Ask Your Question
0

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

asked 2015-03-16 21:48:56 -0600

dylankc gravatar image

I am a beginner learning about ROS. I want to create a similar script to this in C++. I got this from a book called "ROS-by-example" and it is in python. I am not fluent with python and ROS so I am unable to "translate" the language. Can anybody help to translate this into a C++ ROS node equivalent? Maybe just the ROS commands, I can convert the structures such as loops/variable declaration.

I know C++, but I am unfamiliar with the ROS commands such as those with the rospy.(function name). I will be able to replicate the structures (for loops and class) in C++. Your help is greatly appreciated.

#!/usr/bin/env python

""" odom_out_and_back.py - Version 1.1 2013-12-20
A basic demo of using the /odom topic to move a robot a given distance
or rotate through a given angle.
Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2012 Patrick Goebel.  All rights reserved.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.5

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details at:

http://www.gnu.org/licenses/gpl.html

"""

import rospy
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from rbx1_nav.transform_utils import quat_to_angle, normalize_angle
from math import radians, copysign, sqrt, pow, pi

class OutAndBack():
def __init__(self):
    # Give the node a name
    rospy.init_node('out_and_back', anonymous=False)

    # Set rospy to execute a shutdown function when exiting       
    rospy.on_shutdown(self.shutdown)

    # Publisher to control the robot's speed
    self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

    # How fast will we update the robot's movement?
    rate = 20

    # Set the equivalent ROS rate variable
    r = rospy.Rate(rate)

    # Set the forward linear speed to 0.2 meters per second 
    linear_speed = 0.2

    # Set the travel distance in meters
    goal_distance = 1.0

    # Set the rotation speed in radians per second
    angular_speed = 1.0

    # Set the angular tolerance in degrees converted to radians
    angular_tolerance = radians(2.5)

    # Set the rotation angle to Pi radians (180 degrees)
    goal_angle = pi

    # Initialize the tf listener
    self.tf_listener = tf.TransformListener()

    # Give tf some time to fill its buffer
    rospy.sleep(2)

    # Set the odom frame
    self.odom_frame = '/odom'

    # Find out if the robot uses /base_link or /base_footprint
    try:
        self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
        self.base_frame = '/base_footprint'
    except (tf.Exception, tf.ConnectivityException, tf.LookupException):
        try:
            self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
            self.base_frame = '/base_link'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
            rospy.signal_shutdown("tf Exception")  

    # Initialize the position variable as a Point ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2015-09-01 19:31:18 -0600

samer.hanoun gravatar image

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 ...
(more)
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

2 followers

Stats

Asked: 2015-03-16 21:48:56 -0600

Seen: 2,090 times

Last updated: Sep 01 '15