reset /joint_states and gazebo plugins state in gazebo

[jacknlliu]

We can't reset /joint_states after resetting gazebo world online, and this will cause the joint_state_controller and joint_trajectory_controller fail.

And stop/start gazebo ros controller manager help nothing!

And /joint_states will provide wrong data which is very large and exceeds the joint limit after the robot has a collision with an object in gazebo. And it will keep this wrong data, and it will not back to normal.

/joint_states:

header: 
  seq: 14205682
  stamp: 
    secs: 71085
    nsecs: 101000000
  frame_id: ''
name: ['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
position: [-8.51584976541768, -22393.868370150947, -23210.903847882473, -23059.99195414233, 7.4320796024417355, 9.18637489720941]
velocity: [0.00021875832846863282, 0.00585327683488699, 0.003416905486500389, -0.0001427822974524656, 6.232113829809081e-05, 1.6280227906504893e-06]
effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

We also find that the large joint data may change directly from very large negative number to very large positive number.

Possible related issues:

• Joint::SetPosition does not set the position correctly for some position ranges on HingeJoint
• LinkState stores angular velocities as Quaternions, so they overflow when higher than pi rad/s

Reference

• /joint_states return error data when robot collision with object in gazebo ros-controls/ros_controllers#313
• /joint_states keep providing wrong data after robot collision with some object in Gazebo ros-simulation/gazebo_ros_pkgs#682
• spawn_model -J initial joint positions not working ros-simulation/gazebo_ros_pkgs#93 (comment)
• smart_grasping_sandbox reset world
• How can I make a robot joints and links reset completely in gazebo from python?
• calling /gazebo/reset_simulation seems to break plugins.  ros-simulation/gazebo_ros_pkgs#169
• Force torque sensor stops publishing
• Multiple controller_manager spawn calls freeze a controller manager #14
• Properly restarting the gazebo simulation for iterative learning

[jacknlliu]

This issue may be related to gazebo, gazebo_ros_control, joint_state_controller , joint_trajectory_controller and ros controller manager.

We find gazebo_ros_control get the joint_state_ from gazebo is wrong using this transform, but gazebo plugin gazebo_ros_joint_state_publisher get the reasonable joint state using origin gazebo API, see here.