与ROS通讯
- 1 Camera 相机
- 2 IMU 传感器
- 3 GPS 传感器
- 4 Lidar传感器
- 参考资料
在上一个的教程中我们描述了把如何从ROS节点发布控制命令到webos中,这篇博客中我们开始介绍把webots中搭建的模型的传感器数据发布在ROS 中的Topic上面。文中的代码参考了博客。 本篇博客所使用的模型文件和修改的ROS包可在此处下载: 模型文件 、ROS包
1 Camera 相机
在上一篇博客中创建ros_test.cpp中添加以下代码段并编译:
// enable camera
ros::ServiceClient set_camera_client;
webots_ros::set_int camera_srv;
ros::Subscriber sub_camera;
set_camera_client = n->serviceClient<webots_ros::set_int>(ROS_NODE_NAME+"/camera/enable");
camera_srv.request.value = 64;
set_camera_client.call(camera_srv);;
启动节点
rosrun webots_ros ros_test
利用rviz既可以看到如下内容:
2 IMU 传感器
在上一篇博客中创建ros_test.cpp中添加以下代码段并编译:
void broadcastTransform() {
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin(tf::Vector3(-GPSValues[2], GPSValues[0], GPSValues[1]));
tf::Quaternion q(inertialUnitValues[0], inertialUnitValues[1], inertialUnitValues[2], inertialUnitValues[3]);
q = q.inverse();
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", "/ros_key_test/Sick_LMS_291"));
}
void inertialUnitCallback(const sensor_msgs::Imu::ConstPtr &values) {
inertialUnitValues[0] = values->orientation.x;
inertialUnitValues[1] = values->orientation.y;
inertialUnitValues[2] = values->orientation.z;
inertialUnitValues[3] = values->orientation.w;
broadcastTransform();
}
// enable inertial unit
ros::ServiceClient set_inertial_unit_client;
webots_ros::set_int inertial_unit_srv;
ros::Subscriber sub_inertial_unit;
set_inertial_unit_client = n->serviceClient<webots_ros::set_int>(ROS_NODE_NAME+"/inertial_unit/enable");
inertial_unit_srv.request.value = 32;
if (set_inertial_unit_client.call(inertial_unit_srv) && inertial_unit_srv.response.success) {
sub_inertial_unit = n->subscribe(ROS_NODE_NAME+"/imu/roll_pitch_yaw", 1, inertialUnitCallback);
while (sub_inertial_unit.getNumPublishers() == 0);
ROS_INFO("Inertial unit enabled.");
}
else
{
if (!inertial_unit_srv.response.success)
ROS_ERROR("Sampling period is not valid.");
ROS_ERROR("Failed to enable inertial unit.");
return 1;
}
启动节点
rosrun webots_ros ros_test
利用rostopic list 既可以看到webots输出的IMU信息
3 GPS 传感器
在上一篇博客中创建ros_test.cpp中添加以下代码段并编译:
void GPSCallback(const sensor_msgs::NavSatFix::ConstPtr &values) {
GPSValues[0] = values->latitude;
GPSValues[1] = values->altitude;
GPSValues[2] = values->longitude;
broadcastTransform();
}
ros::ServiceClient set_GPS_client;
webots_ros::set_int GPS_srv;
ros::Subscriber sub_GPS;
set_GPS_client = n->serviceClient<webots_ros::set_int>(ROS_NODE_NAME+"/gps/enable");
GPS_srv.request.value = 32;
if (set_GPS_client.call(GPS_srv) && GPS_srv.response.success)
{
sub_GPS = n->subscribe(ROS_NODE_NAME+"/gps/values", 1, GPSCallback);
while (sub_GPS.getNumPublishers() == 0) ;
ROS_INFO("GPS enabled.");
}
else
{
if (!GPS_srv.response.success)
ROS_ERROR("Sampling period is not valid.");
ROS_ERROR("Failed to enable GPS.");
return 1;
}
启动节点
rosrun webots_ros ros_test
利用rostopic list 既可以看到webots输出的GPS信息
4 Lidar传感器
在上一篇博客中创建ros_test.cpp中添加以下代码段并编译:
// enable lidar3d
ros::ServiceClient set_lidar3d_client;
ros::ServiceClient set_lidar3d1_client;
webots_ros::set_int lidar3d_srv;
webots_ros::set_bool lidar3d1_srv;
ros::Subscriber sub_lidar3d_scan;
set_lidar3d_client = n->serviceClient<webots_ros::set_int>(ROS_NODE_NAME+"/lidar3d/enable");
set_lidar3d1_client = n->serviceClient<webots_ros::set_bool>(ROS_NODE_NAME+"/lidar3d/enable_point_cloud");
lidar3d_srv.request.value = TIME_STEP;
lidar3d1_srv.request.value = true;
if (set_lidar3d_client.call(lidar3d_srv) && lidar3d_srv.response.success)
{
ROS_INFO("lidar3d enabled.");
sub_lidar3d_scan = n->subscribe(ROS_NODE_NAME+"/lidar3d/point_cloud", 10, lidar3dCallback);
ROS_INFO("Topic for lidar3d initialized.");
//while (sub_lidar3d_scan.getNumPublishers() == 0) {
//}
ROS_INFO("Topic for lidar3d scan connected.");
if (set_lidar3d1_client.call(lidar3d1_srv) && lidar3d1_srv.response.success)
ROS_INFO("lidar3d pointclouds enabled .");
}
else
{
if (!lidar3d_srv.response.success)
ROS_ERROR("Sampling period is not valid.");
ROS_ERROR("Failed to enable lidar3d.");
return 1;
}
启动节点
```bash
rosrun webots_ros ros_test
利用rostopic list 既可以看到webots输出的雷达的信息,这里使用RVIZ也是可以的,由于我自己搭建的小车模型太小,是的比例不协调,所以这里扫描不到障碍物信息
在这一系列的博客中我介绍了如何在机器人中搭建模型,并且与ROS建立通讯,在ROS中发布webots机器人的传感器值,同时通过ROS控制机器人移动。后续搭积木的工作就留给广大的网友了,最后再附上实验室小伙伴搭建的环境模型。
参考资料
[1] https://blog.csdn.net/weixin_38172545/article/details/106149041 [2] https://blog.csdn.net/weixin_38172545/article/details/105731062 如果大家觉得文章对你有所帮助,请帮忙点个赞。O(∩_∩)O 欢迎大家在评论区交流讨论