Station keeping practice worlds (#582)

* Station keeping practice worlds

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

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 = [