一、功能介绍
使机器人直行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;
cmdVelPub = node.advertise<geometry_msgs::Twist>(topic, 1);
double rate = 50;
ros::Rate loopRate(rate);
signal(SIGINT, shutdown);
ROS_INFO("timed_out_and_back.cpp start...");
float linear_speed = 0.2;
float goal_distance = 1.0;
float linear_duration = goal_distance / linear_speed;
float angular_speed = 1.0;
float goal_angle = M_PI;
float angular_duration = goal_angle / angular_speed;
int count = 0;
int ticks;
geometry_msgs::Twist speed;
while (ros::ok())
{
speed.linear.x = linear_speed;
ticks = int(linear_duration * rate);
for(int i = 0; i < ticks; i++)
{
cmdVelPub.publish(speed);
loopRate.sleep();
}
cmdVelPub.publish(geometry_msgs::Twist());
ROS_INFO("rotation...!");
speed.linear.x = 0;
speed.angular.z = angular_speed;
ticks = int(angular_duration * rate);
for(int i = 0; i < ticks; i++)
{
cmdVelPub.publish(speed);
loopRate.sleep();
}
speed.angular.z = 0;
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})
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
图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;
for(int i = 0;i < 2;i++)
{
speed.linear.x = linear_speed;
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秒。
speed.linear.x = 0;
/Set the angular speed
speed.angular.z = angular_speed;
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消息使机器人停止。
一、功能介绍
使机器人直行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)
{
double rate = 20;
int count = 0;
ros::init(argc, argv, "go_and_back");
std::string topic = "/cmd_vel";
ros::NodeHandle node;
cmdVelPub = node.advertise<geometry_msgs::Twist>(topic, 1);
ros::Rate loopRate(rate);
geometry_msgs::Twist speed;
signal(SIGINT, shutdown);
ROS_INFO("odom_out_and_back.cpp start...");
float linear_speed = 0.2;
float goal_distance = 1.0;
float angular_speed = 0.5;
double goal_angle = M_PI;
double angular_tolerance = 2.5*M_PI/180;
tf::TransformListener listener;
tf::StampedTransform transform;
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();
}
}
for(int i = 0;i < 2;i++)
{
ROS_INFO("go straight...!");
speed.linear.x = linear_speed;
listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);
float x_start = transform.getOrigin().x();
float y_start = transform.getOrigin().y();
float distance = 0;
while( (distance < goal_distance) && (ros::ok()) )
{
cmdVelPub.publish(speed);
loopRate.sleep();
listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);
float x = transform.getOrigin().x();
float y = transform.getOrigin().y();
distance = sqrt(pow((x - x_start), 2) + pow((y - y_start), 2));
}
cmdVelPub.publish(geometry_msgs::Twist());
ros::Duration(1).sleep();
ROS_INFO("rotation...!");
speed.linear.x = 0;
speed.angular.z = angular_speed;
double last_angle = fabs(tf::getYaw(transform.getRotation()));
double turn_angle = 0;
while( (fabs(turn_angle + angular_tolerance) < M_PI) && (ros::ok()) )
{
cmdVelPub.publish(speed);
loopRate.sleep();
listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);
double rotation = fabs(tf::getYaw(transform.getRotation()));
double delta_angle = fabs(rotation - last_angle);
turn_angle += delta_angle;
last_angle = rotation;
}
speed.angular.z = 0;
cmdVelPub.publish(geometry_msgs::Twist());
ros::Duration(1).sleep();
}
cmdVelPub.publish(geometry_msgs::Twist());
ros::Duration(1).sleep();
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})
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();
}
}
- 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坐标。
for(int i = 0;i < 2;i++)
{
ROS_INFO("go straight...!");
speed.linear.x = linear_speed;
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()) )
{
cmdVelPub.publish(speed);
loopRate.sleep();
listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);
float x = transform.getOrigin().x();
float y = transform.getOrigin().y();
distance = sqrt(pow((x - x_start), 2) + pow((y - y_start), 2));
}
这个循环是使机器人直行1米。
while( (fabs(turn_angle + angular_tolerance) < M_PI) && (ros::ok()) )
{
cmdVelPub.publish(speed);
loopRate.sleep();
listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);
double rotation = fabs(tf::getYaw(transform.getRotation()));
double delta_angle = fabs(rotation - last_angle);
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.