From a81dae037303ce7492fbc9e747a96ca99c174da7 Mon Sep 17 00:00:00 2001 From: Withanage Perera Date: Sat, 2 Aug 2025 14:40:59 +0200 Subject: [PATCH 1/3] Got the publisher to code to work by injecting it in the DVL plugin. Need to duplicate it and create a separate plugin. --- .../urdf/sub9.urdf.xacro | 11 ++ .../urdf/xacro/position_sensor.xacro | 27 ++++ .../subjugator_gazebo/CMakeLists.txt | 15 +- .../subjugator_gazebo/include/DepthSensor.hh | 2 + .../subjugator_gazebo/include/RealPosition.hh | 58 ++++++++ .../subjugator_gazebo/src/DepthSensor.cc | 32 +++++ .../subjugator_gazebo/src/RealPosition.cc | 132 ++++++++++++++++++ 7 files changed, 269 insertions(+), 8 deletions(-) create mode 100644 src/subjugator/simulation/subjugator_description/urdf/xacro/position_sensor.xacro create mode 100644 src/subjugator/simulation/subjugator_gazebo/include/RealPosition.hh create mode 100644 src/subjugator/simulation/subjugator_gazebo/src/RealPosition.cc diff --git a/src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro b/src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro index fd37561b..28cda9a5 100644 --- a/src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro +++ b/src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro @@ -5,6 +5,7 @@ + @@ -97,6 +98,14 @@ + + + base_link + /my_robot/pose + 10 + + + @@ -104,6 +113,8 @@ + + + + + + + + + + + + + + + + + + + + + + + /my_robot/pose + 10 + + + + diff --git a/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt b/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt index 02bd7cc6..0b734efb 100644 --- a/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt +++ b/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt @@ -52,12 +52,15 @@ add_library(DepthSensor SHARED src/DepthSensor.cc) # Plugin 3: Hydrophone add_library(Hydrophone SHARED src/Hydrophone.cc) +add_library(RealPosition SHARED src/RealPosition.cc) + # Ensures plugin can see include directory to access its header files target_include_directories(ExamplePlugin PRIVATE include) target_include_directories(UnderwaterCamera PRIVATE include) target_include_directories(DVLBridge PRIVATE include) +target_include_directories(RealPosition PRIVATE include) target_include_directories(DepthSensor PRIVATE include) @@ -76,6 +79,7 @@ target_link_libraries(UnderwaterCamera gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} # Link libraries for DVL Bridge plugin target_link_libraries(DVLBridge gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) +target_link_libraries(RealPosition gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) # Link libraries for Hydrophone plugin target_link_libraries(Hydrophone gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) @@ -84,24 +88,19 @@ target_link_libraries(Hydrophone gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) target_link_libraries(DepthSensor gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) # Specify dependencies using ament_target_dependencies -ament_target_dependencies( - ExamplePlugin - # - # Here is where you would place any dependencies for your plugin - # -) ament_target_dependencies(UnderwaterCamera rclcpp sensor_msgs) ament_target_dependencies(DVLBridge nav_msgs rclcpp geometry_msgs std_msgs) +ament_target_dependencies(RealPosition nav_msgs rclcpp geometry_msgs std_msgs) ament_target_dependencies(DepthSensor rclcpp mil_msgs sensor_msgs) ament_target_dependencies(Hydrophone rclcpp geometry_msgs mil_msgs) # Install the plugins -install(TARGETS UnderwaterCamera ExamplePlugin DVLBridge Hydrophone DepthSensor - LIBRARY DESTINATION lib/${PROJECT_NAME}) +install(TARGETS UnderwaterCamera ExamplePlugin DVLBridge RealPosition Hydrophone + DepthSensor LIBRARY DESTINATION lib/${PROJECT_NAME}) # Install headers install(DIRECTORY include/ DESTINATION include) diff --git a/src/subjugator/simulation/subjugator_gazebo/include/DepthSensor.hh b/src/subjugator/simulation/subjugator_gazebo/include/DepthSensor.hh index de1d885a..c5beea43 100644 --- a/src/subjugator/simulation/subjugator_gazebo/include/DepthSensor.hh +++ b/src/subjugator/simulation/subjugator_gazebo/include/DepthSensor.hh @@ -52,6 +52,8 @@ class DepthSensor : public gz::sim::System, public gz::sim::ISystemConfigure, pu double noiseStdDev_ = 0.0; // default to 0 => no noise if not specified std::mt19937 randomEngine_; std::normal_distribution noiseDist_{ 0.0, 0.0 }; + + std::string entityName; }; } // namespace depth_sensor diff --git a/src/subjugator/simulation/subjugator_gazebo/include/RealPosition.hh b/src/subjugator/simulation/subjugator_gazebo/include/RealPosition.hh new file mode 100644 index 00000000..e65bef45 --- /dev/null +++ b/src/subjugator/simulation/subjugator_gazebo/include/RealPosition.hh @@ -0,0 +1,58 @@ +#ifndef position_HH_ +#define position_HH_ + +#include + +#include + +#include +#include +#include +#include +#include + +namespace real_position +{ +class RealPosition : public gz::sim::System, public gz::sim::ISystemConfigure, public gz::sim::ISystemPostUpdate +{ + public: + RealPosition(); + + public: + ~RealPosition() override; + + /// \brief Configure the system + /// \param[in] _entity The entity this plugin is attached to. + /// \param[in] _sdf The SDF Element associated with this system plugin. + /// \param[in] _ecm The EntityComponentManager of the given simulation instance. + /// \param[in] _eventMgr The EventManager of the given simulation instance. + void Configure(gz::sim::Entity const &entity, std::shared_ptr const &sdf, + gz::sim::EntityComponentManager &ecm, gz::sim::EventManager &eventMgr) override; + + public: + void PostUpdate(gz::sim::UpdateInfo const &_info, gz::sim::EntityComponentManager const &_ecm) override; + + private: + /// \brief Transport node for publishing + gz::transport::Node node; + + /// \brief Publisher for pose messages + gz::transport::Node::Publisher posePub; + + /// \brief Entity ID that this plugin is attached to + gz::sim::Entity entityId; + + /// \brief Topic name for publishing pose + std::string topicName; + + /// \brief Entity name for logging + std::string entityName; + + /// \brief Update frequency control + int updateCounter = 0; + int publishFrequency = 10; // Publish every 10 updates (adjust as needed) +}; + +} // namespace real_position + +#endif diff --git a/src/subjugator/simulation/subjugator_gazebo/src/DepthSensor.cc b/src/subjugator/simulation/subjugator_gazebo/src/DepthSensor.cc index d7815b5b..965757e2 100644 --- a/src/subjugator/simulation/subjugator_gazebo/src/DepthSensor.cc +++ b/src/subjugator/simulation/subjugator_gazebo/src/DepthSensor.cc @@ -1,12 +1,18 @@ #include "DepthSensor.hh" +#include + #include #include #include #include +#include #include // For GZ_ADD_PLUGIN +#include +#include +#include // Register plugin so Gazebo can see it GZ_ADD_PLUGIN(depth_sensor::DepthSensor, gz::sim::System, depth_sensor::DepthSensor::ISystemConfigure, @@ -25,6 +31,16 @@ void DepthSensor::Configure(gz::sim::Entity const &_entity, std::shared_ptrmodelEntity_ = _entity; + auto nameComp = _ecm.Component(_entity); + if (nameComp) + { + this->entityName = nameComp->Data(); + } + else + { + this->entityName = "unknown_entity_" + std::to_string(_entity); + } + // Getting values from SDF if (_sdf->HasElement("frame_id")) this->frameName_ = _sdf->Get("frame_id"); @@ -87,6 +103,22 @@ void DepthSensor::PostUpdate(gz::sim::UpdateInfo const &_info, gz::sim::EntityCo return; } + auto const &pose = poseComp->Data(); + + { + // Print to terminal + std::cout << "=== Entity Position Update ===" << std::endl; + std::cout << "Entity: " << this->entityName << " (ID: " << this->modelEntity_ << ")" << std::endl; + std::cout << "Position - X: " << pose.Pos().X() << ", Y: " << pose.Pos().Y() << ", Z: " << pose.Pos().Z() + << std::endl; + std::cout << "Orientation - Roll: " << pose.Rot().Roll() << ", Pitch: " << pose.Rot().Pitch() + << ", Yaw: " << pose.Rot().Yaw() << std::endl; + std::cout << "Quaternion - W: " << pose.Rot().W() << ", X: " << pose.Rot().X() << ", Y: " << pose.Rot().Y() + << ", Z: " << pose.Rot().Z() << std::endl; + std::cout << "Simulation time: " << _info.simTime.count() / 1e9 << " seconds" << std::endl; + std::cout << "===============================" << std::endl; + } + // To avoid the deprecated operator+, we manually combine: gz::math::Pose3d finalPose = poseComp->Data(); finalPose.Pos() += this->offset_.Pos(); diff --git a/src/subjugator/simulation/subjugator_gazebo/src/RealPosition.cc b/src/subjugator/simulation/subjugator_gazebo/src/RealPosition.cc new file mode 100644 index 00000000..3e3ebb69 --- /dev/null +++ b/src/subjugator/simulation/subjugator_gazebo/src/RealPosition.cc @@ -0,0 +1,132 @@ +#include "RealPosition.hh" + +#include + +#include + +#include +#include +#include +#include + +// Add plugins to gazebo simulator alongside its dependencies +GZ_ADD_PLUGIN(real_position::RealPosition, gz::sim::System, real_position::RealPosition::ISystemPostUpdate) + +using namespace real_position; + +RealPosition::RealPosition() +{ +} + +RealPosition::~RealPosition() +{ +} + +void RealPosition::Configure(gz::sim::Entity const &_entity, std::shared_ptr const &_sdf, + gz::sim::EntityComponentManager &_ecm, gz::sim::EventManager &_eventManager) +{ + // Store the entity ID + this->entityId = _entity; + + // Get the entity name for logging and topic naming + auto nameComp = _ecm.Component(_entity); + if (nameComp) + { + this->entityName = nameComp->Data(); + } + else + { + this->entityName = "unknown_entity_" + std::to_string(_entity); + } + + // Configure topic name - can be customized via SDF parameters + if (_sdf->HasElement("topic")) + { + this->topicName = _sdf->Get("topic"); + } + else + { + this->topicName = "/gazebo/pose/" + this->entityName; + } + + // Configure publish frequency if specified in SDF + if (_sdf->HasElement("publish_frequency")) + { + this->publishFrequency = _sdf->Get("publish_frequency"); + } + + // Create publisher + this->posePub = this->node.Advertise(this->topicName); + + std::cout << "RealPosition initialized for entity: " << this->entityName << " (ID: " << _entity << ")" << std::endl; + std::cout << "Publishing pose on topic: " << this->topicName << std::endl; + std::cout << "Publish frequency: every " << this->publishFrequency << " updates" << std::endl; +} + +void RealPosition::PostUpdate(gz::sim::UpdateInfo const &_info, gz::sim::EntityComponentManager const &_ecm) +{ + // Only runs code if the simulation is active + if (!_info.paused) + { + // Increment update counter + this->updateCounter++; + + // Only publish at specified frequency to avoid spam + if (this->updateCounter % this->publishFrequency != 0) + { + return; + } + + // Get the pose component of the entity + auto poseComp = _ecm.Component(this->entityId); + + if (poseComp) + { + // Get the pose data + auto const &pose = poseComp->Data(); + + // Print to terminal + std::cout << "=== Entity Position Update ===" << std::endl; + std::cout << "Entity: " << this->entityName << " (ID: " << this->entityId << ")" << std::endl; + std::cout << "Position - X: " << pose.Pos().X() << ", Y: " << pose.Pos().Y() << ", Z: " << pose.Pos().Z() + << std::endl; + std::cout << "Orientation - Roll: " << pose.Rot().Roll() << ", Pitch: " << pose.Rot().Pitch() + << ", Yaw: " << pose.Rot().Yaw() << std::endl; + std::cout << "Quaternion - W: " << pose.Rot().W() << ", X: " << pose.Rot().X() << ", Y: " << pose.Rot().Y() + << ", Z: " << pose.Rot().Z() << std::endl; + std::cout << "Simulation time: " << _info.simTime.count() / 1e9 << " seconds" << std::endl; + std::cout << "===============================" << std::endl; + + // Create and populate the pose message for publishing + gz::msgs::Pose poseMsg; + + // Set position + poseMsg.mutable_position()->set_x(pose.Pos().X()); + poseMsg.mutable_position()->set_y(pose.Pos().Y()); + poseMsg.mutable_position()->set_z(pose.Pos().Z()); + + // Set orientation (quaternion) + poseMsg.mutable_orientation()->set_w(pose.Rot().W()); + poseMsg.mutable_orientation()->set_x(pose.Rot().X()); + poseMsg.mutable_orientation()->set_y(pose.Rot().Y()); + poseMsg.mutable_orientation()->set_z(pose.Rot().Z()); + + // Set timestamp + auto timestamp = poseMsg.mutable_header()->mutable_stamp(); + timestamp->set_sec(_info.simTime.count() / 1000000000); + timestamp->set_nsec(_info.simTime.count() % 1000000000); + + // Set frame ID + poseMsg.mutable_header()->add_data()->set_key("frame_id"); + poseMsg.mutable_header()->mutable_data(0)->add_value(this->entityName); + + // Publish the message + this->posePub.Publish(poseMsg); + } + else + { + std::cerr << "Warning: Could not get pose component for entity " << this->entityName + << " (ID: " << this->entityId << ")" << std::endl; + } + } +} From 386f5dd98bb5d8bae71a212d18118e1c320488a4 Mon Sep 17 00:00:00 2001 From: Withanage Perera Date: Sat, 2 Aug 2025 23:46:35 +0200 Subject: [PATCH 2/3] Created a separate plugin and created a ros2 msg to define the published data format. --- src/mil_common/mil_msgs/CMakeLists.txt | 3 +- src/mil_common/mil_msgs/msg/TruePosition.msg | 2 + .../urdf/sub9.urdf.xacro | 12 +-- .../urdf/xacro/position_sensor.xacro | 27 ------ .../urdf/xacro/true_position.xacro | 50 ++++++++++ .../subjugator_gazebo/CMakeLists.txt | 25 +++-- .../subjugator_gazebo/include/TruePosition.hh | 49 ++++++++++ .../subjugator_gazebo/src/DepthSensor.cc | 14 --- .../subjugator_gazebo/src/TruePosition.cc | 91 +++++++++++++++++++ 9 files changed, 214 insertions(+), 59 deletions(-) create mode 100644 src/mil_common/mil_msgs/msg/TruePosition.msg delete mode 100644 src/subjugator/simulation/subjugator_description/urdf/xacro/position_sensor.xacro create mode 100644 src/subjugator/simulation/subjugator_description/urdf/xacro/true_position.xacro create mode 100644 src/subjugator/simulation/subjugator_gazebo/include/TruePosition.hh create mode 100644 src/subjugator/simulation/subjugator_gazebo/src/TruePosition.cc diff --git a/src/mil_common/mil_msgs/CMakeLists.txt b/src/mil_common/mil_msgs/CMakeLists.txt index 196e45f2..448634f1 100644 --- a/src/mil_common/mil_msgs/CMakeLists.txt +++ b/src/mil_common/mil_msgs/CMakeLists.txt @@ -33,7 +33,8 @@ set(msg_files "msg/HoughTransform.msg" "msg/Lines.msg" "msg/ObjectDetection.msg" - "msg/ObjectDetections.msg") + "msg/ObjectDetections.msg" + "msg/TruePosition.msg") set(srv_files "srv/CameraToLidarTransform.srv" "srv/ObjectDBQuery.srv" "srv/SetGeometry.srv") diff --git a/src/mil_common/mil_msgs/msg/TruePosition.msg b/src/mil_common/mil_msgs/msg/TruePosition.msg new file mode 100644 index 00000000..6aa33d75 --- /dev/null +++ b/src/mil_common/mil_msgs/msg/TruePosition.msg @@ -0,0 +1,2 @@ +float64[3] position +float64[3] orientation diff --git a/src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro b/src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro index 28cda9a5..8e193eb8 100644 --- a/src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro +++ b/src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro @@ -5,7 +5,7 @@ - + @@ -98,14 +98,6 @@ - - - base_link - /my_robot/pose - 10 - - - @@ -113,7 +105,7 @@ - + - - - - - - - - - - - - - - - - - - - - - /my_robot/pose - 10 - - - - diff --git a/src/subjugator/simulation/subjugator_description/urdf/xacro/true_position.xacro b/src/subjugator/simulation/subjugator_description/urdf/xacro/true_position.xacro new file mode 100644 index 00000000..47341f15 --- /dev/null +++ b/src/subjugator/simulation/subjugator_description/urdf/xacro/true_position.xacro @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + true + true + + + + + ${frame_id} + 0 0 0 0 0 0 + /depth + ${update_rate} + ${noise_mean} + ${noise_stddev} + + + + diff --git a/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt b/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt index 0b734efb..61422fdd 100644 --- a/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt +++ b/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt @@ -49,10 +49,11 @@ add_library(DVLBridge SHARED src/DVLBridge.cc) # Plugin 3: DepthSensor add_library(DepthSensor SHARED src/DepthSensor.cc) -# Plugin 3: Hydrophone +# Plugin 4: Hydrophone add_library(Hydrophone SHARED src/Hydrophone.cc) -add_library(RealPosition SHARED src/RealPosition.cc) +# Plugin 5: True Position Plugin +add_library(TruePosition SHARED src/TruePosition.cc) # Ensures plugin can see include directory to access its header files target_include_directories(ExamplePlugin PRIVATE include) @@ -60,12 +61,13 @@ target_include_directories(ExamplePlugin PRIVATE include) target_include_directories(UnderwaterCamera PRIVATE include) target_include_directories(DVLBridge PRIVATE include) -target_include_directories(RealPosition PRIVATE include) target_include_directories(DepthSensor PRIVATE include) target_include_directories(Hydrophone PRIVATE include) +target_include_directories(TruePosition PRIVATE include) + # Add additional needed libraries target_link_libraries( ExamplePlugin gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} # Links gazebo @@ -79,7 +81,6 @@ target_link_libraries(UnderwaterCamera gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} # Link libraries for DVL Bridge plugin target_link_libraries(DVLBridge gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) -target_link_libraries(RealPosition gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) # Link libraries for Hydrophone plugin target_link_libraries(Hydrophone gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) @@ -87,20 +88,30 @@ target_link_libraries(Hydrophone gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) # Link libraries for DepthSensor plugin target_link_libraries(DepthSensor gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) +# Link libraries for TruePosition plugin +target_link_libraries(TruePosition gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) + # Specify dependencies using ament_target_dependencies +ament_target_dependencies( + ExamplePlugin + # + # Here is where you would place any dependencies for your plugin + # +) ament_target_dependencies(UnderwaterCamera rclcpp sensor_msgs) ament_target_dependencies(DVLBridge nav_msgs rclcpp geometry_msgs std_msgs) -ament_target_dependencies(RealPosition nav_msgs rclcpp geometry_msgs std_msgs) ament_target_dependencies(DepthSensor rclcpp mil_msgs sensor_msgs) ament_target_dependencies(Hydrophone rclcpp geometry_msgs mil_msgs) +ament_target_dependencies(TruePosition rclcpp mil_msgs) + # Install the plugins -install(TARGETS UnderwaterCamera ExamplePlugin DVLBridge RealPosition Hydrophone - DepthSensor LIBRARY DESTINATION lib/${PROJECT_NAME}) +install(TARGETS UnderwaterCamera ExamplePlugin DVLBridge Hydrophone DepthSensor + TruePosition LIBRARY DESTINATION lib/${PROJECT_NAME}) # Install headers install(DIRECTORY include/ DESTINATION include) diff --git a/src/subjugator/simulation/subjugator_gazebo/include/TruePosition.hh b/src/subjugator/simulation/subjugator_gazebo/include/TruePosition.hh new file mode 100644 index 00000000..b3f51b62 --- /dev/null +++ b/src/subjugator/simulation/subjugator_gazebo/include/TruePosition.hh @@ -0,0 +1,49 @@ +#ifndef TRUE_POSITION_HPP +#define TRUE_POSITION_HPP + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace true_position +{ + +class TruePosition : public gz::sim::System, public gz::sim::ISystemConfigure, public gz::sim::ISystemPostUpdate +{ + public: + TruePosition(); + + ~TruePosition() override = default; + + // System Configure + void Configure(gz::sim::Entity const &entity, std::shared_ptr const &sdf, + gz::sim::EntityComponentManager &ecm, gz::sim::EventManager &eventMgr) override; + + void PostUpdate(gz::sim::UpdateInfo const &_info, gz::sim::EntityComponentManager const &_ecm) override; + + private: + std::shared_ptr ros_node_; + gz::sim::Entity modelEntity_{ gz::sim::kNullEntity }; + + // ROS node + publisher + rclcpp::Publisher::SharedPtr positionPub_; + + // Time bookkeeping in seconds + double lastPubTime_{ 0.0 }; + double updatePeriod_{ 0.1 }; // default = 10 Hz + + std::string entityName; +}; +} // namespace true_position + +#endif diff --git a/src/subjugator/simulation/subjugator_gazebo/src/DepthSensor.cc b/src/subjugator/simulation/subjugator_gazebo/src/DepthSensor.cc index 965757e2..b6724249 100644 --- a/src/subjugator/simulation/subjugator_gazebo/src/DepthSensor.cc +++ b/src/subjugator/simulation/subjugator_gazebo/src/DepthSensor.cc @@ -105,20 +105,6 @@ void DepthSensor::PostUpdate(gz::sim::UpdateInfo const &_info, gz::sim::EntityCo auto const &pose = poseComp->Data(); - { - // Print to terminal - std::cout << "=== Entity Position Update ===" << std::endl; - std::cout << "Entity: " << this->entityName << " (ID: " << this->modelEntity_ << ")" << std::endl; - std::cout << "Position - X: " << pose.Pos().X() << ", Y: " << pose.Pos().Y() << ", Z: " << pose.Pos().Z() - << std::endl; - std::cout << "Orientation - Roll: " << pose.Rot().Roll() << ", Pitch: " << pose.Rot().Pitch() - << ", Yaw: " << pose.Rot().Yaw() << std::endl; - std::cout << "Quaternion - W: " << pose.Rot().W() << ", X: " << pose.Rot().X() << ", Y: " << pose.Rot().Y() - << ", Z: " << pose.Rot().Z() << std::endl; - std::cout << "Simulation time: " << _info.simTime.count() / 1e9 << " seconds" << std::endl; - std::cout << "===============================" << std::endl; - } - // To avoid the deprecated operator+, we manually combine: gz::math::Pose3d finalPose = poseComp->Data(); finalPose.Pos() += this->offset_.Pos(); diff --git a/src/subjugator/simulation/subjugator_gazebo/src/TruePosition.cc b/src/subjugator/simulation/subjugator_gazebo/src/TruePosition.cc new file mode 100644 index 00000000..8bf4c8d7 --- /dev/null +++ b/src/subjugator/simulation/subjugator_gazebo/src/TruePosition.cc @@ -0,0 +1,91 @@ +#include "TruePosition.hh" + +#include + +#include + +#include +#include // For GZ_ADD_PLUGIN +#include +#include +#include + +GZ_ADD_PLUGIN(true_position::TruePosition, gz::sim::System, true_position::TruePosition::ISystemConfigure, + true_position::TruePosition::ISystemPostUpdate) + +namespace true_position +{ + +TruePosition::TruePosition() +{ +} + +void TruePosition::Configure(gz::sim::Entity const &_entity, std::shared_ptr const &_sdf, + gz::sim::EntityComponentManager &_ecm, gz::sim::EventManager &_eventManager) +{ + this->modelEntity_ = _entity; + + auto nameComp = _ecm.Component(_entity); + if (nameComp) + { + this->entityName = nameComp->Data(); + } + else + { + this->entityName = "unknown_entity_" + std::to_string(_entity); + } + + if (_sdf->HasElement("update_rate")) + { + float rate = _sdf->Get("update_rate"); + if (rate > 0.0) + { + this->updatePeriod_ = 1.0 / rate; + } + } + + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + + this->ros_node_ = std::make_shared("true_position_node"); + this->positionPub_ = this->ros_node_->create_publisher("/true_position", 10); +} + +void TruePosition::PostUpdate(gz::sim::UpdateInfo const &_info, gz::sim::EntityComponentManager const &_ecm) +{ + if (!_info.paused) + { + rclcpp::spin_some(this->ros_node_); + + // Convert simTime (steady_clock::duration) to float (seconds) + float nowSec = std::chrono::duration_cast>(_info.simTime).count(); + + float dt = nowSec - this->lastPubTime_; + + // Haven't met our update threshold yet + if (dt < this->updatePeriod_) + return; + + this->lastPubTime_ = nowSec; + + // Get the pose component which has our position data + auto poseComp = _ecm.Component(this->modelEntity_); + if (!poseComp) + { + gzerr << "[TruePosition] Pose component not found for entity [" << this->modelEntity_ << "]\n"; + return; + } + + auto const &pose = poseComp->Data(); + + // Publish our position/orientation data + mil_msgs::msg::TruePosition msg; + msg.position = { pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z() }; + msg.orientation = { pose.Rot().Roll(), pose.Rot().Pitch(), pose.Rot().Yaw() }; + + this->positionPub_->publish(msg); + } +} +} // namespace true_position From b2de39dc0b3d830fe13ef35ac128655e7e7f7584 Mon Sep 17 00:00:00 2001 From: Withanage Perera Date: Sat, 2 Aug 2025 23:53:05 +0200 Subject: [PATCH 3/3] Minor cleanup. --- .../subjugator_gazebo/include/RealPosition.hh | 58 -------- .../subjugator_gazebo/include/TruePosition.hh | 3 - .../subjugator_gazebo/src/RealPosition.cc | 132 ------------------ .../subjugator_gazebo/src/TruePosition.cc | 3 +- 4 files changed, 2 insertions(+), 194 deletions(-) delete mode 100644 src/subjugator/simulation/subjugator_gazebo/include/RealPosition.hh delete mode 100644 src/subjugator/simulation/subjugator_gazebo/src/RealPosition.cc diff --git a/src/subjugator/simulation/subjugator_gazebo/include/RealPosition.hh b/src/subjugator/simulation/subjugator_gazebo/include/RealPosition.hh deleted file mode 100644 index e65bef45..00000000 --- a/src/subjugator/simulation/subjugator_gazebo/include/RealPosition.hh +++ /dev/null @@ -1,58 +0,0 @@ -#ifndef position_HH_ -#define position_HH_ - -#include - -#include - -#include -#include -#include -#include -#include - -namespace real_position -{ -class RealPosition : public gz::sim::System, public gz::sim::ISystemConfigure, public gz::sim::ISystemPostUpdate -{ - public: - RealPosition(); - - public: - ~RealPosition() override; - - /// \brief Configure the system - /// \param[in] _entity The entity this plugin is attached to. - /// \param[in] _sdf The SDF Element associated with this system plugin. - /// \param[in] _ecm The EntityComponentManager of the given simulation instance. - /// \param[in] _eventMgr The EventManager of the given simulation instance. - void Configure(gz::sim::Entity const &entity, std::shared_ptr const &sdf, - gz::sim::EntityComponentManager &ecm, gz::sim::EventManager &eventMgr) override; - - public: - void PostUpdate(gz::sim::UpdateInfo const &_info, gz::sim::EntityComponentManager const &_ecm) override; - - private: - /// \brief Transport node for publishing - gz::transport::Node node; - - /// \brief Publisher for pose messages - gz::transport::Node::Publisher posePub; - - /// \brief Entity ID that this plugin is attached to - gz::sim::Entity entityId; - - /// \brief Topic name for publishing pose - std::string topicName; - - /// \brief Entity name for logging - std::string entityName; - - /// \brief Update frequency control - int updateCounter = 0; - int publishFrequency = 10; // Publish every 10 updates (adjust as needed) -}; - -} // namespace real_position - -#endif diff --git a/src/subjugator/simulation/subjugator_gazebo/include/TruePosition.hh b/src/subjugator/simulation/subjugator_gazebo/include/TruePosition.hh index b3f51b62..f7c7b190 100644 --- a/src/subjugator/simulation/subjugator_gazebo/include/TruePosition.hh +++ b/src/subjugator/simulation/subjugator_gazebo/include/TruePosition.hh @@ -1,10 +1,7 @@ #ifndef TRUE_POSITION_HPP #define TRUE_POSITION_HPP -#include #include -#include -#include #include #include diff --git a/src/subjugator/simulation/subjugator_gazebo/src/RealPosition.cc b/src/subjugator/simulation/subjugator_gazebo/src/RealPosition.cc deleted file mode 100644 index 3e3ebb69..00000000 --- a/src/subjugator/simulation/subjugator_gazebo/src/RealPosition.cc +++ /dev/null @@ -1,132 +0,0 @@ -#include "RealPosition.hh" - -#include - -#include - -#include -#include -#include -#include - -// Add plugins to gazebo simulator alongside its dependencies -GZ_ADD_PLUGIN(real_position::RealPosition, gz::sim::System, real_position::RealPosition::ISystemPostUpdate) - -using namespace real_position; - -RealPosition::RealPosition() -{ -} - -RealPosition::~RealPosition() -{ -} - -void RealPosition::Configure(gz::sim::Entity const &_entity, std::shared_ptr const &_sdf, - gz::sim::EntityComponentManager &_ecm, gz::sim::EventManager &_eventManager) -{ - // Store the entity ID - this->entityId = _entity; - - // Get the entity name for logging and topic naming - auto nameComp = _ecm.Component(_entity); - if (nameComp) - { - this->entityName = nameComp->Data(); - } - else - { - this->entityName = "unknown_entity_" + std::to_string(_entity); - } - - // Configure topic name - can be customized via SDF parameters - if (_sdf->HasElement("topic")) - { - this->topicName = _sdf->Get("topic"); - } - else - { - this->topicName = "/gazebo/pose/" + this->entityName; - } - - // Configure publish frequency if specified in SDF - if (_sdf->HasElement("publish_frequency")) - { - this->publishFrequency = _sdf->Get("publish_frequency"); - } - - // Create publisher - this->posePub = this->node.Advertise(this->topicName); - - std::cout << "RealPosition initialized for entity: " << this->entityName << " (ID: " << _entity << ")" << std::endl; - std::cout << "Publishing pose on topic: " << this->topicName << std::endl; - std::cout << "Publish frequency: every " << this->publishFrequency << " updates" << std::endl; -} - -void RealPosition::PostUpdate(gz::sim::UpdateInfo const &_info, gz::sim::EntityComponentManager const &_ecm) -{ - // Only runs code if the simulation is active - if (!_info.paused) - { - // Increment update counter - this->updateCounter++; - - // Only publish at specified frequency to avoid spam - if (this->updateCounter % this->publishFrequency != 0) - { - return; - } - - // Get the pose component of the entity - auto poseComp = _ecm.Component(this->entityId); - - if (poseComp) - { - // Get the pose data - auto const &pose = poseComp->Data(); - - // Print to terminal - std::cout << "=== Entity Position Update ===" << std::endl; - std::cout << "Entity: " << this->entityName << " (ID: " << this->entityId << ")" << std::endl; - std::cout << "Position - X: " << pose.Pos().X() << ", Y: " << pose.Pos().Y() << ", Z: " << pose.Pos().Z() - << std::endl; - std::cout << "Orientation - Roll: " << pose.Rot().Roll() << ", Pitch: " << pose.Rot().Pitch() - << ", Yaw: " << pose.Rot().Yaw() << std::endl; - std::cout << "Quaternion - W: " << pose.Rot().W() << ", X: " << pose.Rot().X() << ", Y: " << pose.Rot().Y() - << ", Z: " << pose.Rot().Z() << std::endl; - std::cout << "Simulation time: " << _info.simTime.count() / 1e9 << " seconds" << std::endl; - std::cout << "===============================" << std::endl; - - // Create and populate the pose message for publishing - gz::msgs::Pose poseMsg; - - // Set position - poseMsg.mutable_position()->set_x(pose.Pos().X()); - poseMsg.mutable_position()->set_y(pose.Pos().Y()); - poseMsg.mutable_position()->set_z(pose.Pos().Z()); - - // Set orientation (quaternion) - poseMsg.mutable_orientation()->set_w(pose.Rot().W()); - poseMsg.mutable_orientation()->set_x(pose.Rot().X()); - poseMsg.mutable_orientation()->set_y(pose.Rot().Y()); - poseMsg.mutable_orientation()->set_z(pose.Rot().Z()); - - // Set timestamp - auto timestamp = poseMsg.mutable_header()->mutable_stamp(); - timestamp->set_sec(_info.simTime.count() / 1000000000); - timestamp->set_nsec(_info.simTime.count() % 1000000000); - - // Set frame ID - poseMsg.mutable_header()->add_data()->set_key("frame_id"); - poseMsg.mutable_header()->mutable_data(0)->add_value(this->entityName); - - // Publish the message - this->posePub.Publish(poseMsg); - } - else - { - std::cerr << "Warning: Could not get pose component for entity " << this->entityName - << " (ID: " << this->entityId << ")" << std::endl; - } - } -} diff --git a/src/subjugator/simulation/subjugator_gazebo/src/TruePosition.cc b/src/subjugator/simulation/subjugator_gazebo/src/TruePosition.cc index 8bf4c8d7..a3b4323a 100644 --- a/src/subjugator/simulation/subjugator_gazebo/src/TruePosition.cc +++ b/src/subjugator/simulation/subjugator_gazebo/src/TruePosition.cc @@ -35,6 +35,7 @@ void TruePosition::Configure(gz::sim::Entity const &_entity, std::shared_ptrentityName = "unknown_entity_" + std::to_string(_entity); } + // Grab our update rate from the xacro file if (_sdf->HasElement("update_rate")) { float rate = _sdf->Get("update_rate"); @@ -80,7 +81,7 @@ void TruePosition::PostUpdate(gz::sim::UpdateInfo const &_info, gz::sim::EntityC auto const &pose = poseComp->Data(); - // Publish our position/orientation data + // Create a msg object to contain our position/orientation data, then publish it mil_msgs::msg::TruePosition msg; msg.position = { pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z() }; msg.orientation = { pose.Rot().Roll(), pose.Rot().Pitch(), pose.Rot().Yaw() };