Getting Robot Orientation

edit

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.