Webots+ROS学习记录（7）——在ROS中读取webots传感器数据

在webots中，选中器件，右键点击help，可以查找对应的帮助文档，有简单ros以及编程基础就能轻松应用。这里总结一下camera、imu、gps、lidar等的使用方法。

一、Camera

1. 首先在模型中添加camera，放置在合适的位置，在子节点中添加tramsform，在transform中添加子节点圆柱形shape。注意相机正方向为-z。在width以及height中可以设置相机的宽、高等参数。
   
   
2. ros节点中使能

// 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_key_test/camera/enable");
  camera_srv.request.value = 64;
  set_camera_client.call(camera_srv);

3. 启动仿真后，可以由rqt_image_view读取图像
   

二、IMU

在webots中 InertialUnit 只对外提供x.y.z.w四个数据

1. 在Robot中添加 InertialUnit，在其子节点中添加shape节点
2. ROS使能函数

 // 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_key_test/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_key_test/inertial_unit/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;
  }

三、GPS

建模方法同IMU

// enable gps
  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_key_test/gps/enable");
  GPS_srv.request.value = 32;
  if (set_GPS_client.call(GPS_srv) && GPS_srv.response.success) {
    sub_GPS = n->subscribe("ros_key_test/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;
  }

四、Lidar

1. Webots中有很多现成的雷达，我们选用一款16线3d激光雷达VelodyneVLP-16，为其命名为lidar3d。
   
   注意lidar在使能时候有两个步骤，一个是enable lidar，一个是enable point cloud。

 // 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_key_test/lidar3d/enable");
  set_lidar3d1_client = n->serviceClient<webots_ros::set_bool>("ros_key_test/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_key_test/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;
  }

2. 2.要想在rviz中看见点云，还需要发布tf

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/lidar3d"));
}