add support for both sonar and lidar rotation determination by transf…

…orm checks
PX4 · Aug 17, 2019 · dae0fde · dae0fde
1 parent 9915be5
commit dae0fde
Show file tree

Hide file tree

Showing 12 changed files with 191 additions and 126 deletions.
diff --git a/include/common.h b/include/common.h
@@ -22,6 +22,7 @@
 
 
 #include <tinyxml.h>
+#include <typeinfo>
 #include <Eigen/Dense>
 #include <gazebo/gazebo.hh>
 #include <ignition/math.hh>
@@ -220,27 +221,10 @@ const ignition::math::Quaterniond getSensorOrientation(const gazebo::physics::Mo
                                                        const std::string& parentSensorModelName,
                                                        T& parentSensor)
 {
-  // Get the `base_link` rotation WRT world
-  gazebo::physics::LinkPtr baseLink = nullptr;
-  std::vector<gazebo::physics::LinkPtr> linkList = rootModel->GetLinks(); // Get list of all links in the root model
-  for(auto link : linkList) {
-    std::string linkName = link->GetName();
-    if(linkName.find("::base_link") != std::string::npos) {
-      baseLink = rootModel->GetLink(linkName); // Get the pointer to the `base_link`
-      break;
-    }
-  }
-  if (!baseLink)
-    gzthrow("Sensor plugin requires the `base_link` element to be defined");
-
-  // This is the rotation of the 'base_link` WRT world
-#if GAZEBO_MAJOR_VERSION >= 9
-  ignition::math::Quaterniond q_wb = baseLink->WorldPose().Rot();
-#else
-  ignition::math::Quaterniond q_wb = ignitionFromGazeboMath(baseLink->GetWorldPose()).Rot();
-#endif
+  // Get list of all links in the root model
+  std::vector<gazebo::physics::LinkPtr> linkList = rootModel->GetLinks();
 
-  // Get the parent sensor link rotation WRT world
+  // Get the parent sensor link rotation WRT parent entity
   gazebo::physics::LinkPtr parentSensorLink = nullptr;
   for(auto link : linkList) {
     std::string linkName = link->GetName();
@@ -251,19 +235,70 @@ const ignition::math::Quaterniond getSensorOrientation(const gazebo::physics::Mo
   }
   if (!parentSensorLink)
     gzthrow("Sensor plugin requires a `link` element for its parent sensor to be defined");
-
-  // This is the rotation of the parentSensorLink WRT world
 #if GAZEBO_MAJOR_VERSION >= 9
-  ignition::math::Quaterniond q_wl = parentSensorLink->WorldPose().Rot();
+  ignition::math::Quaterniond q_pl = parentSensorLink->RelativePose().Rot();
 #else
-  ignition::math::Quaterniond q_wl = ignitionFromGazeboMath(parentSensorLink->GetWorldPose()).Rot();
+  ignition::math::Quaterniond q_pl = ignitionFromGazeboMath(parentSensorLink->RelativePose()).Rot();
 #endif
 
   // Calculate parent sensor rotation WRT `base_link`
-  ignition::math::Quaterniond q_bl = q_wb.Inverse() * q_wl; // This is the rotation of the parent sensor link WRT `base_link`
-  ignition::math::Quaterniond q_ls = parentSensor->Pose().Rot(); // This is the rotation of the parent sensor WRT parent sensor link
-
-  return (q_bl * q_ls).Inverse(); // Return the rotation of the parent sensor WRT `base_link`
+  ignition::math::Quaterniond q_ls = parentSensor->Pose().Rot();
+
+  /**
+   * @note Given a bug on the sonar sensor which doesn't allow to obtain the
+   * correct sensor to link rotation one must use the rotation of base_link WRT
+   * to the sensor link so to get the proper rotation.
+   *
+   * Also, it's assumed on the SDF of the sonar sensor a -90 degrees rotation in
+   * pitch so the sensor ray matches the X-axis of the link:
+   * <sensor name="sonar" type="sonar">
+   *   <pose>0 0 0 0 -1.57 0</pose>
+   *   <sonar>
+   *     ...
+   *   </sonar>
+   *   <plugin name="SonarPlugin" filename="libgazebo_sonar_plugin.so">
+   *     <robotNamespace></robotNamespace>
+   *   </plugin>
+   * </sensor>
+   *
+   * - Example of a forward facing sonar link:
+   * <include>
+   *   <uri>model://sonar</uri>
+   *   <pose frame="base_link">0 0 -0.05 0 0 0</pose>
+   * </include>
+   *
+   * - For a lidar sensor link however, the direction would depend on the sensor
+   * pose orientation, since all the transforms are taken into account.
+   *
+   * - Ex lidar sensor:
+   * <sensor name="laser" type="ray">
+   *   <pose>0 0 0 0 1.57 0</pose>
+   *   <ray>
+   *
+   *   </ray>
+   *   <plugin name="LaserPlugin" filename="libgazebo_lidar_plugin.so">
+   *     <robotNamespace></robotNamespace>
+   *   </plugin>
+   * </sensor>
+   *
+   * - For the above sensor to be poiting forward, then the link should have the
+   * following pose:
+   * <include>
+   *   <uri>model://lidar</uri>
+   *   <pose frame="base_link">0 0 -0.05 0 -1.57 0</pose>
+   * </include>
+   */
+
+  // Check if the sensor is of type SonarSensor
+  std::string sensorType = typeid(parentSensor).name();
+  if (sensorType.find("SonarSensor") != std::string::npos) {
+    return q_pl.Inverse();
+
+  } else {
+
+    ignition::math::Quaterniond q_bs = (q_pl * q_ls).Inverse();
+    return q_bs; // Return the rotation of the parent sensor WRT `base_link`
+  }
 }
 
 /**

diff --git a/include/gazebo_lidar_plugin.h b/include/gazebo_lidar_plugin.h
@@ -63,7 +63,7 @@ namespace gazebo
     public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
 
     /// \brief Pointer to parent
-    protected: physics::WorldPtr world;
+    protected: physics::WorldPtr world_;
 
     /// \brief The parent sensor
     private:
@@ -79,8 +79,8 @@ namespace gazebo
 
     /// \brief The connection tied to RayPlugin::OnNewLaserScans()
     private:
-      event::ConnectionPtr newLaserScansConnection;
-      sensor_msgs::msgs::Range lidar_message;
+      event::ConnectionPtr newLaserScansConnection_;
+      sensor_msgs::msgs::Range lidar_message_;
   };
 }
 #endif
diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h
@@ -107,7 +107,7 @@ static const std::string kDefaultMotorVelocityReferencePubTopic = "/gazebo/comma
 static const std::string kDefaultImuTopic = "/imu";
 static const std::string kDefaultLidarTopic = "/link/lidar";
 static const std::string kDefaultOpticalFlowTopic = "/px4flow/link/opticalFlow";
-static const std::string kDefaultSonarTopic = "/sonar_model/link/sonar";
+static const std::string kDefaultSonarTopic = "/link/sonar";
 static const std::string kDefaultIRLockTopic = "/camera/link/irlock";
 static const std::string kDefaultGPSTopic = "/gps";
 static const std::string kDefaultVisionTopic = "/vision_odom";

diff --git a/include/gazebo_sonar_plugin.h b/include/gazebo_sonar_plugin.h
@@ -22,16 +22,24 @@
 #ifndef _GAZEBO_SONAR_PLUGIN_HH_
 #define _GAZEBO_SONAR_PLUGIN_HH_
 
+#include <gazebo/gazebo.hh>
+#include <gazebo/common/common.hh>
 #include <gazebo/common/Plugin.hh>
+#include <gazebo/util/system.hh>
+#include "gazebo/physics/physics.hh"
+#include "gazebo/transport/transport.hh"
+
+#include "gazebo/msgs/msgs.hh"
 #include <gazebo/sensors/SensorTypes.hh>
 #include <gazebo/sensors/SonarSensor.hh>
-#include <gazebo/util/system.hh>
+
+#include <common.h>
 
 #include <Range.pb.h>
 
 namespace gazebo
 {
-  static constexpr uint8_t kDefaultRotation = 0; // current types are described as https://github.com/PX4/Firmware/blob/master/msg/distance_sensor.msg
+  static constexpr auto kDefaultSonarTopic = "sonar";
 
   /// \brief A Ray Sensor Plugin
   class GAZEBO_VISIBLE SonarPlugin : public SensorPlugin
@@ -50,20 +58,22 @@ namespace gazebo
     public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
 
     /// \brief Pointer to parent
-    protected: physics::WorldPtr world;
+    protected: physics::WorldPtr world_;
 
     /// \brief The parent sensor
     private:
-      sensors::SonarSensorPtr parentSensor;
+      sensors::SonarSensorPtr parentSensor_;
+      std::string sonar_topic_;
       transport::NodePtr node_handle_;
       transport::PublisherPtr sonar_pub_;
       std::string namespace_;
-      int rotation_;
 
-    /// \brief The connection tied to RayPlugin::OnNewLaserScans()
+      gazebo::msgs::Quaternion orientation_;
+
+    /// \brief The connection tied to SonarPlugin::OnNewScans()
     private:
-      event::ConnectionPtr newScansConnection;
-      sensor_msgs::msgs::Range sonar_message;
+      event::ConnectionPtr newScansConnection_;
+      sensor_msgs::msgs::Range sonar_message_;
   };
 }
 #endif
diff --git a/models/iris_irlock/iris_irlock.sdf b/models/iris_irlock/iris_irlock.sdf
@@ -7,7 +7,7 @@
 
     <include>
       <uri>model://lidar</uri>
-      <pose>-0.12 0 0 0 3.1415 0</pose>
+      <pose>-0.12 0 0 0 0 0</pose>
     </include>
     <joint name="lidar_joint" type="revolute">
       <child>lidar::link</child>
@@ -38,4 +38,4 @@
     </joint>
 
   </model>
-</sdf>
+</sdf>
diff --git a/models/iris_opt_flow/iris_opt_flow.sdf b/models/iris_opt_flow/iris_opt_flow.sdf
@@ -24,13 +24,13 @@
 
     <!--lidar-->
     <include>
-      <uri>model://sf10a</uri>
+      <uri>model://lidar</uri>
       <pose>0 0 -0.05 0 0 0</pose>
     </include>
 
-    <joint name="sf10a_joint" type="fixed">
+    <joint name="lidar_joint" type="fixed">
       <parent>iris::base_link</parent>
-      <child>sf10a::link</child>
+      <child>lidar::link</child>
     </joint>
 
   </model>

diff --git a/models/lidar/model.sdf b/models/lidar/model.sdf
@@ -2,6 +2,7 @@
 <sdf version="1.4">
   <model name="lidar">
     <link name="link">
+      <pose>0 0 0 0 0 0</pose>
       <inertial>
         <pose>0 0 0 0 0 0</pose>
         <mass>0.01</mass>
@@ -25,7 +26,7 @@
         </material>
       </visual>
       <sensor name="laser" type="ray">
-        <pose>0 0 0 0 1.570896 0</pose>
+        <pose>0 0 0 0 1.57079633 0</pose>
         <ray>
           <scan>
             <horizontal>

diff --git a/models/mb1240-xl-ez4/mb1240-xl-ez4.sdf.jinja b/models/mb1240-xl-ez4/mb1240-xl-ez4.sdf.jinja
@@ -6,6 +6,7 @@
 {%- set ixx = m/12*(w**2 + h**2) -%}
 {%- set iyy = m/12*(l**2 + h**2) -%}
 {%- set izz = m/12*(l**2 + w**2) -%}
+{%- set deg2rad = 0.01745329251 -%}
 
 {%- macro box(l, w, h) -%}
 <geometry>
@@ -46,6 +47,7 @@
       </collision>
 
       <sensor name="sonar" type="sonar">
+        <pose>0 0 0 0 {{ -90*deg2rad }} 0</pose>
         <sonar>
           <min>0</min>
           <max>2.00</max>

diff --git a/models/px4flow/px4flow.sdf.jinja b/models/px4flow/px4flow.sdf.jinja
@@ -43,13 +43,14 @@
       </collision>
 
       <sensor name="px4flow" type="imu">
-        <pose>0 0 0 0 -1.5707963259 3.141592652</pose>
+        <pose>0 0 0 0 -{{ 90*deg2rad }} {{ 180*deg2rad }}</pose>
         <always_on>1</always_on>
         <update_rate>200</update_rate>
       </sensor>
 
       <!-- Sonar: HRLV EZ4-->
       <!--<sensor name="sonar" type="sonar">-->
+        <!--<pose>0 0 0 0 {{ -90*deg2rad }} 0</pose>-->
         <!--<sonar>-->
           <!--<min>0</min>-->
           <!--<max>5.00</max>-->

diff --git a/models/sonar/model.sdf b/models/sonar/model.sdf
@@ -1,5 +1,6 @@
+<?xml version="1.0" ?>
 <sdf version="1.4">
-  <model name="sonar_model">
+  <model name="sonar">
     <link name="link">
       <pose>0 0 0 0 0 0</pose>
       <inertial>
@@ -16,22 +17,26 @@
       </inertial>
       <visual name="visual">
         <geometry>
-          <box><size>0.005 0.015 0.005</size></box>
+          <box>
+            <size>0.005 0.015 0.005</size>
+          </box>
         </geometry>
         <material>
-	  <script>
-      	    <name>Gazebo/Black</name>
-      	  </script>
+          <script>
+            <name>Gazebo/Black</name>
+          </script>
         </material>
       </visual>
       <sensor name="sonar" type="sonar">
-        <pose>0 0 0 0 1.57 0 </pose>
-          <sonar>
-            <min>0.02</min>
-            <max>5.0</max>
-            <radius>1.33974388395</radius>
-          </sonar>
-        <plugin name="SonarPlugin" filename="libgazebo_sonar_plugin.so"/>
+        <pose>0 0 0 0 -1.57 0</pose>
+        <sonar>
+          <min>0.02</min>
+          <max>5.0</max>
+          <radius>1.33974388395</radius>
+        </sonar>
+        <plugin name="SonarPlugin" filename="libgazebo_sonar_plugin.so">
+          <robotNamespace></robotNamespace>
+        </plugin>
         <always_on>1</always_on>
         <update_rate>20</update_rate>
         <visualize>true</visualize>