ROS By Example：python 转 C++ 之 odom_out_and_back

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()))，效果相同。