From f78f3376c9f076f82909880c2cc5d7179172b3f3 Mon Sep 17 00:00:00 2001 From: Hamza El-Kebir Date: Mon, 14 Jun 2021 22:20:03 -0500 Subject: [PATCH 1/2] Add CraterCatalogPlugin, which provides a single notification of the planar relative location of the nearest crater, as well as its name. This commit adds a new Crater.proto message, and a plugin gazebo_crater_catalog_plugin.h, which exposes a ROS topic at `~/lander/crater_catalog`. An example subscriber implementation is included in gazebo_acs_controller_plugin.cpp. lander.sdf was modified to accommodate the new plugin, as was CMakeLists.txt. --- CMakeLists.txt | 3 + include/gazebo_acs_controller_plugin.h | 29 +++++ include/gazebo_crater_catalog_plugin.h | 139 ++++++++++++++++++++++ models/lander/lander.sdf | 3 + msgs/Crater.proto | 9 ++ src/gazebo_acs_controller_plugin.cpp | 14 +++ src/gazebo_crater_catalog_plugin.cpp | 158 +++++++++++++++++++++++++ 7 files changed, 355 insertions(+) create mode 100644 include/gazebo_crater_catalog_plugin.h create mode 100644 msgs/Crater.proto create mode 100644 src/gazebo_crater_catalog_plugin.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 707c2d4..6a780b4 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/include/gazebo_acs_controller_plugin.h b/include/gazebo_acs_controller_plugin.h index 6dc5cc1..d7ee064 100644 --- a/include/gazebo_acs_controller_plugin.h +++ b/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 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 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 { diff --git a/include/gazebo_crater_catalog_plugin.h b/include/gazebo_crater_catalog_plugin.h new file mode 100644 index 0000000..03ba095 --- /dev/null +++ b/include/gazebo_crater_catalog_plugin.h @@ -0,0 +1,139 @@ +// +// Created by helkebir on 6/14/21. +// + +#ifndef _GAZEBO_CRATER_CATALOG_PLUGIN_HH_ +#define _GAZEBO_CRATER_CATALOG_PLUGIN_HH_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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 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; + using Vector2d = ignition::math::Vector2; + + + 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 connections_; + transport::NodePtr nodeHandle_; + + std::vector craters_; //! Vector of registered crater model pointers. + std::vector craterNames_; //! Vector of registered crater names. + + transport::PublisherPtr craterPub_; + std::string craterPubTopic_; + + ignition::math::Vector3 vehiclePos_; //! Current vehicle position. + + ClosestCrater closestCrater_; //! Struct containing closest crater characteristics. + }; + +} + +#endif //_GAZEBO_CRATER_CATALOG_PLUGIN_HH_ diff --git a/models/lander/lander.sdf b/models/lander/lander.sdf index 75c8077..b696560 100644 --- a/models/lander/lander.sdf +++ b/models/lander/lander.sdf @@ -2325,6 +2325,9 @@ + + + 0 0 0 0 0 0 diff --git a/msgs/Crater.proto b/msgs/Crater.proto new file mode 100644 index 0000000..1669804 --- /dev/null +++ b/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; +} \ No newline at end of file diff --git a/src/gazebo_acs_controller_plugin.cpp b/src/gazebo_acs_controller_plugin.cpp index 8caaa1c..1628ce4 100644 --- a/src/gazebo_acs_controller_plugin.cpp +++ b/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(new_xy_pub_topic_, 10); roll_pitch_status_pub_ = node_handle_->Advertise(roll_pitch_pub_topic_, 10); roll_pitch_setpoint_pub_ = node_handle_->Advertise(roll_pitch_setpoint_pub_topic_, 10); thruster_status_pub_ = node_handle_->Advertise(thruster_pub_topic_, 10); + + // subscriber + crater_sub_ = node_handle_->Subscribe(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() { diff --git a/src/gazebo_crater_catalog_plugin.cpp b/src/gazebo_crater_catalog_plugin.cpp new file mode 100644 index 0000000..943e484 --- /dev/null +++ b/src/gazebo_crater_catalog_plugin.cpp @@ -0,0 +1,158 @@ +// +// Created by helkebir on 6/14/21. +// + +#include + +using namespace gazebo; + +GZ_REGISTER_MODEL_PLUGIN(CraterCatalogPlugin) + +void gazebo::CraterCatalogPlugin::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) +{ + model_ = model; + world_ = model_->GetWorld(); + sdf_ = sdf; + + namespace_.clear(); + + if (sdf_->HasElement("robotNamespace")) { + namespace_ = sdf_->GetElement("robotNamespace")->Get(); + } else { + gzerr << "[gazebo_crater_catalog] Please specify a robotNamespace.\n"; + } + + nodeHandle_ = transport::NodePtr(new transport::Node()); + nodeHandle_->Init(namespace_); + + craterPubTopic_ = "~/" + model_->GetName() + "/crater_catalog"; + craterPub_ = nodeHandle_->Advertise(craterPubTopic_, 10); + + vehiclePos_ = Vector3d::Zero; + closestCrater_.name = "N/A"; + closestCrater_.relPos = Vector3d::Zero; + + initCatalog(); +} + +void gazebo::CraterCatalogPlugin::Init() +{ + connections_.push_back(event::Events::ConnectWorldUpdateBegin(boost::bind(&CraterCatalogPlugin::OnUpdate, this))); + + std::cout << "CraterCatalogPlugin::Init" << std::endl; +} + +void gazebo::CraterCatalogPlugin::OnUpdate() +{ + updateCatalog(); + + sendCraterMessage(); +} + +void gazebo::CraterCatalogPlugin::initCatalog() +{ + addCraterByName("crater_1"); + addCraterByName("crater_2"); + addCraterByName("crater_3"); + addCraterByName("crater_4"); +} + +void gazebo::CraterCatalogPlugin::updateCatalog() +{ + // Obtain absolute vehicle position. + auto pose = model_->WorldPose(); + vehiclePos_ = pose.Pos(); + + // Get closest crater characteristics. + size_t closestCraterIdx = getClosestCraterIdx(vehiclePos_); + // auto craterPos = getCraterPositionByIndex(closestCraterIdx); + auto craterName = getCraterNameByIndex(closestCraterIdx); + auto craterRelPos = getCraterPositionByIndex(closestCraterIdx, vehiclePos_); + + // Update closest crater. + closestCrater_.name = craterName; + closestCrater_.relPos = craterRelPos; + + std::cout << "Closest crater is: " << craterName << " @ <" << craterRelPos.X() << ", " << craterRelPos.Y() << ", " << craterRelPos.Z() << ">" << std::endl; + + sendCraterMessage(); +} + +void gazebo::CraterCatalogPlugin::sendCraterMessage() const +{ + sensor_msgs::msgs::Crater msg; + + msg.set_name(closestCrater_.name); + msg.set_xrel(closestCrater_.relPos.X()); + msg.set_yrel(closestCrater_.relPos.Y()); + // z location is optional. + msg.set_zrel(-1); + // msg.set_zrel(closestCrater_.relPos.Z()); + + craterPub_->Publish(msg); +} + +bool gazebo::CraterCatalogPlugin::addCraterByName(const std::string &craterName) +{ + physics::ModelPtr modPtr = world_->ModelByName(craterName); + if (modPtr != nullptr) { + craters_.push_back(modPtr); + craterNames_.push_back(craterName); + + return true; + } else + return false; +} + +const std::string &gazebo::CraterCatalogPlugin::getCraterNameByIndex(const size_t idx) const +{ + assert((idx < craters_.size()) && "Crater index is out of bounds."); + + return craterNames_[idx]; +} + +gazebo::CraterCatalogPlugin::Vector3d gazebo::CraterCatalogPlugin::getCraterPositionByIndex(const size_t idx, + const Vector3d &origin) const +{ + if (idx >= craters_.size()) { + assert((idx < craters_.size()) && "Crater index is out of bounds."); + + return Vector3d::Zero; + } else { + return craters_[idx]->WorldPose().Pos() - origin; + } +} + +size_t gazebo::CraterCatalogPlugin::getClosestCraterIdx(const Vector3d &pos) const +{ + size_t idx = 0; + double distMin = INFINITY; + double distCur; + + for (int i = 0; i < craters_.size(); i++) { + distCur = pos.Distance(getCraterPositionByIndex(i)); + if (distCur < distMin) { + idx = i; + distMin = distCur; + } + } + + return idx; +} + +gazebo::CraterCatalogPlugin::Vector3d gazebo::CraterCatalogPlugin::getClosestCraterPos(const Vector3d &pos) const +{ + return craters_[getClosestCraterIdx(pos)]->WorldPose().Pos(); +} + +gazebo::CraterCatalogPlugin::Vector2d gazebo::CraterCatalogPlugin::getClosestCraterPosPlanar(const Vector3d &pos) const +{ + auto relPos = getClosestCraterPos(pos); + return Vector2d{relPos.X(), relPos.Y()}; +} + + + + + + From 51f64143b8bd6e46b9db4ad42c1e3b42c4e22470 Mon Sep 17 00:00:00 2001 From: Hamza El-Kebir Date: Mon, 14 Jun 2021 22:23:14 -0500 Subject: [PATCH 2/2] Add CraterCatalogPlugin, which provides a single notification of the planar relative location of the nearest crater, as well as its name. This commit adds a new Crater.proto message, and a plugin gazebo_crater_catalog_plugin.h, which exposes a ROS topic at `~/lander/crater_catalog`. An example subscriber implementation is included in gazebo_acs_controller_plugin.cpp. lander.sdf was modified to accommodate the new plugin, as was CMakeLists.txt. --- include/gazebo_crater_catalog_plugin.h | 2 +- src/gazebo_crater_catalog_plugin.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/gazebo_crater_catalog_plugin.h b/include/gazebo_crater_catalog_plugin.h index 03ba095..e474af9 100644 --- a/include/gazebo_crater_catalog_plugin.h +++ b/include/gazebo_crater_catalog_plugin.h @@ -1,5 +1,5 @@ // -// Created by helkebir on 6/14/21. +// Created by Hamza El-Kebir on 6/14/21. // #ifndef _GAZEBO_CRATER_CATALOG_PLUGIN_HH_ diff --git a/src/gazebo_crater_catalog_plugin.cpp b/src/gazebo_crater_catalog_plugin.cpp index 943e484..f4274f0 100644 --- a/src/gazebo_crater_catalog_plugin.cpp +++ b/src/gazebo_crater_catalog_plugin.cpp @@ -1,5 +1,5 @@ // -// Created by helkebir on 6/14/21. +// Created by Hamza El-Kebir on 6/14/21. // #include