Differential Rover: Ported R1 to GZ and introduced new GZ wheel interface

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

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

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

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

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;
 }
 

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -35,6 +35,7 @@
 
 #include "GZMixingInterfaceESC.hpp"
 #include "GZMixingInterfaceServo.hpp"
+#include "GZMixingInterfaceWheel.hpp"
 
 #include <px4_platform_common/atomic.h>
 #include <px4_platform_common/defines.h>
@@ -56,6 +57,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/vehicle_odometry.h>
+#include <uORB/topics/wheel_encoders.h>
 
 #include <gz/math.hh>
 #include <gz/msgs.hh>
@@ -63,6 +65,7 @@
 
 #include <gz/msgs/imu.pb.h>
 #include <gz/msgs/fluid_pressure.pb.h>
+#include <gz/msgs/model.pb.h>
 #include <gz/msgs/odometry_with_covariance.pb.h>
 
 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<uint64_t> _world_time_us{0};
 

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<gz::msgs::Actuators>(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);
+}

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 <lib/mixer_module/mixer_module.hpp>
+
+#include <gz/msgs.hh>
+#include <gz/transport.hh>
+
+
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/wheel_encoders.h>
+
+
+// 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_s> _wheel_encoders_pub{ORB_ID(wheel_encoders)};
+};

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