与ROS节点通讯

1 设置webots控制器
2 创建ROS节点
3 代码分析
- 3.1 设置电机位置
- 3.2 设置电机速度
- 3.3 读取时间节拍
参考资料

在前面的教程中我们描述了如何在webots中添加传感器（IMU 相机　雷达 GPS）,但是我们使用webots的目的还是希望用webots来模拟真实的硬件并与ROS相连接。这篇博客中我们开始介绍webots中搭建的模型如何把数据发布在ROS 中的Topic上面，具体通过创建一个ROS节点发布控制电机的速度实现webots与ROS之间的通讯。 webots与ROS之间相连接有两种方式 [1]，第一种方法就是使用webots自带的标准ROS控制器。第二种方法就是像我们之前的方式一样手写与ROS通讯的控制器。这里推荐使用webots自带的ROS控制器。本篇博客所使用的模型文件和修改的ROS包可在此处下载：模型文件、ROS包

1 设置webots控制器

step 1. 在webots中为机器人选择系统自带的ROS控制器
Webots 机器人仿真平台(十二) 与ROS节点通讯 step 2 : 为控制器指定参数　“ –name=ros_test ”

2 创建ROS节点

step 3 : 拷贝webots对应的ROS包，并兴建　ros_test.cpp　填入以下内容：

#include "ros/ros.h"
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/NavSatFix.h>
#include <signal.h>
#include <std_msgs/String.h>
#include<tf/transform_broadcaster.h>
#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
#define TIME_STEP 32
#define NMOTORS 4
#define MAX_SPEED 10
ros::NodeHandle *n;
static int controllerCount;
static std::vector<std::string> controllerList; 
ros::ServiceClient timeStepClient;
webots_ros::set_int timeStepSrv;
std::string ROS_NODE_NAME = "ros_test";
static const char *motorNames[NMOTORS] ={"wheel1", "wheel2", "wheel3","wheel4"};//匹配之前定义好的电机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>(ROS_NODE_NAME+std::string("/")
																+ 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_NODE_NAME,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();
 
timeStepClient = n->serviceClient<webots_ros::set_int>(ROS_NODE_NAME+"/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>(ROS_NODE_NAME+std::string("/")
																+ 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>(ROS_NODE_NAME+std::string("/")
		+ 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;
}

step 4 : 修改CmakeList.txt文件
Webots 机器人仿真平台(十二) 与ROS节点通讯 step 5 :最后先启动roscore, 然后启动webots加载刚刚编辑的模型, 最后再新建一个终端启动我们在step3中新建的ROS节点。

rosrun webots_ros ros_test

这时候你会发现webots中的小车会向前运动
Webots 机器人仿真平台(十二) 与ROS节点通讯

3 代码分析

ROS 节点通过调用指定的服务来实现与webots数据交换,而这个服务的名称通常是webots中控制器的名称（在这篇博客前面设置的参数　–name=ros_test　）+ 设备名称（如轮子的名称为”wheel1″）+操作名称(如 “/set_position”) 如下得到一个值: “ros_test/wheel1/set_position”

ROS_NODE_NAME+std::string("/")+ std::string(motorNames[i]) + std::string("/set_position")
//值为　ros_test/wheel1/set_position

这点在启动webots和运行roscore以后，我们使用rosservice list可以看到系统会有很多的ros 服务输出，这就是webots发布的服务
Webots 机器人仿真平台(十二) 与ROS节点通讯其次,上述代码中我们给定四个电机的转速是一个恒定的值，可以根据其他话题的值对电机的转速进行设置。

3.1 设置电机位置

1　上述代码中以下字段为调用ROS中的服务实现把位置命令发送给webots的

ros::ServiceClient set_position_client;   
webots_ros::set_float set_position_srv;   
set_position_client = n->serviceClient<webots_ros::set_float>(ROS_NODE_NAME+std::string("/")
　　　+ 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]);

3.2 设置电机速度

ros::ServiceClient set_velocity_client;
webots_ros::set_float set_velocity_srv;   
set_velocity_client =n->serviceClient<webots_ros::set_float>(ROS_NODE_NAME+std::string("/")										+ 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]);

3.3 读取时间节拍

timeStepClient = n->serviceClient<webots_ros::set_int>(ROS_NODE_NAME+"/robot/time_step");
if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success)
{  
	ROS_ERROR("Failed to call service time_step for next step.");   
}

参考资料

[1] https://blog.csdn.net/weixin_38172545/article/details/106149041 [2] https://blog.csdn.net/weixin_38172545/article/details/105731062 如果大家觉得文章对你有所帮助，请大家帮忙点个赞。O(∩_∩)O 欢迎大家在评论区交流讨论（cenruping@vip.qq.com）