注意:
webots版本:2020b rev1
ros版本:melodic
在前面几章中分别介绍了在webots中如何创建自己的机器人、添加传感器以及使用手柄或键盘驱动它在仿真环境中移动。在本章中,你会学习到ROS系统最强大的特性之一,它能够让你的机器人自主导航和运动。
在图中,能够看到白色、灰色和虚线三种框。白框表示其中的这些功能包集已经在ROS中集成了,并且它们提供的多种节点能够为机器人实现自主导航。
在webots中可以直接使用GPS进行定位。并且利用IMU传感器获取惯性信息来补偿位置和方向值。
姿态(位置+方向):在ROS中,机器人的位置(position:x,y,z)和方向(orientation:x,y,z,w)被定义为姿态。2.1 在webots中加入GPS和IMU
rostopic list
查看gps是否发布了话题/robot/gps/values
在控制台下输入以下命令获取数据类型:$ rostopic info /robot/gps/values
Type: sensor_msgs/NavSatFix
Publishers:
* /robot (http://mckros-GL553VD:39691/)
Subscribers: None从上面可以看到数据类型为sensor_msgs/NavSatFix
,那我们写程序时头文件就要加入#include <sensor_msgs/NavSatFix.h>
介绍了gps数据类型获取的方法,其他两个元件类似。
5. 需要分别对其使能才能在webots中正常运行。webots_demo/src
文件夹下创建一个robot_broadcaster.cpp
。rosservice list
查看服务,找到/robot/gps/enable
。
在控制台中输入以下命令对其使能:头文件:
#include <sensor_msgs/NavSatFix.h>
订阅gps服务:
ros::ServiceClient gps_Client;
webots_ros::set_int gps_Srv;
ros::Subscriber gps_sub;
gps_Client = n->serviceClient<webots_ros::set_int>("/robot/gps/enable"); // 使能GPS服务
gps_Srv.request.value = TIME_STEP;
// 判断gps使能服务是否成功
if (gps_Client.call(gps_Srv) && gps_Srv.response.success) {
ROS_INFO("gps enabled.");
// 订阅gps话题,获取数据
gps_sub = n->subscribe("/robot/gps/values", 1, gpsCallback);
ROS_INFO("Topic for gps initialized.");
while (gps_sub.getNumPublishers() == 0) {
}
ROS_INFO("Topic for gps connected.");
} else {
if (!gps_Srv.response.success)
ROS_ERROR("Failed to enable gps.");
return 1;
}
gps回调函数:
void gpsCallback(const sensor_msgs::NavSatFix::ConstPtr &values)
{
GPSvalues[0] = values->latitude;// 纬度
GPSvalues[1] = values->altitude;// 海拔高度
GPSvalues[2] = values->longitude;// 经度
broadcastTransform(); // tf坐标转换
}
头文件:
#include <sensor_msgs/Imu.h>
订阅IMU服务
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"); //订阅IMU使能服务
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/roll_pitch_yaw", 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("Failed to enable inertial_unit.");
return 1;
}
回调函数:
void inertial_unitCallback(const sensor_msgs::Imu::ConstPtr &values)
{
Inertialvalues[0] = values->orientation.x;
Inertialvalues[1] = values->orientation.y;
Inertialvalues[2] = values->orientation.z;
Inertialvalues[3] = values->orientation.w;
broadcastTransform(); // tf坐标转换
}
✌Bye
原创声明:本文系作者授权腾讯云开发者社区发表,未经许可,不得转载。
如有侵权,请联系 cloudcommunity@tencent.com 删除。
原创声明:本文系作者授权腾讯云开发者社区发表,未经许可,不得转载。
如有侵权,请联系 cloudcommunity@tencent.com 删除。
扫码关注腾讯云开发者
领取腾讯云代金券
Copyright © 2013 - 2025 Tencent Cloud. All Rights Reserved. 腾讯云 版权所有
深圳市腾讯计算机系统有限公司 ICP备案/许可证号:粤B2-20090059 深公网安备号 44030502008569
腾讯云计算(北京)有限责任公司 京ICP证150476号 | 京ICP备11018762号 | 京公网安备号11010802020287
Copyright © 2013 - 2025 Tencent Cloud.
All Rights Reserved. 腾讯云 版权所有