Webots 机器人仿真平台(十三) 在ROS中发布传感器值

在上一个的教程中我们描述了把如何从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

欢迎大家在评论区交流讨论（[email protected]）

上一篇：Webots 机器人仿真平台(十二) 与ROS节点通讯