Groundtruth plugin works

CMakeLists.txt

@@ -10,9 +10,23 @@ find_package(gz-plugin2 REQUIRED COMPONENTS register)
 set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
 
 set(GAZEBO_VERSION 8)
-find_package(gz-sim8 REQUIRED)
+find_package(gz-sim8 8.2.0 REQUIRED)
+if(NOT ${gz-sim8_VERSION} EQUAL 8.2.0)
+  message(FATAL_ERROR "gz-sim8 must be version 8.2.0. You have ${gz-sim8_VERSION}")
+endif ()
 find_package(gz-msgs10)
+if(NOT ${gz-msgs10_VERSION} EQUAL 10.1.1)
+  message(FATAL_ERROR "gz-msgs10 must be version 10.1.1. You have ${gz-msgs10_VERSION}")
+endif ()
 find_package(gz-sensors8 REQUIRED)
+if(NOT ${gz-sensors8_VERSION} EQUAL 8.0.1)
+  message(FATAL_ERROR "gz-sensors8 must be version 8.0.1. You have ${gz-sensors8_VERSION}")
+endif ()
+find_package(gz-transport13 13.1.0 REQUIRED)
+if(NOT ${gz-transport13_VERSION} EQUAL 13.1.0)
+  message(FATAL_ERROR "gz-transport13 must be version 13.1.0. You have ${gz-transport13_VERSION}")
+endif ()
+
 if(gz-msgs10_FOUND)
   set(GZ_MSG_VER 10)
   set(Protobuf_IMPORT_DIRS ${gz-msgs10_INCLUDE_DIRS})
@@ -27,7 +41,10 @@ find_package(MAVLink)
 find_package(Protobuf REQUIRED)
 
 #Generate messages files for collision plugin
-PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS msgs/custom_message.proto)
+PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS msgs/CollisionObject.proto)
+
+#Generate messages files for groundtruth plugin
+PROTOBUF_GENERATE_CPP(GT_PROTO_SRCS GT_PROTO_HDRS msgs/Groundtruth.proto)
 
 include_directories(${CMAKE_BINARY_DIR})
 
@@ -61,9 +78,11 @@ target_link_libraries(collision_plugin
         PRIVATE gz-sim${GAZEBO_VERSION}::gz-sim${GAZEBO_VERSION}
 )
 
-add_library(groundtruth_plugin SHARED src/groundtruth_plugin.cpp)
+add_library(groundtruth_plugin SHARED src/groundtruth_plugin.cpp ${GT_PROTO_SRCS})
 target_link_libraries(groundtruth_plugin
         PRIVATE gz-sim${GAZEBO_VERSION}::gz-sim${GAZEBO_VERSION}
         PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
         PRIVATE gz-msgs10::gz-msgs10
-)
+)
+# ln -s ./libgroundtruth_plugin.so /usr/lib/x86_64-linux-gnu/gz-sim-8/plugins
+# ln -s ./libcollision_plugin.so /usr/lib/x86_64-linux-gnu/gz-sim-8/plugins

include/groundtruth_plugin.h

@@ -6,8 +6,7 @@
 #include <gz/transport/Node.hh>
 #include "gz/sim/Model.hh"
 #include "common.h"
-#include <gz/msgs/pose_v.pb.h>
-#include <gz/msgs/navsat.pb.h>
+#include "Groundtruth.pb.h"
 
 namespace gz {
     namespace sim {
@@ -56,8 +55,7 @@ namespace gz {
                     /// \brief Gazebo communication node.
                     transport::Node node;
 
-                    /// \brief publisher for pose data
-                    transport::Node::Publisher posePub;
+                    /// \brief publisher for groundtruth data
                     transport::Node::Publisher navPub;
 
                     /// \brief Model interface
@@ -74,12 +72,6 @@ namespace gz {
                     /// parameter
                     std::chrono::steady_clock::duration updatePeriod{0};
 
-                    /// \brief A variable that gets populated with poses. This also here as a
-                    /// member variable to avoid repeated memory allocations and improve
-                    /// performance.
-                    msgs::Pose poseMsg;
-                    msgs::NavSat navMsg;
-
                     /// \brief Model name
                     std::string model_name_{};
 

include/harmonic_collision.h

@@ -5,7 +5,7 @@
 #include <gz/transport/Node.hh>
 #include <gz/sim/Model.hh>
 
-#include "custom_message.pb.h"
+#include "CollisionObject.pb.h"
 
 namespace gz {
     using namespace sim;

msgs/Groundtruth.proto

@@ -0,0 +1,51 @@
+syntax = "proto3";
+package gz.msgs;
+option java_package = "com.gz.msgs";
+option java_outer_classname = "GroundtruthProtos";
+
+/// \ingroup gz.msgs
+/// \interface Groundtruth
+/// \brief Data from a Groundtruth plugin
+/// This is for GPS message.
+
+message Groundtruth
+{
+  /// \brief Sequence number
+  uint32 seq_num = 1;
+
+  /// \brief Time
+  int64 time_usec = 2;
+
+  /// \brief Latitude in degrees
+  double latitude_rad = 3;
+
+  /// \brief Longitude in degrees
+  double longitude_rad = 4;
+
+  /// \brief Altitude in meters
+  double altitude_m = 5;
+
+  /// \brief East velocity in the ENU frame, in m / s
+  double velocity_east = 6;
+
+  /// \brief North velocity in the ENU frame, in m / s
+  double velocity_north = 7;
+
+  /// \brief Up velocity in the ENU frame, in m / s
+  double velocity_up = 8;
+
+  /// \brief Quaternion - altitude_w
+  double attitude_q_w = 9;
+
+  /// \brief Quaternion - altitude_x
+  double attitude_q_x = 10;
+
+  /// \brief Quaternion - altitude_y
+  double attitude_q_y = 11;
+
+  /// \brief Quaternion - altitude_z
+  double attitude_q_z = 12;
+
+  /// \brief Reference name
+  string frame_id = 13;
+}

src/groundtruth_plugin.cpp

@@ -1,6 +1,5 @@
 #include "groundtruth_plugin.h"
 
-#include <gz/msgs/pose.pb.h>
 #include <gz/msgs/time.pb.h>
 
 #include <string>
@@ -12,11 +11,9 @@
 #include <gz/plugin/Register.hh>
 
 #include "gz/sim/Util.hh"
-#include "gz/sim/components/Name.hh"
 #include "gz/sim/components/ParentEntity.hh"
 #include "gz/sim/components/Pose.hh"
 #include "gz/sim/Conversions.hh"
-#include "gz/sim/Util.hh"
 
 using namespace gz;
 using namespace sim;
@@ -48,15 +45,14 @@ void GroundtruthPlugin::Configure(const Entity &_entity,
     }
 
     // create publisher
-    std::string poseTopic = scopedName(_entity, _ecm) + "/groundtruth";
+    std::string poseTopic = model_name_ + "/groundtruth";
     poseTopic = transport::TopicUtils::AsValidTopic(poseTopic);
     if (poseTopic.empty()) {
         poseTopic = "/groundtruth";
         gzerr << "Empty pose topic generated for pose_publisher system. "
               << "Setting to " << poseTopic << std::endl;
     }
 
-    auto world_entiry = gz::sim::worldEntity(_ecm);
     auto world_has_origin{false};
 
     // Use environment variables if set for home position.
@@ -96,8 +92,7 @@ void GroundtruthPlugin::Configure(const Entity &_entity,
         alt_home_ = _sdf->Get<double>("homeAltitude", alt_home_).first;
     }
 
-    posePub = node.Advertise<msgs::Pose>(poseTopic);
-    navPub = node.Advertise<msgs::NavSat>(poseTopic + "/gps");
+    navPub = node.Advertise<msgs::Groundtruth>(poseTopic);
 }
 
 //////////////////////////////////////////////////
@@ -136,62 +131,42 @@ void GroundtruthPlugin::PublishPose(const EntityComponentManager &_ecm,
     GZ_PROFILE("PosePublisher::PublishPoses");
 
     // publish poses
-    msgs::Pose *msg{nullptr};
-    msgs::NavSat *n_Msg{nullptr};
-
     auto pose = _ecm.Component<components::Pose>(model.Entity());
     if (!pose)
         return;
 
-    this->poseMsg.Clear();
-    msg = &this->poseMsg;
-    navMsg.Clear();
-    n_Msg = &navMsg;
-
-
     // fill pose msg
     // pose is the transform from parent_name_ to model_name_
-    GZ_ASSERT(msg != nullptr, "Pose msg is null");
-    GZ_ASSERT(n_Msg != nullptr, "Navigation msg is null");
-    auto header = msg->mutable_header();
-
-    auto n_header = n_Msg->mutable_header();
-
-    header->mutable_stamp()->CopyFrom(_stampMsg);
-    n_header->mutable_stamp()->CopyFrom(_stampMsg);
     const math::Pose3d &transform = pose->Data();
-    auto childFrame = header->add_data();
-    childFrame->set_key("model_name");
-    childFrame->add_value(model_name_);
-
     auto pos_W_I = transform.Pos();
     auto att_W_I = transform.Rot();
 
-    auto pos = sphericalCoordinates(model.Entity(), _ecm).value();
-
     // reproject position into geographic coordinates
     auto latlon_gt = reproject(pos_W_I, lat_home_, lon_home_, alt_home_);
 
 //  TODO: Velocity
-
 //    auto one = relativeVel(model.Entity(), _ecm);
 //    gzwarn << "Velocity: " << one << std::endl;
-//    gz::sim::worldPose(model.Entity());
-//    gzwarn << "-Raw Pos(): " << transform.Pos() << std::endl;
-//    gzwarn << "-Ready: " << latlon_gt.first << " " << latlon_gt.second << " " << pos_W_I.Z() + alt_home_ << " "
-//           << att_W_I.W() << " " << att_W_I.X() << " " << att_W_I.Y() << " " << att_W_I.Z() << std::endl;
-//    gzwarn << "-Converted: " << latlon_gt.first * 180 / M_PI << " " << latlon_gt.second * 180 / M_PI << std::endl;
-    // set pose
-    msg->set_name(model_name_);
-    msgs::Set(msg, transform);
 
-    n_Msg->set_latitude_deg(latlon_gt.first * 180 / M_PI);
-    n_Msg->set_longitude_deg(latlon_gt.second * 180 / M_PI);
-    n_Msg->set_altitude(pos_W_I.Z() + alt_home_);
+    // set pose
+    static uint32_t i{0};
+    msgs::Groundtruth gtMsg;
+    gtMsg.set_seq_num(i++);
+    gtMsg.set_time_usec(_stampMsg.sec());
+    gtMsg.set_latitude_rad(latlon_gt.first);
+    gtMsg.set_longitude_rad(latlon_gt.second);
+    gtMsg.set_altitude_m(pos_W_I.Z() + alt_home_);
+    gtMsg.set_velocity_east(0);
+    gtMsg.set_velocity_north(0);
+    gtMsg.set_velocity_up(0);
+    gtMsg.set_attitude_q_w(att_W_I.W());
+    gtMsg.set_attitude_q_x(att_W_I.X());
+    gtMsg.set_attitude_q_y(att_W_I.X());
+    gtMsg.set_attitude_q_z(att_W_I.Z());
+    gtMsg.set_frame_id(model_name_);
 
     // publish individual pose msgs
-    posePub.Publish(this->poseMsg);
-    navPub.Publish(navMsg);
+    navPub.Publish(gtMsg);
 }
 
 GZ_ADD_PLUGIN(GroundtruthPlugin,