diff --git a/docs/world_global.rst b/docs/world_global.rst index c85bfbc6..3f6b4395 100644 --- a/docs/world_global.rst +++ b/docs/world_global.rst @@ -16,6 +16,9 @@ Available parameters (all are optional): - ``9.81``. Gravity acceleration (m/s²), in the negative Z axis direction (i.e. downwards). +- ``false``. If enabled, a gamepad or joystick + can be used to control the selected vehicle. (Default is ``false`` hence no responding to gamepads). + - ``localhost``. The IP address the server must be bound to. Refer to :ref:`architecture`. diff --git a/modules/simulator/include/mvsim/ControllerBase.h b/modules/simulator/include/mvsim/ControllerBase.h index 1c93668b..2fb92590 100644 --- a/modules/simulator/include/mvsim/ControllerBase.h +++ b/modules/simulator/include/mvsim/ControllerBase.h @@ -11,6 +11,8 @@ #include +#include + 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 js; + + TeleopInput() = default; }; + struct TeleopOutput { std::string append_gui_lines; diff --git a/modules/simulator/include/mvsim/Joystick.h b/modules/simulator/include/mvsim/Joystick.h new file mode 100644 index 00000000..f1cb2077 --- /dev/null +++ b/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 + +/*--------------------------------------------------------------- + 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 m_joystate_btns; + /** Using an event system we only have deltas, need to keep the whole + * joystick state (Linux only) */ + std::vector 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& 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 diff --git a/modules/simulator/include/mvsim/World.h b/modules/simulator/include/mvsim/World.h index 331c6636..bafcf0a2 100644 --- a/modules/simulator/include/mvsim/World.h +++ b/modules/simulator/include/mvsim/World.h @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -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 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_; + /** 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 `` diff --git a/modules/simulator/include/mvsim/basic_types.h b/modules/simulator/include/mvsim/basic_types.h index 99572795..8b169b3a 100644 --- a/modules/simulator/include/mvsim/basic_types.h +++ b/modules/simulator/include/mvsim/basic_types.h @@ -14,53 +14,51 @@ #include #include -#include // uintptr_t +#include // uintptr_t #include +#include class b2World; class b2Body; class b2Fixture; -namespace rapidxml -{ +namespace rapidxml { // Forward declarations -template -class xml_node; -template -class xml_attribute; -template -class xml_document; -} // namespace rapidxml +template class xml_node; +template class xml_attribute; +template 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 -class JointXMLnode; +template 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 buttons; + + TJoyStickEvent() = default; +}; + +} // namespace mvsim diff --git a/modules/simulator/src/Joystick.cpp b/modules/simulator/src/Joystick.cpp new file mode 100644 index 00000000..bdd99741 --- /dev/null +++ b/modules/simulator/src/Joystick.cpp @@ -0,0 +1,259 @@ +/*+-------------------------------------------------------------------------+ + | 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 | + +------------------------------------------------------------------------+ */ + +#include +#include + +#ifdef _WIN32 +#define WIN32_LEAN_AND_MEAN +#include +// +#include + +#if !defined(__GNUC__) +#pragma comment(lib, "WINMM.LIB") +#endif +#endif + +#if defined(MRPT_OS_LINUX) || defined(__APPLE__) +// Linux +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#if defined(MRPT_OS_LINUX) && defined(HAVE_LINUX_INPUT_H) +#include +#include +#endif +#endif + +#include + +using mvsim::Joystick; + +/*--------------------------------------------------------------- + Constructor + ---------------------------------------------------------------*/ +Joystick::Joystick() { setLimits(); } +/*--------------------------------------------------------------- + Destructor + ---------------------------------------------------------------*/ +Joystick::~Joystick() +{ +#if defined(MRPT_OS_LINUX) && defined(HAVE_LINUX_INPUT_H) + // Close joystick, if open: + if (m_joy_fd > 0) + { + close(m_joy_fd); + m_joy_fd = -1; + } +#endif +} + +/*--------------------------------------------------------------- + getJoysticksCount + Returns the number of Joysticks in the computer. + ---------------------------------------------------------------*/ +int Joystick::getJoysticksCount() +{ + MRPT_START +#ifdef _WIN32 + return joyGetNumDevs(); +#elif defined(MRPT_OS_LINUX) && defined(HAVE_LINUX_INPUT_H) + // Try to open several joy devs: + int joy_fd = -1; + int nJoys = 0; + + do + { + if (-1 != + (joy_fd = open( + mrpt::format("/dev/input/js%i", nJoys).c_str(), O_RDONLY))) + { + nJoys++; + close(joy_fd); + } + } while (joy_fd != -1); + + return nJoys; +#else + // Apple: + return 0; +#endif + MRPT_END +} + +/*--------------------------------------------------------------- + Gets joystick information. + + \return Returns true if successfull, false on error, for example, if joystick + is not present. + ---------------------------------------------------------------*/ +bool Joystick::getJoystickPosition( + int nJoy, float& x, float& y, float& z, std::vector& buttons, + int* raw_x_pos, int* raw_y_pos, int* raw_z_pos) +{ + MRPT_START +#ifdef _WIN32 + JOYINFO jinfo; + + int ID = JOYSTICKID1 + nJoy; + + // Get joy pos: + if (JOYERR_NOERROR != joyGetPos(ID, &jinfo)) return false; // Error. + + // Output data: + x = (jinfo.wXpos - m_x_min) / (float)(m_x_max - m_x_min); + y = (jinfo.wYpos - m_y_min) / (float)(m_y_max - m_y_min); + z = (jinfo.wZpos - m_z_min) / (float)(m_z_max - m_z_min); + + x = 2 * x - 1; + y = 2 * y - 1; + z = 2 * z - 1; + + buttons.resize(4); + + buttons[0] = 0 != (jinfo.wButtons & JOY_BUTTON1); + buttons[1] = 0 != (jinfo.wButtons & JOY_BUTTON2); + buttons[2] = 0 != (jinfo.wButtons & JOY_BUTTON3); + buttons[3] = 0 != (jinfo.wButtons & JOY_BUTTON4); + + if (raw_x_pos) *raw_x_pos = jinfo.wXpos; + if (raw_y_pos) *raw_y_pos = jinfo.wYpos; + if (raw_z_pos) *raw_z_pos = jinfo.wZpos; + + return true; +#elif defined(MRPT_OS_LINUX) && defined(HAVE_LINUX_INPUT_H) + // Already open? + if (m_joy_index == nJoy && m_joy_fd != -1) + { + // Ok + } + else + { + // Close previous opened joy? + if (m_joy_fd != -1) close(m_joy_fd); + + // Go, try open joystick: + if ((m_joy_fd = open( + mrpt::format("/dev/input/js%i", nJoy).c_str(), O_RDONLY)) < 0) + return false; + + // Perfect! + m_joy_index = nJoy; + + // Read in non-blocking way: **** Refer to sources of "jstest"!!!! *** + fcntl(m_joy_fd, F_SETFL, O_NONBLOCK); + } + + struct js_event js; + + while (read(m_joy_fd, &js, sizeof(struct js_event)) == + sizeof(struct js_event)) + { + // Button? + if (js.type & JS_EVENT_BUTTON) + { + // js.number: Button number + if (m_joystate_btns.size() < (size_t)js.number + 1) + m_joystate_btns.resize(js.number + 1); + m_joystate_btns[js.number] = js.value != 0; + } + + // Axes? + if (js.type & JS_EVENT_AXIS) + { + // std::cout << "joy: event axis" << std::endl; + if (m_joystate_axes.size() < (size_t)js.number + 1) + m_joystate_axes.resize(js.number + 1); + m_joystate_axes[js.number] = js.value; + } + } + + if (errno != EAGAIN) + { + // Joystick disconnected? + m_joy_fd = -1; + m_joy_index = -1; + return false; + } + + // Fill out data: + const size_t nAxis = m_joystate_axes.size(); + if (nAxis >= 1) + { + x = -1 + + 2 * (m_joystate_axes[0] - m_x_min) / (float)(m_x_max - m_x_min); + if (raw_x_pos) *raw_x_pos = m_joystate_axes[0]; + } + + if (nAxis >= 2) + { + y = -1 + + 2 * (m_joystate_axes[1] - m_y_min) / (float)(m_y_max - m_y_min); + if (raw_y_pos) *raw_y_pos = m_joystate_axes[1]; + } + + if (nAxis >= 3) + { + z = -1 + + 2 * (m_joystate_axes[2] - m_z_min) / (float)(m_z_max - m_z_min); + if (raw_z_pos) *raw_z_pos = m_joystate_axes[2]; + } + else + { + z = 0; + } + + buttons = m_joystate_btns; + + return true; +#else + // Apple. + return false; +#endif + MRPT_END +} + +/** Set the axis limit values, for computing a [-1,1] position index easily. + * It seems that these values must been calibrated for each joystick model. + * + * \sa getJoystickPosition + */ +void Joystick::setLimits( + int x_min, int x_max, int y_min, int y_max, int z_min, int z_max) +{ + m_x_max = x_max; + m_x_min = x_min; + + m_y_max = y_max; + m_y_min = y_min; + + m_z_max = z_max; + m_z_min = z_min; +} diff --git a/modules/simulator/src/World.cpp b/modules/simulator/src/World.cpp index 12cf951b..3f3162de 100644 --- a/modules/simulator/src/World.cpp +++ b/modules/simulator/src/World.cpp @@ -405,3 +405,21 @@ bool World::sensor_has_to_create_egl_context() first = false; return ret; } + +std::optional World::getJoystickState() const +{ + if (!joystickEnabled_) return {}; + + if (!joystick_) + { + joystick_.emplace(); + } + + mvsim::TJoyStickEvent js; + + const int nJoy = 0; // TODO: Expose param for multiple joysticks? + + joystick_->getJoystickPosition(nJoy, js.x, js.y, js.z, js.buttons); + + return js; +} diff --git a/mvsim-cli/mvsim-cli-launch.cpp b/mvsim-cli/mvsim-cli-launch.cpp index 21974189..4116bbe2 100644 --- a/mvsim-cli/mvsim-cli-launch.cpp +++ b/mvsim-cli/mvsim-cli-launch.cpp @@ -103,7 +103,8 @@ void mvsim_install_signal_handler() // returns log strings std::string mvsim_launch_handle_teleop( - const mvsim::World::TGUIKeyEvent keyevent) + const mvsim::World::TGUIKeyEvent keyevent, + const std::optional& js) { using namespace mvsim; @@ -145,6 +146,7 @@ std::string mvsim_launch_handle_teleop( ControllerBaseInterface::TeleopInput teleop_in; ControllerBaseInterface::TeleopOutput teleop_out; teleop_in.keycode = keyevent.keycode; + teleop_in.js = js; controller->teleop_interface(teleop_in, teleop_out); txt2gui_tmp += teleop_out.append_gui_lines; } @@ -298,7 +300,9 @@ Available options: break; }; - const auto txt2gui_tmp = mvsim_launch_handle_teleop(keyevent); + const auto js = app->world.getJoystickState(); + + const auto txt2gui_tmp = mvsim_launch_handle_teleop(keyevent, js); // Clear the keystroke buffer gui_key_events_mtx.lock(); diff --git a/mvsim_node_src/include/mvsim/mvsim_node_core.h b/mvsim_node_src/include/mvsim/mvsim_node_core.h index e4ec1e6e..c169ed5c 100644 --- a/mvsim_node_src/include/mvsim/mvsim_node_core.h +++ b/mvsim_node_src/include/mvsim/mvsim_node_core.h @@ -248,6 +248,7 @@ class MVSimNode /// for teleoperation from the GUI (selects the focused" vehicle) size_t teleop_idx_veh_ = 0; mvsim::World::TGUIKeyEvent gui_key_events_; + mvsim::TJoyStickStatus gui_js_status_; std::string msg2gui_; std::thread thGUI_; diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index 21cb472b..f5c65970 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -345,6 +345,7 @@ void MVSimNode::spin() std::string txt2gui_tmp; World::TGUIKeyEvent keyevent = gui_key_events_; + const auto js = gui_js_status_; // Global keys: switch (keyevent.keycode) @@ -388,6 +389,7 @@ void MVSimNode::spin() ControllerBaseInterface::TeleopInput teleop_in; ControllerBaseInterface::TeleopOutput teleop_out; teleop_in.keycode = keyevent.keycode; + teleop_in.joystick = js; controller->teleop_interface(teleop_in, teleop_out); txt2gui_tmp += teleop_out.append_gui_lines; }