(五)turtlebot3用代码控制机器人直线及圆弧运动(/cmd_vel geometry_msgs/Twist的使用)

本文实现通过代码发送控制命令给机器人，让机器人安装特定的轨迹行走．希望实现的功能为直走和走一个圆形．

1.启动gazebo并加载空地图

roslaunch turtlebot3_gazebo turtlebot3_empty_world.launch

此时如果启动键盘控制的话机器人就可动起来了。
为了用代码控制机器人运动，需要将键盘控制程序替换为自定义的程序。

2.启动slam并在rviz中显示

roslaunch turtlebot3_slam turtlebot3_slam.launch

3.ros发送cmd_vel

rostopic list 发现控制命令是通过cmd_vel发送的。
rostopic echo /cmd_vel 可看到命令形式如

linear: 
  x: 0.0
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: 0.0
---

先让机器人转圈，输入这个命令

rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.1, y: 0, z: 0}, angular: {x: 0, y: 0, z: -0.5}}'

让机器人直行，输入

rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.1, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}'

4./cmd_vel geometry_msgs/Twist使用示例

新建一个节点不断发送/cmd_vel 控制机器人运动的节点

mkdir -p cmdveltest/src
cd cmdveltest/src
catkin_create_pkg cmdveltest geometry_msgs
mkdir src
cd src
touch cmdveltest.cpp
cd cmdveltest/src
catkin_make

在CMakeLists.txt中加入

add_executable(cmdveltest src/cmdveltest.cpp)
target_link_libraries(cmdveltest ${catkin_LIBRARIES})

cmdveltest.cpp内容如下

#include 
#include 
#include 
#include 
#include

using namespace std;
int main(int argc,char** argv)
{
    ros::init(argc, argv, "cmdveltest");
     ros::NodeHandle cmdh;
    ros::Publisher cmdpub= cmdh.advertise("cmd_vel", 1, true);;
    ros::Rate r(60);
    while(1){
        geometry_msgs::Twist twist;
        geometry_msgs::Vector3 linear;
        linear.x=0.1;
        linear.y=0;
        linear.z=0;
        geometry_msgs::Vector3 angular;
        angular.x=0;
        angular.y=0;
        //直行
        //angular.z=0;
        //转圈
        angular.z=-0.5;
        twist.linear=linear;
        twist.angular=angular;

        cmdpub.publish(twist);
        cout<<"hello"<// ros::spinOnce();
        //r.sleep();
        sleep(1);
    }
    return 0;
}

编译后启动gazebo,启动测试程序

rosrun cmdveltest cmdveltest

发现机器人跑得飞起