From adabdf42d05044651badab00d2b34ce1662d776a Mon Sep 17 00:00:00 2001 From: Balduin Date: Fri, 28 Feb 2025 15:31:07 +0100 Subject: [PATCH] moving platform: enable movement We now have setPlatformVelocity, which commands a velocity command to the platform. The platform handles it with the gz::sim::systems::VelocityControl plugin. We could also set the velocity once and forget about it (has no effect if doing it directly after creating it though, need to wait a bit, 2 seconds worked). But by setting it constantly we can easily change to more realistic boat-like movement. --- src/modules/simulation/gz_bridge/GZBridge.cpp | 69 ++++++++++++------- src/modules/simulation/gz_bridge/GZBridge.hpp | 5 ++ 2 files changed, 50 insertions(+), 24 deletions(-) diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index abe9d6bc20..3e54674e41 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -69,8 +69,8 @@ GZBridge::~GZBridge() int GZBridge::init() { // TODO document this somehow - char *moving_platform = std::getenv("GZ_MOVING_PLATFORM"); - bool platform = ((moving_platform != nullptr) && (std::strcmp(moving_platform, "1") == 0)); + const char *moving_platform = std::getenv("GZ_MOVING_PLATFORM"); + _has_platform = ((moving_platform != nullptr) && (std::strcmp(moving_platform, "1") == 0)); if (!_model_sim.empty()) { @@ -105,7 +105,7 @@ int GZBridge::init() position->set_y(model_pose_v[1]); position->set_z(model_pose_v[2]); - if (platform) { + if (_has_platform) { // why does this only look good if way bigger than actual platform height? float platform_height = 0.5; position->set_z(fmaxf(platform_height, model_pose_v[2])); @@ -182,12 +182,16 @@ int GZBridge::init() } } - if (platform) { + if (_has_platform) { if (!createMovingPlatform()) { return PX4_ERROR; } + // Initialise publisher for setPlatformVelocity + // TODO less hardcoding of these topic strings + std::string cmd_vel_topic = "/model/flat_platform/link/platform_link/cmd_vel"; + _platform_twist_pub = _node.Advertise(cmd_vel_topic); } // clock @@ -265,7 +269,7 @@ int GZBridge::init() return PX4_ERROR; } - if (platform) { + if (_has_platform) { std::string platform_pose_topic = "/world/" + _world_name + "/model/flat_platform/link/platform_link/sensor/navsat_sensor/navsat"; @@ -299,6 +303,24 @@ int GZBridge::init() return OK; } + +void GZBridge::setPlatformVelocity(float vx, float vy, float vz) +{ + + gz::msgs::Twist twist; + gz::msgs::Set(twist.mutable_linear(), + gz::math::Vector3d(vx, vy, vz)); + + if (_platform_twist_pub) { + _platform_twist_pub.Publish(twist); + + } else { + PX4_ERR("wanted to set platform velocity but publisher not initialized"); + } + + // TODO unified error handling? PX4_ERR at the lowest level or return success bool? +} + bool GZBridge::createMovingPlatform() { @@ -308,8 +330,7 @@ bool GZBridge::createMovingPlatform() // https://gazebosim.org/api/gazebo/6/entity_creation.html // This just as a first hacky quick solution. - // Surely there is a good place for such a file without putting it in the gz repo? - + // Ultimately we should probably put this in the PX4-gazebo-models repo std::string platform_sdf = R""""( @@ -354,29 +375,35 @@ bool GZBridge::createMovingPlatform() 1 30 + + + + + platform_link + + )""""; + req.set_sdf(platform_sdf); // base dir: Tools/simulation/gz/models // req.set_sdf_filename("platform.sdf"); - // req.set_name("platform"); req.set_allow_renaming(false); - // position: origin (might want to adapt to model position) + // Set position & orientation. Origin hardcoded -- adapt to model pose? gz::msgs::Pose *p = req.mutable_pose(); gz::msgs::Vector3d *position = p->mutable_position(); position->set_x(0.); position->set_y(0.); position->set_z(0.05); - // gz::math::Quaterniond q(model_pose_v[3], model_pose_v[4], model_pose_v[5]); - - // orientation: unit quaternion gz::msgs::Quaternion *orientation = p->mutable_orientation(); orientation->set_x(0.); orientation->set_y(0.); @@ -413,6 +440,7 @@ bool GZBridge::setPlatformPose(gz::msgs::Pose &pose) PX4_WARN("set_pose service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); return false; } + return true; } @@ -561,8 +589,8 @@ void GZBridge::platformNavsatCallback(const gz::msgs::NavSat &nav_sat) pthread_mutex_lock(&_node_mutex); // print d n'ebugging pas - PX4_INFO("Moving Platform NavSat pos: lat=%.2f, lon=%.2f, alt=%.2f", - nav_sat.latitude_deg(), nav_sat.longitude_deg(), nav_sat.altitude()); + // PX4_INFO("Moving Platform NavSat pos: lat=%.2f, lon=%.2f, alt=%.2f", + // nav_sat.latitude_deg(), nav_sat.longitude_deg(), nav_sat.altitude()); // TODO publish the corresponding uOrb msg. then in reality we just have // to convert the mavlink msg from the ground station to exactly the @@ -1195,16 +1223,9 @@ void GZBridge::Run() _gimbal.updateParams(); } - // float time_s = hrt_absolute_time() / 1e6; // abstime in microseconds - // float vel = 1.0f; - - // gz::msgs::Pose pose; - // gz::msgs::Vector3d *position = pose.mutable_position(); - // position->set_x(time_s * vel); - // position->set_y(0.); - // position->set_z(0.05); - - // setPlatformPose(pose); + if (_has_platform) { + setPlatformVelocity(1.0, 0.0, 0.0); + } ScheduleDelayed(10_ms); diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 0b7d00c69e..2a7d62307f 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -120,6 +120,8 @@ private: bool setPlatformPose(gz::msgs::Pose &pose); + void setPlatformVelocity(float vx, float vy, float vz); + bool createMovingPlatform(); /** @@ -192,6 +194,8 @@ private: GZMixingInterfaceWheel _mixing_interface_wheel{_node, _node_mutex}; GZGimbal _gimbal{_node, _node_mutex}; + bool _has_platform{false}; + px4::atomic _world_time_us{0}; pthread_mutex_t _node_mutex; @@ -212,4 +216,5 @@ private: float _temperature{288.15}; // 15 degrees gz::transport::Node _node; + gz::transport::Node::Publisher _platform_twist_pub; };