[ros2] Add ackermann drive plugin

[shiveshkhaitan]

Added ackermann drive plugin for ROS2. It subscribe to geometry_msgs/Twist for commands.

Example usage:

<plugin name="gazebo_ros_ackermann_drive" filename="libgazebo_ros_ackermann_drive.so">
  <ros>
    <namespace>demo</namespace>
    <argument>cmd_vel:=cmd_demo</argument>
    <argument>odom:=odom_demo</argument>
    <argument>distance:=distance_demo</argument>
  </ros>

  <update_rate>100.0</update_rate>

  <!-- wheels -->
  <front_left_joint>front_left_wheel_joint</front_left_joint>
  <front_right_joint>front_right_wheel_joint</front_right_joint>
  <rear_left_joint>rear_left_wheel_joint</rear_left_joint>
  <rear_right_joint>rear_right_wheel_joint</rear_right_joint>
  <left_steering_joint>front_left_steer_joint</left_steering_joint>
  <right_steering_joint>front_right_steer_joint</right_steering_joint>
  <steering_wheel_joint>steering_joint</steering_wheel_joint>

  <!-- Max absolute steer angle for tyre in radians-->
  <max_steer>0.6458</max_steer>
  <!-- Max absolute steering angle of steering wheel -->
  <max_steering_angle>7.85</max_steering_angle>
  <!-- Max absolute linear speed in m/s -->
  <max_speed>20</max_speed>

  <odometry_frame>odom_demo</odometry_frame>
  <robot_base_frame>chassis</robot_base_frame>
</plugin>

[chapulina]

I did a first pass, and the Prius is drivable 👍

Besides my other comments, I also noticed that there's a problem with the front wheels TF:

[shiveshkhaitan]

Besides my other comments, I also noticed that there's a problem with the front wheels TF:

Isn't it weird? The car seems to be fine in gazebo. Any idea what could be the problem?

[chapulina]

@shiveshkhaitan , mind fixing conflicts?

[chapulina]

The car seems to be fine in gazebo. Any idea what could be the problem?

With the latest commits, the front wheels look and steering wheel look good to me, but the rear wheels are not showing.

This is printing some wrong info:

gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp

Lines 397 to 398 in 2484790

	"Publishing wheel transforms between [%s], [%s] and [%s]",
	impl_->robot_base_frame_.c_str(), joint->GetName().c_str(), joint->GetName().c_str());

More specifically:

[INFO] [demo.ackermann_drive]: Publishing wheel transforms between [chassis], [rear_right_wheel_joint] and [rear_right_wheel_joint]
[INFO] [demo.ackermann_drive]: Publishing wheel transforms between [chassis], [rear_left_wheel_joint] and [rear_left_wheel_joint]

Inspecting the TF tree on RViz, we can see that the rear wheels are disconnected from the rest of the tree. The parent of the rear wheels is the rear_axle, which is the child of chassis. We either need:

• To publish transforms from rear_axle to chassis
• To calculate transform from rear_* wheels to chassis and publish that.

I think the 2nd option is better to be generalized for other kinematic structures which don't have an intermediate link like rear_axle.

[shiveshkhaitan]

To calculate transform from rear_* wheels to `chassis and publish that.

Should this be published only for the demo? Because if we integrate into the code, then it won't generalise again

[chapulina]

The user chooses the base link (robot_base_frame) and the joints. So if we want to be general, we can provide the transform from that link to each joint. In some cases, that link will be the immediate parent of the joints. But in other cases, like this demo, there will be links in between.

So what we can do is get the parent until we find one which matches the base link name. Here:

gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp

Line 557 in 2484790

auto pose_wheel = joint->GetChild()->RelativePose();

Instead of getting just the relative pose from the joint to its child link, we need to accumulate the relative poses of all joints and links until we find the base link. I think you'll need to use Joint::GetParent and Link::GetParentJointsLinks.

Or we can just, for now, not publish the wheel transforms in case the joint parent's name is different from robot_base_frame.

[chapulina]

@osrf-jenkins run tests please

[chapulina]

Great addition, LGTM!

[chapulina]

I just realized that I merged this without checking CI and the test is failing sometimes. Can you take a look, @shiveshkhaitan ?