mecanum: add control folder

src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2024 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.
+#
+############################################################################
+
+px4_add_library(RoverMecanumControl
+	RoverMecanumControl.cpp
+)
+
+target_link_libraries(RoverMecanumControl PUBLIC pid)
+target_include_directories(RoverMecanumControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp

@@ -0,0 +1,142 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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
+
+// PX4 includes
+#include <px4_platform_common/module_params.h>
+#include <lib/pure_pursuit/PurePursuit.hpp>
+
+// uORB includes
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/rover_mecanum_guidance_status.h>
+
+// Standard libraries
+#include <lib/pid/pid.h>
+#include <matrix/matrix/math.hpp>
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+#include <math.h>
+
+using namespace matrix;
+
+/**
+ * @brief Class for mecanum rover guidance.
+ */
+class RoverMecanumGuidance : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for RoverMecanumGuidance.
+	 * @param parent The parent ModuleParams object.
+	 */
+	RoverMecanumGuidance(ModuleParams *parent);
+	~RoverMecanumGuidance() = default;
+
+	struct mecanum_setpoint {
+		float forward_throttle{0.f};
+		float lateral_throttle{0.f};
+		float yaw_rate{0.f};
+		float heading{0.f};
+		bool closed_loop_yaw_rate{false};
+		bool closed_loop_speed{false};
+		bool closed_loop_heading{false};
+	};
+
+	/**
+	 * @brief Compute guidance for the vehicle.
+	 * @param yaw The yaw orientation of the vehicle in radians.
+	 * @param forward_speed The velocity of the vehicle in m/s.
+	 * @param dt The time step in seconds.
+	 * @param nav_state Navigation state of the rover.
+	 */
+	RoverMecanumGuidance::mecanum_setpoint computeGuidance(float yaw, float forward_speed, float lateral_speed,
+			int nav_state);
+
+	/**
+	 * @brief Update global/ned waypoint coordinates
+	 */
+	void updateWaypoints();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	// uORB subscriptions
+	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
+	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
+	uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
+	uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
+
+	// uORB publications
+	uORB::Publication<rover_mecanum_guidance_status_s> _rover_mecanum_guidance_status_pub{ORB_ID(rover_mecanum_guidance_status)};
+	rover_mecanum_guidance_status_s _rover_mecanum_guidance_status{};
+
+	// Variables
+	MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates.
+	PurePursuit _pure_pursuit{this}; // Pure pursuit library
+	hrt_abstime _timestamp{0};
+	float _max_yaw_rate{0.f};
+	float _theta{0.f};
+
+	// Waypoints
+	Vector2d _curr_pos{};
+	Vector2f _curr_pos_ned{};
+	Vector2d _prev_wp{};
+	Vector2f _prev_wp_ned{};
+	Vector2d _curr_wp{};
+	Vector2f _curr_wp_ned{};
+	Vector2d _next_wp{};
+	Vector2f _next_wp_ned{};
+	Vector2d _home_position{};
+
+	// Constants
+	static constexpr float SLOW_DOWN_THRESHOLD =
+		2.61799f; // Slow down threshold for the maximum angle between the prev-curr and curr-next waypoint segments.
+
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RM_MAX_SPEED>) _param_rm_max_speed,
+		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
+		(ParamFloat<px4::params::RM_MAX_JERK>) _param_rm_max_jerk,
+		(ParamFloat<px4::params::RM_MAX_ACCEL>) _param_rm_max_accel,
+		(ParamFloat<px4::params::RM_MISS_SPD_DEF>) _param_rm_miss_spd_def,
+		(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate
+	)
+};

src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp

@@ -138,7 +138,7 @@ RoverMecanumGuidance::mecanum_setpoint RoverMecanumGuidance::computeGuidance(con
 	_rover_mecanum_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance();
 	_rover_mecanum_guidance_status.pid_heading_integral = 0.f;
 	_rover_mecanum_guidance_status.heading_error_deg = M_RAD_TO_DEG_F * heading_error;
-	_rover_mecanum_guidance_status.state_machine = (uint8_t) _currentState;
+	_rover_mecanum_guidance_status.state_machine = 0.f;
 	_rover_mecanum_guidance_status_pub.publish(_rover_mecanum_guidance_status);
 
 

src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp

@@ -116,7 +116,6 @@ class RoverMecanumGuidance : public ModuleParams
 
 	// Variables
 	MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates.
-	GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of guidance.
 	PurePursuit _pure_pursuit{this}; // Pure pursuit library
 	hrt_abstime _timestamp{0};
 	float _max_yaw_rate{0.f};