（十一）简单的2维机器人仿真器

概述

STDR是一个简单的2维机器人仿真器，它的安装和启动命令如下。

安装
sudo apt-get install ros-$ROS_DISTRO-stdr-simulator
启动
roslaunch stdr_launchers server_with_map_and_gui_plus_robot.launch

启动以后就可以看到一个二维的平面图，喏，就长这样子。

那这个仿真器都是啥意思呢？地图我就不介绍了，不傻都能看懂。就说那个机器人部分吧，迷宫左下角有一个小圆圈，代表了一个抽象的机器人；红线表示假想激光雷达传感器的仿真激光射线，有距离范围；绿线表示仿真的声纳信号，同样有距离范围。
控制机器人的主题是/robot0/cmd_vel，我们可以通过在这个主题上发布消息来控制机器人的移动。

主题信息
rostopic info /robot0/cmd_vel
消息信息
rosmsg show geometry_msgs/Twist

命令行控制机器人移动

第一次控制

rostopic pub -r 2 /robot0/cmd_vel geometry_msgs/Twist '{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}'

这时候控制机器人以0.5m/s的速度向前行进，遇到障碍物后会停止，但是这时候的速度仍然是0.5m/s，需要归零，也就是以下命令。

rostopic pub -r 2 /robot0/cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}'

第二次控制

rostopic pub -r 2 /robot0/cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.1}}'

这个命令的意思是让机器人逆时针旋转，人眼是可以看出来的，当你看到它差不多转到90度的时候，赶紧执行归零命令行，如下。

rostopic pub -r 2 /robot0/cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}'

这样机器人就会停止转动了，像这样子。

第三次控制

rostopic pub -r 2 /robot0/cmd_vel geometry_msgs/Twist '{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}'

仍然让它以0.5m/s的速度向前行进，碰到障碍物后它自己会停下来的。

好了，我们就展示这么多吧，指令控制实在是太麻烦了，其实还是使用ROS节点控制比较方便。

编程控制机器人移动

新建程序包stdr_control，新建cpp文件stdr_open_loop_commander.cpp，别问我为什么这名字这么恶心，书上这么写的，我也懒得自己改了。

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>Simulator via cmd_vel
int main(int argc, char **argv) {
    ros::init(argc, argv, "stdr_commander"); 
    ros::NodeHandle n; 
    ros::Publisher twist_commander = n.advertise<geometry_msgs::Twist>("/robot0/cmd_vel", 1);
    double sample_dt = 0.01; //采样时间，多久发布一次控制信息
    double speed = 1.0; // 前进速度
    double yaw_rate = 0.5; //角速度
    double time_3_sec = 3.0; // 移动时间
    //初始化
    geometry_msgs::Twist twist_cmd;
    twist_cmd.linear.x=0.0;
    twist_cmd.linear.y=0.0;    
    twist_cmd.linear.z=0.0;
    twist_cmd.angular.x=0.0;
    twist_cmd.angular.y=0.0;
    twist_cmd.angular.z=0.0;   
    ros::Rate loop_timer(1/sample_dt);
    double timer=0.0;//控制移动时间
    //归零
    for (int i=0;i<10;i++) {
      twist_commander.publish(twist_cmd);
      loop_timer.sleep();
    }
    //前进
    twist_cmd.linear.x=speed; 
    while(timer<time_3_sec) {
          twist_commander.publish(twist_cmd);
          timer+=sample_dt;
          loop_timer.sleep();
          }
    //归零并旋转
    twist_cmd.linear.x=0.0;
    twist_cmd.angular.z=yaw_rate;
    timer=0.0;//重新定时
    while(timer<time_3_sec) {
          twist_commander.publish(twist_cmd);
          timer+=sample_dt;
          loop_timer.sleep();
          }
    //归零并前进
    twist_cmd.angular.z=0.0;
    twist_cmd.linear.x=speed; 
    timer=0.0; //重新定时
    while(timer<time_3_sec) {
          twist_commander.publish(twist_cmd);
          timer+=sample_dt;
          loop_timer.sleep();
          }
    //归零
    twist_cmd.angular.z=0.0; 
    twist_cmd.linear.x=0.0; 
    for (int i=0;i<10;i++) {
      twist_commander.publish(twist_cmd);
      loop_timer.sleep();
    }               
}

编写CMakeLists.txt文件。

视频不好展示，还是放截图吧。