Ask Your Question
0

How to know whether the robot turns over

asked 2016-11-23 18:52:43 -0500

yangzhaoyang6 gravatar image

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?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2016-11-23 21:19:35 -0500

DavidN gravatar image

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.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2016-11-23 18:52:43 -0500

Seen: 18 times

Last updated: Nov 23 '16