publish platform twist on /cmd_vel

for the velocity control plugin.
PX4 · Mar 7, 2025 · 7ad10bc · 7ad10bc
1 parent a9b6ffb
commit 7ad10bc
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 15 deletions.
diff --git a/src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp b/src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
@@ -33,18 +33,6 @@
 
 #include "MovingPlatformController.hpp"
 
-#include <gz/plugin/Register.hh>
-
-#include "gz/sim/components/Pose.hh"
-#include <gz/sim/components/Model.hh>
-#include <gz/sim/EntityComponentManager.hh>
-#include <gz/sim/Util.hh>
-
-#include <gz/math.hh>
-#include <gz/math/Rand.hh>
-#include <gz/math/Pose3.hh>
-
-
 using namespace custom;
 
 // Register the plugin
@@ -62,6 +50,10 @@ void MovingPlatformController::Configure(const gz::sim::Entity &entity,
 		gz::sim::EventManager &eventMgr)
 {
 	_entity = entity;
+
+	// refrain from hardcoding world name / path?
+	std::string cmd_vel_topic = "/model/flat_platform/link/platform_link/cmd_vel";
+	_platform_twist_pub = _node.Advertise<gz::msgs::Twist>(cmd_vel_topic);
 }
 
 void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
@@ -149,5 +141,9 @@ void MovingPlatformController::updatePose(const gz::sim::EntityComponentManager
 
 void MovingPlatformController::sendVelocityCommands()
 {
+	gz::msgs::Twist twist_msg;
+	gz::msgs::Set(twist_msg.mutable_linear(), _platform_v);
+	gz::msgs::Set(twist_msg.mutable_angular(), _platform_w);
 
+	_platform_twist_pub.Publish(twist_msg);
 }
diff --git a/src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.hpp b/src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.hpp
@@ -34,7 +34,19 @@
 #pragma once
 
 #include <gz/sim/System.hh>
+#include "gz/sim/components/Pose.hh"
+#include <gz/sim/components/Model.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/Util.hh>
+
 #include <gz/transport/Node.hh>
+#include <gz/plugin/Register.hh>
+#include <gz/msgs/Utility.hh>
+#include <gz/msgs/twist.pb.h>
+
+#include <gz/math.hh>
+#include <gz/math/Rand.hh>
+#include <gz/math/Pose3.hh>
 
 namespace custom
 {
@@ -61,22 +73,25 @@ class MovingPlatformController:
 
 private:
 
-	gz::transport::Node _node;
-
 	void updatePose(const gz::sim::EntityComponentManager &ecm);
 	void updateVelocityCommands(const gz::math::Vector3d &mean_velocity);
 	void sendVelocityCommands();
 
+	gz::transport::Node _node;
 	gz::sim::Entity &_entity;
 
-	// low-pass filtered white noise for driving boat motion
+	// Low-pass filtered white noise for driving boat motion.
 	gz::math::Vector3d _noise_v_lowpass{0., 0., 0.};
 	gz::math::Vector3d _noise_w_lowpass{0., 0., 0.};
 
+	// Platform linear & angular velocity.
 	gz::math::Vector3d _platform_v{0., 0., 0.};
 	gz::math::Vector3d _platform_w{0., 0., 0.};
 
+	// Platform position & orientation for feedback.
 	gz::math::Vector3d _platform_position{0., 0., 0.};
 	gz::math::Quaterniond _platform_orientation{1., 0., 0., 0.};
+
+	gz::transport::Node::Publisher _platform_twist_pub;
 };
 } // end namespace custom