Skip to content

Commit

Permalink
Service logic added
Browse files Browse the repository at this point in the history
  • Loading branch information
@KiselevIlia
KiselevIlia committed Mar 29, 2024
1 parent adc01a3 commit 250aaff
Show file tree
Hide file tree
Showing 3 changed files with 69 additions and 63 deletions.
18 changes: 11 additions & 7 deletions include/groundtruth_plugin.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,8 @@
#include <gz/sim/config.hh>
#include <gz/sim/System.hh>
#include <gz/transport/Node.hh>
#include "gz/sim/Model.hh"
#include <gz/msgs.hh>
#include <gz/sim/Model.hh>
#include "common.h"
#include "Groundtruth.pb.h"

Expand Down Expand Up @@ -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;
Expand All @@ -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_{};

Expand All @@ -88,8 +96,4 @@ namespace gz {
}
}
}
}

#ifdef DEBUG
void callback(gz::msgs::Groundtruth const& msg);
#endif
}
29 changes: 16 additions & 13 deletions msgs/Groundtruth.proto
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
85 changes: 42 additions & 43 deletions src/groundtruth_plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <gz/transport/AdvertiseOptions.hh>

using namespace gz;
using namespace sim;
Expand All @@ -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<const sdf::Element> &_sdf,
Expand Down Expand Up @@ -93,11 +100,11 @@ void GroundtruthPlugin::Configure(const Entity &_entity,
}

navPub = node.Advertise<msgs::Groundtruth>(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;
}
}

//////////////////////////////////////////////////
Expand Down Expand Up @@ -126,13 +133,15 @@ void GroundtruthPlugin::PostUpdate(const UpdateInfo &_info,
return;
}

PublishPose(_ecm, convert<msgs::Time>(_info.simTime));
fillPose(_ecm, convert<msgs::Time>(_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
Expand All @@ -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 = &gtMsg;
_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,
Expand All @@ -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
bool GroundtruthPlugin::responseCallback(const gz::msgs::StringMsg &, gz::msgs::Groundtruth &rep)
{
rep.Clear();
rep.CopyFrom(stash::glob_gt_msg);

return true;
}

0 comments on commit 250aaff

Please sign in to comment.