ros-mobile-robots · joeuser846 · Mar 15, 2022 · Mar 15, 2022
diff --git a/diffbot_bringup/launch/bringup_all.launch b/diffbot_bringup/launch/bringup_all.launch
@@ -0,0 +1,39 @@
+<launch>
+    <arg name="laser_frame_id" default="rplidar_laser_link" />
+    <!--arg name="model" default="$(env DIFFBOT_MODEL)" doc="model type [diffbot, remo]"/-->
+    <arg name="model" default="diffbot" doc="model type [diffbot, remo]"/>
+
+    <!-- The `minimal.launch` is used to load the DiffBot's robot descripton
+        and the controller configuration onto the ROS parameter server using the launch file from the
+        `diffbot_base` package (https://fjp.at/projects/diffbot/ros-packages/base/).
+        It will also setup the ROS controller manager (http://wiki.ros.org/controller_manager) with
+        DiffBot's hardware interface https://fjp.at/projects/diffbot/ros-packages/base/#hardware-inte>
+        For the motor driver the node `motor_driver.py` from the `grove_motor_driver` package is star>
+        And for the encoders rosserial communicates with the Teensy microcontroller to publish the en>
+    -->
+    <include file="$(find diffbot_bringup)/launch/minimal.launch">
+        <arg name="model" value="$(arg model)" />
+    </include>
+
+    <!-- Starting SLAMTEC RPLidar A1/A2 -->
+    <include file="$(find rplidar_ros)/launch/rplidar.launch" ns="diffbot">
+        <arg name="laser_frame_id" value="$(arg laser_frame_id)" />
+    </include>
+
+    <!-- Starting Adafruit BNO055 IMU -->
+    <include file="$(find imu_bno055)/launch/imu.launch" ns="diffbot">
+    </include>
+
+    <!-- Starting robot_localization to fuse wheel encoder and IMU sensors -->
+    <include file="$(find robot_localization)/launch/ekf_template.launch" ns="diffbot">
+    </include>
+
+    <!-- Starting robot state publish which will publish tf -->
+    <!-- This is needed to publish transforms between all links -->
+    <!-- diff_drive_controller publishes only a single transform between odom and base_footprint -->
+    <!-- The robot_state_publisher reads the joint states published by ros control's joint_state_controller -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
+        output="screen" ns="diffbot" />
+</launch>
+
+
diff --git a/diffbot_bringup/launch/bringup_with_laser_imu.launch b/diffbot_bringup/launch/bringup_with_laser_imu.launch
@@ -0,0 +1,35 @@
+<launch>
+    <arg name="laser_frame_id" default="rplidar_laser_link" />
+    <!--arg name="model" default="$(env DIFFBOT_MODEL)" doc="model type [diffbot, remo]"/-->
+    <arg name="model" default="diffbot" doc="model type [diffbot, remo]"/>
+
+    <!-- The `minimal.launch` is used to load the DiffBot's robot descripton
+        and the controller configuration onto the ROS parameter server using the launch file from the
+        `diffbot_base` package (https://fjp.at/projects/diffbot/ros-packages/base/).
+        It will also setup the ROS controller manager (http://wiki.ros.org/controller_manager) with
+        DiffBot's hardware interface https://fjp.at/projects/diffbot/ros-packages/base/#hardware-inte>
+        For the motor driver the node `motor_driver.py` from the `grove_motor_driver` package is star>
+        And for the encoders rosserial communicates with the Teensy microcontroller to publish the en>
+    -->
+    <include file="$(find diffbot_bringup)/launch/minimal.launch">
+        <arg name="model" value="$(arg model)" />
+    </include>
+
+    <!-- Starting robot state publish which will publish tf -->
+    <!-- This is needed to publish transforms between all links -->
+    <!-- diff_drive_controller publishes only a single transfrom between odom and base_footprint -->
+    <!-- The robot_state_publisher reads the joint states published by ros control's joint_state_controller -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
+        output="screen" ns="diffbot" />
+
+    <!-- Starting SLAMTEC RPLidar A1/A2 -->
+    <include file="$(find rplidar_ros)/launch/rplidar.launch" ns="diffbot">
+        <arg name="laser_frame_id" value="$(arg laser_frame_id)" />
+    </include>
+
+    <!-- Starting Adafruit BNO055 IMU -->
+    <include file="$(find imu_bno055)/launch/imu.launch" ns="diffbot">
+    </include>
+</launch>
+
+
diff --git 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:

diff --git a/diffbot_description/urdf/diffbot.urdf.xacro b/diffbot_description/urdf/diffbot.urdf.xacro
@@ -10,19 +10,19 @@
   <xacro:include filename="$(find ${package_name})/urdf/include/robot.gazebo.xacro" />
 
   <xacro:property name="caster_wheel_yaml" value="$(find ${package_name})/config/${robot_name}/caster_wheel.yaml" />
-  <xacro:property name="caster_wheel_props" value="${xacro.load_yaml(caster_wheel_yaml)}"/>
+  <xacro:property name="caster_wheel_props" value="${load_yaml(caster_wheel_yaml)}"/>
 
   <xacro:property name="front_wheel_yaml" value="$(find ${package_name})/config/${robot_name}/front_wheel.yaml" />
-  <xacro:property name="front_wheel_props" value="${xacro.load_yaml(front_wheel_yaml)}"/>
+  <xacro:property name="front_wheel_props" value="${load_yaml(front_wheel_yaml)}"/>
 
   <xacro:property name="motor_yaml" value="$(find ${package_name})/config/${robot_name}/motor.yaml" />
-  <xacro:property name="motor_props" value="${xacro.load_yaml(motor_yaml)}"/>
+  <xacro:property name="motor_props" value="${load_yaml(motor_yaml)}"/>
 
   <xacro:property name="base_yaml" value="$(find ${package_name})/config/${robot_name}/base.yaml" />
-  <xacro:property name="base_props" value="${xacro.load_yaml(base_yaml)}"/>
+  <xacro:property name="base_props" value="${load_yaml(base_yaml)}"/>
 
   <xacro:property name="sensor_yaml" value="$(find ${package_name})/config/${robot_name}/sensors.yaml" />
-  <xacro:property name="sensor_prop" value="${xacro.load_yaml(sensor_yaml)}"/>
+  <xacro:property name="sensor_prop" value="${load_yaml(sensor_yaml)}"/>
 
 
   <!-- Footprint link -->
@@ -71,4 +71,15 @@
   <xacro:gazebo_ros_control robotNamespace="/diffbot">
   </xacro:gazebo_ros_control>
 
+
+  <joint name="imu_sensor_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="imu_sensor_link"/>
+    <origin xyz="0.0 -0.01 0.00"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+
+  <link name="imu_sensor_link">
+  </link>
+
 </robot>
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -0,0 +1,11 @@
+<launch>
+  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
+    <rosparam command="load" file="$(find robot_localization)/params/ekf_template.yaml" />
+
+    <!--  Placeholder for output topic remapping
+    <remap from="odometry/filtered" to=""/>
+    <remap from="accel/filtered" to=""/>
+    -->
+
+  </node>
+</launch>