ROS机器人直行1米，接着旋转180°，再返回到起始点。之一

系统版本：Ubuntu14.04，ROS indigo
机器人底盘：kobuki
摄像头：Asus Xtion
编程语言：C++
参考书本：ROS by Example

一、功能介绍

　　使机器人直行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 speed
  cmdVelPub = node.advertise<geometry_msgs::Twist>(topic, 1);

  //How fast will we update the robot's movement?
  double rate = 50;
  //Set the equivalent ROS rate variable
  ros::Rate loopRate(rate);

  //execute a shutdown function when exiting
  signal(SIGINT, shutdown);
  ROS_INFO("timed_out_and_back.cpp start...");

  //Set the forward linear speed to 0.2 meters per second
  float linear_speed = 0.2;

  //Set the travel distance to 1.0 meters
  float 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 second
  float 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 trip
  int ticks;
  geometry_msgs::Twist speed; // 控制信号载体 Twist message
  while (ros::ok())
  {

    speed.linear.x = linear_speed; // 设置线速度，正为前进，负为后退
    // Move forward for a time to go 1 meter
    ticks = int(linear_duration * rate);
    for(int i = 0; i < ticks; i++)
    {
      cmdVelPub.publish(speed); // 将刚才设置的指令发送给机器人
      loopRate.sleep();
    }
    //Stop the robot before the rotation
    cmdVelPub.publish(geometry_msgs::Twist());
    //loopRate.sleep();
    ROS_INFO("rotation...!");

    //Now rotate left roughly 180 degrees
    speed.linear.x = 0;
    //Set the angular speed
    speed.angular.z = angular_speed; // 设置角速度，正为左转，负为右转
    //Rotate for a time to go 180 degrees
    ticks = 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 leg
    cmdVelPub.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;
}

特别说明：代码中的注释大部分引用《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})

3.编译程序

在catkin_ws目录下，进行catkin_make编译，得到timed_out_and_back执行程序。

4.测试程序

4.1 启动roscore

   roscore

4.2 启动机器人

4.2.1若是运行仿真机器人

   roslaunch aicroboxi_bringup fake_aicroboxi.launch

4.2.2若是运行真实的机器人平台

   roslaunch aicroboxi_bringup minimal.launch

4.3 启动 rviz 图形化显示程序

   roslaunch aicroboxi_rviz view_mobile.launch

4.4 启动timed_out_and_back程序，效果如图1

   rosrun robot_move timed_out_and_back

rviz4

图1

四、解释部分代码

signal(SIGINT, shutdown);

这覆盖了默认的ROS信号句柄。这必须在创建第一个NodeHandle之后设置。当在终端按下Ctrl-C将调用shutdown函数，执行一些必要的清除。本程序的目的是停止移动机器人。

//Set the forward linear speed to 0.2 meters per second
float linear_speed = 0.2;

//Set the travel distance to 1.0 meters
float goal_distance = 1.0;

//How long should it take us to get there?
float linear_duration = goal_distance / linear_speed;

初始化直行速度为0.2 m/s和目标距离为1米，然后计算所需时间。

//Set the rotation speed to 1.0 radians per second
float 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;

设置旋转角速度为1 rad/s和目标角度为180°或者π弧度。

geometry_msgs::Twist speed; // 控制信号载体 Twist message

for(int i = 0;i < 2;i++)
{

    speed.linear.x = linear_speed; // 设置线速度，正为前进，负为后退
    // Move forward for a time to go 1 meter
    ticks = int(linear_duration * rate);
    for(int i = 0; i < ticks; i++)
    {
      cmdVelPub.publish(speed); // 将刚才设置的指令发送给机器人
      loopRate.sleep();
    }

　　不断地发布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 degrees
speed.linear.x = 0;
/Set the angular speed
speed.angular.z = angular_speed; // 设置角速度，正为左转，负为右转
//Rotate for a time to go 180 degrees
ticks = int(angular_duration * rate);
for(int i = 0; i < ticks; i++)
{
   cmdVelPub.publish(speed); // 将刚才设置的指令发送给机器人
   loopRate.sleep();
}

这是循环的第二部分，机器人直行停止后以angular_speed rad/s旋转到180°。

cmdVelPub.publish(geometry_msgs::Twist());

通过发布一个空的Twist消息使机器人停止。

源代码地址：https://github.com/KeoChi/robot_move

个人学习笔记，欢迎交流学习。