Can you suggest the Gazebo plugin for the Mecanum wheel robot ? Because planar move and ros force based move don't work for me
Hi, I am quite new to ROS and Gazebo. I have try the planar move plugin for my Mecanum robot. It seem to be good except that the this plugin override physics. That means even I change mass, inertia, friction in URDF of my robot. There is no effect. When I move robot 1000 kg or 1 kg it yield the same results. Therefore I use ros force based plugin for my multiple Mecanum robots instead of the planar move. refer to https://answers.ros.org/question/212889/gazebo-planar-move-plugin-for-omni-directional-wheel/
You can check the rosforcebased_move plugin here
https://github.com/tu-darmstadt-ros-pkg/hector_gazebo/tree/kinetic-devel/hector_gazebo_plugins/src
However, I still found a problem here.
- When I spawn the robot, there are always offset of robot position in my map and also the yaw direction offset for 90 degrees. Hence, I have to edit the arguments for static transform node in order to solve this problem. Here are some parts of my launch file to call a single robot with the namespace.
<node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 /map $(arg robot_name)/odom 10" />
Note that when I set to 0 0 0 0 0 0 the robot will offset in Rviz when compare to the Gazebo So I have to change the offset here. But when I use the original planar move there is no problem.
- When I move robot sideways in the y-axis, the odometry in Rviz does not change even though in gazebo it has change position already. For move straight and backward in X-axis, there is no problem.
Here is the part of my URDF file to call the plugin in
<gazebo>
<plugin name="ros_force_based_controller" filename="libgazebo_ros_force_based_move.so">
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<odometryRate>100.0</odometryRate>
<robotBaseFrame>base_link</robotBaseFrame>
<publishOdometryTf>true</publishOdometryTf>
<yaw_velocity_p_gain>500</yaw_velocity_p_gain>
<x_velocity_p_gain>1000</x_velocity_p_gain>
<y_velocity_p_gain>1000</y_velocity_p_gain>
<cmdVelTimeOut>0.25</cmdVelTimeOut>
</plugin>
</gazebo>
Finally, I have studied from ridgeback robot simulation from Clearpath robotics. It seems that they modify the ros force based move to not publish odometryTF. But they use this information from "gazeboroscontrol" instead. This simulation is work for me. But I can't migrate code to make multiple Mecanum robots. So I still looking for the best solution plugin for my multiple mecanum robot.
The link of Ridgeback robot URDF is here
Asked by kamoonty on 2019-07-14 05:51:37 UTC
Answers
Hi,
I may be a bit late to answer but concerning the problem of measuring the odometry on the y-axis, I think it's because the implementation of the force_move_based_plugin simply ignores it, as we can see in the function computing odometry:
tf::Transform GazeboRosForceBasedMove::getTransformForMotion(double linear_vel_x, double angular_vel, double timeSeconds) const
I think a solution is to reimplement this function correctly by taking the transversal axis into account. I tried to replace it with the following and it looks like it works now but I still have to test it more thoroughly:
tf::Transform GazeboRosForceBasedMove::getTransformForMotion(double linear_vel_x, double linear_vel_y, double angular_vel, double timeSeconds) const
{
tf::Transform tmp;
tmp.setIdentity();
const double linear_x = linear_vel_x * timeSeconds;
const double linear_y = linear_vel_y * timeSeconds;
const double angular = angular_vel * timeSeconds;
if (std::abs(angular_vel) < 0.0001) {
tmp.setOrigin(tf::Vector3(linear_x, 0.0, 0.0));
} else {
const double delta_x = linear_x*cos(angular) - linear_y*sin(angular);
const double delta_y = linear_x*sin(angular) + linear_y*cos(angular);
tmp.setOrigin(tf::Vector3(delta_x, delta_y, 0.0));
tmp.setRotation(tf::createQuaternionFromYaw(angular));
}
return tmp;
}
Asked by gurbain on 2020-04-01 04:42:10 UTC
Comments