Communication with ROS

1 Camera
2 IMU sensor
3 GPS sensor
4 Lidar sensor
Reference

In the previous tutorial, we described how to issue control commands from ROS nodes to webos. In this blog, we started to introduce the release of sensor data of models built in webots on Topic in ROS. The code in the article refers to the blog .

The model files and modified ROS packages used in this blog can be downloaded here: model files , ROS packages

1 Camera

Create ros_test.cpp in the previous blog, add the following code snippet and compile it:

// 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);;

Start node

rosrun webots_ros ros_test

Using rviz, you can see the following:
Insert picture description here

2 IMU sensor

Create ros_test.cpp in the previous blog, add the following code snippet and compile it:

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;
  }

Start node

rosrun webots_ros ros_test

Use rostopic list to see the IMU information output by webots
Insert picture description here

3 GPS sensor

Create ros_test.cpp in the previous blog, add the following code snippet and compile it:

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;
}

Start node

rosrun webots_ros ros_test

Use rostopic list to see the GPS information output by webots
Insert picture description here

4 Lidar sensor

Create ros_test.cpp in the previous blog, add the following code snippet and compile it:

 // 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

You can use rostopic list to see the radar information output by webots. It is also possible to use RVIZ here. Since the car model I built is too small and the scale is not coordinated, the obstacle information cannot be scanned here.
Insert picture description here

In this series In the blog, I introduced how to build a model in the robot, establish communication with ROS, publish the sensor values of the webots robot in ROS, and control the movement of the robot through ROS. The subsequent work of building blocks is left to the majority of netizens, and finally the environmental model built by the laboratory partners is attached .