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

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

一、功能介绍

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

二、实现方法

　　根据/odom和/base_footprint（或/base_link）坐标之间的转换来监视机器人的位置和方向。
　　此方法使用术语“里程信息”(“Odometry”)表示内部位置数据，ROS提供一种消息类型来存储这些信息，既是“nav_msgs/Odometry”。使用以下指令查看消息的数据结构，见图1。

rosmsg show nav_msgs/Odometry

图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 trip
  ros::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 variable
  ros::Rate loopRate(rate);
  geometry_msgs::Twist speed; // 控制信号载体 Twist message
  signal(SIGINT, shutdown);
  ROS_INFO("odom_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;


  //Set the rotation speed to 0.5 radians per second
  float 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 radians
  double angular_tolerance = 2.5*M_PI/180; //角度转换成弧度:deg*PI/180

  tf::TransformListener listener;
  tf::StampedTransform transform;

  //Find out if the robot uses /base_link or /base_footprint
  std::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 trip
  for(int i = 0;i < 2;i++)
  {
    ROS_INFO("go straight...!");
    speed.linear.x = linear_speed; // 设置线速度，正为前进，负为后退
    //Get the starting position values
    listener.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 traveled
    float distance = 0;
    while( (distance < goal_distance) && (ros::ok()) )
    {
         //Publish the Twist message and sleep 1 cycle
         cmdVelPub.publish(speed);
         loopRate.sleep();
         listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);
         //Get the current position
         float x = transform.getOrigin().x();
         float y = transform.getOrigin().y();
         //Compute the Euclidean distance from the start
         distance = sqrt(pow((x - x_start), 2) +  pow((y - y_start), 2));
    }
    //Stop the robot before the rotation
    cmdVelPub.publish(geometry_msgs::Twist());
    ros::Duration(1).sleep(); // sleep for  a second
    ROS_INFO("rotation...!");
    //Now rotate left roughly 180 degrees
    speed.linear.x = 0;
    //Set the angular speed
    speed.angular.z = angular_speed; // 设置角速度，正为左转，负为右转

    //yaw是围绕Y轴旋转，也叫偏航角
    //Track the last angle measured
    double last_angle = fabs(tf::getYaw(transform.getRotation()));
    //Track how far we have turned
    double turn_angle = 0;
    while( (fabs(turn_angle + angular_tolerance) < M_PI) && (ros::ok()) )
    {
        //Publish the Twist message and sleep 1 cycle
        cmdVelPub.publish(speed);
        loopRate.sleep();
        // Get the current rotation
        listener.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 loop
        double delta_angle = fabs(rotation - last_angle);

        //Add to the running total
        turn_angle += delta_angle;

        last_angle = rotation;
    }
    //Stop the robot before the rotation
    //Set the angular speed
    speed.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 second
  ROS_INFO("odom_out_and_back.cpp ended!");
  ros::shutdown();
  return 0;
}

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

3.编译程序

在catkin_ws目录下，进行catkin_make编译，得到odom_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 启动odom_out_and_back程序，效果如图2

   rosrun robot_move odom_out_and_back

图2

四、解释部分代码

tf::TransformListener listener;
tf::StampedTransform transform;

//Find out if the robot uses /base_link or /base_footprint
std::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();
  }
}

　　创建StampedTransform对象来获取变换信息，创建TransformListener对象监听坐标变换。我们需要/odom坐标和/base_footprint坐标或者/base_link坐标之间的变换。首先测试是否存在/base_footprint坐标，如果不存在，再测试/base_link坐标。结果将保存在base_frame变量，以便之后使用。在本次实验使用的是/base_footprint坐标。

//Loop once for each leg of the trip
for(int i = 0;i < 2;i++)
{
    ROS_INFO("go straight...!");
    speed.linear.x = linear_speed; // 设置线速度，正为前进，负为后退
    //Get the starting position values
    listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);
    float x_start = transform.getOrigin().x();
    float y_start = transform.getOrigin().y();

　　执行两次循环，每次循环都是机器人移动直行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 cycle
     cmdVelPub.publish(speed);
     loopRate.sleep();
     listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);
     //Get the current position
     float x = transform.getOrigin().x();
     float y = transform.getOrigin().y();
     //Compute the Euclidean distance from the start
     distance = sqrt(pow((x - x_start), 2) +  pow((y - y_start), 2));
}

这个循环是使机器人直行1米。

while( (fabs(turn_angle + angular_tolerance) < M_PI) && (ros::ok()) )
{
    //Publish the Twist message and sleep 1 cycle
    cmdVelPub.publish(speed);
    loopRate.sleep();
    // Get the current rotation
    listener.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 loop
    double delta_angle = fabs(rotation - last_angle);

    //Add to the running total
    turn_angle += delta_angle;

    last_angle = rotation;
}

　　这个循环使机器人在一定angular_tolerance角度误差内旋转180°。abs()求得是正数的绝对值,fabs()求得是浮点数的绝对值。tf::getYaw(transform.getRotation())获取旋转的角度。

五、使用Odometry行走一个正方形

　　设置四个导航点，使机器人移动成一个正方形。效果如图3.

图3

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

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