ros机器人直行1米再旋转180度，最终回到起点

一、功能介绍

　　使机器人直行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;
}

 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101

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

3.编译程序

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

4.测试程序

4.1 启动roscore

   roscore
 
 
1

4.2 启动机器人

4.2.1若是运行仿真机器人

   roslaunch aicroboxi_bringup fake_aicroboxi.launch
 
 
1

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

   roslaunch aicroboxi_bringup minimal.launch
 
 
1

4.3 启动 rviz 图形化显示程序

   roslaunch aicroboxi_rviz view_mobile.launch
 
 
1

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

   rosrun robot_move timed_out_and_back
 
 
1

图1

四、解释部分代码

signal(SIGINT, shutdown);
 
 
1

这覆盖了默认的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;
 
 
1
2
3
4
5
6
7
8

初始化直行速度为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
2
3
4
5
6
7
8

设置旋转角速度为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();
    }
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13

　　不断地发布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();
}
 
 
1
2
3
4
5
6
7
8
9
10
11

这是循环的第二部分，机器人直行停止后以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/Odometry
 
 
1

 

图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;
}



 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145

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

3.编译程序

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

4.测试程序

4.1 启动roscore

   roscore
 
 
1

4.2 启动机器人

4.2.1若是运行仿真机器人

   roslaunch aicroboxi_bringup fake_aicroboxi.launch
 
 
1

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

   roslaunch aicroboxi_bringup minimal.launch
 
 
1

4.3 启动 rviz 图形化显示程序

   roslaunch aicroboxi_rviz view_mobile.launch
 
 
1

4.4 启动odom_out_and_back程序，效果如图2

   rosrun robot_move odom_out_and_back
 
 
1

图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();
  }
}



 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

　　创建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
2
3
4
5
6
7
8
9
10
11
12

　　执行两次循环，每次循环都是机器人移动直行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
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

这个循环是使机器人直行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;
}


 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

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

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

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