Station keeping practice worlds (#582)

* Station keeping practice worlds

Signed-off-by: Carlos Agüero <[email protected]>

Author: caguero

vrx_gz/CMakeLists.txt

@@ -81,6 +81,21 @@ install(
   TARGETS ScoringPlugin
   DESTINATION lib)
 
+# Stationkeeping scoring plugin
+add_library(StationkeepingScoringPlugin SHARED
+  src/StationkeepingScoringPlugin.cc
+  src/WaypointMarkers
+)
+target_link_libraries(StationkeepingScoringPlugin PUBLIC
+  gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER}
+  gz-sim${GZ_SIM_VER}::core
+  gz-math${GZ_MATH_VER}
+  ScoringPlugin
+)
+install(
+  TARGETS StationkeepingScoringPlugin
+  DESTINATION lib)
+
 # Wayfinding scoring plugin
 add_library(WayfindingScoringPlugin SHARED
   src/WayfindingScoringPlugin.cc
@@ -108,7 +123,6 @@ list(APPEND VRX_GZ_PLUGINS
   PublisherPlugin
   ScanDockScoringPlugin
   SimpleHydrodynamics
-  StationkeepingScoringPlugin
   Surface
   WaveVisual
   WildlifeScoringPlugin

vrx_gz/src/StationkeepingScoringPlugin.cc

@@ -24,6 +24,7 @@
 #include <gz/plugin/Register.hh>
 
 #include "StationkeepingScoringPlugin.hh"
+#include "WaypointMarkers.hh"
 
 using namespace gz;
 using namespace vrx;
@@ -91,31 +92,12 @@ class StationkeepingScoringPlugin::Implementation
   /// \brief Vehicle to score.
   public: sim::Entity vehicleEntity{sim::kNullEntity};
 
-  /// \brief Waypoint visualization markers
-  public: bool markers = false;
-
-  /// \brief Display goal marker (currently disabled)
-  public: void AddMarker();
-
   /// \brief Display or suppress state changes
   public: bool silent = false;
-};
 
-/////////////////////////////////////////////////
-void StationkeepingScoringPlugin::Implementation::AddMarker()
-{
-  msgs::Marker markerMsg;
-  markerMsg.set_id(0);
-  markerMsg.set_action(msgs::Marker::ADD_MODIFY);
-  markerMsg.set_type(msgs::Marker::TRIANGLE_FAN);
-
-  msgs::Material *matMsg = markerMsg.mutable_material();
-  matMsg->mutable_script()->set_name("Gazebo/Red");
-  msgs::Set(markerMsg.mutable_pose(),
-            math::Pose3d(this->goalX, this->goalY, 1, 0, 0, this->goalYaw));
-  bool markerRequest = this->node.Request("/marker", markerMsg);
-  this->markers = markerRequest;
-}
+  /// \brief Waypoint visualization markers
+  public: WaypointMarkers waypointMarkers{"station_keeping_marker"};
+};
 
 /////////////////////////////////////////////////
 StationkeepingScoringPlugin::StationkeepingScoringPlugin()
@@ -235,10 +217,15 @@ void StationkeepingScoringPlugin::Configure(const sim::Entity &_entity,
   if (_sdf->HasElement("head_error_on"))
     this->dataPtr->headErrorOn = _sdf->Get<bool>("head_error_on");
 
-  // if (_sdf->HasElement("markers"))
-  // {
-  //   this->dataPtr->markers = true;
-  // }
+  if (_sdf->HasElement("markers"))
+  {
+    this->dataPtr->waypointMarkers.Load(_sdf->Clone()->GetElement("markers"));
+    if (!this->dataPtr->waypointMarkers.DrawMarker(0,
+           this->dataPtr->goalX, this->dataPtr->goalY, this->dataPtr->goalYaw))
+    {
+      gzerr << "Error creating visual marker" << std::endl;
+    }
+  }
 }
 
 //////////////////////////////////////////////////
@@ -251,12 +238,6 @@ void StationkeepingScoringPlugin::PreUpdate( const sim::UpdateInfo &_info,
 
   ScoringPlugin::PreUpdate(_info, _ecm);
 
-  // Check to see if we need to publish the marker(s)
-  // if (this->dataPtr->markers)
-  // {
-  //   this->dataPtr->AddMarker();
-  // }
-
   if (this->TaskState() == "finished")
     return;
 

vrx_gz/src/WayfindingScoringPlugin.cc

@@ -200,7 +200,7 @@ void WayfindingScoringPlugin::Configure(const sim::Entity &_entity,
     for (const auto waypoint : this->dataPtr->localWaypoints)
     {
       if (!this->dataPtr->waypointMarkers.DrawMarker(markerId, waypoint.X(),
-            waypoint.Y(), waypoint.Z(), std::to_string(markerId)))
+            waypoint.Y(), waypoint.Z()))
       {
         gzerr << "Error creating visual marker" << std::endl;
       }

vrx_gz/src/WaypointMarkers.cc

@@ -26,16 +26,13 @@ using namespace vrx;
 
 /////////////////////////////////////////////////
 WaypointMarkers::WaypointMarkers(const std::string &_namespace)
-  : ns(_namespace), material("Gazebo/Red"), scaling{0.2, 0.2, 1.5}, height(4.0)
+  : ns(_namespace), scaling{0.2, 0.2, 1.5}, height(4.0)
 {
 }
 
 /////////////////////////////////////////////////
 void WaypointMarkers::Load(const std::shared_ptr<const sdf::Element> &_sdf)
 {
-  if (_sdf->HasElement("material"))
-    this->material = _sdf->Get<std::string>("material");
-
   if (_sdf->HasElement("scaling"))
     this->scaling = _sdf->Get<math::Vector3d>("scaling");
 
@@ -47,22 +44,27 @@ void WaypointMarkers::Load(const std::shared_ptr<const sdf::Element> &_sdf)
 }
 
 /////////////////////////////////////////////////
-bool WaypointMarkers::DrawMarker(double _x, double _y, double _yaw,
-  const std::string &_text)
+bool WaypointMarkers::DrawMarker(double _x, double _y, double _yaw)
 {
-  return this->DrawMarker(this->id++, _x, _y, _yaw, _text);
+  return this->DrawMarker(this->id++, _x, _y, _yaw);
 }
 
 /////////////////////////////////////////////////
 bool WaypointMarkers::DrawMarker(int _markerId, double _x, double _y,
-  double _yaw, const std::string &_text)
+  double _yaw)
 {
- //TODO: Fix below. The markers are not showing up.
   msgs::Marker markerMsg;
   markerMsg.set_ns(this->ns);
   markerMsg.set_action(msgs::Marker_Action_ADD_MODIFY);
-  msgs::Material *matMsg = markerMsg.mutable_material();
-  matMsg->mutable_script()->set_name(this->material);
+  markerMsg.set_visibility(gz::msgs::Marker::GUI);
+  markerMsg.mutable_material()->mutable_ambient()->set_r(1);
+  markerMsg.mutable_material()->mutable_ambient()->set_g(1);
+  markerMsg.mutable_material()->mutable_ambient()->set_b(1);
+  markerMsg.mutable_material()->mutable_ambient()->set_a(1);
+  markerMsg.mutable_material()->mutable_diffuse()->set_r(1);
+  markerMsg.mutable_material()->mutable_diffuse()->set_g(1);
+  markerMsg.mutable_material()->mutable_diffuse()->set_b(1);
+  markerMsg.mutable_material()->mutable_diffuse()->set_a(1);
 
   // draw cylinder
   markerMsg.set_type(msgs::Marker_Type_CYLINDER);
@@ -79,25 +81,9 @@ bool WaypointMarkers::DrawMarker(int _markerId, double _x, double _y,
   msgs::Set(markerMsg.mutable_scale(), this->scaling);
   msgs::Set(markerMsg.mutable_pose(),  math::Pose3d(
     _x + cos(_yaw), _y + sin(_yaw), this->height + this->scaling.Z() / 2.0,
-    0, M_PI/2, _yaw));
+    0, M_PI / 2, _yaw));
   markerMsg.set_id((_markerId + 1) * 1000);
   result = node.Request("/marker", markerMsg);
-  if (!result)
-    return false;
 
-  // draw text
-//  if (!_text.empty())
-//  {
-//    markerMsg.set_type(msgs::Marker_Type_TEXT);
-//    markerMsg.set_text(_text);
-//    msgs::Set(markerMsg.mutable_scale(),
-//                        math::Vector3d(1.0, 1.0, 1.0));
-//    msgs::Set(markerMsg.mutable_pose(),
-//                        math::Pose3d(_x, _y - 0.2,
-//                            this->height + this->scaling.Z() + 0.8,
-//                            0, 0, 0));
-//    markerMsg.set_id((_markerId + 1) * 10000);
-//    result = node.Request("/marker", markerMsg);
-//  }
   return result;
 }

vrx_gz/src/WaypointMarkers.hh

@@ -27,19 +27,15 @@
 namespace vrx
 {
   /// \brief This class is used to display waypoint markers.
-  /// Cylindrical Gazebo markers are drawn with text on top
   ///
   /// The marker properties can be set using sdf:
-  /// material: Optional parameter (string type) to specify the material
-  ///           for marker. Default: Gazebo/Green
   /// scaling: Optional parameter (vector type) to specify marker scaling.
   ///          Default: 0.2 0.2 1.5
   /// height: Optional parameter (double type) height of marker above water.
   /// initial_id: Optional parameter (int type) to be used as initial ID when
   /// drawing markers without explicitly specifying ID.
   /// E.g.
   /// <markers>
-  ///   <material>Gazebo/Green</material>
   ///   <scaling>0.2 0.2 2.0</scaling>
   ///   <height>0.5</height>
   /// </markers>
@@ -59,31 +55,24 @@ namespace vrx
     /// \param[in] _x X coordinate of waypoint marker
     /// \param[in] _y Y coordinate of waypoint marker
     /// \param[in] _yaw orientation of waypoint marker in radians
-    /// \param[in] _text (optional) Text above waypoint marker
     /// \return Returns true if marker is successfully sent to Gazebo
     public: bool DrawMarker(int _markerId,
                             double _x,
                             double _y,
-                            double _yaw,
-                            const std::string &_text = "");
+                            double _yaw);
 
     /// \brief Draw a new waypoint marker in Gazebo
     /// \param[in] _x X coordinate of waypoint marker
     /// \param[in] _y Y coordinate of waypoint marker
     /// \param[in] _yaw orientation of waypoint marker in radians
-    /// \param[in] _text (optional) Text above waypoint marker
     /// \return Returns true if marker is successfully sent to Gazebo
     public: bool DrawMarker(double _x,
                             double _y,
-                            double _yaw,
-                            const std::string &_text = "");
+                            double _yaw);
 
     /// \brief Namespace for Gazebo markers
     private: std::string ns;
 
-    /// \brief Name of Gazebo material for marker
-    private: std::string material;
-
     /// \brief Scaling factor for cylinder marker
     private: gz::math::Vector3d scaling;
 
@@ -93,7 +82,7 @@ namespace vrx
     /// \brief If an ID is not specified, the markers will start using this one.
     private: int id = 0;
 
-    /// \brief gazebo transport node
+    /// \brief Gazebo transport node
     private: gz::transport::Node node;
   };
 }

vrx_gz/src/vrx_gz/launch.py

@@ -42,7 +42,10 @@
 ]
 
 STATIONKEEPING_WORLDS = [
-  'stationkeeping_task'
+  'stationkeeping_task',
+  'practice_2022_stationkeeping0_task',
+  'practice_2022_stationkeeping1_task',
+  'practice_2022_stationkeeping2_task',
 ]
 
 WAYFINDING_WORLDS = [