Add rangesensor module (#2158)

* added rangesensors to gazebo model and NPS * added module to handle the range sensors * added sender IDS and added to abi messages * added abi message to flight plan guided
2026-05-30 19:47:50 +08:00 · 2017-11-06 17:16:13 +01:00
parent a2991ee9b3
commit e84ff368c2
35 changed files with 2419 additions and 806 deletions
@@ -76,7 +76,7 @@ using namespace std;
 #endif

 // Add video handling functions if req'd.
-#ifdef NPS_SIMULATE_VIDEO
+#if NPS_SIMULATE_VIDEO
 extern "C" {
 #include "modules/computer_vision/cv.h"
 #include "modules/computer_vision/video_thread_nps.h"
@@ -97,14 +97,24 @@ static struct gazebocam_t gazebo_cams[VIDEO_THREAD_MAX_CAMERAS] =
 { { NULL, 0 } };
 #endif

-struct gazebo_actuators_t
-{
+struct gazebo_actuators_t {
  string names[NPS_COMMANDS_NB];
  double thrusts[NPS_COMMANDS_NB];
  double torques[NPS_COMMANDS_NB];
 };

-struct gazebo_actuators_t gazebo_actuators = {NPS_ACTUATOR_NAMES, {NPS_ACTUATOR_THRUSTS}, {NPS_ACTUATOR_THRUSTS}};
+struct gazebo_actuators_t gazebo_actuators = {NPS_ACTUATOR_NAMES, NPS_ACTUATOR_THRUSTS, NPS_ACTUATOR_THRUSTS};
+
+
+#if NPS_SIMULATE_LASER_RANGE_ARRAY
+extern "C" {
+#include "subsystems/abi.h"
+}
+
+static void gazebo_init_range_sensors(void);
+static void gazebo_read_range_sensors(void);
+
+#endif

 /// Holds all necessary NPS FDM state information
 struct NpsFdm fdm;
@@ -168,6 +178,15 @@ inline struct DoubleRates to_pprz_rates(gazebo::math::Vector3 body_g)
 }

 inline struct DoubleEulers to_pprz_eulers(gazebo::math::Quaternion quat)
+{
+  struct DoubleEulers eulers;
+  eulers.psi = -quat.GetYaw();
+  eulers.theta = -quat.GetPitch();
+  eulers.phi = quat.GetRoll();
+  return eulers;
+}
+
+inline struct DoubleEulers to_global_pprz_eulers(gazebo::math::Quaternion quat)
 {
  struct DoubleEulers eulers;
  eulers.psi = -quat.GetYaw() - M_PI / 2;
@@ -218,8 +237,11 @@ void nps_fdm_run_step(
  if (!gazebo_initialized) {
    init_gazebo();
    gazebo_read();
-#ifdef NPS_SIMULATE_VIDEO
+#if NPS_SIMULATE_VIDEO
    init_gazebo_video();
+#endif
+#if NPS_SIMULATE_LASER_RANGE_ARRAY
+    gazebo_init_range_sensors();
 #endif
    gazebo_initialized = true;
  }
@@ -229,9 +251,13 @@ void nps_fdm_run_step(
  gazebo::sensors::run_once();
  gazebo_write(act_commands, commands_nb);
  gazebo_read();
-#ifdef NPS_SIMULATE_VIDEO
+#if NPS_SIMULATE_VIDEO
  gazebo_read_video();
 #endif
+#if NPS_SIMULATE_LASER_RANGE_ARRAY
+  gazebo_read_range_sensors();
+#endif
+
 }

 // TODO Atmosphere functions have not been implemented yet.
@@ -290,16 +316,32 @@ static void init_gazebo(void)
  gazebo::common::SystemPaths::Instance()->AddModelPaths(
    gazebodir + "models/");

-  cout << "Load world: " << gazebodir + "world/" + NPS_GAZEBO_WORLD << endl;
+  // get vehicles
+  cout << "Load vehicle: " << gazebodir + "models/" + NPS_GAZEBO_AC_NAME + "/" + NPS_GAZEBO_AC_NAME + ".sdf" << endl;
+  sdf::SDFPtr vehicle_sdf(new sdf::SDF());
+  sdf::init(vehicle_sdf);
+  sdf::readFile(gazebodir + "models/" + NPS_GAZEBO_AC_NAME + "/" + NPS_GAZEBO_AC_NAME + ".sdf", vehicle_sdf);

+  // add sensors
+#if NPS_SIMULATE_LASER_RANGE_ARRAY
+  vehicle_sdf->Root()->GetFirstElement()->AddElement("include")->GetElement("uri")->Set("model://range_sensors");
+  sdf::ElementPtr range_joint = vehicle_sdf->Root()->GetFirstElement()->AddElement("joint");
+  range_joint->GetAttribute("name")->Set("range_sensor_joint");
+  range_joint->GetAttribute("type")->Set("fixed");
+  range_joint->GetElement("parent")->Set("chassis");
+  range_joint->GetElement("child")->Set("range_sensors::base");
+#endif
+
+  // get world
+  cout << "Load world: " << gazebodir + "world/" + NPS_GAZEBO_WORLD << endl;
  sdf::SDFPtr world_sdf(new sdf::SDF());
  sdf::init(world_sdf);
  sdf::readFile(gazebodir + "world/" + NPS_GAZEBO_WORLD, world_sdf);

-  world_sdf->Root()->GetFirstElement()->AddElement("include")->GetElement("uri")->Set((string)"model://" + NPS_GAZEBO_AC_NAME);
-  world_sdf->Write(pprz_home + "/var/gazebo.world");
+  // add vehicles
+  world_sdf->Root()->GetFirstElement()->InsertElement(vehicle_sdf->Root()->GetFirstElement());

-  // TODO add sensors to the gazebo model in the same way as the vehicle.
+  world_sdf->Write(pprz_home + "/var/gazebo.world");

  gazebo::physics::WorldPtr world = gazebo::loadWorld(pprz_home + "/var/gazebo.world");
  if (!world) {
@@ -330,8 +372,7 @@ static void init_gazebo(void)
 #ifdef MOTOR_MIXING_YAW_COEF
  const double yaw_coef[] = MOTOR_MIXING_YAW_COEF;

-  for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++)
-  {
+  for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
    gazebo_actuators.torques[i] = -fabs(gazebo_actuators.torques[i]) * yaw_coef[i] / fabs(yaw_coef[i]);
  }
 #endif
@@ -422,17 +463,12 @@ static void gazebo_read(void)
  fdm.ltpprz_ecef_accel = fdm.ltp_ecef_accel; // ???
  fdm.body_inertial_accel = fdm.body_ecef_accel; // Approximate, unused.

-  // only use accelerometer if no collisions or if not on ground
-  if (ct->Contacts().contact_size() || pose.pos.z < 0.03){
-    fdm.body_accel = to_pprz_body(
-                     pose.rot.RotateVectorReverse(-world->Gravity()));
-  } else {
-    fdm.body_accel = to_pprz_body(
+  fdm.body_accel = to_pprz_body(
                     pose.rot.RotateVectorReverse(accel.Ign() - world->Gravity()));
-  }
+
  /* attitude */
  // ecef_to_body_quat: unused
-  fdm.ltp_to_body_eulers = to_pprz_eulers(local_to_global_quat * pose.rot);
+  fdm.ltp_to_body_eulers = to_global_pprz_eulers(local_to_global_quat * pose.rot);
  double_quat_of_eulers(&(fdm.ltp_to_body_quat), &(fdm.ltp_to_body_eulers));
  fdm.ltpprz_to_body_quat = fdm.ltp_to_body_quat; // unused
  fdm.ltpprz_to_body_eulers = fdm.ltp_to_body_eulers; // unused
@@ -460,7 +496,7 @@ static void gazebo_read(void)
 #else
  // Gazebo versions < 8 do not have atmosphere or wind support
  // Use placeholder values. Valid for low altitude, low speed flights.
-  fdm.wind = (struct DoubleVect3){0, 0, 0};
+  fdm.wind = (struct DoubleVect3) {0, 0, 0};
  fdm.pressure_sl = 101325; // Pa

  fdm.airspeed = 0;
@@ -509,7 +545,7 @@ static void gazebo_write(double act_commands[], int commands_nb)
  }
 }

-#ifdef NPS_SIMULATE_VIDEO
+#if NPS_SIMULATE_VIDEO
 /**
 * Set up cameras.
 *
@@ -593,8 +629,8 @@ static void gazebo_read_video(void)
    struct image_t img;
    read_image(&img, cam);

-#ifdef NPS_DEBUG_VIDEO
-    cv::Mat RGB_cam(cam->ImageHeight(),cam->ImageWidth(),CV_8UC3,(uint8_t *)cam->ImageData());
+#if NPS_DEBUG_VIDEO
+    cv::Mat RGB_cam(cam->ImageHeight(), cam->ImageWidth(), CV_8UC3, (uint8_t *)cam->ImageData());
    cv::cvtColor(RGB_cam, RGB_cam, cv::COLOR_RGB2BGR);
    cv::namedWindow(cameras[i]->dev_name, cv::WINDOW_AUTOSIZE);  // Create a window for display.
    cv::imshow(cameras[i]->dev_name, RGB_cam);
@@ -653,6 +689,133 @@ static void read_image(
  img->pprz_ts = ts.Double() * 1e6;
  img->buf_idx = 0; // unused
 }
+#endif  // NPS_SIMULATE_VIDEO
+
+#if NPS_SIMULATE_LASER_RANGE_ARRAY
+/*
+ * Simulate range sensors:
+ *
+ * In the airframe file, set NPS_SIMULATE_LASER_RANGE_ARRAY if you want to make use of the integrated Ray sensors.
+ * These are defined in their own model which is added to the ardrone.sdf (called range_sensors). Here you can add
+ * single ray point sensors with a specified position and orientation. It is also possible add noise.
+ *
+ * Within the airframe file (see e.g ardrone2_rangesensors_gazebo.xml), the amount of sensors
+ * (LASER_RANGE_ARRAY_NUM_SENSORS) and their orientations
+ * (LASER_RANGE_ARRAY_ORIENTATIONS={phi_1,theta_1,psi_1...phi_n,theta_n,psi_n}  n = number of sensors) need to be
+ * specified as well. This is to keep it generic since this need to be done on the real platform with an external
+ * ray sensor. The function will compare the orientations from the ray sensors of gazebo, with the ones specified
+ * in the airframe, and will fill up an array to send and abi message to be used by other modules.
+ *
+ * NPS_GAZEBO_RANGE_SEND_AGL defines if the sensor that is defined as down should be used to send an AGL message.
+ * to send an OBSTACLE_DETECTION message.
+ *
+ * Functions:
+ *   gazebo_init_range_sensors() -> Finds and initializes all ray sensors in gazebo
+ *   gazebo_read_range_sensors() -> Reads and evaluates the ray sensors values, and sending it to other pprz modules
+ */
+
+struct gazebo_ray_t {
+  gazebo::sensors::RaySensorPtr sensor;
+  gazebo::common::Time last_measurement_time;
+};
+
+static struct gazebo_ray_t rays[LASER_RANGE_ARRAY_NUM_SENSORS] = {{NULL, 0}};
+static float range_orientation[] = LASER_RANGE_ARRAY_ORIENTATIONS;
+static uint8_t ray_sensor_agl_index = 255;
+
+#define VL53L0_MAX_VAL 8.191f
+
+static void gazebo_init_range_sensors(void)
+{
+  gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
+  gazebo::sensors::Sensor_V sensors = mgr->GetSensors();  // list of all sensors
+
+  uint16_t sensor_count = model->GetSensorCount();
+
+  gazebo::sensors::RaySensorPtr ray_sensor;
+  uint8_t ray_sensor_count_selected = 0;
+
+  // Loop though all sensors and only select ray sensors, which are saved within a struct
+  for (uint16_t i = 0; i < sensor_count; i++) {
+    if (ray_sensor_count_selected > LASER_RANGE_ARRAY_NUM_SENSORS) {
+      break;
+    }
+
+    if (sensors.at(i)->Type() == "ray") {
+      ray_sensor = static_pointer_cast<gazebo::sensors::RaySensor>(sensors.at(i));
+
+      if (!ray_sensor) {
+        cout << "ERROR: Could not get pointer to raysensor " << i << "!" << endl;
+        continue;
+      }
+
+      // Read out the pose from per ray sensors in gazebo relative to body
+      struct DoubleEulers pose_sensor = to_pprz_eulers(ray_sensor->Pose().Rot());
+
+      struct DoubleQuat q_ray, q_def;
+      double_quat_of_eulers(&q_ray, &pose_sensor);
+
+      /* Check the orientations of the ray sensors found in gazebo, if they are similar (within 5 deg) to the orientations
+       * given in the airframe file in LASER_RANGE_ARRAY_RANGE_ORIENTATION
+       */
+      for (int j = 0; j < LASER_RANGE_ARRAY_NUM_SENSORS; j++) {
+        struct DoubleEulers def = {0, range_orientation[j*2], range_orientation[j*2 + 1]};
+        double_quat_of_eulers(&q_def, &def);
+        // get angle between required angle and ray angle
+        double angle = acos(QUAT_DOT_PRODUCT(q_ray, q_def));
+
+        if (fabs(angle) < RadOfDeg(5)) {
+          ray_sensor_count_selected++;
+          rays[j].sensor = ray_sensor;
+          rays[j].sensor->SetActive(true);
+
+#if LASER_RANGE_ARRAY_SEND_AGL
+          // find the sensor pointing down
+          if (fabs(range_orientation[j*2] + M_PI_2) < RadOfDeg(5)) {
+            ray_sensor_agl_index = j;
+          }
 #endif
+          break;
+        }
+      }
+    }
+  }
+
+  if (ray_sensor_count_selected != LASER_RANGE_ARRAY_NUM_SENSORS) {
+    cout << "ERROR: you have defined " << LASER_RANGE_ARRAY_NUM_SENSORS << " sensors in your airframe file, but only "
+         << ray_sensor_count_selected << " sensors have been found in the gazebo simulator, "
+         "with the same orientation as in the airframe file " << endl;
+    exit(0);
+  }
+  cout << "Initialized laser range array" << endl;
+}
+
+static void gazebo_read_range_sensors(void)
+{
+  static float range;
+
+  // Loop through all ray sensors found in gazebo
+  for (int i = 0; i < LASER_RANGE_ARRAY_NUM_SENSORS; i++) {
+    if ((rays[i].sensor->LastMeasurementTime() - rays[i].last_measurement_time).Float() < 0.005
+        || rays[i].sensor->LastMeasurementTime() == 0) { continue; }
+
+    if (rays[i].sensor->Range(0) < 1e-5 || rays[i].sensor->Range(0) > VL53L0_MAX_VAL) {
+      range = VL53L0_MAX_VAL;
+    } else {
+      range = rays[i].sensor->Range(0);
+    }
+    AbiSendMsgOBSTACLE_DETECTION(OBS_DETECTION_RANGE_ARRAY_NPS_ID, range, range_orientation[i*2], range_orientation[i*2 + 1]);
+
+    if (i == ray_sensor_agl_index) {
+      float agl = rays[i].sensor->Range(0);
+      // Down range sensor as agl
+      if (agl > 1e-5 && !isinf(agl)) {
+        AbiSendMsgAGL(AGL_RAY_SENSOR_GAZEBO_ID, agl);
+      }
+    }
+    rays[i].last_measurement_time = rays[i].sensor->LastMeasurementTime();
+  }
+}
+#endif  // NPS_SIMULATE_LASER_RANGE_ARRAY

 #pragma GCC diagnostic pop // Ignore -Wdeprecated-declarations