Question

I am new to Gazebo and ROS. I am recently working on a simulation using Gazebo 2 in ROS Indigo. The topic of my research is robot controlling, so it is critical to know whether my commands are appropriate.

Sometimes, a bad command may cause the robot (which is Pioneer3AT in my simulation) to turn over, and that's a situation I need to reset the simulation.

I want to know if there is a way to let me know whether the robot turns over or is about to turn over (big angle with respect to the ground). I note that there is a topic named /gazebo/model_states which shows positions and orientations of the models in the simulation. However, orientations are in Quaternion format, which cannot show the angles robot has rotated in each of the three axises.

Anyone can help?

Asked by yangzhaoyang6 on 2016-11-23 19:52:43 UTC

Answer 1

You can easily convert to Euler angle from quaternion value using something like this:

geometry_msgs::Vector3 rpy_data;
tf::Quaternion tmp_quat;
tf::quaternionMsgToTF(imu_msg.orientation, tmp_quat);
tf::Matrix3x3(tmp_quat).getRPY(rpy_data.x, rpy_data.y, rpy_data.z);

Then you can have some logic to detect when roll or pitch value exceeds some threshold to consider "turn over"

If you are familiar with tf, there is another way. You might want to detect whether the z-vector of your robot is negative. If so, you can rotate a unit vector in z axis (0,0,1) using the quaternion you have (function transformVector should to the job) and check the resulted vector if z value is negative.

Asked by DavidN on 2016-11-23 22:19:35 UTC

How to know whether the robot turns over

Comments

Answers

Comments