ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

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

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
This program is free software; you can redistribute it and/or modify
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:

"""

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

# 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:
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.signal_shutdown("tf Exception")

# Initialize the position variable as a Point ...
edit retag close merge delete

Sort by » oldest newest most voted

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

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
//        {
//        }
//        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