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。

ROS機器人直行1米，接着旋轉180°，再傳回到起始點。之二

圖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 = ;
  int count = ;//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, );

  //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 = ;

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


  //Set the rotation speed to 0.5 radians per second
  float angular_speed = ;

  //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 = *M_PI/; //角度轉換成弧度: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() );
    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() );
        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 = ;i < ;i++)
  {
    ROS_INFO("go straight...!");
    speed.linear.x = linear_speed; // 設定線速度，正為前進，負為後退
    //Get the starting position values
    listener.lookupTransform(odom_frame, base_frame, ros::Time(), transform);
    float x_start = transform.getOrigin().x();
    float y_start = transform.getOrigin().y();
    // Keep track of the distance traveled
    float distance = ;
    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(), 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), ) +  pow((y - y_start), ));
    }
    //Stop the robot before the rotation
    cmdVelPub.publish(geometry_msgs::Twist());
    ros::Duration().sleep(); // sleep for  a second
    ROS_INFO("rotation...!");
    //Now rotate left roughly 180 degrees
    speed.linear.x = ;
    //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 = ;
    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(), 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 = ;
    cmdVelPub.publish(geometry_msgs::Twist());
    ros::Duration().sleep(); // sleep for  a second
  }
  cmdVelPub.publish(geometry_msgs::Twist());
  ros::Duration().sleep(); // sleep for  a second
  ROS_INFO("odom_out_and_back.cpp ended!");
  ros::shutdown();
  return ;
}

特别說明：代碼中的注釋大部分引用《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若是運作仿真機器人

4.2.2若是運作真實的機器人平台

4.3 啟動 rviz 圖形化顯示程式

4.4 啟動odom_out_and_back程式，效果如圖2

rosrun robot_move odom_out_and_back

ROS機器人直行1米，接着旋轉180°，再傳回到起始點。之二

圖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() );
    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() );
    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 = ;i < ;i++)
{
    ROS_INFO("go straight...!");
    speed.linear.x = linear_speed; // 設定線速度，正為前進，負為後退
    //Get the starting position values
    listener.lookupTransform(odom_frame, base_frame, ros::Time(), 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 = ;
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(), 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), ) +  pow((y - y_start), ));
}

這個循環是使機器人直行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(), 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.

ROS機器人直行1米，接着旋轉180°，再傳回到起始點。之二

圖3

源代碼位址：https://github.com/KeoChi/robot_move

個人學習筆記，歡迎交流學習。

ROS機器人直行1米，接着旋轉180°，再傳回到起始點。之二

一、功能介紹

二、實作方法

三、實驗步驟

1.在robot_move/src/裡建立odom_out_and_back.cpp，并粘貼如下代碼：

2.修改robot_move目錄下的CMakeLists.txt

3.編譯程式

4.測試程式

4.1 啟動roscore

4.2 啟動機器人

4.2.1若是運作仿真機器人

4.2.2若是運作真實的機器人平台

4.3 啟動 rviz 圖形化顯示程式

4.4 啟動odom_out_and_back程式，效果如圖2

四、解釋部分代碼

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

繼續閱讀

ROS Navigation之amcl源碼解析（完全詳解）0. 寫在最前面1. amcl是幹什麼的2. 總體情況3. amcl_node.cpp參考

or frame [laser]: No transform to fixed frame [map]

機器人學空間描述和坐标變換

機器人學導論第三章

ros安裝-Ubuntu14.04

ROS Stage學習

Android智能機器人

java實作小豆聊天機器人接口

React實作聊天機器人

2018國際人工智能與機器人科技峰會

從其他領域和智能機器人的聯系，淺談人工智能的前景

OpenAI ChatGPT 人工智能機器人注冊使用，能以中文對答如流的機器人

當人工智能機器人具有七情六欲

人工智能：人類潛在的毀滅者嗎

Python 搭建一個簡易QQ機器人

ROS機器人Diego 1#制作（五）base controller---角速度的标定