Turn the robot using the odometry message
I want to create function turn where I will enter some angle and the robot would turn using the odometry message. I have a problem with waiting for odometry message, because the fuction will be called with the theta_current = 0, which is initial value. I thought that if I use function waitForMessage and afterwards spinOnce, I should get the current value. But I am wrong and I have no idea how to solve that problem. And yes I did a lot of googling but I found only the examples using tf.
//global variables
double x_current = 0; //used to move
double y_current = 0; //used to move
double theta_current = 0; //used to move
int main(int argc, char **argv)
{
ros::init(argc, argv, "robot_node");
ros::NodeHandle n;
velocity_pub = n.advertise<geometry_msgs::Twist>("/robot1/cmd_vel", 1000);
ros::topic::waitForMessage<nav_msgs::Odometry>("/robot1/odom");
ros::Subscriber sub = n.subscribe("/robot1/odom",1000,getOdom);
ros::spinOnce();
turn(90.0, 0.2, 1);
ros::Rate r(10);
while(ros::ok())
{
ros::spinOnce();
r.sleep();
}
return 0;
}
void set_velocities(float lin_vel, float ang_vel)
{
geometry_msgs::Twist msg;
msg.linear.x = lin_vel;
msg.angular.z = ang_vel;
velocity_pub.publish(msg);
}
void turn(float angleDeg, double angularSpeed, int direction)
{
float currentAngleDeg = theta_current*M_PI/180; //convert current angle in radians to degrees
float endAngle = currentAngleDeg + currentAngleDeg;
ros::Rate rate(10);
while(currentAngleDeg < endAngle)
{
set_velocities(0.0, 0.2);
ros::spinOnce();
rate.sleep();
}
set_velocities(0.0, 0.0);
}
void getOdom(const nav_msgs::Odometry::ConstPtr& msg)
{
theta_current = msg->twist.twist.angular.z;
x_current = msg->pose.pose.position.x;
y_current = msg->pose.pose.position.y;
}
Asked by horczech on 2016-05-24 13:01:50 UTC
Answers
Try using tf
to get the robot's yaw from the odometry message, e.g.:
#include <tf/transform_datatypes.h>
...
...
...
void getOdom(const nav_msgs::Odometry::ConstPtr& msg)
{
theta_current = tf::getYaw(msg->pose.pose.orientation);
x_current = msg->pose.pose.position.x;
y_current = msg->pose.pose.position.y;
}
EDIT: I guess you were looking for a way not using the tf
library? Regardless, you'll need a way to transform the msg->pose.pose.orientation
(a Quaternion) into the robot's yaw angle. If you don't want to use tf
, you could try implementing your own conversion function. Here's the math: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
Asked by jacobperron on 2016-05-25 17:30:40 UTC
Comments
There are many funky things going on here, but one of the important ones is that
msg->twist.twist.angular.z
is the angular velocity, not the orientation. In other words, you are reading the wrong field of theOdometry
message. Trymsg->pose.pose.orientation
instead.Asked by spmaniato on 2016-05-24 14:51:24 UTC