This is an EKF base filetr implementation adpted to floating base system and then update to be use in ROS2 Framework and add some module to manage wheeled system.
The system offer a span of module to manage different sensors subscribing to different topics. The available list of module contains:
- ins: inertial navigation system(IMU)
- legodo: legged odometry
- bias_lock: bias updater when sytem is standing
- scan_matcher: lidar scan matcher
- qualysis_mt: qualysis motion tracker
- pose_topic: Estimated Pose topic name
- pose_frame_id: pose tf frame name
- twist_topic: Estimated Twist topic name
- publish_pose: Bool if true publish pose
- publish_tf: Bool if true publish tf
- tf_child_frame_id
- republish_sensors
- init_sensors: sensor list used to init the filter
- active_sensors: sensor list used to update the estimation
- utime_history_span: max update temporal span dimension
- sigma0: init covariance
- x0: init state
- topic: module subcrition topic name
- roll_forward_on_receive: Bool if true update the estimation when the message is recieved
- publish_head_on_message: Bool if true publish the estimarion when the message is recieved
- q_gyro: gyro measures covariance
- q_accel: accelerometer measure covariance
- q_gyro_bias: gyro bias covariance
- q_accel_bias: accelerometer bias covariance
- num_to_init: message number needed to initialize ins module
- gyro/accel_bias_initial: bias initial value
- gyro/accel_bias_recalc_at_start: Bool if true recalc bias during the module initialization
- gyro/accel_bias_update_online: Bool if true recalc bias during the module initialization
- frame: imu tf frame name
- base_frame: base tf frame name
- legodo_mode: it desctibe the covariance computation mode between: STATIC_SIGMA, VAR_SIGMA,IMPACT_SIGMA, WEIGHTED_AVG and ALPHA_FILTER
- stance_mode: it describe the contact detection mode between: THRESHOLD, HYSTERESIS, and REGRESSION
- stance_threshold: threshold value
- stance_hysteresis_low
- stance_hysteresis_high
- stance_hysteresis_delay_low
- stance_hysteresis_delay_high
- stance_alpha
- stance_regression_beta_size
- stance_regression_beta
- r_vx: init covariance in x direction
- r_vy: init covariance in y direction
- r_vz: init covariance in z direction
- sim: Bool if true the robot use the sensor_msgs otherwise it use pi3hat_msgs
- torque_threshold: minimum torque on knee to be in contact with the ground
- velocity_threshold: maximum velocity to consider the robot standing
- secondary_topic: joint state topic name
- sim: Bool if true the robot use the sensor_msgs otherwise it use pi3hat_msgs
- mode: it describe the correction mode between position, yaw and position with yaw
- r_yaw: yaw covariance
- r_pxy: position covariance
- mode: it describe the correction mode between linear velocity, agular_velocity and both
- r_linear: velocity covariance
- r_chi: angular covariance
- robot_name: qualisys rigid body name
- r_xyz position covariance
- r_chi: orientation covariance
- mode: it describe the correction mode between position, yaw and position with yaw, orientation and position with orientation
The estimator node can be start in this way:
ros2 launch pronto_ros2_node pronto_node.launch.py xacro_pkg:=<robot_description_package> xacro_name:=<xacro_file_name> config_name:=<configuration_file name>
The config file must be add to the config folder in pronto_ros2_node package.
The Benchmarking launch allows the user to start a list of pronto instance to compare the obtained estimations. The input is a bag cointains all the filter measures input, or at least it should be consistent with the configuration files. The configuration files has to be placed into the config folder and named omnicar_tune_i.yaml, each file is associated with a pronto instace. The launch file cointain the global variable to set the bag name and the list of instance number, subset of the configuration files' list. To start the filters the command line is:
ros2 launch pronto_ros2_node bench_pronto.launch.py
Eigen3