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 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):
rospy.loginfo("TF Exception")
return
return (Point(*trans), quat_to_angle(Quaternion(*rot)))
def shutdown(self):
# Always stop the robot when shutting down the node.
rospy.loginfo("Stopping the robot...")
self.cmd_vel.publish(Twist())
rospy.sleep(1)
if __name__ == '__main__':
try:
OutAndBack()
except:
rospy.loginfo("Out-and-Back node terminated.")
On a side note, this code is very useful for beginners such as myself as the comments explain the code line by line and you are able to understand the thought process. Also, the book has a more detailed explanation (much like the brilliant ROS tutorials format)!
Asked by dylankc on 2015-03-16 21:48:56 UTC
Answers
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;
}
// An out and back robot is required to move forward 5m, turn 180, move forward 5m
// Robot specific variables
// -----------------------
// Set the forward linear speed to 1.0 meters per second
double linear_speed = 1.0;
// Set the travel distance in meters
double travel_distance = 5.0;
// Set the rotation speed in radians per second
double angular_speed = 0.1;
// Set the rotation angle to PI radians (180 degrees)
double turn_angle = M_PI;
move(cmd_VelPublisher, travel_distance, linear_speed, 1);
turn(cmd_VelPublisher, turn_angle, angular_speed, 1);
move(cmd_VelPublisher, travel_distance, linear_speed, 1);
delete odom_listener;
return 0;
}
Asked by samer.hanoun on 2015-09-01 19:30:05 UTC
Comments