系统版本: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
四、解释部分代码
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
个人学习笔记,欢迎交流学习。