Comment déterminer si un robot ROS va vers l'avant ou vers l'arrière?

Question

ROS fournit un message d'odométrie, qui me dit ce qui suit en référence à un plan xy.

• La composante x de la vitesse du robot en (m/sec).
• La composante y de la vitesse du robot en (m/sec).
• orientation angulaire représentée par un quaternion (z et des composants w en radians)

ROS fournit l'supplémentaire suivante C++ bibliothèques

• API quaternion de robot: http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1Quaternion.html

• Vector 3 API: http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1Vector3.html

J'ai une instruction si-géante inélégante qui utilise le quadrant que le robot fait face avec la direction des composantes x et y. Je préférerais apprendre à tirer parti des quaternions.

Answer 1

Je ne vois pas la nécessité de travailler avec des quaternions ici. Le problème semble beaucoup plus simple. De nav_msgs/Odometry: "La torsion de ce message doit être spécifiée dans le cadre de coordonnées donné par le child_frame_id". Ainsi, la torsion exprime la vitesse (linéaire et angulaire), par rapport à child_frame_id. Dans la plupart des configurations de robot, child_frame_id sera un cadre de coordonnées fixé au robot, par exemple "base_link". Ainsi, la vitesse de la torsion est donnée par rapport à un cadre fixe au robot - vous pouvez simplement vérifier si le vecteur twist.linear pointe vers le plan positif la moitié, quelque chose comme:

if (odom_msg.twist.linear.x >= 0.0 && odom_msg.twist.linear.y >= 0.0) 
    // robot going forward