前往小程序,Get更优阅读体验!
立即前往
首页
学习
活动
专区
工具
TVP
发布
社区首页 >专栏 >ROS联合webots实战案例(三)在webots中使用ROS控制小机器人[1]

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

原创
作者头像
锡城筱凯
修改2021-02-07 10:22:49
1.3K0
修改2021-02-07 10:22:49
举报
文章被收录于专栏:ROS联合webots实战

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

注意:

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

博客地址:https://blog.csdn.net/xiaokai1999

webots版本:2020b rev1

ros版本:melodic

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

  1. 我们找一篇篇幅较短的代码,比如官方提供的 keyboard_teleop.cpp 我们先跑一下这个例程看看:$ roslaunch webots_ros keyboard_teleop.launch
  2. 分别按方向键可以实现机器人的移动,运行后的效果:
    1.gif
    1.gif
  3. 肯定会有同学会好奇,webots中的传感器或者电机是怎么通过ros获取或者控制的呢? 我们需要注意到webots在机器人控制上可以使用5种语言来控制,分别为c、c++、java、python和matlab 通过向导->新机器人控制器可以查看,打开后点击下一步界面如下:
    1.jpg
    1.jpg
    但是如果使用ROS控制的话,我们不需要创建,因为官方帮我们写好了底层的控制器代码,我们直接使用就行。
  4. 在案例弹出的webots软件场景树中,可以看到如下:
    2.jpg
    2.jpg
    其中有两个关键的参数:controller:选择控制器

3.jpg
3.jpg
controllerArgs:控制器参数属性,如果在同一个场景中有多台使用ros控制器的机器人,配置这个参数进行区分 5. 配置完后重新运行可以看到webots已经成功连接上了ros:
4.jpg
4.jpg
6. 在终端分别运行rostopic list rosservice list可以看到所有关于传感器的函数都已经发布了
5.jpg
5.jpg
看到这里其实大家都已经知道webots和ros是怎么运行的了。 7. 大家也可以通过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 = controllerList0; 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 = controllerListwantedController - 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$ cd ~/.catkin_ws $ catkin_makecmake_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 ## #############<build_depend>message_generation</build_depend> <exec_depend>message_runtime</exec_depend>
  2. 编译功能包
  3. 移植webots_ros的srv和msg文件夹 进入/usr/local/webots/projects/default/controllers/ros/include/文件夹下面的srvmsg文件夹复制刚刚创建的webots_demo功能包内
  4. 配置CMakeList.txt文件
  5. 配置package.xml文件,添加:
  6. webots_demo功能包内创建worlds文件夹,并且将第二章创建的机器人地图放在worlds文件夹中,方便我们直接调用。
  7. 修改webots中机器人控制器:
    6.jpg
    6.jpg
  8. 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})$ cd ~/.catkin_ws $ catkin_make$ roslaunch webots_demo webots.launch$ rosrun webots_demo velocity_keyboard
  2. 编译功能包
  3. 运行launch文件,可能不会那么快可以TAB出来,打进去一样可以运行
  4. 运行编写好的代码
  5. 效果undefined
    2.gif
    2.gif

结语

本文也是基于笔者的学习和使用经验总结的,主观性较强,如果有哪些不对的地方或者不明白的地方,欢迎评论区留言交流~

用手柄控制机器人的操作会单独写个文档发出来哦~~

下一节加入雷达试试吧。

✌Bye

原创声明:本文系作者授权腾讯云开发者社区发表,未经许可,不得转载。

如有侵权,请联系 cloudcommunity@tencent.com 删除。

原创声明:本文系作者授权腾讯云开发者社区发表,未经许可,不得转载。

如有侵权,请联系 cloudcommunity@tencent.com 删除。

评论
登录后参与评论
0 条评论
热度
最新
推荐阅读
目录
  • 在webots中使用ROS控制小机器人
    • 1.从官方案例入手了解基本结构
      • 2.从官方案例入手了解基本源码
        • 3.控制前配置
          • 5.最后
            • 结语
            领券
            问题归档专栏文章快讯文章归档关键词归档开发者手册归档开发者手册 Section 归档