Multiple publishers for joint_state cause RViz to flicker
Hi all, I have a car simulation in ROS Melodic
with 4 wheels and a joint on top of it and I am getting behavior that I don't know how to solve even after searching that in the forum.
The wheels are actuated according to the following plugin
<!-- Add Skid Steer Plugin -->
<gazebo>
<plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
<updateRate>100.0</updateRate>
<robotNamespace>/my_car</robotNamespace>
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<covariance_x>0.0001</covariance_x>
<covariance_y>0.0001</covariance_y>
<covariance_yaw>0.0001</covariance_yaw>
<leftFrontJoint>base_wheel_front_left</leftFrontJoint>
<rightFrontJoint>base_wheel_front_right</rightFrontJoint>
<leftRearJoint>base_wheel_rear_left</leftRearJoint>
<rightRearJoint>base_wheel_rear_right</rightRearJoint>
<wheelSeparation>2.11</wheelSeparation>
<wheelDiameter>0.71</wheelDiameter>
<robotBaseFrame>base_footprint</robotBaseFrame>
<torque>1000000</torque>
<topicName>cmd_vel</topicName>
<broadcastTF>false</broadcastTF>
</plugin>
</gazebo>
While the joints on top of the car are actuated using the transmission element like this one:
<transmission name="chassi_boom_transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="base_mount_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="chassi_boom_actuator">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
The problem is... Whenever I spawn the joint_state_publisher, both gazebo and joint_state_publisher publish to the topic joint_states
. However, the gazebo
never updates its joint_state
causing my simulation model to flicker.
$ rostopic info /my_car/joint_states
Type: sensor_msgs/JointState
Publishers:
* /gazebo (http://ps-imr:42743/)
* /joint_state_publisher (http://ps-imr:38597/)
Subscribers:
* /robot_state_publisher (http://ps-imr:38075/)
I currently have one launch file to start the simulation and another one to start the ros_control layer.
part of My_car.launch:
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<remap from="/joint_states" to="/my_car/joint_states" />
</node>
<!-- Spawn the example robot -->
<arg name="my_car_model" default="tesla" />
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find my_car_description)/urdf/tesla_my_car.urdf.xacro' my_car_model:=$(arg my_car_model)" />
<node pkg="gazebo_ros" type="spawn_model" name="spawn_model" args="-urdf -param /robot_description -model my_car" />
<!-- Publish TFs -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="50.0" />
<remap from="/joint_states" to="/my_car/joint_states" />
</node>
And that is part of my control.launch
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find belt_loader_control)/config/belt_loader_control.yaml" command="load" />
<!-- load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/my_car" args="my_car_controller flex_ramp_controller joint_state_controller" />
Any thoughts about why that could be happening?
why do you do this?
because otherwise I won't see the wheels in place in RViz. Even though I am still able to move it by publishing a message in
/cmd_vel
I mean ... they are not event shown in my Robot Model. It states
No transform from [wheel_front_left] to [base_footprint]
The JSP will publish
JointState
messages for alljoint
s in your URDF. So starting the JSP in addition to Gazebo will cause conflicting messages to be published on/joint_states
. That's probably the reason you see "flickering" (which is really just two different states being visualised in rapid succession by RViz).