ROS联合webots实战案例(三[1])在webots中使用ROS控制机器人

在webots中使用ROS控制小机器人

为了能和读者进一步讨论问题，建立了一个微信群，方便给大家解答问题，也可以一起讨论问题。
加群链接
注意：

• 再学习本系列教程时，应该已经安装过ROS了并且需要有一些ROS的基本知识,本章节代码过长。

webots版本：2020b rev1
ros版本：melodic

1.从官方案例入手了解基本结构

1. 我们找一篇篇幅较短的代码，比如官方提供的 keyboard_teleop.cpp
   我们先跑一下这个例程看看:

$ roslaunch webots_ros keyboard_teleop.launch

2. 分别按方向键可以实现机器人的移动，运行后的效果：
   

3. 肯定会有同学会好奇，webots中的传感器或者电机是怎么通过ros获取或者控制的呢？
   我们需要注意到webots在机器人控制上可以使用5种语言来控制，分别为c、c++、java、python和matlab
   通过向导->新机器人控制器可以查看，打开后点击下一步界面如下：
   

但是如果使用ROS控制的话，我们不需要创建，因为官方帮我们写好了底层的控制器代码，我们直接使用就行。
4. 在案例弹出的webots软件场景树中，可以看到如下：

其中有两个关键的参数：

controller：选择控制器

controllerArgs：控制器参数属性，如果在同一个场景中有多台使用ros控制器的机器人，配置这个参数进行区分

5. 配置完后重新运行可以看到webots已经成功连接上了ros：
6. 
7. 在终端分别运行rostopic list rosservice list可以看到所有关于传感器的函数都已经发布了
   
   看到这里其实大家都已经知道webots和ros是怎么运行的了。
8. 大家也可以通过ros的一些基本命令对topic或service进行查看

2.从官方案例入手了解基本源码

接下来看一下官方给的keyboard_teleop.cpp代码：

在下面的代码中我会进行注释，如遇到关键点我会标明序号，到代码下面找到对应序号查看

#include "ros/ros.h"

// webots_ros的一些数据服务[1]
#include <webots_ros/Int32Stamped.h>
#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
#include <webots_ros/robot_get_device_list.h>

#include <std_msgs/String.h>

#include <signal.h>
#include <stdio.h>

#define TIME_STEP 32
// 连接控制器所需的变量
static int controllerCount;
static std::vector<std::string> controllerList;
static std::string controllerName;
// 机器人左右轮位置传感器
static double lposition = 0;
static double rposition = 0;
// 左轮服务客户端
ros::ServiceClient leftWheelClient;
webots_ros::set_float leftWheelSrv;
// 右轮服务客户端
ros::ServiceClient rightWheelClient;
webots_ros::set_float rightWheelSrv;
// 时限服务客户端
ros::ServiceClient timeStepClient;
webots_ros::set_int timeStepSrv;
// 键盘服务客户端
ros::ServiceClient enableKeyboardClient;
webots_ros::set_int enableKeyboardSrv;

// 获取在ROS网络中可获得的控制器的名称
void controllerNameCallback(const std_msgs::String::ConstPtr &name) {
  controllerCount++;
  controllerList.push_back(name->data);
  ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
}

// 当用户在控制台中按下ctrl+c 返回的函数
void quit(int sig) {
  enableKeyboardSrv.request.value = 0;
  enableKeyboardClient.call(enableKeyboardSrv);
  timeStepSrv.request.value = 0;
  timeStepClient.call(timeStepSrv);
  ROS_INFO("User stopped the 'keyboard_teleop' node.");
  ros::shutdown();
  exit(0);
}
// 键盘返回函数
void keyboardCallback(const webots_ros::Int32Stamped::ConstPtr &value) {
  int key = value->data;
  int send = 0;

  switch (key) {
    //方向键左键
    case 314:
      lposition += -0.2;
      rposition += 0.2;
      send = 1;
      break;
    //方向键右键
    case 316:
      lposition += 0.2;
      rposition += -0.2;
      send = 1;
      break;
    //方向键上键
    case 315:
      lposition += 0.2;
      rposition += 0.2;
      send = 1;
      break;
    //方向键下键
    case 317:
      lposition += -0.2;
      rposition += -0.2;
      send = 1;
      break;
    case 312:
      ROS_INFO("END.");
      quit(-1);
      break;
    default:
      send = 0;
      break;
  }

  leftWheelSrv.request.value = lposition;
  rightWheelSrv.request.value = rposition;
  if (send) {
    // 判断service服务是否成功发送
    if (!leftWheelClient.call(leftWheelSrv) || !rightWheelClient.call(rightWheelSrv) || !leftWheelSrv.response.success ||
        !rightWheelSrv.response.success)
      ROS_ERROR("Failed to send new position commands to the robot.");
  }
  return;
}

int main(int argc, char **argv) {

  // 在ROS网络上创建一个名称为‘keyboard_teleop’节点
  ros::init(argc, argv, "keyboard_teleop", ros::init_options::AnonymousName);
  ros::NodeHandle n;
  // SIGNAL 具体可参考unix下信号[2]
  signal(SIGINT, quit);

  // 订阅model_name话题获取可用控制器的列表
  ros::Subscriber nameSub = n.subscribe("model_name", 100, controllerNameCa