ROS联合Webots仿真
文章开始之前首先提出以下几点:
- 本文的目的:通过在Webots中搭建仿真环境使读者快速入门Webots,能使用Webots开展ROS下的仿真工作,为实际项目的落地打好基础。
- 笔者的工作内容:整合关于Webots的一些较好的入门资料,通过跑通一些例程:一如建图、导航等,与读者一起学习并且实操Webots这款仿真软件,后续的工作笔者也在学习当中,敬待更新;
- 为什么要学Webots:Webots是一款类似于Gazebo的物理仿真软件,其优点在于集成了大量的场景文件,同时在社区内有许多开源的资源可使用(当然这些优点其实大部分仿真软件都具备,但笔者一直认为:会用工具本身也是一种能力,多掌握一种工具不是一件坏事,这样面对实际的问题也有多种工具可供选择)令我感觉最深刻的是,需要自己编写控制器代码,这点虽然不是新手友好型,但是这点可以当作一种算法验证的工具。
注:本文Ubuntu系统是16.04,ros版本为kinetic,所需的Webots版本为2020a rev1。
一、Webots的安装和配置
1. 下载并安装Webots
由于git和Webots官网下载速度较慢,这里附上百度网盘链接(实际上读者也可以尝试前面两种下载方式,因为网盘下载速度更慢…):
链接:https://pan.baidu.com/s/1uAafKhKggayauXdWxTbXJA
提取码:zlg7
- 下载完成后进入文件所在目录,打开终端输入以下指令解压安装包:
dpkg -i webots_2020a-rev1_amd64.deb
- 安装webots_ros功能包:
sudo apt install ros-kinetic-webots-ros
2. 配置Webots
- 安装完成后进入
/usr/local/webots/projects/languages/
文件夹下,里面有四个文件夹。
- 将其中的webots_ros文件复制到
catkin_ws/src
下;- 把controllers、plugings、worlds剩余三个文件夹复制到
catkin_ws/src
下刚刚复制过来的webots_ros下;- 进入
/usr/local/webots/projects/default/controllers/ros/include/
下,将其中的srv和msg文件夹也复制到catkin_ws/src/webots_ros
下,完成复制结果如下图:
- 添加WEBOTS_HOME环境变量。打开bashrc文件:
gedit ~/.bashrc
在文件最后一行加入下列代码,点击保存:
export WEBOTS_HOME=/usr/local/webots
- 进入
catkin_ws/src
打开终端编译:
catkin_make
- 打开官方例程pioneer3at机器人:
roslaunch webots_ros pioneer3at.launch
可以看到下面提示栏里提示:
[ros] [ INFO] [1634120441.840263312]: The controller is now connected to the ROS master.
至此也完成了Webots的安装与配置。
二、搭建仿真环境
上一章节中我们安装并配置了Webots,并且运行了官方的Webots例程。本章将为读者讲解如何搭建一个简单的仿真环境,在Webots中搭建一个属于自己的双轮差速机器人。
1. 创建一个新世界
- step 1:
打开 Webots,设置中文模式:按下Win键,搜索Webots,为了方便起见,建议把图标固定到任务栏;菜单栏选择
Tools->Preferences
进入如下界面把Language设置为中文。
这里设置为中文后,软件内只有部分位置被翻译过来,不过没关系,后续笔者会对用到的地方做出详细的解释。
- step 2:
菜单栏选择
文件->新世界
,创建一个新的地图。
- step 3:
左侧窗口我们称之为场景树,在场景树中点击
worldinfo
,可以看到有个“+”按钮被激活,点击该按钮。
- step 4:
在弹出的窗口中为环境添加节点:找到
PROTO nodes(Webots Projects)->objects->backgrounds
,将展开后的两个节点均Add进新建的世界。
- step 5:
添加地板:找到
PROTO nodes(Webots Projects)->objects->floors
,将展开后的RectangleArena(solid)
加入世界中。
以上操作过后,显示的世界如下图:
我们可以改变场景树中的RectangleArena->floorsize
:双击后改为x=5m、y=5m,方便后面进行小车的仿真。
2. 创建小车模型
- step 1:建立车身:
- 在场景树中添加Robot节点;
- 双击
Robot->children
节点,创建Base_nodes->Shape
节点;- 在场景树中单击
Robot->children->Shape
,下面会弹出一个DEF框,添加定义为body
;
- 双击
appearance
添加颜色纹理Base_nodes->Appearance
,双击geometry
添加形状Base_nodes->Box
;- 双击
appearance->material
添加金属属性,可以修改其节点下的diffusecolor
属性更改颜色;- 在
geometry
节点添加几何属性,设置车体大小:x=0.5m、y=0.15m、z=0.5m,在translation
节点中将其在y上坐标设置高一些,方便后面加轮子。(注意:translation
节点的参数值都是相对于世界坐标系的物体坐标)现在得到的车身如下图所示:
- 在
Robot
节点下双击boundingObject
:因为上面DEF了body,直接use body即可;同理双击Robot
节点下重力属性physics
(只要创建出physics子节点即可)
- step 2:加入车轮:
- 在
Robot->children
下创建一个Base_nodes->HingeJoint
节点,DEF为left_wheel,该节点下会出现三个节点,如下图所示:
- 在
HingeJoint->jointParameters
节点下创建子节点HingeJointParameters
;该节点主要用来设置:
- 铰链轴的方向(axis,电机旋转轴的平行向量)
- 铰链轴的位置(anchor,表示电机转轴相对于父节点的坐标偏移量)
- 弹簧系数(springConstant)
- 阻尼系数系数(dampingConstant)
- 静摩擦系数(staticFriction)
- 点击
查看–>可选显示–>show joint Axes
或者同时按下ctrl+F5
,可以显示出旋转轴,方便我们调整旋转轴及旋转原点;- 在
HingeJoint->device
节点下添加电机Rotatioal Motor
,Rotatioal Motor->name
命名为left_motor
;该节点主要用来设置:
- 电机加速度(acceleration)
- 消耗因数(comsuptionFactor)
- PID参数(controlPID,从左到右为P、I、D,位置控制模式)
- 电机最大速度(maxVelocity)
- 电机最大转矩(maxTorque)
- 在
HingeJoint->endPoint
节点下,增加Base_nodes->soild
节点,该节点主要是设置电机模型以及其他功能参数,之后在soild->children
节点下创建一个shape
节点,在shape
节点下设置轮子的形状为Cylinder
;- 通过调整
translation
和rotation
,将轮子设置到合适的位置,轮子大小设置为(height=0.08 radius=0.12)如图:
- 接下来通过调整
axis
和anchor
,将电机旋转轴调整到正确位置,比如我这里电机的位置相对于车体(x=0.31、y=0、z=0),于是我将旋转轴相对于父节点(body)的位置通过anchor
调整至此,并且修改axis
的各个值,观察到我的轮子是绕x轴旋转,所以将axis设置为(1 0 0)。可以看到,轮子处有一个黑线(如上图中所示),表示旋转轴在这里,黑线表示的就是电机旋转轴的位置和方向。笔者在这里设置的(axis=2 0 0)m和(anchor=0.31 0 0)m- 最后,在
HingeJoint->endPoint
节点下给轮子增加碰撞属性boundingObject
(与轮子大小一致)及重力属性physics
(创建一个physics子节点)- 接下来复制出另一个轮子,按照上面的方法调整
axis
和anchor
,并且更改电机name为right_motor
以上操作完成后,模型变为如下图所示:
- step 3:添加万向轮
- 在
Robot->children
下创建一个Base_nodes->HingeJoint
节点;- 创建
jointParameters
节点,axis设置为(0 1 0),anchor设置为(0 -0.09 0.23);- 创建
endpoint
节点,万向轮是被动轮,不需要电机驱动(HingeJoint->device
节点不需要创建子节点);- 在
HingeJoint->endPoint
节点下,增加Base_nodes->soild
节点,soild->children
节点下创建一个shape
节点,设置轮子的形状为sphere
;- 在
HingeJoint->endPoint
节点下给轮子增加碰撞属性boundingObject
及重力属性physics
;- 复制一个万向轮到anchor(0,-0.09,0.23)。
可以打开ctrl+F5
调整axis
和anchor
,最后模型如下图所示:
- step 4:加入激光雷达
Robot->children
节点下双击添加PROTO nodes—>devices->sick->SickLms291(lidar)
;在sick(Sensor Intelligence)公司官网上可以查到SickLms291这款雷达的各项参数:
工作范围示意图如下图所示:
这里注意到:其扫描范围只有前方一个半圆区域,这点不同于常见的激光雷达(例如Rplidar);
- 改变雷达的
translation
,移动到合适的位置,并且保证:雷达的坐标轴各轴方向与车体各轴方向相同。
- step 5:加入GPS和IMU
Robot->children
下添GPS"gps"
和InertialUnit"inertial unit"
至此,小车的模型我们就建立完成,点击窗口上的运行按钮,便可以看到小车由于自身的重力属性落在了地板上。
最后建议在环境中加入一些障碍物,例如墙体、箱体之类,便于后面仿真,对于此便不再操作和赘述。
此时,我们将世界保存为webots_map.wbt
3. 创建控制器
前面两小节我们已经完成世界模型和小车模型的搭建,本节在此基础上需要创建一个控制器,通过ROS来实现对小车的控制。在实现控制前,我们需要一些前置的工作。
(1)配置功能包
- 命令行创建功能包:
cd ~/.catkin_ws/src
catkin_create_pkg webots_demo std_msgs rospy roscpp sensor_msgs
- 编译:
cd ~/.catkin_ws
catkin_make
- 把
/usr/local/webots/projects/default/controllers/ros/include/
下的srv
和msg
文件夹复制到catkin_ws/src/webots_demo
下; - 进入
webots_demo
的CMakeList.txt
文件进行配置,将附录中[1]段代码粘贴进文件并保存; - 进入同目录下webots_demo的
package.xml
文件,加入两行:
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
- 同目录下建立
world
、launch
和controller
文件夹,controller
下建立my_controller
文件夹; - 将上一小节建立的世界文件
webots_map.wbt
放入world
中; - 在
launch
文件夹中创建webots.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>
其实也可以手动连接ROS master
,开启终端输入如下指令后,用Webots打开webots_demo下的webots_map.wbt
世界文件即可
roscore
- 用
Webots
打开世界文件,展开Robot
节点,双击controller
,将控制器从void
改为ros
,controllerArgs
改为--name=robot
;
(2)编写控制器(键盘控制小车)
- cpp文件代码见附录中[2];
- 将该文件命名为my_controller.cpp,分别复制到
catkin_ws/src/webots_demo/controller/my_controller
和catkin_ws/src/webots_demo/src
下 - 在
catkin_ws/src/webots_demo/CMakeList.txt
最后补充:
add_executable(my_controller src/my_controller.cpp)
add_dependencies(my_controller webots_ros_generate_messages_cpp)
target_link_libraries(my_controller ${catkin_LIBRARIES})
这三行使得我们编译功能包后会生成对应的可执行文件。
三、ROS联合Webots进行仿真
在前面两章的基础之上,我们已经完成了仿真环境的搭建,本章节将动手实操,用我们写好的键盘控制器来控制仿真环境中的机器人,并对其中的一些数据进行分析。
1. 运行仿真环境
- 编译功能包
webots_demo
:
cd ~/.catkin_ws
catkin_make
- 运行
webots.launch
文件,启动世界:
roslaunch webots_demo webots.launch
这里如果出现一些问题,例如笔者出现了启动launch文件后Webots闪退,可以打开launch文件检查一下做出相应的修改,也可以用第二章第3小节里提到的手动连接ROS master
的方法。此处旨在让仿真环境连接上ROS,才能进行后续操作。
- 世界文件运行起来后,运行控制器,启动键盘控制节点:
rosrun webots_demo my_controller
这个时候鼠标点击仿真环境地图内,按下键盘上的方向键即可操控小车。
- 查看rosservice:
rosservice list
可以看到如下服务:
- 开启雷达服务:
rosservice call /robot/Sick_LMS_291/enable "value: 1"
终端显示:
success: True
- 查看rostopic:
rostopic list
这里可以看到:键盘节点、雷达均有话题发布
键盘话题:/robot/keyboard/key
雷达发布的话题:/robot/Sick_LMS_291/laser_scan/layer0
、
/robot/Sick_LMS_291/range_image
- 为了可视化的看到雷达的数据,我们可以打开rviz:
rviz
将左手Displays栏里的Fixed Frame改为robot/Sick_LMS_291
点击Add->By topic
订阅话题/robot->/Sick_LMS_291->/laser_scan->/layer0->LaserScan
和/robot->/Sick_LMS_291->/range_image->Image
可以看到雷达扫描的数据被可视化:
在Webots中用鼠标点击一下background,然后用键盘上的方向键来控制小车运动,可以看到rviz中LaserScan图像在实时变化:
2. 导航前置
在前文的基础之上,我们可以像以往在Gazebo里仿真一样,实现移动机器人的自主导航。前文加入了三个传感器:
GPS:用于Webots中移动小车的定位;
IMU:利用获取的惯性信息来补偿小车位姿;
Sick_LMS_291:识别障碍信息,用于地图构建和避障。
具体实现小车导航的仿真,可转到Reference[1]中的https://blog.csdn.net/xiaokai1999/article/details/112596613
这里说明一下,笔者也是一步一步跟着这位博主学习操作Webots的,本文中有许多的步骤,都是与这位博主的相同,但是笔者都有自己去动手操作,毕竟这东西光看是学不会的,更别提看着教程操作有的地方会出现不同的状况了。所以,我尽量中和了一些关键的博客,部分文字是自己总结出来的,方便读者理解和上手。
在跟随这篇博文操作后,可以查看到tf_tree:
其中:
- odom:odometry,里程计节点,其基本内容包含了位姿(x、y、theta)和x、y方向的角速度和线速度(有注意到在第二章节建模的时候,机器人运动平面是xoz并非xoy!y轴是垂直地面的);
- base_link:机器人基坐标系,已经刚性地固连到移动机器人的底座(车体),在本案例中,base_link就是黄色车体的坐标系;
- robot/Sick_LMS_291:激光雷达坐标系。
2021.10.17
更新
不足与展望
- 后续希望自己搭建出一个地图,用gmapping或者cartographer进行建图;
- 后续可实现Lidar Slam的仿真;
- 后续可实现VSlam的仿真;
- 后续希望搭建机械臂相关仿真环境;
- 整个项目希望利用git进行管理。
Reference
[1] CSDN.锡城筱凯.ROS联合webots实战案例目录
[2] CSDN.JameScottX.Webots 舵轮底盘小教程
附录
catkin_ws/src/webots_demo/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 ##
#############
catkin_ws/src/webots_demo/controller/my_controller/my_controller.cpp
#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;
}