一、功能介绍
使机器人直行1米，接着旋转180°，再返回到起始点。
二、实现方法
使用时间和速度估算距离和角度。定时发送Twist命令，使机器人直行一个确定的距离，旋转180°，再次以相同的时间和相同的速度直行并停到起始点。最后机器人再一次旋转180°与原始方向相吻合。
三、实验步骤
1.在robot_move/src/里创建timed_out_and_back.cpp，并粘贴如下代码：
#include <ros/ros.h>#include <signal.h>#include <geometry_msgs/Twist.h>#include <string.h>ros::Publisher cmdVelPub;void shutdown(int sig){cmdVelPub.publish(geometry_msgs::Twist());ROS_INFO("timed_out_and_back.cpp ended!");ros::shutdown();}int main(int argc, char** argv){ros::init(argc, argv, "go_and_back");std::string topic = "/cmd_vel";ros::NodeHandle node;//Publisher to control the robot's speedcmdVelPub = node.advertise<geometry_msgs::Twist>(topic, 1);//How fast will we update the robot's movement?double rate = 50;//Set the equivalent ROS rate variableros::Rate loopRate(rate);//execute a shutdown function when exitingsignal(SIGINT, shutdown);ROS_INFO("timed_out_and_back.cpp start...");//Set the forward linear speed to 0.2 meters per secondfloat linear_speed = 0.2;//Set the travel distance to 1.0 metersfloat goal_distance = 1.0;//How long should it take us to get there?float linear_duration = goal_distance / linear_speed;//Set the rotation speed to 1.0 radians per secondfloat angular_speed = 1.0;//Set the rotation angle to Pi radians (180 degrees)float goal_angle = M_PI;//How long should it take to rotate?float angular_duration = goal_angle / angular_speed;int count = 0;//Loop through the two legs of the tripint ticks;geometry_msgs::Twist speed; // 控制信号载体 Twist messagewhile (ros::ok()){speed.linear.x = linear_speed; // 设置线速度，正为前进，负为后退// Move forward for a time to go 1 meterticks = int(linear_duration * rate);for(int i = 0; i < ticks; i++){cmdVelPub.publish(speed); // 将刚才设置的指令发送给机器人loopRate.sleep();}//Stop the robot before the rotationcmdVelPub.publish(geometry_msgs::Twist());//loopRate.sleep();ROS_INFO("rotation...!");//Now rotate left roughly 180 degreesspeed.linear.x = 0;//Set the angular speedspeed.angular.z = angular_speed; // 设置角速度，正为左转，负为右转//Rotate for a time to go 180 degreesticks = int(angular_duration * rate);for(int i = 0; i < ticks; i++){cmdVelPub.publish(speed); // 将刚才设置的指令发送给机器人loopRate.sleep();}speed.angular.z = 0;//Stop the robot before the next legcmdVelPub.publish(geometry_msgs::Twist());count++;if(count == 2){count = 0;cmdVelPub.publish(geometry_msgs::Twist());ROS_INFO("timed_out_and_back.cpp ended!");ros::shutdown();}else{ROS_INFO("go back...!");}}return 0;}12345678910111213141516171819222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101
特别说明：代码中的注释大部分引用《ros_by_example_indigo_volume_1》这本书的，英文注释简单易懂，所以不作翻译了。
2.修改robot_move目录下的CMakeLists.txt
在CMakeLists.txt文件末尾加入几条语句:
add_executable(timed_out_and_back src/timed_out_and_back.cpp)target_link_libraries(timed_out_and_back ${catkin_LIBRARIES})12
3.编译程序
在catkin_ws目录下，进行catkin_make编译，得到timed_out_and_back执行程序。
4.测试程序
4.1 启动roscore
roscore1
4.2 启动机器人
4.2.1若是运行仿真机器人
roslaunch aicroboxi_bringup fake_aicroboxi.launch1
4.2.2若是运行真实的机器人平台
roslaunch aicroboxi_bringup minimal.launch1
4.3 启动 rviz 图形化显示程序
roslaunch aicroboxi_rviz view_mobile.launch1
4.4 启动timed_out_and_back程序，效果如图1
rosrun robot_move timed_out_and_back1
图1
四、解释部分代码
signal(SIGINT, shutdown);1
这覆盖了默认的ROS信号句柄。这必须在创建第一个NodeHandle之后设置。当在终端按下Ctrl-C将调用shutdown函数，执行一些必要的清除。本程序的目的是停止移动机器人。
//Set the forward linear speed to 0.2 meters per secondfloat linear_speed = 0.2;//Set the travel distance to 1.0 metersfloat goal_distance = 1.0;//How long should it take us to get there?float linear_duration = goal_distance / linear_speed;12345678
初始化直行速度为0.2 m/s和目标距离为1米，然后计算所需时间。
//Set the rotation speed to 1.0 radians per secondfloat angular_speed = 1.0;//Set the rotation angle to Pi radians (180 degrees)float goal_angle = M_PI;//How long should it take to rotate?float angular_duration = goal_angle / angular_speed;12345678
设置旋转角速度为1 rad/s和目标角度为180°或者π弧度。
geometry_msgs::Twist speed; // 控制信号载体 Twist messagefor(int i = 0;i < 2;i++){speed.linear.x = linear_speed; // 设置线速度，正为前进，负为后退// Move forward for a time to go 1 meterticks = int(linear_duration * rate);for(int i = 0; i < ticks; i++){cmdVelPub.publish(speed); // 将刚才设置的指令发送给机器人loopRate.sleep();}12345678910111213
不断地发布geometry_msgs::Twist类型消息促使机器人保持移动。为了使机器人以linear_speed m/s的速度直行linear_distance米，我们每隔1/rate秒发布speed消息，之前就定义了ros::Rate loopRate(rate)，那么loopRate.sleep()表示loopRate.sleep(1/rate)，休眠1/rate秒。
//Now rotate left roughly 180 degreesspeed.linear.x = 0;/Set the angular speedspeed.angular.z = angular_speed; // 设置角速度，正为左转，负为右转//Rotate for a time to go 180 degreesticks = int(angular_duration * rate);for(int i = 0; i < ticks; i++){cmdVelPub.publish(speed); // 将刚才设置的指令发送给机器人loopRate.sleep();}1234567891011
这是循环的第二部分，机器人直行停止后以angular_speed rad/s旋转到180°。
cmdVelPub.publish(geometry_msgs::Twist());1
通过发布一个空的Twist消息使机器人停止。
一、功能介绍
使机器人直行1米，接着旋转180°，再返回到起始点。
二、实现方法
根据/odom和/base_footprint（或/base_link）坐标之间的转换来监视机器人的位置和方向。
此方法使用术语“里程信息”(“Odometry”)表示内部位置数据，ROS提供一种消息类型来存储这些信息，既是“nav_msgs/Odometry”。使用以下指令查看消息的数据结构，见图1。
rosmsg show nav_msgs/Odometry1
图1
nav_msgs/Odometry提供了机器人frame_id坐标系到child_id坐标系的相对位置。geometry_msgs/Pose消息提供了机器人的位姿信息，消息geometry_msgs/Twist提供了速度信息。线速度x为正时，机器人向前移动，为负时，机器人向后移动。角速度z为正时，机器人向左转，为负时，机器人向右转。
因为里程Odometry其实就是两个坐标系之间的位移，那么我们就有必要发布两个坐标系之间的坐标变换信息。一般ROS的里程测量使用/odom作为父坐标ID（固定坐标），/base_footprint（或/base_link）作为子坐标ID（机器人自身）。这些变换是指机器人相对/odom坐标移动。
三、实验步骤
1.在robot_move/src/里创建odom_out_and_back.cpp，并粘贴如下代码：
#include <ros/ros.h>#include <signal.h>#include <geometry_msgs/Twist.h>#include <tf/transform_listener.h>#include <nav_msgs/Odometry.h>#include <string.h>ros::Publisher cmdVelPub;void shutdown(int sig){cmdVelPub.publish(geometry_msgs::Twist());ROS_INFO("odom_out_and_back.cpp ended!");ros::shutdown();}int main(int argc, char** argv){//How fast will we update the robot's movement?double rate = 20;int count = 0;//Loop through the two legs of the tripros::init(argc, argv, "go_and_back");std::string topic = "/cmd_vel";ros::NodeHandle node;cmdVelPub = node.advertise<geometry_msgs::Twist>(topic, 1);//Set the equivalent ROS rate variableros::Rate loopRate(rate);geometry_msgs::Twist speed; // 控制信号载体 Twist messagesignal(SIGINT, shutdown);ROS_INFO("odom_out_and_back.cpp start...");//Set the forward linear speed to 0.2 meters per secondfloat linear_speed = 0.2;//Set the travel distance to 1.0 metersfloat goal_distance = 1.0;//Set the rotation speed to 0.5 radians per secondfloat angular_speed = 0.5;//Set the rotation angle to Pi radians (180 degrees)double goal_angle = M_PI;//Set the angular tolerance in degrees converted to radiansdouble angular_tolerance = 2.5*M_PI/180; //角度转换成弧度:deg*PI/180tf::TransformListener listener;tf::StampedTransform transform;//Find out if the robot uses /base_link or /base_footprintstd::string odom_frame = "/odom";std::string base_frame;try{listener.waitForTransform(odom_frame, "/base_footprint", ros::Time(), ros::Duration(2.0) );base_frame = "/base_footprint";ROS_INFO("base_frame = /base_footprint");}catch (tf::TransformException & ex){try{listener.waitForTransform(odom_frame, "/base_link", ros::Time(), ros::Duration(2.0) );base_frame = "/base_link";ROS_INFO("base_frame = /base_link");}catch (tf::TransformException ex){ROS_INFO("Cannot find transform between /odom and /base_link or /base_footprint");cmdVelPub.publish(geometry_msgs::Twist());ros::shutdown();}}//Loop once for each leg of the tripfor(int i = 0;i < 2;i++){ROS_INFO("go straight...!");speed.linear.x = linear_speed; // 设置线速度，正为前进，负为后退//Get the starting position valueslistener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);float x_start = transform.getOrigin().x();float y_start = transform.getOrigin().y();// Keep track of the distance traveledfloat distance = 0;while( (distance < goal_distance) && (ros::ok()) ){//Publish the Twist message and sleep 1 cyclecmdVelPub.publish(speed);loopRate.sleep();listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);//Get the current positionfloat x = transform.getOrigin().x();float y = transform.getOrigin().y();//Compute the Euclidean distance from the startdistance = sqrt(pow((x - x_start), 2) + pow((y - y_start), 2));}//Stop the robot before the rotationcmdVelPub.publish(geometry_msgs::Twist());ros::Duration(1).sleep(); // sleep for a secondROS_INFO("rotation...!");//Now rotate left roughly 180 degreesspeed.linear.x = 0;//Set the angular speedspeed.angular.z = angular_speed; // 设置角速度，正为左转，负为右转//yaw是围绕Y轴旋转，也叫偏航角//Track the last angle measureddouble last_angle = fabs(tf::getYaw(transform.getRotation()));//Track how far we have turneddouble turn_angle = 0;while( (fabs(turn_angle + angular_tolerance) < M_PI) && (ros::ok()) ){//Publish the Twist message and sleep 1 cyclecmdVelPub.publish(speed);loopRate.sleep();// Get the current rotationlistener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);//C++: abs()求得是正数的绝对值,fabs()求得是浮点数的绝对值;python:abs(x),参数可以是：负数、正数、浮点数或者长整形double rotation = fabs(tf::getYaw(transform.getRotation()));//Compute the amount of rotation since the last loopdouble delta_angle = fabs(rotation - last_angle);//Add to the running totalturn_angle += delta_angle;last_angle = rotation;}//Stop the robot before the rotation//Set the angular speedspeed.angular.z = 0;cmdVelPub.publish(geometry_msgs::Twist());ros::Duration(1).sleep(); // sleep for a second}cmdVelPub.publish(geometry_msgs::Twist());ros::Duration(1).sleep(); // sleep for a secondROS_INFO("odom_out_and_back.cpp ended!");ros::shutdown();return 0;}1234567891011121314151617181922232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811911122123124125126127128129130131132133134135136137138139140141142143144145
特别说明：代码中的注释大部分引用《ros_by_example_indigo_volume_1》这本书的，英文注释简单易懂，所以不作翻译了。
2.修改robot_move目录下的CMakeLists.txt
在CMakeLists.txt文件末尾加入几条语句:
add_executable(odom_out_and_back src/odom_out_and_back.cpp)target_link_libraries(odom_out_and_back ${catkin_LIBRARIES})12
3.编译程序
在catkin_ws目录下，进行catkin_make编译，得到odom_out_and_back执行程序。
4.测试程序
4.1 启动roscore
roscore1
4.2 启动机器人
4.2.1若是运行仿真机器人
roslaunch aicroboxi_bringup fake_aicroboxi.launch1
4.2.2若是运行真实的机器人平台
roslaunch aicroboxi_bringup minimal.launch1
4.3 启动 rviz 图形化显示程序
roslaunch aicroboxi_rviz view_mobile.launch1
4.4 启动odom_out_and_back程序，效果如图2
rosrun robot_move odom_out_and_back1
图2
四、解释部分代码
tf::TransformListener listener;tf::StampedTransform transform;//Find out if the robot uses /base_link or /base_footprintstd::string odom_frame = "/odom";std::string base_frame;try{listener.waitForTransform(odom_frame, "/base_footprint", ros::Time(), ros::Duration(2.0) );base_frame = "/base_footprint";ROS_INFO("base_frame = /base_footprint");}catch (tf::TransformException & ex){try{listener.waitForTransform(odom_frame, "/base_link", ros::Time(), ros::Duration(2.0) );base_frame = "/base_link";ROS_INFO("base_frame = /base_link");}catch (tf::TransformException ex){ROS_INFO("Cannot find transform between /odom and /base_link or /base_footprint");cmdVelPub.publish(geometry_msgs::Twist());ros::shutdown();}}12345678910111213141516171819222324252627282930
创建StampedTransform对象来获取变换信息，创建TransformListener对象监听坐标变换。我们需要/odom坐标和/base_footprint坐标或者/base_link坐标之间的变换。首先测试是否存在/base_footprint坐标，如果不存在，再测试/base_link坐标。结果将保存在base_frame变量，以便之后使用。在本次实验使用的是/base_footprint坐标。
//Loop once for each leg of the tripfor(int i = 0;i < 2;i++){ROS_INFO("go straight...!");speed.linear.x = linear_speed; // 设置线速度，正为前进，负为后退//Get the starting position valueslistener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);float x_start = transform.getOrigin().x();float y_start = transform.getOrigin().y();123456789101112
执行两次循环，每次循环都是机器人移动直行1米，然后旋转180°。每次循环一开始，我们都记录起始点的位置。使用listener对象来查看odom_frame和base_frame坐标的变换，并记录在transform。通过transform.getOrigin().x()和transform.getOrigin().y()获得起始点位置。
float distance = 0;while( (distance < goal_distance) && (ros::ok()) ){//Publish the Twist message and sleep 1 cyclecmdVelPub.publish(speed);loopRate.sleep();listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);//Get the current positionfloat x = transform.getOrigin().x();float y = transform.getOrigin().y();//Compute the Euclidean distance from the startdistance = sqrt(pow((x - x_start), 2) + pow((y - y_start), 2));}12345678910111213141516
这个循环是使机器人直行1米。
while( (fabs(turn_angle + angular_tolerance) < M_PI) && (ros::ok()) ){//Publish the Twist message and sleep 1 cyclecmdVelPub.publish(speed);loopRate.sleep();// Get the current rotationlistener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);//C++: abs()求得是正数的绝对值,fabs()求得是浮点数的绝对值;python:abs(x),参数可以是：负数、正数、浮点数或者长整形double rotation = fabs(tf::getYaw(transform.getRotation()));//Compute the amount of rotation since the last loopdouble delta_angle = fabs(rotation - last_angle);//Add to the running totalturn_angle += delta_angle;last_angle = rotation;}1234567891011121314151617181920
这个循环使机器人在一定angular_tolerance角度误差内旋转180°。abs()求得是正数的绝对值,fabs()求得是浮点数的绝对值。tf::getYaw(transform.getRotation())获取旋转的角度。
五、使用Odometry行走一个正方形
设置四个导航点，使机器人移动成一个正方形。效果如图3.
