ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
The current heading of the robot can be found by using the following conditions:
double Z=msg2->pose.pose.orientation.z;
double W=msg2->pose.pose.orientation.w;
if (Z<0.707 && Z>-0.707) // This corresponds to heading [-pi/2,pi/2]
curr_head=asin(2*Z*W);
else if (Z>0.707) // Heading>pi/2 and Heading<pi
curr_head=3.14-asin(2*Z*W);
else if (Z<-0.707) // Heading (-pi/2,-pi)
curr_head=-3.14-asin(2*Z*W);
2 | No.2 Revision |
The current heading of the robot can be found by using the following conditions:
double Z=msg2->pose.pose.orientation.z;
double W=msg2->pose.pose.orientation.w;
if (Z<0.707 && Z>-0.707) // This corresponds to heading [-pi/2,pi/2]
(-pi/2,pi/2)
curr_head=asin(2*Z*W);
else if (Z>0.707) // Heading>pi/2 and Heading<pi
curr_head=3.14-asin(2*Z*W);
else if (Z<-0.707) // Heading (-pi/2,-pi)
curr_head=-3.14-asin(2*Z*W);