1 写在前面
笔者近期学习了《ROS by Example. Do-It-Yourself Guide to the Robot Operating System》,书中的代码全部是用python来编写的,为了保证完全理解其代码的含义及代码背后的相关知识,这里我将其代码用C++实现了一遍,写下此文章用作记录存留。
2 主要内容
本文对应第7.8章节中的内容,实现了 odom_out_and_back.py 到 odom_out_and_back.cpp 的转换。
开发环境:Ubuntu14.04 + ROS-Indigo
3 原始python脚本
odom_out_and_back.py
#!/usr/bin/env python
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.15 meters per second
linear_speed = 0.15
# Set the travel distance in meters
goal_distance = 1.0
# Set the rotation speed in radians per second
angular_speed = 0.5
# Set the angular tolerance in degrees converted to radians
angular_tolerance = radians(1.0)
# 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.")
4 转换后的C++代码
odom_out_and_back.cpp
// sys
#include <math.h>
#include <signal.h>
#include <vector>
// ros
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Point.h"
#include "geometry_msgs/Quaternion.h"
#include "tf/transform_listener.h"
ros::Publisher cmd_vel_pub;
void MySigintHandler(int sig)
{
ROS_INFO("node \"out_and_back\" shutting down!");
cmd_vel_pub.publish(geometry_msgs::Twist());
sleep(1);
ros::shutdown();
exit(sig);
}
// Get the current transform between the odom and base frames
bool get_odom(const tf::TransformListener &listener,
const std::string &target_frame,
const std::string &source_frame,
tf::StampedTransform &transform)
{
try
{
listener.lookupTransform(target_frame, source_frame, ros::Time(0), transform);
}
catch(...)
{
ROS_ERROR("TF Exception");
return false;
}
return true;
}
// from Quaternion get Euler yaw
double quat_to_radianYaw(const tf::Quaternion &quat)
{
double qx = quat.x();
double qy = quat.y();
double qz = quat.z();
double qw = quat.w();
// yaw (z-axis rotation)
double siny_cosp = +2.0 * (qw * qz + qx * qy);
double cosy_cosp = +1.0 - 2.0 * (qy * qy + qz * qz);
return atan2(siny_cosp, cosy_cosp);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "out_and_back", ros::init_options::NoSigintHandler);
ros::NodeHandle node;
signal(SIGINT, MySigintHandler);
cmd_vel_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
int rate = 20; //Hz
ros::Rate loop_rate(rate);
double linear_speed = 0.15; // Set the forward linear speed to 0.15 meters per second
double goal_distance = 1.0; // Set the travel distance in meters
double angular_speed = 0.5; // Set the rotation speed in radians per second
double angular_tolerance = M_PI/180.0; // Set the angular tolerance in degrees converted to radians
double goal_angle = M_PI; // Set the rotation angle to Pi radians (180 degrees)
// Initialize the tf listener
tf::TransformListener tf_listener;
// Give tf some time to fill its buffer
sleep(2);
// Set the odom frame
std::string odom_frame = "/odom";
std::string base_frame = "";
// Find out if the robot uses /base_link or /base_footprint
try
{
tf_listener.waitForTransform(odom_frame, "/base_footprint", ros::Time(), ros::Duration(1.0));
base_frame = "/base_footprint";
ROS_INFO("base frame: /base_footprint");
}
catch(...)
{
try
{
tf_listener.waitForTransform(odom_frame, "/base_link", ros::Time(), ros::Duration(1.0));
base_frame = "/base_link";
ROS_INFO("base frame: /base_link");
}
catch(...)
{
ROS_ERROR("Cannot find transform between /odom and /base_link or /base_footprint");
ROS_ERROR("TF Exception");
ros::shutdown();
return 0;
}
}
// Initialize the position variable as a Point type
geometry_msgs::Point position;
// Loop once for each leg of the trip
for (int i = 0; i < 2; ++i)
{
geometry_msgs::Twist move_cmd;
move_cmd.linear.x = linear_speed;
// Get the starting position values
tf::StampedTransform transform;
get_odom(tf_listener, odom_frame, base_frame, transform);
double x_start = transform.getOrigin().x();
double y_start = transform.getOrigin().y();
// Keep track of the distance traveled
double distance = 0;
// Enter the loop to move along a side
while (distance < goal_distance && node.ok())
{
// Publish the Twist message and sleep 1 cycle
cmd_vel_pub.publish(move_cmd);
loop_rate.sleep();
// Get the current position
get_odom(tf_listener, odom_frame, base_frame, transform);
position.x = transform.getOrigin().x();
position.y = transform.getOrigin().y();
// 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.linear.x = 0;
cmd_vel_pub.publish(move_cmd);
loop_rate.sleep();
// Set the movement command to a rotation
move_cmd.angular.z = angular_speed;
// Track the last angle measured
get_odom(tf_listener, odom_frame, base_frame, transform);
double last_angle = fabs(quat_to_radianYaw(transform.getRotation()));
// Track how far we have turned
double turn_angle = 0;
while (fabs(turn_angle + angular_tolerance) < fabs(goal_angle) && node.ok())
{
// Publish the Twist message and sleep 1 cycle
cmd_vel_pub.publish(move_cmd);
loop_rate.sleep();
// Get the current rotation
get_odom(tf_listener, odom_frame, base_frame, transform);
// Compute the amount of rotation since the last loop
double current_angle = fabs(quat_to_radianYaw(transform.getRotation()));
double delta_angle = fabs(current_angle - last_angle);
// Add to the running total
turn_angle += delta_angle;
last_angle = current_angle;
}
// Stop the robot before the next leg
move_cmd.angular.z = 0;
cmd_vel_pub.publish(move_cmd);
loop_rate.sleep();
}
// Stop the robot for good
cmd_vel_pub.publish(geometry_msgs::Twist());
return 0;
}
在编译前,还需要在CMakeLists.txt文件中加入以下内容:
add_executable(odom_out_and_back src/odom_out_and_back.cpp)
target_link_libraries(odom_out_and_back ${catkin_LIBRARIES})
在上述C++代码中,关于四元数转欧拉角的内容,本人参考于此篇文章, 且具体理论知识此处不做详述。
此外,TF自带通过四元数计算偏航角的函数 double getYaw(const Quaternion& bt_q)
,因此可以直接将第164行替换为double current_angle = fabs(tf::getYaw(transform.getRotation()))
,效果相同。