From 158510d0339f2bde75658e9e342057966e39e6e3 Mon Sep 17 00:00:00 2001 From: Joe User Date: Mon, 14 Mar 2022 21:17:03 -0400 Subject: [PATCH 1/2] Changes related to sbc BNO055 IMU --- diffbot_bringup/launch/bringup_all.launch | 39 ++++ .../launch/bringup_with_laser_imu.launch | 35 ++++ diffbot_description/urdf/diffbot.urdf.xacro | 21 +- .../config/base_local_planner_params.yaml | 4 +- .../config/costmap_common_params.yaml | 2 +- .../config/costmap_global_params.yaml | 6 +- .../config/costmap_local_params.yaml | 4 +- .../config/dwa_local_planner_params.yaml | 4 +- robot_localization/launch/ekf_template.launch | 11 ++ robot_localization/params/ekf_template.yaml | 179 ++++++++++++++++++ ros-imu-bno055/launch/imu.launch | 10 + 11 files changed, 300 insertions(+), 15 deletions(-) create mode 100644 diffbot_bringup/launch/bringup_all.launch create mode 100644 diffbot_bringup/launch/bringup_with_laser_imu.launch create mode 100644 robot_localization/launch/ekf_template.launch create mode 100644 robot_localization/params/ekf_template.yaml create mode 100644 ros-imu-bno055/launch/imu.launch diff --git a/diffbot_bringup/launch/bringup_all.launch b/diffbot_bringup/launch/bringup_all.launch new file mode 100644 index 00000000..b11321f1 --- /dev/null +++ b/diffbot_bringup/launch/bringup_all.launch @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/diffbot_bringup/launch/bringup_with_laser_imu.launch b/diffbot_bringup/launch/bringup_with_laser_imu.launch new file mode 100644 index 00000000..0f3aa4b6 --- /dev/null +++ b/diffbot_bringup/launch/bringup_with_laser_imu.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/diffbot_description/urdf/diffbot.urdf.xacro b/diffbot_description/urdf/diffbot.urdf.xacro index ba208316..937b7491 100644 --- a/diffbot_description/urdf/diffbot.urdf.xacro +++ b/diffbot_description/urdf/diffbot.urdf.xacro @@ -10,19 +10,19 @@ - + - + - + - + - + @@ -71,4 +71,15 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/diffbot_navigation/config/base_local_planner_params.yaml b/diffbot_navigation/config/base_local_planner_params.yaml index 58b91340..993eb7b9 100644 --- a/diffbot_navigation/config/base_local_planner_params.yaml +++ b/diffbot_navigation/config/base_local_planner_params.yaml @@ -14,8 +14,8 @@ TrajectoryPlannerROS: acc_lim_theta: 0.6 # Goal Tolerance Parameters - xy_goal_tolerance: 0.10 - yaw_goal_tolerance: 0.05 + xy_goal_tolerance: 0.2 + yaw_goal_tolerance: 0.75 # Differential-drive robot configuration holonomic_robot: false diff --git a/diffbot_navigation/config/costmap_common_params.yaml b/diffbot_navigation/config/costmap_common_params.yaml index f9cb83f4..5475ee55 100644 --- a/diffbot_navigation/config/costmap_common_params.yaml +++ b/diffbot_navigation/config/costmap_common_params.yaml @@ -10,7 +10,7 @@ raytrace_range: 3.5 # [ [x1, y1], [x2, y2], ...., [xn, yn] ]. The footprint specification assumes the center point of the robot is at # (0.0, 0.0) in the robot_base_frame and that the points are specified in meters, # both clockwise and counter-clockwise orderings of points are supported. -footprint: [[-0.08, -0.075], [-0.08, 0.075], [0.105, 0.075], [0.105, -0.075]] +footprint: [[-0.05, -0.05], [-0.05, 0.05], [0.05, 0.05], [0.05, -0.05]] #robot_radius: 0.17 # The radius in meters to which the map inflates obstacle cost values. diff --git a/diffbot_navigation/config/costmap_global_params.yaml b/diffbot_navigation/config/costmap_global_params.yaml index f4ed4c60..11555ebc 100644 --- a/diffbot_navigation/config/costmap_global_params.yaml +++ b/diffbot_navigation/config/costmap_global_params.yaml @@ -6,9 +6,9 @@ global_costmap: robot_base_frame: base_footprint # The frequency in Hz for the map to be updated. - update_frequency: 10.0 + update_frequency: 10.0 # The frequency in Hz for the map to publish display information. - publish_frequency: 10.0 + publish_frequency: 30.0 # Specifies the delay in transform (tf) data that is tolerable in seconds. # This parameter serves as a safeguard to losing a link in the tf tree while # still allowing an amount of latency the user is comfortable with to exist in the system. @@ -17,7 +17,7 @@ global_costmap: # If the tf transform between the coordinate frames specified by the global_frame and # robot_base_frame parameters is transform_tolerance seconds older than ros::Time::now(), # then the navigation stack will stop the robot. - transform_tolerance: 0.5 + transform_tolerance: 1.0 # Whether or not to use the static map to initialize the costmap. # If the rolling_window parameter is set to true, this parameter must be set to false. diff --git a/diffbot_navigation/config/costmap_local_params.yaml b/diffbot_navigation/config/costmap_local_params.yaml index e43a51b4..a5c355ea 100644 --- a/diffbot_navigation/config/costmap_local_params.yaml +++ b/diffbot_navigation/config/costmap_local_params.yaml @@ -8,7 +8,7 @@ local_costmap: # The frequency in Hz for the map to be updated. update_frequency: 10.0 # The frequency in Hz for the map to be publish display information. - publish_frequency: 10.0 + publish_frequency: 30.0 # Specifies the delay in transform (tf) data that is tolerable in seconds. # This parameter serves as a safeguard to losing a link in the tf tree while # still allowing an amount of latency the user is comfortable with to exist in the system. @@ -17,7 +17,7 @@ local_costmap: # If the tf transform between the coordinate frames specified by the global_frame and # robot_base_frame parameters is transform_tolerance seconds older than ros::Time::now(), # then the navigation stack will stop the robot. - transform_tolerance: 0.5 + transform_tolerance: 1.0 # Whether or not to use the static map to initialize the costmap. # If the rolling_window parameter is set to true, this parameter must be set to false. diff --git a/diffbot_navigation/config/dwa_local_planner_params.yaml b/diffbot_navigation/config/dwa_local_planner_params.yaml index 2542fca2..b1b0e6ad 100644 --- a/diffbot_navigation/config/dwa_local_planner_params.yaml +++ b/diffbot_navigation/config/dwa_local_planner_params.yaml @@ -19,8 +19,8 @@ DWAPlannerROS: acc_lim_theta: 3.2 # Goal Tolerance Parametes - xy_goal_tolerance: 0.1 - yaw_goal_tolerance: 0.5 + xy_goal_tolerance: 0.2 + yaw_goal_tolerance: 0.75 latch_xy_goal_tolerance: false # Forward Simulation Parameters diff --git a/robot_localization/launch/ekf_template.launch b/robot_localization/launch/ekf_template.launch new file mode 100644 index 00000000..c5c2bbdc --- /dev/null +++ b/robot_localization/launch/ekf_template.launch @@ -0,0 +1,11 @@ + + + + + + + + diff --git a/robot_localization/params/ekf_template.yaml b/robot_localization/params/ekf_template.yaml new file mode 100644 index 00000000..b49b9a67 --- /dev/null +++ b/robot_localization/params/ekf_template.yaml @@ -0,0 +1,179 @@ +# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin +# computation until it receives at least one message from one of the inputs. It will then run continuously at the +# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. +frequency: 30 + +silent_tf_failure: true +# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict +# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the +# filter will generate new output. Defaults to 1 / frequency if not specified. +sensor_timeout: 0.2 + +# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is +# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar +# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected +# by, for example, an IMU. Defaults to false if unspecified. +two_d_mode: true + +# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for +# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if +# unspecified. +transform_time_offset: 0.0 + +# Use this parameter to provide specify how long the tf listener should wait for a transform to become available. +# Defaults to 0.0 if unspecified. +transform_timeout: 0.2 + +# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is +# unhappy with any settings or data. +print_diagnostics: false + +# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by +# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious +# effects on the performance of the node. Defaults to false if unspecified. +debug: false + +# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. +debug_out_file: /path/to/debug/file.txt + +# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. +publish_tf: true + +# Whether to publish the acceleration state. Defaults to false if unspecified. +publish_acceleration: false + +# Whether we'll allow old measurements to cause a re-publication of the updated state +permit_corrected_publication: false + +# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and +# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. +# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be +# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom +# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your +# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based +# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. +# ekf_localization_node and ukf_localization_node are not concerned with the earth frame. +# Here is how to use the following settings: +# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. +# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of +# odom_frame. +# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set +# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. +# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates +# from landmark observations) then: +# 3a. Set your "world_frame" to your map_frame value +# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state +# estimation node from robot_localization! However, that instance should *not* fuse the global data. +map_frame: map # Defaults to "map" if unspecified +odom_frame: odom # Defaults to "odom" if unspecified +base_link_frame: base_footprint # Defaults to "base_link" if unspecified +world_frame: odom # Defaults to the value of odom_frame if unspecified + +# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, +# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, +# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, +# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no +# default values, and must be specified. +odom0: /diffbot/mobile_base_controller/odom # Topic published by base_controller using encoder values + +# Each sensor reading updates some or all of the filter's state. These options give you greater control over which +# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only +# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the +# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types +# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message +# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false +# if unspecified, effectively making this parameter required for each sensor. +odom0_config: [false, false, false, # x, y, z + false, false, false, # roll, pitch, yaw + true, true, false, # vx, vy, vz vx, vy from wheel encoders + false, false, false, # vroll, vpitch, vyaw + false, false, false] # ax, ay, az + + +# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase +# the size of the subscription queue so that more measurements are fused. +odom0_queue_size: 30 + +# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result +# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's +# algorithm. +odom0_nodelay: false + +# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- +# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they +# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also +# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't +# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose +# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then +# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true +# for twist measurements has no effect. +odom0_differential: false + +# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" +# for all future measurements. While you can achieve the same effect with the differential paremeter, the key +# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before +# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. +odom0_relative: true + +# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to +# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to +# numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not +# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. +# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying +# the thresholds. +odom0_pose_rejection_threshold: 5 +odom0_twist_rejection_threshold: 1 + + + +imu0: /diffbot/imu/data # Topic direct from IMU +imu0_config: [false, false, false, # x, y, z + false, false, true, # roll, pitch, yaw + false, false, false, # vx, vy, vz + false, false, true, # vroll, vpitch, vyaw Yaw from IMU + false, false, false] # ax, ay, az + + +imu0_nodelay: false +imu0_differential: false +imu0_relative: true +imu0_queue_size: 10 +imu0_pose_rejection_threshold: 0.5 # Note the difference in parameter names (formerly 0.8) +imu0_twist_rejection_threshold: 0.5 # +imu0_linear_acceleration_rejection_threshold: 0.5 # + +# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set +# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. +imu0_remove_gravitational_acceleration: true + +# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no +# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During +# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be +# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When +# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially +# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance +# for the velocity variable in question, or decrease the variance of the variable in question in the measurement +# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we +# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during +# predicition. Note that if an acceleration measurement for the variable in question is available from one of the +# inputs, the control term will be ignored. + +# Whether or not we use the control input during prediction. Defaults to false. +use_control: false +# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to +# false. +stamped_control: false +# The last issued control command will be used in prediction for this period. Defaults to 0.2. +control_timeout: 0.2 +# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. +control_config: [true, false, false, false, false, true] +# Places limits on how large the acceleration term will be. Should match your robot's kinematics. +acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] +# Acceleration and deceleration limits are not always the same for robots. +deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] +# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these +# gains +acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] +# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these +# gains +deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] diff --git a/ros-imu-bno055/launch/imu.launch b/ros-imu-bno055/launch/imu.launch new file mode 100644 index 00000000..21be550c --- /dev/null +++ b/ros-imu-bno055/launch/imu.launch @@ -0,0 +1,10 @@ + + + + + + + + + + From c63535919536e22e04423149d2b5ec14b0cfcdab Mon Sep 17 00:00:00 2001 From: Joe User Date: Mon, 14 Mar 2022 21:23:43 -0400 Subject: [PATCH 2/2] Suppress base_footprint -> odom transform broadcast --- diffbot_control/config/diffbot_control.yaml | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/diffbot_control/config/diffbot_control.yaml b/diffbot_control/config/diffbot_control.yaml index aa7b3916..88c88358 100644 --- a/diffbot_control/config/diffbot_control.yaml +++ b/diffbot_control/config/diffbot_control.yaml @@ -24,7 +24,7 @@ diffbot: type: diff_drive_controller/DiffDriveController publish_rate: 50 - left_wheel: 'front_left_wheel_joint' + left_wheel: 'front_left_wheel_joint' right_wheel: 'front_right_wheel_joint' # Wheel separation and diameter. These are both optional. @@ -36,11 +36,18 @@ diffbot: # Odometry covariances for the encoder output of the robot. These values should # be tuned to your robot's sample odometry data, but these values are a good place # to start - pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] + pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] # Top level frame (link) of the robot description - base_frame_id: base_footprint + # Don't publish base_footprint->odom transform here it will be done by robot_localization after sensor fusion + # base_frame_id: base_footprint + + # Odometry fused with IMU is published by robot_localization, so + # no need to publish a TF based on encoders alone. + # This was the breakthrough that allowed sensor fusion while using differential_drive_controller + enable_odom_tf: false + # Velocity and acceleration limits for the robot linear: