add facing parameter to lidar and sonar plugins

PX4 · Aug 17, 2019 · 4c38627 · 4c38627
1 parent 22df947
commit 4c38627
Show file tree

Hide file tree

Showing 6 changed files with 52 additions and 29 deletions.
diff --git a/include/gazebo_lidar_plugin.h b/include/gazebo_lidar_plugin.h
@@ -22,20 +22,21 @@
 #ifndef _GAZEBO_RAY_PLUGIN_HH_
 #define _GAZEBO_RAY_PLUGIN_HH_
 
-#include "gazebo/common/Plugin.hh"
-#include "gazebo/sensors/SensorTypes.hh"
-#include "gazebo/sensors/RaySensor.hh"
-#include "gazebo/util/system.hh"
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/sensors/SensorTypes.hh>
+#include <gazebo/sensors/RaySensor.hh>
+#include <gazebo/util/system.hh>
 
-#include "Range.pb.h"
-
-#define SENSOR_MIN_DISTANCE   0.06 // values smaller than that cause issues
-#define SENSOR_MAX_DISTANCE  35.0 // values bigger than that cause issues
-#define DEFAULT_MIN_DISTANCE  0.2
-#define DEFAULT_MAX_DISTANCE 15.0
+#include <Range.pb.h>
 
 namespace gazebo
 {
+  static constexpr double kSensorMinDistance = 0.06; // values smaller than that cause issues
+  static constexpr double kSensorMaxDistance = 35.0; // values bigger than that cause issues
+  static constexpr double kDefaultMinDistance = 0.2;
+  static constexpr double kDefaultMaxDistance = 15.0;
+  static constexpr uint8_t kDefaultFacing = 25; // current types are described as https://github.com/PX4/Firmware/blob/master/msg/distance_sensor.msg
+
   /// \brief A Ray Sensor Plugin
   class GAZEBO_VISIBLE RayPlugin : public SensorPlugin
   {
@@ -63,7 +64,7 @@ namespace gazebo
       std::string namespace_;
       double min_distance_;
       double max_distance_;
-
+      int facing_;
 
     /// \brief The connection tied to RayPlugin::OnNewLaserScans()
     private:

diff --git a/include/gazebo_sonar_plugin.h b/include/gazebo_sonar_plugin.h
@@ -22,15 +22,17 @@
 #ifndef _GAZEBO_SONAR_PLUGIN_HH_
 #define _GAZEBO_SONAR_PLUGIN_HH_
 
-#include "gazebo/common/Plugin.hh"
-#include "gazebo/sensors/SensorTypes.hh"
-#include "gazebo/sensors/SonarSensor.hh"
-#include "gazebo/util/system.hh"
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/sensors/SensorTypes.hh>
+#include <gazebo/sensors/SonarSensor.hh>
+#include <gazebo/util/system.hh>
 
-#include "Range.pb.h"
+#include <Range.pb.h>
 
 namespace gazebo
 {
+  static constexpr uint8_t kDefaultFacing = 0; // current types are described as https://github.com/PX4/Firmware/blob/master/msg/distance_sensor.msg
+
   /// \brief A Ray Sensor Plugin
   class GAZEBO_VISIBLE SonarPlugin : public SensorPlugin
   {
@@ -56,7 +58,7 @@ namespace gazebo
       transport::NodePtr node_handle_;
       transport::PublisherPtr sonar_pub_;
       std::string namespace_;
-
+      int facing_;
 
     /// \brief The connection tied to RayPlugin::OnNewLaserScans()
     private:

diff --git a/msgs/Range.proto b/msgs/Range.proto
@@ -8,7 +8,8 @@ message Range
   required float min_distance                 = 2;
   required float max_distance                 = 3;
   required float current_distance             = 4;
-  optional float h_fov                        = 5;
-  optional float v_fov                        = 6;
-  optional gazebo.msgs.Quaternion orientation = 7;
+  required int32 rotation                     = 5;
+  optional float h_fov                        = 6;
+  optional float v_fov                        = 7;
+  optional gazebo.msgs.Quaternion orientation = 8;
 }
diff --git a/src/gazebo_lidar_plugin.cpp b/src/gazebo_lidar_plugin.cpp
@@ -80,23 +80,31 @@ void RayPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
   // get minimum distance
   if (_sdf->HasElement("min_distance")) {
     min_distance_ = _sdf->GetElement("min_distance")->Get<double>();
-    if (min_distance_ < SENSOR_MIN_DISTANCE) {
-      min_distance_ = SENSOR_MIN_DISTANCE;
+    if (min_distance_ < kSensorMinDistance) {
+      min_distance_ = kSensorMinDistance;
     }
   } else {
-    gzwarn << "[gazebo_lidar_plugin] Using default minimum distance: 0.3\n";
-    min_distance_ = DEFAULT_MIN_DISTANCE;
+    gzwarn << "[gazebo_lidar_plugin] Using default minimum distance: " << kDefaultMinDistance << "\n";
+    min_distance_ = kDefaultMinDistance;
   }
 
   // get maximum distance
   if (_sdf->HasElement("max_distance")) {
     max_distance_ = _sdf->GetElement("max_distance")->Get<double>();
-    if (max_distance_ > SENSOR_MAX_DISTANCE) {
-      max_distance_ = SENSOR_MAX_DISTANCE;
+    if (max_distance_ > kSensorMaxDistance) {
+      max_distance_ = kSensorMaxDistance;
     }
   } else {
-    gzwarn << "[gazebo_lidar_plugin] Using default maximum distance: 15\n";
-    max_distance_ = DEFAULT_MAX_DISTANCE;
+    gzwarn << "[gazebo_lidar_plugin] Using default maximum distance: " << kDefaultMaxDistance << "\n";
+    max_distance_ = kDefaultMaxDistance;
+  }
+
+  // get facing of the sensor according to https://github.com/PX4/Firmware/blob/master/msg/distance_sensor.msg
+  if (_sdf->HasElement("facing")) {
+    facing_ = _sdf->GetElement("facing")->Get<int>();
+  } else {
+    gzwarn << "[gazebo_lidar_plugin] Using default facing: " << kDefaultFacing << "\n";
+    facing_ = kDefaultFacing;
   }
 
   node_handle_ = transport::NodePtr(new transport::Node());
@@ -134,6 +142,7 @@ void RayPlugin::OnNewLaserScans()
   }
 
   lidar_message.set_current_distance(current_distance);
+  lidar_message.set_facing(facing_);
 
   lidar_pub_->Publish(lidar_message);
 }
diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp
@@ -849,7 +849,7 @@ void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message) {
   sensor_msg.current_distance = lidar_message->current_distance() * 100.0;
   sensor_msg.type = 0;
   sensor_msg.id = 0;
-  sensor_msg.orientation = 25;//downward facing
+  sensor_msg.orientation = lidar_message->facing();
   sensor_msg.covariance = 0;
 
   //distance needed for optical flow message
@@ -931,6 +931,7 @@ void GazeboMavlinkInterface::SonarCallback(SonarPtr& sonar_message) {
 
   sensor_msg.type = 1;
   sensor_msg.id = 1;
+  sensor_msg.orientation = sonar_message->rotation();
   sensor_msg.covariance = 0;
   sensor_msg.horizontal_fov = sonar_message->h_fov();
   sensor_msg.vertical_fov = sonar_message->v_fov();

diff --git a/src/gazebo_sonar_plugin.cpp b/src/gazebo_sonar_plugin.cpp
@@ -74,6 +74,14 @@ void SonarPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
   else
     gzwarn << "[gazebo_sonar_plugin] Please specify a robotNamespace.\n";
 
+  // get facing of the sensor according to https://github.com/PX4/Firmware/blob/master/msg/distance_sensor.msg
+  if (_sdf->HasElement("facing")) {
+    facing_ = _sdf->GetElement("facing")->Get<int>();
+  } else {
+    gzwarn << "[gazebo_sonar_plugin] Using default facing: " << kDefaultFacing << "\n";
+    facing_ = kDefaultFacing;
+  }
+
   node_handle_ = transport::NodePtr(new transport::Node());
   node_handle_->Init(namespace_);
 
@@ -97,6 +105,7 @@ void SonarPlugin::OnNewScans()
   sonar_message.set_min_distance(parentSensor->RangeMin());
   sonar_message.set_max_distance(parentSensor->RangeMax());
   sonar_message.set_current_distance(parentSensor->Range());
+  sonar_message.set_facing(facing_);
 
   sonar_message.set_h_fov(2.0f * atan(parentSensor->Radius() / parentSensor->RangeMax()));
   sonar_message.set_v_fov(2.0f * atan(parentSensor->Radius() / parentSensor->RangeMax()));