WIP: Add joystick control

docs/world_global.rst

@@ -16,6 +16,9 @@ Available parameters (all are optional):
 - ``<gravity>9.81</gravity>``. Gravity acceleration (m/s²), in the negative Z axis 
   direction (i.e. downwards).
 
+- ``<joystick_enabled>false</joystick_enabled>``. If enabled, a gamepad or joystick 
+  can be used to control the selected vehicle. (Default is ``false`` hence no responding to gamepads).
+
 - ``<server_address>localhost</server_address>``. The IP address the server
   must be bound to. Refer to :ref:`architecture`.
 

modules/simulator/include/mvsim/ControllerBase.h

@@ -11,6 +11,8 @@
 
 #include <mvsim/basic_types.h>
 
+#include <optional>
+
 namespace mvsim
 {
 /** Interface of ControllerBaseTempl<> for teleoperation, etc.
@@ -21,9 +23,12 @@ class ControllerBaseInterface
    public:
 	struct TeleopInput
 	{
-		int keycode;
-		TeleopInput() : keycode(0) {}
+		int keycode = 0;
+		std::optional<TJoyStickEvent> js;
+
+		TeleopInput() = default;
 	};
+
 	struct TeleopOutput
 	{
 		std::string append_gui_lines;

modules/simulator/include/mvsim/Joystick.h

@@ -0,0 +1,112 @@
+/*+-------------------------------------------------------------------------+
+  |                       MultiVehicle simulator (libmvsim)                 |
+  |                                                                         |
+  | Copyright (C) 2014-2023  Jose Luis Blanco Claraco                       |
+  | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
+  | Distributed under 3-clause BSD License                                  |
+  |   See COPYING                                                           |
+  +-------------------------------------------------------------------------+ */
+
+// NOTE: This file is borrowed from mrpt-hwdrivers (BSD-3 License)
+
+/* +------------------------------------------------------------------------+
+   |                     Mobile Robot Programming Toolkit (MRPT)            |
+   |                          https://www.mrpt.org/                         |
+   |                                                                        |
+   | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file     |
+   | See: https://www.mrpt.org/Authors - All rights reserved.               |
+   | Released under BSD License. See: https://www.mrpt.org/License          |
+   +------------------------------------------------------------------------+ */
+#pragma once
+
+#include <vector>
+
+/*---------------------------------------------------------------
+	Class
+  ---------------------------------------------------------------*/
+namespace mvsim
+{
+/** Access to joysticks and gamepads (read buttons and position), and request
+ * number of joysticks in the system.
+ */
+class Joystick
+{
+   private:
+	/** The axis limits:
+	 */
+	int m_x_min, m_x_max, m_y_min, m_y_max, m_z_min, m_z_max;
+
+#if defined(MRPT_OS_LINUX)
+	/** File FD for the joystick, or -1 if not open (Linux only) */
+	int m_joy_fd{-1};
+	/** The index of the joystick open in m_joy_fd (Linux only) */
+	int m_joy_index{-1};
+	/** Using an event system we only have deltas, need to keep the whole
+	 * joystick state (Linux only) */
+	std::vector<bool> m_joystate_btns;
+	/** Using an event system we only have deltas, need to keep the whole
+	 * joystick state (Linux only) */
+	std::vector<int> m_joystate_axes;
+#endif
+
+   public:
+	/** Constructor
+	 */
+	Joystick();
+
+	/** Destructor
+	 */
+	virtual ~Joystick();
+
+	/** Returns the number of Joysticks in the computer.
+	 */
+	static int getJoysticksCount();
+
+	/** Gets joystick information.
+	 *
+	 *   This method will try first to open the joystick, so you can safely call
+	 * it while the joystick is plugged and removed arbitrarly.
+	 *
+	 * \param nJoy The index of the joystick to query: The first one is 0, the
+	 * second 1, etc... See Joystick::getJoysticksCount to discover the number
+	 * of joysticks in the system.
+	 * \param x The x axis position, range [-1,1]
+	 * \param y The y axis position, range [-1,1]
+	 * \param z The z axis position, range [-1,1]
+	 * \param buttons Each element will hold true if buttons are pressed. The
+	 * size of the vector will be set automatically to the number of buttons.
+	 * \param raw_x_pos If it is desired the raw integer measurement from
+	 * JoyStick, set this pointer to a desired placeholder.
+	 * \param raw_y_pos If it is desired the raw integer measurement from
+	 * JoyStick, set this pointer to a desired placeholder.
+	 * \param raw_z_pos If it is desired the raw integer measurement from
+	 * JoyStick, set this pointer to a desired placeholder.
+	 *
+	 * \return Returns true if successfull, false on error, for example, if
+	 * joystick is not present.
+	 *
+	 * \sa setLimits
+	 */
+	bool getJoystickPosition(
+		int nJoy, float& x, float& y, float& z, std::vector<bool>& buttons,
+		int* raw_x_pos = nullptr, int* raw_y_pos = nullptr,
+		int* raw_z_pos = nullptr);
+
+/** Set the axis limit values, for computing a [-1,1] position index easily
+ * (Only required to calibrate analog joystick).
+ *   It seems that these values must been calibrated for each joystick model.
+ *
+ * \sa getJoystickPosition
+ */
+#ifdef _WIN32
+	void setLimits(
+		int x_min = 0, int x_max = 0xFFFF, int y_min = 0, int y_max = 0xFFFF,
+		int z_min = 0, int z_max = 0xFFFF);
+#else
+	void setLimits(
+		int x_min = -32767, int x_max = 32767, int y_min = -32767,
+		int y_max = 32767, int z_min = -32767, int z_max = 32767);
+#endif
+};	// End of class def.
+
+}  // namespace mvsim

modules/simulator/include/mvsim/World.h

@@ -25,6 +25,7 @@
 #include <mrpt/system/CTimeLogger.h>
 #include <mvsim/Block.h>
 #include <mvsim/Comms/Client.h>
+#include <mvsim/Joystick.h>
 #include <mvsim/RemoteResourcesManager.h>
 #include <mvsim/TParameterDefinitions.h>
 #include <mvsim/VehicleBase.h>
@@ -393,6 +394,12 @@ class World : public mrpt::system::COutputLogger
 		return userDefinedVariables_;
 	}
 
+	/** If joystick usage is enabled (via XML file option, for example),
+	 *  this will read the joystick state and return it. Otherwise (or on device
+	 * error or disconnection), a null optional variable is returned.
+	 */
+	std::optional<mvsim::TJoyStickEvent> getJoystickState() const;
+
    private:
 	friend class VehicleBase;
 	friend class Block;
@@ -412,6 +419,9 @@ class World : public mrpt::system::COutputLogger
 	 */
 	mutable double simulTimestep_ = 0;
 
+	bool joystickEnabled_ = false;
+	mutable std::optional<Joystick> joystick_;
+
 	/** Velocity and position iteration count (refer to libbox2d docs) */
 	int b2dVelIters_ = 8, b2dPosIters_ = 3;
 
@@ -423,6 +433,7 @@ class World : public mrpt::system::COutputLogger
 		{"simul_timestep", {"%lf", &simulTimestep_}},
 		{"b2d_vel_iters", {"%i", &b2dVelIters_}},
 		{"b2d_pos_iters", {"%i", &b2dPosIters_}},
+		{"joystick_enabled", {"%bool", &joystickEnabled_}},
 	};
 
 	/** User-defined variables as defined via `<variable name='' value='' />`

modules/simulator/include/mvsim/basic_types.h

@@ -14,53 +14,51 @@
 #include <mrpt/math/TPose2D.h>
 #include <mrpt/math/TTwist2D.h>
 
-#include <cstdint>	// uintptr_t
+#include <cstdint> // uintptr_t
 #include <string>
+#include <vector>
 
 class b2World;
 class b2Body;
 class b2Fixture;
 
-namespace rapidxml
-{
+namespace rapidxml {
 // Forward declarations
-template <class Ch>
-class xml_node;
-template <class Ch>
-class xml_attribute;
-template <class Ch>
-class xml_document;
-}  // namespace rapidxml
+template <class Ch> class xml_node;
+template <class Ch> class xml_attribute;
+template <class Ch> class xml_document;
+} // namespace rapidxml
 
-namespace mrpt
-{
-namespace opengl
-{
+namespace mrpt {
+namespace opengl {
 class CSetOfObjects;
-}  // namespace opengl
-namespace slam
-{
+} // namespace opengl
+namespace slam {
 class CObservation;
 }
-}  // namespace mrpt
+} // namespace mrpt
 
-namespace mvsim
-{
+namespace mvsim {
 class World;
 class VehicleBase;
-template <typename Ch = char>
-class JointXMLnode;
+template <typename Ch = char> class JointXMLnode;
 
 /** Simulation context for simulable objects updates */
-struct TSimulContext
-{
-	b2World* b2_world = nullptr;
-	World* world = nullptr;
-	double simul_time = 0;	//!< Current time in the simulated world
-	double dt = 0;	//!< timestep
+struct TSimulContext {
+  b2World *b2_world = nullptr;
+  World *world = nullptr;
+  double simul_time = 0; //!< Current time in the simulated world
+  double dt = 0;         //!< timestep
 };
 
 /// Used to signal a Box2D fixture as "invisible" to sensors.
 constexpr uintptr_t INVISIBLE_FIXTURE_USER_DATA = 1;
 
-}  // namespace mvsim
+struct TJoyStickEvent {
+  float x = .0f, y = .0f, z = .0f;
+  std::vector<bool> buttons;
+
+  TJoyStickEvent() = default;
+};
+
+} // namespace mvsim