Physics Engine Position Plugin

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")

src/mil_common/mil_msgs/msg/TruePosition.msg

@@ -0,0 +1,2 @@
+float64[3] position
+float64[3] orientation

src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro

@@ -5,6 +5,7 @@
   <xacro:include filename="$(find subjugator_description)/urdf/xacro/camera.xacro"/>
   <xacro:include filename="$(find subjugator_description)/urdf/xacro/imu_magnetometer.xacro"/>
   <xacro:include filename="$(find subjugator_description)/urdf/xacro/dvl.xacro"/>
+  <xacro:include filename="$(find subjugator_description)/urdf/xacro/true_position.xacro"/>
   <xacro:include filename="$(find subjugator_description)/urdf/xacro/hydrophone.xacro"/>
   <xacro:include filename="$(find subjugator_description)/urdf/xacro/collision.xacro"/>
   <xacro:include filename="$(find subjugator_description)/urdf/xacro/thruster.xacro"/>
@@ -104,6 +105,8 @@
   <!-- DVL Sensor -->
   <xacro:mil_dvl_sensor name="dvl_sensor" parent="base_link" xyz="0.3937 0 -0.2413" rpy="3.14159 0.0 0.0"/>
 
+  <xacro:mil_true_position name="true_position" parent="base_link" xyz="0 0 0" rpy="0.0 0.0 0.0"/>
+
   <!-- IMU, Magnetometer -->
   <xacro:mil_imu_magnetometer
     name="imu"

src/subjugator/simulation/subjugator_description/urdf/xacro/true_position.xacro

@@ -0,0 +1,50 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro
+    name="mil_true_position"
+    params="
+      name
+      namespace='/true_position'
+      parent='base_link'
+      xyz='0 0 0'
+      rpy='0 0 0'
+      update_rate='10'
+      frame_id='true_position_frame'
+      noise_mean='0.0'
+      noise_stddev='0.0'
+    "
+  >
+    <link name="${name}_link">
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="0.1"/>
+        <inertia ixx="0.001" iyy="0.001" izz="0.001" ixy="0" ixz="0" iyz="0"/>
+      </inertial>
+    </link>
+
+    <!-- Joint -->
+    <joint name="${name}_joint" type="fixed">
+      <parent link="${parent}"/>
+      <child link="${name}_link"/>
+      <origin xyz="${xyz}" rpy="${rpy}"/>
+    </joint>
+
+    <!-- This ensures that the fixed link is not simplified away -->
+    <gazebo reference="${name}_joint">
+      <disableFixedJointLumping>true</disableFixedJointLumping>
+      <preserveFixedJoint>true</preserveFixedJoint>
+    </gazebo>
+
+    <gazebo>
+      <plugin name="true_position::TruePosition" filename="TruePosition">
+        <frame_id>${frame_id}</frame_id>
+        <offset>0 0 0 0 0 0</offset>
+        <topic>/depth</topic>
+        <update_rate>${update_rate}</update_rate>
+        <noise_mean>${noise_mean}</noise_mean>
+        <noise_stddev>${noise_stddev}</noise_stddev>
+      </plugin>
+    </gazebo>
+  </xacro:macro>
+</robot>

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)

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<double> noiseDist_{ 0.0, 0.0 };
+
+    std::string entityName;
 };
 }  // namespace depth_sensor
 

src/subjugator/simulation/subjugator_gazebo/include/TruePosition.hh

@@ -0,0 +1,46 @@
+#ifndef TRUE_POSITION_HPP
+#define TRUE_POSITION_HPP
+
+#include <memory>
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <gz/math/Pose3.hh>
+#include <gz/sim/System.hh>
+#include <gz/sim/components/Pose.hh>
+#include <mil_msgs/msg/true_position.hpp>
+#include <sdf/sdf.hh>
+
+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<sdf::Element const> 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<rclcpp::Node> ros_node_;
+    gz::sim::Entity modelEntity_{ gz::sim::kNullEntity };
+
+    // ROS node + publisher
+    rclcpp::Publisher<mil_msgs::msg::TruePosition>::SharedPtr positionPub_;
+
+    // Time bookkeeping in seconds
+    double lastPubTime_{ 0.0 };
+    double updatePeriod_{ 0.1 };  // default = 10 Hz
+
+    std::string entityName;
+};
+}  // namespace true_position
+
+#endif

src/subjugator/simulation/subjugator_gazebo/src/DepthSensor.cc

@@ -1,12 +1,18 @@
 #include "DepthSensor.hh"
 
+#include <gz/msgs/pose.pb.h>
+
 #include <iostream>
 #include <random>
 
 #include <rclcpp/rclcpp.hpp>
 
 #include <gz/common/Console.hh>
+#include <gz/plugin/Register.hh>
 #include <gz/plugin/Register.hh>  // For GZ_ADD_PLUGIN
+#include <gz/sim/Util.hh>
+#include <gz/sim/components/Name.hh>
+#include <gz/sim/components/Pose.hh>
 
 // 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_ptr<sdf:
     // std::cout << "[DepthSensor] Attached to entity ID:" << _entity << std::endl;
     this->modelEntity_ = _entity;
 
+    auto nameComp = _ecm.Component<gz::sim::components::Name>(_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<std::string>("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();

src/subjugator/simulation/subjugator_gazebo/src/TruePosition.cc

@@ -0,0 +1,92 @@
+#include "TruePosition.hh"
+
+#include <gz/msgs/pose.pb.h>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <gz/common/Console.hh>
+#include <gz/plugin/Register.hh>  // For GZ_ADD_PLUGIN
+#include <gz/sim/Util.hh>
+#include <gz/sim/components/Name.hh>
+#include <gz/sim/components/Pose.hh>
+
+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<sdf::Element const> const &_sdf,
+                             gz::sim::EntityComponentManager &_ecm, gz::sim::EventManager &_eventManager)
+{
+    this->modelEntity_ = _entity;
+
+    auto nameComp = _ecm.Component<gz::sim::components::Name>(_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<float>("update_rate");
+        if (rate > 0.0)
+        {
+            this->updatePeriod_ = 1.0 / rate;
+        }
+    }
+
+    if (!rclcpp::ok())
+    {
+        rclcpp::init(0, nullptr);
+    }
+
+    this->ros_node_ = std::make_shared<rclcpp::Node>("true_position_node");
+    this->positionPub_ = this->ros_node_->create_publisher<mil_msgs::msg::TruePosition>("/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<std::chrono::duration<float>>(_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<gz::sim::components::Pose>(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