# Revision history [back]

### 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 type
position = Point()

# Loop once for each leg of the trip
for i in range(2):
# Initialize the movement command
move_cmd = Twist()

# Set the movement command to forward motion
move_cmd.linear.x = linear_speed

# Get the starting position values
(position, rotation) = self.get_odom()

x_start = position.x
y_start = position.y

# Keep track of the distance traveled
distance = 0

# Enter the loop to move along a side
while distance < goal_distance and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)

r.sleep()

# Get the current position
(position, rotation) = self.get_odom()

# Compute the Euclidean distance from the start
distance = sqrt(pow((position.x - x_start), 2) +
pow((position.y - y_start), 2))

# Stop the robot before the rotation
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)

# Set the movement command to a rotation
move_cmd.angular.z = angular_speed

# Track the last angle measured
last_angle = rotation

# Track how far we have turned
turn_angle = 0

while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()

# Get the current rotation
(position, rotation) = self.get_odom()

# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(rotation - last_angle)

# Add to the running total
turn_angle += delta_angle
last_angle = rotation

# Stop the robot before the next leg
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)

# Stop the robot for good
self.cmd_vel.publish(Twist())

def get_odom(self):
# Get the current transform between the odom and base frames
try:
(trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
return

return (Point(*trans), quat_to_angle(Quaternion(*rot)))

def shutdown(self):
# Always stop the robot when shutting down the node.