Plataforma de simulación de robot Webots (12) que se comunica con los nodos ROS

En el tutorial anterior describimos cómo agregar sensores (GPS de radar de cámara IMU) a webots, pero el propósito de usar webots es usar webots para simular hardware real y conectarse con ROS. En este blog, comenzamos a presentar cómo el modelo integrado en webots publica datos sobre el tema en ROS, específicamente creando un nodo ROS para publicar y controlar la velocidad del motor para realizar la comunicación entre webots y ROS.

Hay dos formas de conectar webots y ROS [1] , el primer método es usar el controlador ROS estándar que viene con webots. El segundo método es escribir a mano el controlador que se comunica con ROS como lo hicimos antes. Se recomienda utilizar el controlador ROS que viene con los webots.

Los archivos de modelo y los paquetes ROS modificados utilizados en este blog se pueden descargar aquí:
archivos de modelo , paquetes ROS

1 Configure el controlador de webots

paso 1. Seleccione el controlador ROS que viene con el sistema para el robot en webots.

Paso 2 : Especifique el parámetro " --name = ros_test "para el controlador

2 Crea un nodo ROS

Paso 3 : Copie el paquete ROS correspondiente a los webots, compile　 ros_test.cpp y　complete el siguiente contenido:

#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;
}

Paso 4 : Modifique el archivo CmakeList.txt

Paso 5 : Finalmente, inicie roscore primero, luego inicie webots para cargar el modelo recién editado, y finalmente cree una nueva terminal para iniciar el nodo ROS que creamos en el paso 3.

rosrun webots_ros ros_test

En este momento, encontrará que el automóvil en webots avanzará

3 análisis de código

El nodo ROS implementa el intercambio de datos con webots llamando al servicio especificado, y el nombre de este servicio suele ser el nombre del controlador en webots (el parámetro establecido anteriormente en este blog --name = ros_test) + el nombre del dispositivo (como el nombre de la rueda) Es "wheel1") + nombre de la operación (como "/ set_position") para
obtener un valor como sigue: "ros_test / wheel1 / set_position"

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

Después de iniciar webots y ejecutar roscore, podemos usar la lista rosservice para ver que el sistema tendrá una gran cantidad de salida de servicio ros. Este es el servicio publicado por webots.

En segundo lugar, en el código anterior, damos un valor constante para la velocidad de los cuatro motores. , Puede configurar la velocidad del motor de acuerdo con el valor de otros temas.

3.1 Configurar la posición del motor

1 Los siguientes campos en el código anterior se utilizan para llamar al servicio en ROS para enviar comandos de ubicación a los 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 Establecer la velocidad del motor

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 Leer tiempo de compás

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.");   
}   

Referencia

[1] https://blog.csdn.net/weixin_38172545/article/details/106149041
[2] https://blog.csdn.net/weixin_38172545/article/details/105731062

Si cree que el artículo es útil para usted, ayúdelo y déle me gusta. O (∩_∩) O

Bienvenidos a todos para intercambiar y discutir en el área de comentarios ([email protected])

Anterior: Webots Robot Simulation Platform (11) Agregar sensor láser (láser)

Siguiente: Webots Robot Simulation Platform (13) Publicar valores de sensor en ROS