ROS By Example：python 转 C++ 之 timed_out_and_back

1 写在前面

笔者近期学习了《ROS by Example. Do-It-Yourself Guide to the Robot Operating System》，书中的代码全部是用python来编写的，为了保证完全理解其代码的含义及代码背后的相关知识，这里我将其代码用C++实现了一遍，写下此文章用作记录存留。

2 主要内容

本文对应第7.6章节中的内容，实现了 timed_out_and_back.py 到 timed_out_and_back.cpp 的转换。

开发环境：Ubuntu14.04 + ROS-Indigo

3 原始python脚本

timed_out_and_back.py

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from math import 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=1)
        
        # How fast will we update the robot's movement?
        rate = 50
        
        # 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 to 1.0 meters
        goal_distance = 1.0
        
        # How long should it take us to get there?
        linear_duration = goal_distance / linear_speed
        
        # Set the rotation speed to 1.0 radians per second
        angular_speed = 1.0
        
        # Set the rotation angle to Pi radians (180 degrees)
        goal_angle = pi
        
        # How long should it take to rotate?
        angular_duration = goal_angle / angular_speed
     
        # Loop through the two legs of the trip  
        for i in range(2):
            # Initialize the movement command
            move_cmd = Twist()
            
            # Set the forward speed
            move_cmd.linear.x = linear_speed
            
            # Move forward for a time to go the desired distance
            ticks = int(linear_duration * rate)
            
            for t in range(ticks):
                self.cmd_vel.publish(move_cmd)
                r.sleep()
            
            # Stop the robot before the rotation
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)
            
            # Now rotate left roughly 180 degrees  
            
            # Set the angular speed
            move_cmd.angular.z = angular_speed

            # Rotate for a time to go 180 degrees
            ticks = int(goal_angle * rate)
            
            for t in range(ticks):           
                self.cmd_vel.publish(move_cmd)
                r.sleep()
                
            # Stop the robot before the next leg
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)    
            
        # Stop the robot
        self.cmd_vel.publish(Twist())
        
    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++代码

timed_out_and_back.cpp

// sys
#include <math.h>  // M_PI
#include <signal.h>
// ros
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"

ros::Publisher cmd_vel_pub;

void MySigintHandler(int sig)
{
	ROS_INFO("shutting down!");
    cmd_vel_pub.publish(geometry_msgs::Twist());
    sleep(1);
	ros::shutdown();
    exit(sig);
}

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", 1);
    int rate = 50;  //Hz
    ros::Rate loop_rate(rate);    

    double linear_speed     = 0.2;  // Set the forward linear speed to 0.2 meters per second 
    double goal_distance    = 1.0;  // Set the travel distance to 1.0 meters
    double linear_duration  = goal_distance / linear_speed; // How long should it take us to get there?
    double angular_speed    = 1.0;  // Set the rotation speed to 1.0 radians per second
    double goal_angle       = M_PI; // Set the rotation angle to Pi radians (180 degrees)
    double angular_duration = goal_angle / angular_speed; // How long should it take to rotate?

    for(int i=0; i<2; ++i)
    {
        geometry_msgs::Twist msg_go;
        msg_go.linear.x = linear_speed;
        // Move forward for a time to go the desired distance
        int ticks = static_cast<int>(linear_duration * rate);
        for (int j = 0; j < ticks; ++j)
        {
            cmd_vel_pub.publish(msg_go);
            loop_rate.sleep();
        } 
        // Stop the robot before the rotation
        geometry_msgs::Twist msg_stop;
        cmd_vel_pub.publish(msg_stop);
        sleep(1);
        // Rotate for a time to go 180 degrees
        geometry_msgs::Twist msg_turn;
        msg_turn.angular.z = angular_speed;
        ticks = static_cast<int>(angular_duration * rate);
        for (int j = 0; j < ticks; ++j)
        {
            cmd_vel_pub.publish(msg_turn);
            loop_rate.sleep();
        }
        // Stop the robot before the next leg
        cmd_vel_pub.publish(msg_stop);
        sleep(1);
    }
    // Stop the robot
    cmd_vel_pub.publish(geometry_msgs::Twist());

    return 0;
}

在编译前，还需要在CMakeLists.txt文件中加入以下内容：

add_executable(timed_out_and_back src/timed_out_and_back.cpp)
target_link_libraries(timed_out_and_back ${catkin_LIBRARIES})