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
  1. 分别按方向键可以实现机器人的移动,运行后的效果:
    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-BM6e7M9R-1610451233766)(1.gif)]

  2. 肯定会有同学会好奇,webots中的传感器或者电机是怎么通过ros获取或者控制的呢?
    我们需要注意到webots在机器人控制上可以使用5种语言来控制,分别为c、c++、java、python和matlab
    通过向导->新机器人控制器可以查看,打开后点击下一步界面如下:
    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-xnQv816X-1610451233769)(1.jpg)]

但是如果使用ROS控制的话,我们不需要创建,因为官方帮我们写好了底层的控制器代码,我们直接使用就行。
4. 在案例弹出的webots软件场景树中,可以看到如下:
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ybnJSN0M-1610451233770)(2.jpg)]

其中有两个关键的参数:

controller:选择控制器
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-k8GdZQrU-1610451233772)(3.jpg)]

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

  1. 配置完后重新运行可以看到webots已经成功连接上了ros:
  2. [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-QPXgrua8-1610451233773)(4.jpg)]
  3. 在终端分别运行rostopic list rosservice list可以看到所有关于传感器的函数都已经发布了
    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ZcjmYeh2-1610451233775)(5.jpg)]
    看到这里其实大家都已经知道webots和ros是怎么运行的了。
  4. 大家也可以通过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, controllerNameCallback);
  while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
    ros::spinOnce();
    ros::Duration(0.5).sleep();
  }
  ros::spinOnce();

  // 如果存在大于一个控制器,需要让用户选择控制器
  if (controllerCount == 1)
    controllerName = controllerList[0];
  else {
    int wantedController = 0;
    std::cout << "Choose the # of the controller you want to use:\n";
    std::cin >> wantedController;
    if (1 <= wantedController && wantedController <= controllerCount)
      controllerName = controllerList[wantedController - 1];
    else {
      ROS_ERROR("Invalid number for controller choice.");
      return 1;
    }
  }
  nameSub.shutdown();
  // 服务设置
  leftWheelClient = n.serviceClient<webots_ros::set_float>(controllerName + "/left_wheel/set_position");
  rightWheelClient = n.serviceClient<webots_ros::set_float>(controllerName + "/right_wheel/set_position");
  timeStepClient = n.serviceClient<webots_ros::set_int>(controllerName + "/robot/time_step");
  timeStepSrv.request.value = TIME_STEP;

  enableKeyboardClient = n.serviceClient<webots_ros::set_int>(controllerName + "/keyboard/enable");
  enableKeyboardSrv.request.value = TIME_STEP;
  // 判断是否成功发送数据
  if (enableKeyboardClient.call(enableKeyboardSrv) && enableKeyboardSrv.response.success) {
    ros::Subscriber sub_keyboard;
    sub_keyboard = n.subscribe(controllerName + "/keyboard/key", 1, keyboardCallback);
    while (sub_keyboard.getNumPublishers() == 0) {
    }
    ROS_INFO("Keyboard enabled.");
    ROS_INFO("Use the arrows in Webots window to move the robot.");
    ROS_INFO("Press the End key to stop the node.");

    // main loop
    while (ros::ok()) {
      ros::spinOnce();
      if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success)
        ROS_ERROR("Failed to call service time_step for next step.");
    }
  } else
    ROS_ERROR("Could not enable keyboard, success = %d.", enableKeyboardSrv.response.success);

  enableKeyboardSrv.request.value = 0;
  if (!enableKeyboardClient.call(enableKeyboardSrv) || !enableKeyboardSrv.response.success)
    ROS_ERROR("Could not disable keyboard, success = %d.", enableKeyboardSrv.response.success);
  timeStepSrv.request.value = 0;
  timeStepClient.call(timeStepSrv);
  ros::shutdown();
  return (0);
}

[1]
这个表格描述了ROS和Webots设备之间的服务

service name service definition
get_bool bool ask

bool value
get_float bool ask

float64 value
get_float_array bool ask

float64[] values
get_int bool ask

int32 value
get_string bool ask

string value
get_uint64 bool ask

uint64 value
set_bool bool value

bool success
set_float float64 value

bool success
set_float_array float64[] values

bool success
set_int int32 value

bool success
set_string string value

bool success

[2]
因为这个不是我们具体的内容,想了解的同学可以看:
Unix信号详解(Signal的信号说明)

3.控制前配置

  1. 创建功能包
$ cd ~/.catkin_ws/src
$ catkin_create_pkg webots_demo std_msgs rospy roscpp sensor_msgs
  1. 编译功能包
$ cd ~/.catkin_ws
$ catkin_make
  1. 移植webots_ros的srv和msg文件夹
    进入/usr/local/webots/projects/default/controllers/ros/include/文件夹下面的srvmsg文件夹复制刚刚创建的webots_demo功能包内
  2. 配置CMakeList.txt文件
cmake_minimum_required(VERSION 2.8.3)
project(webots_demo)

find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs message_generation)

#######################################
## Declare ROS messages and services ##
#######################################

## Generate messages in the 'msg' folder
  add_message_files(
    FILES
    BoolStamped.msg
    Float64Stamped.msg
    Int32Stamped.msg
    Int8Stamped.msg
    RadarTarget.msg
    RecognitionObject.msg
    StringStamped.msg
  )

## Generate services in the 'srv' folder
  add_service_files(
    FILES
    camera_get_focus_info.srv
    camera_get_info.srv
    camera_get_zoom_info.srv
    display_draw_line.srv
    display_draw_oval.srv
    display_draw_pixel.srv
    display_draw_polygon.srv
    display_draw_rectangle.srv
    display_draw_text.srv
    display_get_info.srv
    display_image_copy.srv
    display_image_delete.srv
    display_image_load.srv
    display_image_new.srv
    display_image_paste.srv
    display_image_save.srv
    display_set_font.srv
    field_get_bool.srv
    field_get_color.srv
    field_get_count.srv
    field_get_float.srv
    field_get_int32.srv
    field_get_node.srv
    field_get_rotation.srv
    field_get_string.srv
    field_get_type.srv
    field_get_type_name.srv
    field_get_vec2f.srv
    field_get_vec3f.srv
    field_import_node.srv
    field_import_node_from_string.srv
    field_remove_node.srv
    field_remove.srv
    field_set_bool.srv
    field_set_color.srv
    field_set_float.srv
    field_set_int32.srv
    field_set_rotation.srv
    field_set_string.srv
    field_set_vec2f.srv
    field_set_vec3f.srv
    get_bool.srv
    get_float_array.srv
    get_float.srv
    get_int.srv
    get_string.srv
    get_uint64.srv
    get_urdf.srv
    gps_decimal_degrees_to_degrees_minutes_seconds.srv
    lidar_get_frequency_info.srv
    lidar_get_info.srv
    lidar_get_layer_point_cloud.srv
    lidar_get_layer_range_image.srv
    motor_set_control_pid.srv
    mouse_get_state.srv
    node_add_force_or_torque.srv
    node_add_force_with_offset.srv
    node_get_center_of_mass.srv
    node_get_contact_point.srv
    node_get_field.srv
    node_get_id.srv
    node_get_number_of_contact_points.srv
    node_get_name.srv
    node_get_orientation.srv
    node_get_parent_node.srv
    node_get_position.srv
    node_get_static_balance.srv
    node_get_status.srv
    node_get_type.srv
    node_get_velocity.srv
    node_remove.srv
    node_reset_functions.srv
    node_move_viewpoint.srv
    node_set_visibility.srv
    node_set_velocity.srv
    pen_set_ink_color.srv
    range_finder_get_info.srv
    receiver_get_emitter_direction.srv
    robot_get_device_list.srv
    robot_set_mode.srv
    robot_wait_for_user_input_event.srv
    save_image.srv
    set_bool.srv
    set_float.srv
    set_float_array.srv
    set_int.srv
    set_string.srv
    skin_get_bone_name.srv
    skin_get_bone_orientation.srv
    skin_get_bone_position.srv
    skin_set_bone_orientation.srv
    skin_set_bone_position.srv
    speaker_is_sound_playing.srv
    speaker_speak.srv
    speaker_play_sound.srv
    supervisor_get_from_def.srv
    supervisor_get_from_id.srv
    supervisor_movie_start_recording.srv
    supervisor_set_label.srv
    supervisor_virtual_reality_headset_get_orientation.srv
    supervisor_virtual_reality_headset_get_position.srv
  )

## Generate added messages and services with any dependencies listed here
  generate_messages(
    DEPENDENCIES
    std_msgs
    sensor_msgs
  )

###################################
## catkin specific configuration ##
###################################

catkin_package(
   CATKIN_DEPENDS roscpp rospy std_msgs sensor_msgs message_runtime
)

###########
## Build ##
###########

include_directories(
  ${catkin_INCLUDE_DIRS}
)

## Instructions for keyboard_teleop node

#############
## Install ##
#############

  1. 配置package.xml文件,添加:
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
  1. webots_demo功能包内创建worlds文件夹,并且将第二章创建的机器人地图放在worlds文件夹中,方便我们直接调用。
  2. 修改webots中机器人控制器:
    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-2vML8eoi-1610451233775)(6.jpg)]
  3. webots_demo功能包内创建launch文件夹,并且创建一个名为webots.launch的launch文件,代码如下
    (因为笔者之前在单独开启webots后,即使成功连接上了ros网络,但是依旧无法控制,使用launch文件可以解决这个问题)
<?xml version="1.0"?>
<launch>
  <!-- 启动webots -->
  <arg name="no-gui" default="false," doc="Start Webots with minimal GUI"/>
  <include file="$(find webots_ros)/launch/webots.launch">
    <arg name="mode" value="realtime"/>
    <arg name="no-gui" value="$(arg no-gui)"/>
    <arg name="world" value="$(find webots_demo)/worlds/webots_map.wbt"/>
  </include>
</launch>

4.编写键盘控制机器人的程序

#include <signal.h>
#include <std_msgs/String.h>
#include "ros/ros.h"
 
#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
#include <webots_ros/Int32Stamped.h>
 
using namespace std;
#define TIME_STEP 32    //时钟
#define NMOTORS 2       //电机数量
#define MAX_SPEED 2.0   //电机最大速度
 
ros::NodeHandle *n;

static int controllerCount;
static vector<string> controllerList; 

ros::ServiceClient timeStepClient;          //时钟通讯客户端
webots_ros::set_int timeStepSrv;            //时钟服务数据

ros::ServiceClient set_velocity_client;     //速度设置客户端
webots_ros::set_float set_velocity_srv;     //速度设置数据

ros::ServiceClient set_position_client;     //位置设置客户端
webots_ros::set_float set_position_srv;     //位置设置数据


double speeds[NMOTORS]={0.0,0.0};           //电机速度值 0~100

// 匹配电机名
static const char *motorNames[NMOTORS] ={"left_motor", "right_motor"};
/*******************************************************
* Function name :updateSpeed
* Description   :将速度请求以set_float的形式发送给set_velocity_srv
* Parameter     :无
* Return        :无
**********************************************************/
void updateSpeed() {   
    for (int i = 0; i < NMOTORS; ++i) {
        // 更新速度
        set_velocity_client = n->serviceClient<webots_ros::set_float>(string("/robot/") + string(motorNames[i]) + string("/set_velocity"));   
        set_velocity_srv.request.value = -speeds[i];
        set_velocity_client.call(set_velocity_srv);
    }
}

/*******************************************************
* Function name :controllerNameCallback
* Description   :控制器名回调函数,获取当前ROS存在的机器人控制器
* Parameter     :
        @name   控制器名
* Return        :无
**********************************************************/
void controllerNameCallback(const std_msgs::String::ConstPtr &name) { 
    controllerCount++; 
    controllerList.push_back(name->data);//将控制器名加入到列表中
    ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());

}
/*******************************************************
* Function name :quit
* Description   :退出函数
* Parameter     :
        @sig   信号
* Return        :无
**********************************************************/
void quit(int sig) {
    ROS_INFO("User stopped the '/robot' node.");
    timeStepSrv.request.value = 0; 
    timeStepClient.call(timeStepSrv); 
    ros::shutdown();
    exit(0);
}
/*******************************************************
* Function name :键盘返回函数
* Description   :当键盘动作,就会进入此函数内
* Parameter     :
        @value   返回的值
* Return        :无
**********************************************************/
void keyboardDataCallback(const webots_ros::Int32Stamped::ConstPtr &value)
{
    // 发送控制变量
    int send =0;
    //ROS_INFO("sub keyboard value = %d",value->data);
    switch (value->data)
    {
        // 左转
        case 314:
            speeds[0] = 5.0;
            speeds[1] = -5.0;
            send=1;
            break;
        // 前进
        case 315:
            speeds[0] = 5.0;
            speeds[1] = 5.0;
            send=1;
            break;
        // 右转
        case 316:
            speeds[0] = -5.0;
            speeds[1] = 5.0;
            send=1;
            break;
        // 后退
        case 317:
            speeds[0] = -5.0;
            speeds[1] = -5.0;
            send=1;
            break;
        // 停止
        case 32:
            speeds[0] = 0;
            speeds[1] = 0;
            send=1;
            break;
        default:
            send=0; 
            break;
    }
    //当接收到信息时才会更新速度值
    if (send)
    {
        updateSpeed();
        send=0;
    } 
}

int main(int argc, char **argv) {
    setlocale(LC_ALL, ""); // 用于显示中文字符
    string controllerName;
    // 在ROS网络中创建一个名为robot_init的节点
    ros::init(argc, argv, "robot_init", ros::init_options::AnonymousName);
    n = new ros::NodeHandle;
    // 截取退出信号
    signal(SIGINT, quit);

    // 订阅webots中所有可用的model_name
    ros::Subscriber nameSub = n->subscribe("model_name", 100, controllerNameCallback);
    while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
        ros::spinOnce();
        ros::spinOnce();
        ros::spinOnce();
    }
    ros::spinOnce();
    // 服务订阅time_step和webots保持同步
    timeStepClient = n->serviceClient<webots_ros::set_int>("robot/robot/time_step");
    timeStepSrv.request.value = TIME_STEP;

    // 如果在webots中有多个控制器的话,需要让用户选择一个控制器
    if (controllerCount == 1)
        controllerName = controllerList[0];
    else {
        int wantedController = 0;
        cout << "Choose the # of the controller you want to use:\n";
        cin >> wantedController;
        if (1 <= wantedController && wantedController <= controllerCount)
        controllerName = controllerList[wantedController - 1];
        else {
        ROS_ERROR("Invalid number for controller choice.");
        return 1;
        }
    }
    ROS_INFO("Using controller: '%s'", controllerName.c_str());
    // 退出主题,因为已经不重要了
    nameSub.shutdown();

    //初始化电机
    for (int i = 0; i < NMOTORS; ++i) {
        // position速度控制时设置为缺省值INFINITY   
        set_position_client = n->serviceClient<webots_ros::set_float>(string("/robot/") + string(motorNames[i]) + string("/set_position"));   
        set_position_srv.request.value = INFINITY;
        if (set_position_client.call(set_position_srv) && set_position_srv.response.success)     
            ROS_INFO("Position set to INFINITY for motor %s.", motorNames[i]);   
        else     
            ROS_ERROR("Failed to call service set_position on motor %s.", motorNames[i]);
        // velocity初始速度设置为0   
        set_velocity_client = n->serviceClient<webots_ros::set_float>(string("/robot/") + string(motorNames[i]) + string("/set_velocity"));   
        set_velocity_srv.request.value = 0.0;   
        if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)     
            ROS_INFO("Velocity set to 0.0 for motor %s.", motorNames[i]);   
        else     
            ROS_ERROR("Failed to call service set_velocity on motor %s.", motorNames[i]);
    }   


    // 服务订阅键盘
    ros::ServiceClient keyboardEnableClient;
    webots_ros::set_int keyboardEnablesrv;
   
    keyboardEnableClient = n->serviceClient<webots_ros::set_int>("/robot/keyboard/enable");
    keyboardEnablesrv.request.value = TIME_STEP;
    if (keyboardEnableClient.call(keyboardEnablesrv) && keyboardEnablesrv.response.success)
    {
        ros::Subscriber keyboardSub;
        keyboardSub = n->subscribe("/robot/keyboard/key",1,keyboardDataCallback);
        while (keyboardSub.getNumPublishers() == 0) {}
        ROS_INFO("Keyboard enabled.");
        ROS_INFO("control directions:");
        ROS_INFO("  ↑  ");
        ROS_INFO("← ↓ →");
        ROS_INFO("stop:space");
        ROS_INFO("Use the arrows in Webots window to move the robot.");
        ROS_INFO("Press the End key to stop the node.");
        while (ros::ok()) {   
            ros::spinOnce();
            if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success)
            {  
                ROS_ERROR("Failed to call service time_step for next step.");     
                break;   
            }   
            ros::spinOnce();
        } 
    }
    else
    ROS_ERROR("Could not enable keyboard, success = %d.", keyboardEnablesrv.response.success);
    //退出时时钟清零
    timeStepSrv.request.value = 0;
    timeStepClient.call(timeStepSrv);
    ros::shutdown(); 
    return 0;
}

5.最后

  1. 配置CMakeList.txt,在最下面添加:
add_executable(velocity_keyboard src/velocity_keyboard.cpp)
add_dependencies(velocity_keyboard webots_ros_generate_messages_cpp)
target_link_libraries(velocity_keyboard	${catkin_LIBRARIES})
  1. 编译功能包
$ cd ~/.catkin_ws
$ catkin_make
  1. 运行launch文件,可能不会那么快可以TAB出来,打进去一样可以运行
$ roslaunch webots_demo webots.launch
  1. 运行编写好的代码
$ rosrun webots_demo velocity_keyboard
  1. 效果
    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-JOaGsCcJ-1610451233776)(2.gif)]

结语

本文也是基于笔者的学习和使用经验总结的,主观性较强,如果有哪些不对的地方或者不明白的地方,欢迎评论区留言交流~
用手柄控制机器人的操作会单独写个文档发出来哦~~
下一节加入雷达试试吧。

✌Bye

猜你喜欢

转载自blog.csdn.net/xiaokai1999/article/details/112545405
今日推荐