- 此教程以webots_demo为基础
前言
前段时间升级了webots 2021a,也没写博客记录一下,逞着礼拜天记录一下。
1. Webots2021a更新了什么
来自官网(简单的翻译了一下)
- 新特征
- 添加
wb_supervisor_node_get_from_device
函数去获取设备的节点句柄(#2074) - 现在可以在仿真时向机器人控制器添加设备(#2237)
- 添加
wb_connector_is_locked
函数来返回Connector的目前isLocked 状态(#2087) - 添加在WorldInfo节点下的
coordinateSystem
场地来使用EUN(East-Up-North)坐标系统(#2228) - 添加了在
realtime
模式下禁用渲染的可能性(#2286) - 在
wb_supervisor_node_get_number_of_contact_points
Supervisor API函数中添加了一个新参数,以获取包括模块化节点的联系点的数量(#2228) - 添加了新的
wb_supervisor_node_get_contact_point_node
Supervisor API函数,以获取节点引用联系的有联系的点(#2228) - 向 Camera API添加了两个新功能,分别为
wb_camera_get_exposure
和wb_camera_set_exposure
,以获取和更改Camera的曝光度(#2363) - 在 Recognition 节点和Camera API中添加了新功能,用于生成分段的地面真实图像(#2199)
- 在默认机器人窗口的 Camera 选项卡中添加了用于启用和禁用 recognition 和 segmentation 功能的选项(#2431)
- 向InertialUnit API添加了wb_inertial_unit_get_quaternion函数来获取作为四元数的方向数据(#2424)
- 不推荐使用 InertialUnit.lookupTable 参数,相关的
wb_inertial_unit_get_lookup_table_size
和wb_inertial_unit_get_lookup_table
函数已经移除(#2424) - 添加
PlasticFruitBox
和MetalStorageBox
PROTO 物体(#2427) - 在 Python的 getPointCloud 方法下添加
data_type
参数。相比较以前的版本可以更快的获取bytearray
点的代表
- 添加
- 增强的功能
- 添加支持Python3.9(#2318)
- 给 BiscuitBox PROTO 模型改良
Solid.recognitionColors
值(#2401) - 改进了相机图像格式的文档,以及通过相机API获取到的图像如何在显示设备中显示(#2443)
- 现在,当测量深度超出范围时,Lidar和RangeFinder设备将返回无穷大(#2509)
- 优化的功能
- 依赖更新
2. 以webots_demo为案例 哪些地方发生了更新
- launch文件启动webots时参数
no-gui
要改成no_gui
。以webots_demo 的webots.launch
文件为例<?xml version="1.0"?> <launch> <!-- 启动webots --> <arg name="no_gui" default="false," doc="Start Webots with minimal GUI"/> <include file="$(find webots_ros)/launch/webots.launch"> <arg name="mode" value="realtime"/> <arg name="no_gui" value="$(arg no_gui)"/> <arg name="world" value="$(find webots_demo)/worlds/webots_map.wbt"/> </include> </launch>
- GPS 数据类型从
sensor_msgs::NavSatFix
改为了geometry_msgs/PointStamped
,直接使用2020版本会在webots中报如下错误。[ERROR] [1614430952.900944139]: Client [/robot_cartographer_bringup] wants topic /robot/gps/values to have datatype/md5sum [sensor_msgs/NavSatFix/2d3a8cd499b9b4a0249fb98fd05cfa48], but our version has [geometry_msgs/PointStamped/c63aecb41bfdfd6b7e1fac37c7cbe7bf]. Dropping connection.
解决方案
- 将头文件
#include <sensor_msgs/NavSatFix.h>
改为#include <geometry_msgs/PointStamped.h>
- GPS回调函数数据类型更改,数据格式更改。如下
double GPSvalues[4]; void GPSCallback(const geometry_msgs::PointStamped::ConstPtr &value) { GPSvalues[0] = value->point.x; // 对应rviz坐标轴x GPSvalues[1] = value->point.z; // 对应rviz坐标轴y if (gps_flag) { GPSvalues[2] = value->point.x; GPSvalues[3] = value->point.z; gps_flag=0; } broadcastTransform(); }
- IMU数据改为从quaternion发出 topic名称从
robot/inertial_unit/roll_pitch_yaw
改为了robot/inertial_unit/quaternion
IMU 始能代码改为如下代码:
// 订阅inertial_unit服务
ros::ServiceClient inertial_unit_Client;
webots_ros::set_int inertial_unit_Srv;
ros::Subscriber inertial_unit_sub;
inertial_unit_Client = n->serviceClient<webots_ros::set_int>("/robot/inertial_unit/enable");
inertial_unit_Srv.request.value = TIME_STEP;
if (inertial_unit_Client.call(inertial_unit_Srv) && inertial_unit_Srv.response.success) {
ROS_INFO("inertial_unit enabled.");
inertial_unit_sub = n->subscribe("robot/inertial_unit/quaternion", 1, inertial_unitCallback);
ROS_INFO("Topic for inertial_unit initialized.");
while (inertial_unit_sub.getNumPublishers() == 0) {
}
ROS_INFO("Topic for inertial_unit connected.");
} else {
if (!inertial_unit_Srv.response.success)
ROS_ERROR("Sampling period is not valid.");
ROS_ERROR("Failed to enable inertial_unit.");
return 1;
}
虽然topic名称发生改变,但是数据格式没有改变,依旧是sensor_msgs::Imu
- 坐标轴转换函数改变(如果出现webots转向和rviz不符或者坐标系不符,请检查速度、坐标轴,自行调试)
``` c++
void broadcastTransform()
{
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin(tf::Vector3(GPSvalues[0]-GPSvalues[2],GPSvalues[1]-GPSvalues[3],0));// 设置原点
tf::Quaternion q(Inertialvalues[0],Inertialvalues[2],Inertialvalues[1],-Inertialvalues[3]);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),”odom”,”base_link”));
transform.setIdentity();
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), “base_link”, “/robot/Sick_LMS_291”));
}
```
- webots中 IMU 方向发生改变(注意)
- IMU方向如下图所示,蓝色朝上,绿色向前,红色向右
- robot方向如下图所示
- 在webots2021a版本,需要配置两个
robot_broadcaster
,分别支持gmapping和cartographer 详细见github
✌Bye 此项目github地址:https://github.com/JackyMao1999/webots_demo