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
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 ...