Ask Your Question
0

Getting Robot Orientation

asked 2013-07-11 07:10:03 -0500

this post is marked as community wiki

This post is a wiki. Anyone with karma >75 is welcome to improve it.

I am trying to write a program to have a robot follow a square path. I have two primary functions, forwardCommand and turnCommand, for either going straight to a corner of the square or for turning to the next corner. I am writing in C++.

My turning function is incomplete, because I cannot get values in my program to have the robot know when to stop its turn and go straight to the next waypoint.

void turnCommand(ros::Publisher& pub)
{
    geometry_msgs::Twist command;

    heading = headingTo(state.pose[X], state.pose[Y], goal_x, goal_y) 
    angle_to_go = heading - state.pose[THETA] // When within desired accuracy, go to next waypoint
    z = .2; // Rate of angular turn
    command.angular.z = z; 

    if(angle_to_go < 0.1)
    {
        TFSwitch = true;

        if(k == 4)
        {
            victory = true;
            k = 0;
        }
    }   

    pub.publish(command);
}

headingTo Function:

inline double headingTo(double x1, double y1, double x2, double y2)
{ return atan2((y2-y1), (x2-x1)); }

I am having trouble with getting the orientation of the robot to know when it should stop turning. I have an odometry callback to update my robot class' x, y, and theta coordinates from a nav_msgs::Odometry::ConstPtr& named msg. I need to get state.pose[THETA] to access the polar direction the robot faces, so that when state.pose[THETA] is close to heading the robot moves on.

void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    state.pose[X] = msg->pose.pose.position.x;
    state.pose[Y] = msg->pose.pose.position.y;
    state.pose[THETA] = yawFromQuaternion(msg->pose.pose.orientation);

    state.velocity[X] = msg->twist.twist.linear.x;
    state.velocity[Y] = msg->twist.twist.linear.y;
    state.velocity[THETA] = msg->twist.twist.angular.z;

}

The function yawFromQuaternion:

inline double yawFromQuaternion(double x, double y, double z, double w)
{ return atan2((2.0 * (w*z + x*y)), (1.0 - 2.0 * (y*y + z*z))); }

When I try to reference state.pose[THETA] it does not update.

Can I have some help with the nav_msgs Odometry and pose.pose.orientation? I read some of the other answered questions (How to understand robot orientation from quaternion yaw angle), . I've tried to implement the solutions found there, but I have been unsuccessful.

edit retag flag offensive close merge delete

Comments

Have you tried using the rostopic command line tool to manually publish odom messages? That lets you know if your node works or if you have a communication problem. Anyway, you should post your subscriber initialization and the output of `rostopic list'.

tbh gravatar imagetbh ( 2013-07-11 07:54:24 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2013-07-11 08:24:08 -0500

this post is marked as community wiki

This post is a wiki. Anyone with karma >75 is welcome to improve it.

Use the function tf::getYaw(quat). This extracts the yaw from a quaternion and should be implemented correctly.

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: 2013-07-11 07:10:03 -0500

Seen: 2,761 times

Last updated: Jul 11 '13