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 <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 "ros/ros.h"
#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
#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
开始仿真
- 执行
source devel/setup.bash
roscore- 打开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.- 新建一个终端，执行
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.显示效果图如下
评论（0）
您还未登录，请登录后发表或查看评论