Odometry based out-and-back script (py to c++)

edit

The C++ equivalent version of the python code. Launch stage with willow-erratic world and make sure the robot has enough space to move forward and out without hitting any obstacles before turning and getting back. Run your "out_and_back" node using "rosrun".

/*
 * out_and_back.cpp
 *
 *  Created on:
 *  Author:
 */
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <angles/angles.h>

double x_current = 0;
double y_current = 0;
double theta_current = 0;

tf::TransformListener *odom_listener;

void get_odom()
{
    tf::StampedTransform transform;
    try
    {
        odom_listener->lookupTransform("/odom", "/base_footprint", ros::Time(0), transform);

        x_current = transform.getOrigin().x();
        y_current = transform.getOrigin().y();
        theta_current = angles::normalize_angle_positive(tf::getYaw(transform.getRotation()));
        ROS_INFO("odom (x, y, theta) : (%f, %f, %f)", x_current, y_current, theta_current);
    }
    catch (tf::TransformException &ex)
    {
        ROS_ERROR("%s",ex.what());
    }
}

void move(ros::Publisher cmd_VelPublisher, double move_distance, double linearSpeed, int direction)
{
    // Current position
    double x = x_current;
    double y = y_current;

    // Initialize the movement command message
    geometry_msgs::Twist cmd_velMsg;
    // Set the movement command linear speed for forward motion
    cmd_velMsg.linear.x = direction * linearSpeed;

    // How fast to update the robot's movement?
    // Set the equivalent ROS rate variable
    ros::Rate rate(10.0);

    double d = 0;
    // Enter the loop to move the robot
    while (d < move_distance && ros::ok())
    {
        //Publish the Twist message and sleep 1 cycle
        cmd_VelPublisher.publish(cmd_velMsg);

        ros::spinOnce();
        rate.sleep();

        // Get the current odometry
        get_odom();

        // Compute the Euclidean distance from the start position
        d = sqrt(pow((x_current - x), 2) + pow((y_current - y), 2));
        ROS_INFO("d: %f", d);
    }

    // Stop the robot
    cmd_velMsg.linear.x = 0;
    cmd_VelPublisher.publish(cmd_velMsg);

    return;
}

void turn(ros::Publisher cmd_VelPublisher, double turn_angle, double angularSpeed, int direction)
{
    // Initialize the movement command message
    geometry_msgs::Twist cmd_velMsg;
    //Set the movement command rotation speed
    cmd_velMsg.angular.z = direction * angularSpeed;

    // How fast to update the robot's movement?
    // Set the equivalent ROS rate variable
    ros::Rate rate(10.0);

    // Current angle
    get_odom();
    double last_angle = theta_current;
    double angle = 0;

     while ((angle < turn_angle) && ros::ok())
    {
        //Publish the Twist message and sleep 1 cycle
        cmd_VelPublisher.publish(cmd_velMsg);

        //ROS_INFO("current_theta: %f, angle + turn_angle: %f", angle, (angle + turn_angle));

        ros::spinOnce();
        rate.sleep();

        // Get the current odometry
        get_odom();

        // Compute the amount of rotation since the last loop
        angle += angles::normalize_angle(theta_current - last_angle);
        last_angle = theta_current;

        ROS_INFO("angle: %f", angle);
    }

    // Stop turning the robot
    cmd_velMsg.angular.z = 0;
    cmd_VelPublisher.publish(cmd_velMsg);
}

int main(int argc, char **argv)
{
    // Initiate new ROS node named "out_and_back"
    ros::init(argc, argv, "out_and_back");
    ros::NodeHandle node;

    // Publisher to the robot's velocity command topic
    ros::Publisher cmd_VelPublisher;
    // Advertise a new publisher for the simulated robot's velocity command topic
    cmd_VelPublisher = node.advertise<geometry_msgs::Twist>("cmd_vel", 10);

    odom_listener = new tf::TransformListener();
    // Find out if the robot uses /base_link or /base_footprint
    try
    {
        odom_listener->waitForTransform("/odom", "/base_footprint", ros::Time(0), ros::Duration(1.0));

//        try
//        {
//            odom_listener->waitForTransform("/odom", "/base_link", ros::Time(0), ros::Duration(1.0));
//        }
//        catch (tf::TransformException ex)
//        {
//            ROS_ERROR("Cannot find transform between /base_link and /odom :", ex.what());
//            return 0;
//        }
    }
    catch (tf::TransformException ex)
    {
        ROS_ERROR("Cannot find transform between /base_footprint and /odom :", ex.what());
        return 0 ...