From 056e41af8caa075e1629bdc33124a79e8aedc777 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Fri, 1 Dec 2023 11:30:09 +0100 Subject: [PATCH] Differential Rover: Ported R1 to GZ and introduced new GZ wheel interface --- .../init.d-posix/airframes/4009_gz_r1_rover | 48 +++++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + boards/px4/sitl/default.px4board | 1 + .../simulation/gz_bridge/CMakeLists.txt | 2 + src/modules/simulation/gz_bridge/GZBridge.cpp | 10 ++ src/modules/simulation/gz_bridge/GZBridge.hpp | 4 + .../gz_bridge/GZMixingInterfaceWheel.cpp | 123 ++++++++++++++++++ .../gz_bridge/GZMixingInterfaceWheel.hpp | 88 +++++++++++++ src/modules/simulation/gz_bridge/module.yaml | 9 ++ 9 files changed, 286 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover create mode 100644 src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp create mode 100644 src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover new file mode 100644 index 0000000000..f6e51cea57 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -0,0 +1,48 @@ +#!/bin/sh +# +# @name Aion Robotics R1 Rover +# @type Rover +# @class Rover + +. ${R}etc/init.d/rc.rover_differential_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover} + +param set-default SIM_GZ_EN 1 + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 0 +param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 1 + +param set-default COM_PREARM_MODE 2 + +param set-default NAV_ACC_RAD 0.5 +param set-default NAV_LOITER_RAD 2 + +param set-default GND_MAX_ANG 0.6 +param set-default GND_WHEEL_BASE 2.0 + +param set-default CA_R_REV 3 + +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_MIN1 0 +param set-default SIM_GZ_EC_MAX1 200 +param set-default SIM_GZ_EC_DIS1 100 + +param set-default SIM_GZ_EC_FUNC2 102 +param set-default SIM_GZ_EC_MIN2 0 +param set-default SIM_GZ_EC_MAX2 200 +param set-default SIM_GZ_EC_DIS2 100 + +param set-default SIM_GZ_MT_FUNC1 101 +param set-default SIM_GZ_MT_MIN1 0 +param set-default SIM_GZ_MT_MAX1 200 +param set-default SIM_GZ_MT_DIS1 100 + +param set-default SIM_GZ_MT_FUNC2 102 +param set-default SIM_GZ_MT_MIN2 0 +param set-default SIM_GZ_MT_MAX2 200 +param set-default SIM_GZ_MT_DIS2 100 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 247545c187..ed460d9929 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -80,6 +80,7 @@ px4_add_romfs_files( 4005_gz_x500_vision 4006_gz_px4vision 4008_gz_advanced_plane + 4009_gz_r1_rover 4010_gz_x500_mono_cam 6011_gazebo-classic_typhoon_h480 diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 51f3e092f1..3a40e02a2a 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -12,6 +12,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y +CONFIG_MODULE_DIFFERENTIAL_DRIVE_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_AUX_GLOBAL_POSITION=y CONFIG_MODULES_EVENTS=y diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index d8f0208e1c..6a39c06fab 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -64,6 +64,8 @@ if(gz-transport_FOUND) GZMixingInterfaceESC.hpp GZMixingInterfaceServo.cpp GZMixingInterfaceServo.hpp + GZMixingInterfaceWheel.cpp + GZMixingInterfaceWheel.hpp DEPENDS mixer_module px4_work_queue diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index ae4ba731e2..b9e5c81a60 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -226,6 +226,11 @@ int GZBridge::init() return PX4_ERROR; } + if (!_mixing_interface_motor.init(_model_name)) { + PX4_ERR("failed to init motor output"); + return PX4_ERROR; + } + ScheduleNow(); return OK; } @@ -724,6 +729,7 @@ void GZBridge::Run() _mixing_interface_esc.stop(); _mixing_interface_servo.stop(); + _mixing_interface_motor.stop(); exit_and_cleanup(); return; @@ -739,6 +745,7 @@ void GZBridge::Run() _mixing_interface_esc.updateParams(); _mixing_interface_servo.updateParams(); + _mixing_interface_motor.updateParams(); } ScheduleDelayed(10_ms); @@ -754,6 +761,9 @@ int GZBridge::print_status() PX4_INFO_RAW("Servo outputs:\n"); _mixing_interface_servo.mixingOutput().printStatus(); + PX4_INFO_RAW("Motor outputs:\n"); + _mixing_interface_motor.mixingOutput().printStatus(); + return 0; } diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index b7d40eef36..8c7a128d3c 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -35,6 +35,7 @@ #include "GZMixingInterfaceESC.hpp" #include "GZMixingInterfaceServo.hpp" +#include "GZMixingInterfaceWheel.hpp" #include #include @@ -56,6 +57,7 @@ #include #include #include +#include #include #include @@ -63,6 +65,7 @@ #include #include +#include #include using namespace time_literals; @@ -129,6 +132,7 @@ private: GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex}; GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex}; + GZMixingInterfaceWheel _mixing_interface_motor{_node, _node_mutex}; px4::atomic _world_time_us{0}; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp new file mode 100644 index 0000000000..e072c4c921 --- /dev/null +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "GZMixingInterfaceWheel.hpp" + +bool GZMixingInterfaceWheel::init(const std::string &model_name) +{ + + std::string wheel_speed_topic = "/model/" + model_name + "/command/motor_speed"; + + if (!_node.Subscribe(wheel_speed_topic, &GZMixingInterfaceWheel::wheelSpeedCallback, this)) { + PX4_ERR("failed to subscribe to %s", wheel_speed_topic.c_str()); + return false; + } + + std::string wheel_topic = "/model/" + model_name + "/command/motor_speed"; + _actuators_pub = _node.Advertise(wheel_topic); + + if (!_actuators_pub.Valid()) { + PX4_ERR("failed to advertise %s", wheel_topic.c_str()); + return false; + } + + _wheel_encoders_pub.advertise(); + + ScheduleNow(); + + return true; +} + +bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, + unsigned num_control_groups_updated) +{ + unsigned active_output_count = 0; + + for (unsigned i = 0; i < num_outputs; i++) { + if (_mixing_output.isFunctionSet(i)) { + active_output_count++; + + } else { + break; + } + } + + if (active_output_count > 0) { + gz::msgs::Actuators wheel_velocity_message; + wheel_velocity_message.mutable_velocity()->Resize(active_output_count, 0); + + for (unsigned i = 0; i < active_output_count; i++) { + float output_scaler = 100.0f; + float scaled_output = (float)outputs[i] - output_scaler; + wheel_velocity_message.set_velocity(i, scaled_output); + } + + + if (_actuators_pub.Valid()) { + return _actuators_pub.Publish(wheel_velocity_message); + } + } + + return false; +} + +void GZMixingInterfaceWheel::Run() +{ + pthread_mutex_lock(&_node_mutex); + _mixing_output.update(); + _mixing_output.updateSubscriptions(false); + pthread_mutex_unlock(&_node_mutex); +} + +void GZMixingInterfaceWheel::wheelSpeedCallback(const gz::msgs::Actuators &actuators) +{ + if (hrt_absolute_time() == 0) { + return; + } + + pthread_mutex_lock(&_node_mutex); + + wheel_encoders_s wheel_encoders{}; + + for (int i = 0; i < actuators.velocity_size(); i++) { + // Convert from RPM to rad/s + wheel_encoders.wheel_speed[i] = (float)actuators.velocity(i) * (2.0f * M_PI_F / 60.0f); + } + + if (actuators.velocity_size() > 0) { + wheel_encoders.timestamp = hrt_absolute_time(); + _wheel_encoders_pub.publish(wheel_encoders); + } + + pthread_mutex_unlock(&_node_mutex); +} diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp new file mode 100644 index 0000000000..bf36644302 --- /dev/null +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include + + +#include +#include + + +// GZBridge mixing class for Wheels. +// It is separate from GZBridge to have separate WorkItems and therefore allowing independent scheduling +// All work items are expected to run on the same work queue. +class GZMixingInterfaceWheel : public OutputModuleInterface +{ +public: + static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; + + GZMixingInterfaceWheel(gz::transport::Node &node, pthread_mutex_t &node_mutex) : + OutputModuleInterface(MODULE_NAME "-actuators-wheel", px4::wq_configurations::rate_ctrl), + _node(node), + _node_mutex(node_mutex) + {} + + bool updateOutputs(bool stop_wheels, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) override; + + MixingOutput &mixingOutput() { return _mixing_output; } + + bool init(const std::string &model_name); + + void stop() + { + _mixing_output.unregister(); + ScheduleClear(); + } + +private: + friend class GZBridge; + + void Run() override; + + void wheelSpeedCallback(const gz::msgs::Actuators &actuators); + + gz::transport::Node &_node; + pthread_mutex_t &_node_mutex; + + MixingOutput _mixing_output{"SIM_GZ_MT", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; + + gz::transport::Node::Publisher _actuators_pub; + + uORB::Publication _wheel_encoders_pub{ORB_ID(wheel_encoders)}; +}; diff --git a/src/modules/simulation/gz_bridge/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index d4258bd0e9..81b382945c 100644 --- a/src/modules/simulation/gz_bridge/module.yaml +++ b/src/modules/simulation/gz_bridge/module.yaml @@ -25,3 +25,12 @@ actuator_output: max: { min: 0, max: 1000, default: 1000 } failsafe: { min: 0, max: 1000 } num_channels: 8 + - param_prefix: SIM_GZ_MT + group_label: 'Motors' + channel_label: 'Motors' + standard_params: + disarmed: { min: 0, max: 200, default: 100 } + min: { min: 0, max: 200, default: 0 } + max: { min: 0, max: 200, default: 200 } + failsafe: { min: 0, max: 200 } + num_channels: 2