From 250aaff9f9fa714d3952be896c19aafe071cabbf Mon Sep 17 00:00:00 2001 From: KiselevIlia Date: Fri, 29 Mar 2024 17:02:29 +0400 Subject: [PATCH] Service logic added --- include/groundtruth_plugin.h | 18 +++++--- msgs/Groundtruth.proto | 29 ++++++------ src/groundtruth_plugin.cpp | 85 ++++++++++++++++++------------------ 3 files changed, 69 insertions(+), 63 deletions(-) diff --git a/include/groundtruth_plugin.h b/include/groundtruth_plugin.h index a1c858f..b8f0dfe 100644 --- a/include/groundtruth_plugin.h +++ b/include/groundtruth_plugin.h @@ -4,7 +4,8 @@ #include #include #include -#include "gz/sim/Model.hh" +#include +#include #include "common.h" #include "Groundtruth.pb.h" @@ -48,9 +49,11 @@ namespace gz { /// \brief Publishes pose with the provided time /// stamp. /// \param[in] _stampMsg Time stamp associated with published poses - void PublishPose(const EntityComponentManager &_ecm, + void fillPose(const EntityComponentManager &_ecm, const msgs::Time &_stampMsg); + bool responseCallback(const gz::msgs::StringMsg &, gz::msgs::Groundtruth &rep); + private: /// \brief Gazebo communication node. transport::Node node; @@ -72,6 +75,11 @@ 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::Groundtruth gtMsg; + /// \brief Model name std::string model_name_{}; @@ -88,8 +96,4 @@ namespace gz { } } } -} - -#ifdef DEBUG -void callback(gz::msgs::Groundtruth const& msg); -#endif \ No newline at end of file +} \ No newline at end of file diff --git a/msgs/Groundtruth.proto b/msgs/Groundtruth.proto index 6bcc48e..b27b1c7 100644 --- a/msgs/Groundtruth.proto +++ b/msgs/Groundtruth.proto @@ -13,39 +13,42 @@ message Groundtruth /// \brief Sequence number uint32 seq_num = 1; - /// \brief Time - int64 time_usec = 2; + /// \brief Seconds + int64 time_sec = 2; + + /// \brief Nanoseconds + int32 time_nsec = 3; /// \brief Latitude in degrees - double latitude_rad = 3; + double latitude_rad = 4; /// \brief Longitude in degrees - double longitude_rad = 4; + double longitude_rad = 5; /// \brief Altitude in meters - double altitude_m = 5; + double altitude_m = 6; /// \brief East velocity in the ENU frame, in m / s - double velocity_east = 6; + double velocity_east = 7; /// \brief North velocity in the ENU frame, in m / s - double velocity_north = 7; + double velocity_north = 8; /// \brief Up velocity in the ENU frame, in m / s - double velocity_up = 8; + double velocity_up = 9; /// \brief Quaternion - altitude_w - double attitude_q_w = 9; + double attitude_q_w = 10; /// \brief Quaternion - altitude_x - double attitude_q_x = 10; + double attitude_q_x = 11; /// \brief Quaternion - altitude_y - double attitude_q_y = 11; + double attitude_q_y = 12; /// \brief Quaternion - altitude_z - double attitude_q_z = 12; + double attitude_q_z = 13; /// \brief Reference name - string frame_id = 13; + string frame_id = 14; } \ No newline at end of file diff --git a/src/groundtruth_plugin.cpp b/src/groundtruth_plugin.cpp index ef51da0..7395f4a 100644 --- a/src/groundtruth_plugin.cpp +++ b/src/groundtruth_plugin.cpp @@ -14,6 +14,8 @@ #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/Pose.hh" #include "gz/sim/Conversions.hh" +// Just in case for Advertise function +// #include using namespace gz; using namespace sim; @@ -23,6 +25,11 @@ using namespace systems; GroundtruthPlugin::GroundtruthPlugin() { } +namespace stash { +/// \brief Service callback can't use any of class fields (pub/priv/prot). It can use only global variables. + static msgs::Groundtruth glob_gt_msg; +} + ////////////////////////////////////////////////// void GroundtruthPlugin::Configure(const Entity &_entity, const std::shared_ptr &_sdf, @@ -93,11 +100,11 @@ void GroundtruthPlugin::Configure(const Entity &_entity, } navPub = node.Advertise(poseTopic); - -#ifdef DEBUG - node.Subscribe(poseTopic, callback); - gzwarn << "DEBUG MODE. Echo from " << poseTopic << std::endl; -#endif + auto resp = node.Advertise(poseTopic + "_req", &GroundtruthPlugin::responseCallback, {}); + if (!resp) + { + gzwarn << "Error advertising service" << std::endl; + } } ////////////////////////////////////////////////// @@ -126,13 +133,15 @@ void GroundtruthPlugin::PostUpdate(const UpdateInfo &_info, return; } - PublishPose(_ecm, convert(_info.simTime)); + fillPose(_ecm, convert(_info.simTime)); + // publish individual pose msgs + navPub.Publish(gtMsg); lastPosePubTime = _info.simTime; } ////////////////////////////////////////////////// -void GroundtruthPlugin::PublishPose(const EntityComponentManager &_ecm, - const msgs::Time &_stampMsg) { +void GroundtruthPlugin::fillPose(const EntityComponentManager &_ecm, + const msgs::Time &_stampMsg) { GZ_PROFILE("PosePublisher::PublishPoses"); // publish poses @@ -155,23 +164,24 @@ void GroundtruthPlugin::PublishPose(const EntityComponentManager &_ecm, // 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 - navPub.Publish(gtMsg); + msgs::Groundtruth *_gtMsg = >Msg; + _gtMsg->Clear(); + _gtMsg->set_seq_num(i++); + _gtMsg->set_time_sec(_stampMsg.sec()); + _gtMsg->set_time_nsec(_stampMsg.nsec()); + _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_); + + stash::glob_gt_msg.CopyFrom(gtMsg); } GZ_ADD_PLUGIN(GroundtruthPlugin, @@ -182,21 +192,10 @@ GZ_ADD_PLUGIN(GroundtruthPlugin, GZ_ADD_PLUGIN_ALIAS(GroundtruthPlugin, "gz::sim::systems::GroundtruthPlugin") -#ifdef DEBUG -void callback(gz::msgs::Groundtruth const& msg){ - gzwarn << - "seq_num(): " << msg.seq_num() << std::endl << - "time_usec(): " << msg.time_usec() << std::endl << - "latitude_rad(): " << msg.latitude_rad() << " " << msg.latitude_rad() * 180 / M_PI << std::endl << - "longitude_rad(): " << msg.longitude_rad() << " " << msg.longitude_rad() * 180 / M_PI << std::endl << - "altitude_m(): " << msg.altitude_m() << std::endl << - "velocity_east(): " << msg.velocity_east() << std::endl << - "velocity_north(): " << msg.velocity_north() << std::endl << - "velocity_up(): " << msg.velocity_up() << std::endl << - "attitude_q_w(): " << msg.attitude_q_w() << std::endl << - "attitude_q_x(): " << msg.attitude_q_x() << std::endl << - "attitude_q_y(): " << msg.attitude_q_y() << std::endl << - "attitude_q_z(): " << msg.attitude_q_z() << std::endl << - "frame_id(): " << msg.frame_id() << std::endl; -} -#endif \ No newline at end of file +bool GroundtruthPlugin::responseCallback(const gz::msgs::StringMsg &, gz::msgs::Groundtruth &rep) +{ + rep.Clear(); + rep.CopyFrom(stash::glob_gt_msg); + + return true; +} \ No newline at end of file