Webots 机器人仿真平台(十二) 与ROS节点通讯

119
0
2020年11月12日 09时25分

与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控制器

 

1

 

step 2 : 为控制器指定参数 “ –name=ros_test ”

 

2

 

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文件

 

3

 

step 5 :最后先启动roscore, 然后启动webots加载刚刚编辑的模型, 最后再新建一个终端启动我们在step3中新建的ROS节点。

 

4

 

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发布的服务

 

5

 

其次,上述代码中我们给定四个电机的转速是一个恒定的值,可以根据其他话题的值对电机的转速进行设置。

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

发表评论

后才能评论