Problem with Create 3 Simulator - Galactic

[AlHussain00]

Hello,

I am facing an issue with Create3 simulator. There exist inconsistency between rviz and gazebo when using the APIs to drive the robot to a certain goal point. In order to generate the problem, the following commands are used:

• ros2 action send_goal /undock irobot_create_msgs/action/Undock "{}"
• ros2 action send_goal /navigate_to_position irobot_create_msgs/action/NavigateToPosition "{achieve_goal_heading: true,goal_pose:{pose:{position:{x: 2,y: 2,z: 0.0}, orientation:{x: 0.0,y: 0.0, z: 0.0, w: 1.0}}}}"
• ros2 action send_goal /navigate_to_position irobot_create_msgs/action/NavigateToPosition "{achieve_goal_heading: true,goal_pose:{pose:{position:{x: 1,y: 2,z: 0.0}, orientation:{x: 0.0,y: 0.0, z: 0.0, w: 1.0}}}}"
• ros2 action send_goal /navigate_to_position irobot_create_msgs/action/NavigateToPosition "{achieve_goal_heading: true,goal_pose:{pose:{position:{x: 3,y: 3,z: 0.0}, orientation:{x: 0.0,y: 0.0, z: 0.0, w: 1.0}}}}"

The robot's position in rviz is accurate and has no issue at all, however, this is not the case in gazebo as shown in the attached image (gazebo is on the left, and rviz is on the right). The drift between gazebo and rviz keeps on getting bigger if more driving commands are used. I know that gazebo represents the ground truth, while rviz uses the data published on the topics. Also something like this can happen due to the slippage of the wheels. However, according to Create3 documentation, the pose is estimated based on dead reckoning estimation, hence, I believe this should not happen (at least for this simple task).

I really could not find an explanation for this. Am I misunderstanding something?

I am using ROS2 Galactic and Ubuntu 20.04

Thank you in advance.

[justinIRBT]

The odometry coming from the sim is produced by:

<xacro:if value="${'$(arg gazebo)' == 'classic'}">
    <gazebo>
      <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
        <parameters> $(find irobot_create_control)/config/control.yaml </parameters>
        <ros>
          <remapping>~/odom:=odom</remapping>
        </ros>
      </plugin>
    </gazebo>
  </xacro:if>

  <xacro:if value="${'$(arg gazebo)' == 'ignition'}">
    <gazebo>
      <plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
        <parameters> $(find irobot_create_control)/config/control.yaml </parameters>
        <ros>
          <remapping>~/odom:=odom</remapping>
        </ros>
      </plugin>
    </gazebo>
  </xacro:if>

I believe these libraries will have noise in the odometry estimate. This noise is actually probably worse than the real robot /odom which fuses an optical flow sensor (mouse) and gyro along with the wheel odometry into the final reported odometry. The Control libraries used to produce odom in the sim just use the wheels.

There is separately a ground truth topic in the sim sim_ground_truth_pose, this is produced by:

<gazebo>
      <plugin name="gazebo_ros_p3d_robot" filename="libgazebo_ros_p3d.so">
        <ros>
          <namespace>/</namespace>
          <remapping>odom:=sim_ground_truth_pose</remapping>
        </ros>
        <body_name>base_link</body_name>
        <frame_name>world</frame_name>
        <update_rate>62</update_rate>
        <xyz_offset>0 0 0</xyz_offset>
        <rpy_offset>0.0 0.0 0.0</rpy_offset>
        <gaussian_noise>0.0</gaussian_noise>
      </plugin>
    </gazebo>

I got all these code snippets from irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro if you want to look into the libraries that are generating the various topics.

[AlHussain00]

Thank you for your reply.

I think everything is now clear. I just have 2 questions so far.

1. Is it possible to control the amount of noise in the sim using the control.yaml file?
2. Is there a flag in the sim, which can be used to select the option of either using the wheels as a source of odometry or fuse the sensors' data (to make the sim as close as possible to reality)?

Regards,

[justinIRBT]

1. That file configures the node used from the library. You can see the source of the library here to see if there is anything to tweak https://github.com/ros-controls/ros2_controllers/blob/humble/diff_drive_controller/src/diff_drive_controller.cpp
2. The sim does not model the wheel+gyro+mouse odometry. You could write a node that subscribes to the ground truth, takes motion deltas, and adds a small amount of noise to it if you want a tighter model.

[AlHussain00]

Thank you so much