Mouse sensor output doesn't seem to match up with where the robot is?

[shuhaowu]

Describe the bug When looking at the output of /mouse, it doesn't appear to match what's in /sim_ground_truth_pose at all.

To Reproduce Steps to reproduce the behavior:

1. Launch the empty world demo.
2. ros2 action send_goal /undock irobot_create_msgs/action/Undock "{}"
3. In two terminals: ros2 topic echo /mouse and ros topic echo /sim_ground_truth_pose --field "pose.pose.position"
4. See discrepencies throughout the undocking sequence, with it ending at:

For /mouse:

integrated_x: 0.16436231136322021
integrated_y: -0.28541702032089233 

For /sim_ground_truth_pose:

x: -0.3511189952412123
y: 0.0030276005157999506

Expected behavior Should the integrated_x and integrated_y match? I'm not too familiar with these sensors yet.

[alsora]

In general it's not possible to directly compare the robot position and the mouse reported x, y.

If you look at the TF tree you can see that the mouse sensor has a displament dx, dy with respect to the center of rotation of the robot. This means that when the robot rotates in place, the mouse x, y will be affected by the motion. On the other hand, the robot x, y position is unchanged (only its theta will change).

This is expected to happen when you undock, because the robot does a 180 deg turn.

If you do a different experiment where you start with the robot and no dock, you should see how if you only drive forward /backward with no rotation, then the topics should ageee

[alsora]

In order to use the raw mouse readings to estimate the robot pose, you should take into account the relative position of this sensor on the robot.

[shuhaowu]

If you do a different experiment where you start with the robot and no dock, you should see how if you only drive forward /backward with no rotation, then the topics should ageee

It doesn't agree. That's what motivated me to make this bug issue, but I used the undock action as it was easier to reproduce. But yes I understand fundamentally it should be different, but the difference is quite drastic in the previous cases.

Here's an example of the robot moving ~0.44m only in the forward direction (via a joystick). In this case, there is no dock and the robot starts undocked.

/odom:

y: 1.2091082328241202e-05
z: 0.0

/mouse:

integrated_x: 0.3133462369441986
integrated_y: 0.31339552998542786

In order to use the raw mouse readings to estimate the robot pose, you should take into account the relative position of this sensor on the robot.

Yea in general I think this is true. I looked at the source code for the mouse plugin and came to roughly the same conclusion, but as you can see the discrepancy is very large. Not sure what's going on with it as I don't see any obvious problems with the mouse plugin.

[shuhaowu]

It's also somewhat suspicious that integrated_x appears to almost go at the same rate as integrated_y while moving forward/backward only. Not sure why this is the case. Doesn't quite happen during rotation. It's pretty late here so I won't be able to investigate this until at least tomorrow, tho.

[alsora]

Ok thank you for the detailed report.

According to the urdf, the mouse sensor is rotated of pi/4 with respect to the robot center https://github.com/iRobotEducation/create3_sim/blob/main/irobot_create_description/urdf/create3.urdf.xacro#L102

And 0.31 * sqrt(2) is approximately 0.44.

This should explain the numbers you get while driving forward

[shuhaowu]

Ah that makes a lot more sense. I missed that line and just assumed that the mouse frame is parallel with the base_link frame. So presumably that will be how the real robot work as well? When driving forward the x and y counter will both go up as it is mounted at 45 degrees?

[alsora]

Yes. The values used in the simulation represent the "nominal" location of the sensor. The real create3 robots undergo a calibration procedure in the factory, so the actual values may be slightly different.

Consumers should rely on what is published on the tf topic.

[shuhaowu]

What's published in the tf topic? Is it coming from the wheel odometry as it is in the simulation (from ros2_control)?

[alsora]

The robot publishes on the tf topic the relative transformations between all of its frames. Actually, static transformations are published on tf_static topic. Here you can see the mouse sensor location and also locations of all other sensors.

[shuhaowu]

Ah you mean the transformation between the base and the mouse sensor. That makes sense and is what I would expect. I thought you mean something to do with odometry. Consider this matter resolved. Thanks for all the help!