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 fd37561b..8e193eb8 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 @@ + @@ -104,6 +105,8 @@ + + + + + + + + + + + + + + + + + + + + + + + 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 02bd7cc6..61422fdd 100644 --- a/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt +++ b/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt @@ -49,9 +49,12 @@ 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) +# 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) @@ -63,6 +66,8 @@ 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 @@ -83,6 +88,9 @@ 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 @@ -99,9 +107,11 @@ 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 Hydrophone DepthSensor - LIBRARY DESTINATION lib/${PROJECT_NAME}) + TruePosition 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/TruePosition.hh b/src/subjugator/simulation/subjugator_gazebo/include/TruePosition.hh new file mode 100644 index 00000000..f7c7b190 --- /dev/null +++ b/src/subjugator/simulation/subjugator_gazebo/include/TruePosition.hh @@ -0,0 +1,46 @@ +#ifndef TRUE_POSITION_HPP +#define TRUE_POSITION_HPP + +#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 d7815b5b..b6724249 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,8 @@ void DepthSensor::PostUpdate(gz::sim::UpdateInfo const &_info, gz::sim::EntityCo return; } + auto const &pose = poseComp->Data(); + // 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..a3b4323a --- /dev/null +++ b/src/subjugator/simulation/subjugator_gazebo/src/TruePosition.cc @@ -0,0 +1,92 @@ +#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); + } + + // Grab our update rate from the xacro file + 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(); + + // 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() }; + + this->positionPub_->publish(msg); + } +} +} // namespace true_position