Add crater ROS topic to notify vehicle of nearest crater

CMakeLists.txt

@@ -299,6 +299,7 @@ set(sensor_msgs
   msgs/VehicleAngularRates.proto
   msgs/TVCStatus.proto
   msgs/TVCTarget.proto
+  msgs/Crater.proto
   # ----------------------------------------------------------------------------
   msgs/Airspeed.proto
   msgs/Imu.proto
@@ -337,6 +338,7 @@ add_library(gazebo_contact_plugin SHARED src/gazebo_contact_plugin.cc)
 add_library(gazebo_cg_plugin SHARED src/gazebo_cg_plugin.cpp)
 add_library(gazebo_custom_mavlink_interface SHARED src/gazebo_custom_mavlink_interface.cpp src/mavlink_interface.cpp)
 add_library(gazebo_tvc_controller_plugin SHARED src/gazebo_tvc_controller_plugin.cpp)
+add_library(gazebo_crater_catalog_plugin SHARED src/gazebo_crater_catalog_plugin.cpp)
 # ------------------------------------------------------------------------------
 
 add_library(gazebo_airspeed_plugin SHARED src/gazebo_airspeed_plugin.cpp)
@@ -371,6 +373,7 @@ set(plugins
   gazebo_cg_plugin
   gazebo_custom_mavlink_interface
   gazebo_tvc_controller_plugin
+  gazebo_crater_catalog_plugin
   # ----------------------------------------------------------------------------
 
   gazebo_airspeed_plugin

include/gazebo_acs_controller_plugin.h

@@ -42,10 +42,26 @@
 #include "RollPitchStatus.pb.h"
 #include "RollPitchSetpoint.pb.h"
 #include "ThrusterStatus.pb.h"
+#include "Crater.pb.h"
 // -----------------------------------------------------------------------------
 
 namespace gazebo {
 
+typedef const boost::shared_ptr<const sensor_msgs::msgs::Crater> CraterMsgPtr;
+
+/**
+ * @brief Struct that contains accessible closest crater data.
+ *
+ * @details Contains the name and relative position of the nearest crater. The crater depth can be inferred from the
+ * crater name provided the same naming convention is adhered to as in the sample world.
+ *
+ * Note that in the default configuration, the z-value of \c relaPos will be -1.
+ */
+struct ClosestCrater {
+    std::string name; //! Crater name.
+    ignition::math::Vector3<double> relPos; //! Relative distance to crater (planar vector, z can be ignored by default).
+};
+
 class GAZEBO_VISIBLE ACSControllerPlugin : public ModelPlugin {
 
 public:
@@ -59,6 +75,14 @@ class GAZEBO_VISIBLE ACSControllerPlugin : public ModelPlugin {
   void OnUpdate();
   void handle_control();
   void sendACSStatus();
+  /**
+   * @brief Callback for CraterCatalog messages.
+   *
+   * @details Amends \c closestCrater_ to contain latest nearest crater.
+   *
+   * @param msg Crater message.
+   */
+  void craterCallback(CraterMsgPtr &msg);
 
   physics::ModelPtr _model;
   sdf::ElementPtr _sdf;
@@ -87,12 +111,17 @@ class GAZEBO_VISIBLE ACSControllerPlugin : public ModelPlugin {
   std::string roll_pitch_pub_topic_;
   std::string roll_pitch_setpoint_pub_topic_;
   std::string thruster_pub_topic_;
+  std::string crater_sub_topic_; // Crater message topic.
 
   // status publishers
   transport::PublisherPtr new_xy_status_pub_;
   transport::PublisherPtr roll_pitch_status_pub_;
   transport::PublisherPtr roll_pitch_setpoint_pub_;
   transport::PublisherPtr thruster_status_pub_;
+
+  transport::SubscriberPtr crater_sub_; //! Crater catalog subscriber.
+
+  ClosestCrater closestCrater_; //! Closest crater struct.
 };
 
 class actuator {

include/gazebo_crater_catalog_plugin.h

@@ -0,0 +1,139 @@
+//
+// Created by Hamza El-Kebir on 6/14/21.
+//
+
+#ifndef _GAZEBO_CRATER_CATALOG_PLUGIN_HH_
+#define _GAZEBO_CRATER_CATALOG_PLUGIN_HH_
+
+#include <mutex>
+#include <string>
+#include <vector>
+#include <iostream>
+
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/sensors/sensors.hh>
+#include <gazebo/transport/transport.hh>
+#include <gazebo/util/system.hh>
+#include <ignition/math.hh>
+
+#include "Crater.pb.h"
+
+namespace gazebo {
+
+    /**
+     * @brief Struct that contains accessible closest crater data.
+     *
+     * @details Contains the name and relative position of the nearest crater. The crater depth can be inferred from the
+     * crater name provided the same naming convention is adhered to as in the sample world.
+     *
+     * Note that in the default configuration, the z-value of \c relaPos will be -1.
+     */
+    struct ClosestCrater {
+        std::string name;
+        ignition::math::Vector3<double> relPos;
+    };
+
+    /**
+     * @brief Class that contains logic for realistic closest crater reporting.
+     */
+    class GAZEBO_VISIBLE CraterCatalogPlugin : public ModelPlugin {
+    public:
+        CraterCatalogPlugin() {};
+
+        virtual void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
+
+        virtual void Init();
+
+        bool addCraterByName(const std::string &craterName);
+
+        using Vector3d = ignition::math::Vector3<double>;
+        using Vector2d = ignition::math::Vector2<double>;
+
+
+    protected:
+        void OnUpdate();
+
+        /**
+         * @brief Initializes crater list.
+         */
+        void initCatalog();
+
+        /**
+         * @brief Fetches current vehicle position and computes closest crater characteristics.
+         */
+        void updateCatalog();
+
+        /**
+         * @brief Sends Crater message to ~/lander/crater_catalog
+         */
+        void sendCraterMessage() const;
+
+        /**
+         * @brief Gets crater name from \c craterNames_ by index.
+         *
+         * @details Raises runtime exceptions on range transgression.
+         *
+         * @param idx Crater index.
+         * @return Crater name.
+         */
+        const std::string &getCraterNameByIndex(const size_t idx) const;
+
+        /**
+         * @brief Get crater position by index.
+         *
+         * @details Raises runtime exceptions on range transgression.
+         *
+         * @param idx Crater index.
+         * @param origin Origin relative to which position is computed, default is 0.
+         * @return Crater position in world frame.
+         */
+        Vector3d getCraterPositionByIndex(const size_t idx,
+                                          const Vector3d &origin = Vector3d::Zero) const;
+
+        /**
+         * @brief Computes closest crater index.
+         *
+         * @param pos Vehicle position.
+         * @return Closest crater index.
+         */
+        size_t getClosestCraterIdx(const Vector3d &pos) const;
+
+        /**
+         * @brief Obtains closest crater position.
+         *
+         * @param pos Vehicle position.
+         * @return Closest crater position in world frame.
+         */
+        Vector3d getClosestCraterPos(const Vector3d &pos) const;
+
+        /**
+         * @brief Obtains closest crater position in plane.
+         *
+         * @param pos Vehicle position.
+         * @return Closest crater position in plane in world coordinates.
+         */
+        Vector2d getClosestCraterPosPlanar(const Vector3d &pos) const;
+
+        physics::ModelPtr model_;
+        sdf::ElementPtr sdf_;
+        physics::WorldPtr world_; //! Pointer to world object.
+
+        std::string namespace_;
+        std::vector<event::ConnectionPtr> connections_;
+        transport::NodePtr nodeHandle_;
+
+        std::vector<physics::ModelPtr> craters_; //! Vector of registered crater model pointers.
+        std::vector<std::string> craterNames_; //! Vector of registered crater names.
+
+        transport::PublisherPtr craterPub_;
+        std::string craterPubTopic_;
+
+        ignition::math::Vector3<double> vehiclePos_; //! Current vehicle position.
+
+        ClosestCrater closestCrater_; //! Struct containing closest crater characteristics.
+    };
+
+}
+
+#endif //_GAZEBO_CRATER_CATALOG_PLUGIN_HH_

models/lander/lander.sdf

@@ -2325,6 +2325,9 @@
     <plugin name='tvc_controller' filename='libgazebo_tvc_controller_plugin.so'>
       <robotNamespace/>
     </plugin>
+    <plugin name='crater_catalog_plugin' filename='libgazebo_crater_catalog_plugin.so'>
+      <robotNamespace/>
+    </plugin>
     <!--lidar-->
     <link name="lander/lidar_link">
       <pose>0 0 0 0 0 0</pose>

msgs/Crater.proto

@@ -0,0 +1,9 @@
+syntax = "proto2";
+package sensor_msgs.msgs;
+
+message Crater {
+  required string name = 1;
+  required double xrel = 2;
+  required double yrel = 3;
+  required double zrel = 4;
+}

src/gazebo_acs_controller_plugin.cpp

@@ -84,12 +84,26 @@ void ACSControllerPlugin::Load(physics::ModelPtr _model,
   roll_pitch_pub_topic_ = "~/" + _model->GetName() + "/roll_pitch_status";
   roll_pitch_setpoint_pub_topic_ = "~/" + _model->GetName() + "/roll_pitch_setpoint";
   thruster_pub_topic_ = "~/" + _model->GetName() + "/thruster_status";
+  crater_sub_topic_ = "~/" + _model->GetName() + "/crater_catalog";
 
   // publishers
   new_xy_status_pub_ = node_handle_->Advertise<sensor_msgs::msgs::NewXYStatus>(new_xy_pub_topic_, 10);
   roll_pitch_status_pub_ = node_handle_->Advertise<sensor_msgs::msgs::RollPitchStatus>(roll_pitch_pub_topic_, 10);
   roll_pitch_setpoint_pub_ = node_handle_->Advertise<sensor_msgs::msgs::RollPitchSetpoint>(roll_pitch_setpoint_pub_topic_, 10);
   thruster_status_pub_ = node_handle_->Advertise<sensor_msgs::msgs::ThrusterStatus>(thruster_pub_topic_, 10);
+
+  // subscriber
+  crater_sub_ = node_handle_->Subscribe<sensor_msgs::msgs::Crater>(crater_sub_topic_, &ACSControllerPlugin::craterCallback, this);
+}
+
+void ACSControllerPlugin::craterCallback(CraterMsgPtr &msg)
+{
+    closestCrater_.name = msg->name();
+    closestCrater_.relPos.X(msg->xrel());
+    closestCrater_.relPos.Y(msg->yrel());
+    closestCrater_.relPos.Z(msg->zrel());
+
+    std::cout << "Received crater update: " << closestCrater_.name << "; " << closestCrater_.relPos << std::endl;
 }
 
 void ACSControllerPlugin::Init() {