My Code:
- In main()
ros::Subscriber sub0 = nh.subscribe("panda_link0_contact_sensor_state", 1000, bumperCallback);
- In bumperCallback()
void bumperCallback(const gazebo_msgs::ContactsStateConstPtr& cs)
{
ROS_INFO("Header:");
//uint32 seq
//time stamp
//string frame_id
ROS_INFO_STREAM(cs->header);
ROS_INFO("states:");
ROS_INFO_STREAM(cs->states[0].collision1_name);
ROS_INFO_STREAM(cs->states[0].collision2_name);
//ROS_INFO_STREAM(cs->states[0]);
}
- In Model Description file
<gazebo reference="${arm_id}_link0">
<sensor name="${arm_id}_link0_contact_sensor" type="contact">
<visualize>true</visualize>
<always_on>true</always_on>
<update_rate>30.0</update_rate>
<contact>
<geom>panda_link0_geom</geom>
<collision>panda_link0_collision_collision</collision>
</contact>
<plugin name="${arm_id}_link0_bumper" filename="libgazebo_ros_bumper.so">
<bumperTopicName>panda_link0_contact_sensor_state</bumperTopicName>
<frameName>link0</frameName>
</plugin>
</sensor>
<selfCollide>true</selfCollide>
</gazebo>
References
- the discription of bumper from gazebo webseit
http://gazebosim.org/tutorials?tut=ros_gzplugins#collapseOne
<gazebo>
<plugin name="${name}_gazebo_ros_bumper_controller" filename="libgazebo_ros_bumper.so">
<alwaysOn>true</alwaysOn>
<updateRate>${update_rate}</updateRate>
<bumperTopicName>${name}_bumper</bumperTopicName>
<frameName>world</frameName>
</plugin>
</gazebo>
http://docs.ros.org/api/gazebo_msgs/html/msg/ContactsState.html
Header header # stamp
gazebo_msgs/ContactState[] states # array of geom pairs in contact
uint32 seq
time stamp
string frame_id
string info # text info on this contact
string collision1_name # name of contact collision1
string collision2_name # name of contact collision2
geometry_msgs/Wrench[] wrenches # list of forces/torques
geometry_msgs/Wrench total_wrench # sum of forces/torques in every DOF
geometry_msgs/Vector3[] contact_positions # list of contact position
geometry_msgs/Vector3[] contact_normals # list of contact normals
float64[] depths # list of penetration depths
- Super good for learning Gazebo in franka robot!!!
- https://answers.ros.org/question/29158/how-do-i-use-force-sensor-bumper-sensor-in-gazebo/
- https://answers.ros.org/question/244380/how-to-subscribe-to-a-topic-whom-type-is-gazebo_msgcontactstate/
- Here I found the “selfCollide” tag!!!
- About the priority of the tag in SDF
- About C++ in ROS, I found this code is very important, because without it the system always stop and will not run the continuous subsribers. In Terminal will show that:
Segmentation fault (core dumped)
So the following code can jump the uncollided link.
if (0 == contacts->states.size()) { return; }
rqt_plot
At last, I should plot the There is a topic /joint_states, I can use
$ rostopic echo /joint_states
to know the order of my joints. (Reference)
So I can plot the position, velocity and effort by:
- joint (what we want to run)
rqt_plot /franka/joint1_position_controller/command /franka/joint2_position_controller/command /franka/joint3_position_controller/command /franka/joint4_position_controller/command /franka/joint5_position_controller/command /franka/joint6_position_controller/command /franka/joint7_position_controller/command
or (what we actually run in simulator)
rqt_plot /joint_states/position[2] /joint_states/position[3] /joint_states/position[4] /joint_states/position[5] /joint_states/position[6] /joint_states/position[7] /joint_states/position[8]
- velocity
rqt_plot /joint_states/velocity[2] /joint_states/velocity[3] /joint_states/velocity[4] /joint_states/velocity[5] /joint_states/velocity[6] /joint_states/velocity[7] /joint_states/velocity[8]
- effort
rqt_plot /joint_states/effort[2] /joint_states/effort[3] /joint_states/effort[4] /joint_states/effort[5] /joint_states/effort[6] /joint_states/effort[7] /joint_states/effort[8]
PS.
/joint_states is the feedback from Gazebo simulator, which is what robot actually run in simulation environment.