Webots+ROS学习记录（6）——在ROS中控制你的Webots仿真

Webots+ROS学习记录（6）——在ROS中控制你的Webots仿真

在Webots+ROS学习记录（1）与Webots+ROS学习记录（2）中，我们曾介绍过ROS与Webots的通讯与联合方针，其中用了软件自带的例子Pioneer3，这一篇文档就是与大家探讨一下如何更改Pioneer3的官方例程，来实现用ROS控制你自己机器人。

Webots的ros控制器

当你在ROS上安装好Webots后，你可以在路径$WEBOTS_HOME/projects/default/controllers/ros中找到Webots的ros控制器。

有两种方法可以在Webots上使用ROS。最简单的解决方案是使用标准ROS控制器，也就是刚才提到的控制器。它是Webots默认控制器的一部分，可在任何项目中使用。该控制器可用于Webots中的任何机器人，并充当ROS节点，将所有Webots功能作为service或topic提供给其他ROS节点。第二个更复杂的解决方案是构建您自己的Webots控制器，它也将是使用Webots和ROS库的ROS节点。此解决方案仅应用于标准控制器无法完成的特定应用程序。

我们使用的是第一种解决方案，现在给出它的控制框图。

在ros中搭建你的wbt

在Webots+ROS学习记录（5）中我们已经搭建了一个六轮的山地车，现在将它命名为ros_test.wbt拷贝在catkin_ws/src/six_wheel_robots中。

打开webots软件，在菜单栏打开刚才拷贝过来的ros_test.wbt。然后给它添加控制器wizard-new robot controller，新建一个c++控制器。在左边scene
tree中选定robot，将其下的name改为ros_test,选定controller参数，选择为ros（已经存在于$WEBOTS_HOME/projects/default/controllers/ros），然后将controllerArgs更改为*“–name=ros_test”*。

在这之后，要将6个轮子上的旋转电机device中的name参数分别改为wheel1-wheel6.

到这里，软件端要做的事已经做完了，因为大部分工作我们都在Windows下完成了。

新建你的节点：ros_test.cpp

在catkin_ws/src/webots_ros/src中新建一个文件名为ros_test.cpp。这个程序是我改了改官方的节点pioneer3at.cpp，所以这里只对主要的程序块进行解释。在这里只实现了简单的运动，还未加入传感器等设备，所以程序也比较简单，后续会添加更多的函数进去。

#include 

#include 

#include 

#include 

#include 

#include 

#include

#include "ros/ros.h"

 

#include 

#include 

 

#define TIME_STEP 32

#define NMOTORS 6

#define MAX_SPEED 10
 

ros::NodeHandle *n;

static int controllerCount;

static std::vector<std::string> controllerList; 

ros::ServiceClient timeStepClient;

webots_ros::set_int timeStepSrv;
 

static const char *motorNames[NMOTORS] ={"wheel1", "wheel2", "wheel3","wheel4","wheel5","wheel6"};//匹配之前定义好的电机name

void updateSpeed() {   
double speeds[NMOTORS];
  
speeds[0] = MAX_SPEED;
speeds[1] = MAX_SPEED;
speeds[2] = MAX_SPEED;
speeds[3] = MAX_SPEED;
speeds[4] = MAX_SPEED;   
speeds[5] = MAX_SPEED;


 //set speeds
 
for (int i = 0; i < NMOTORS; ++i) {

   
ros::ServiceClient set_velocity_client;
   
webots_ros::set_float set_velocity_srv;
  
set_velocity_client = n->serviceClient<webots_ros::set_float>(std::string("ros_test/")
+ std::string(motorNames[i]) + std::string("/set_velocity"));   
set_velocity_srv.request.value = speeds[i];
set_velocity_client.call(set_velocity_srv);


 }

}//将速度请求以set_float的形式发送给set_velocity_srv




// catch names of the controllers availables on ROS network
void controllerNameCallback(const std_msgs::String::ConstPtr &name) { 
controllerCount++; 
controllerList.push_back(name->data);
ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());

}


void quit(int sig) {

ROS_INFO("User stopped the 'ros_test' node.");

timeStepSrv.request.value = 0; 
timeStepClient.call(timeStepSrv); 
ros::shutdown();
exit(0);
}
 

int main(int argc, char **argv) {
 
std::string controllerName;
 
// create a node named 'ros_test' on ROS network

 
ros::init(argc, argv, "ros_test",
ros::init_options::AnonymousName);

n = new ros::NodeHandle;  
signal(SIGINT, quit);
 
// subscribe to the topic model_name to get the list of availables controllers

ros::Subscriber nameSub = n->subscribe("model_name", 100, controllerNameCallback);

while (controllerCount == 0 || controllerCount <nameSub.getNumPublishers()) {
ros::spinOnce();
ros::spinOnce();
ros::spinOnce();
 } 
ros::spinOnce();
 
timeStepClient = n->serviceClient<webots_ros::set_int>("ros_test/robot/time_step");
timeStepSrv.request.value = TIME_STEP;


// if there is more than one controller available, it let the user choose 
if (controllerCount == 1)   
controllerName = controllerList[0];

else {
  
int wantedController = 0;
  
std::cout << "Choose the # of the controller you want touse:\n";   
std::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());
 
// leave topic once it is not necessary anymore
 
nameSub.shutdown();
 
// init motors 
for (int i = 0; i < NMOTORS; ++i) {

   
// position，发送电机位置给wheel1-6，速度控制时设置为缺省值INFINITY   
ros::ServiceClient set_position_client;   
webots_ros::set_float set_position_srv;   
set_position_client = n->serviceClient<webots_ros::set_float>(std::string("ros_test/")
+ std::string(motorNames[i]) + std::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]);

    
// speed，发送电机速度给wheel1-6   
ros::ServiceClient set_velocity_client;
webots_ros::set_float set_velocity_srv;   
set_velocity_client =
n->serviceClient<webots_ros::set_float>(std::string("ros_test/")
+ std::string(motorNames[i]) + std::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]);
}   
updateSpeed();
 
// main loop 
while (ros::ok()) {   
if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success)
{  
ROS_ERROR("Failed to call service time_step for next step.");     
break;   
}   
ros::spinOnce();
} 
timeStepSrv.request.value = 0;
timeStepClient.call(timeStepSrv);
ros::shutdown(); 
return 0;

}

配置你的节点

节点程序编写好之后，需要在catkin_ws/src/webots_ros/CMakeLists.txt中添加

## Instructions for ros_test node
add_executable(ros_test src/ros_test.cpp) 
add_dependencies(ros_test
webots_ros_generate_messages_cpp) 
target_link_libraries(ros_test
${catkin_LIBRARIES}
)

配置好之后，执行catkin_make

开始仿真

1. 执行

source devel/setup.bash
roscore

2. 打开webots，打开ros_test.wbt
   开始仿真，会显示

INFO: ros: Starting controller:
"/snap/webots/14/usr/share/webots/projects/default/controllers/ros/ros"
--name=ros_test

[ros] [ INFO] [1587636702.847755813]:
Robot's unique name is ros_test.

3. 新建一个终端，执行

source devel/setup.bash
rosrun webots_ros ros_test

会显示

[ INFO] [1587631747.676473982]: Controller #1: ros_test.
[ INFO] [1587631747.676885551]: Using controller: 'ros_test'
[ INFO] [1587631747.697647281]: Position set to INFINITY for motor wheel1.
[ INFO] [1587631747.730389653]: Velocity set to 0.0 for motor wheel1.
[ INFO] [1587631747.762513035]: Position set to INFINITY for motor wheel2.
[ INFO] [1587631747.794688194]: Velocity set to 0.0 for motor wheel2.
[ INFO] [1587631747.832344113]: Position set to INFINITY for motor wheel3.
[ INFO] [1587631747.862172553]: Velocity set to 0.0 for motor wheel3.
[ INFO] [1587631747.895410180]: Position set to INFINITY for motor wheel4.
[ INFO] [1587631747.926774109]: Velocity set to 0.0 for motor wheel4.
[ INFO] [1587631747.956902746]: Position set to INFINITY for motor wheel5.
[ INFO] [1587631747.990213588]: Velocity set to 0.0 for motor wheel5.
[ INFO] [1587631748.022360634]: Position set to INFINITY for motor wheel6.
[ INFO] [1587631748.054141159]: Velocity set to 0.0 for motor wheel6.

显示效果图如下