diff --git a/joy/CMakeLists.txt b/joy/CMakeLists.txt index 61708679..e39e9f2e 100644 --- a/joy/CMakeLists.txt +++ b/joy/CMakeLists.txt @@ -38,6 +38,27 @@ rclcpp_components_register_node(joy PLUGIN "joy::Joy" EXECUTABLE joy_node) +add_library(game_controller SHARED src/game_controller.cpp) +target_include_directories(game_controller PUBLIC + "$" + "$") +ament_target_dependencies(game_controller + rclcpp + rclcpp_components + sensor_msgs) +target_link_libraries(game_controller + SDL2::SDL2) + +install(TARGETS game_controller EXPORT export_game_controller + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) + +rclcpp_components_register_node(game_controller + PLUGIN "joy::GameController" + EXECUTABLE game_controller_node) + add_executable(joy_enumerate_devices src/joy_enumerate_devices.cpp) target_link_libraries(joy_enumerate_devices @@ -52,6 +73,7 @@ install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) ament_export_targets(export_joy) +ament_export_targets(export_game_controller) ament_export_dependencies( "rclcpp" "sensor_msgs" diff --git a/joy/README.md b/joy/README.md index 9c8ac77f..0417575c 100644 --- a/joy/README.md +++ b/joy/README.md @@ -1,10 +1,49 @@ -# ROS 2 Driver for Generic Joysticks +# ROS 2 Driver for Generic Joysticks and Game Controllers -The joy package contains joy_node, a node that interfaces a generic joystick to ROS 2. This node publishes a "sensor_msgs/msg/Joy" message, which contains the current state of each one of the joystick's buttons and axes. +The joy package contains joy_node, and game_controller node which interface generic joysticks and game controllers to ROS 2. These nodes publish "sensor_msgs/msg/Joy" messages, which contain the state of the devices button and axes. Examples of game controllers are ones that come with recently released game consoles and their after market clones. + +The game_controller_node uses SDL2's built device mapping database to give buttons and axes a consistent order in the "sensor_msgs/msg/Joy". Custom mappings can also be supplied using SDL's SDL_GAMECONTROLLERCONFIG environment variable. A third party tool can be used to create the mapping string. The joy_node supports both joysticks and game controllers but the order that buttons and axes appear with the message will dependend on the manufacturer of the device. + +For game_controller_node the following tables detail the indexes of buttons and axes. + +| Index | Button | +| - | - | +| 0 | A (CROSS) | +| 1 | B (CIRCLE) | +| 2 | X (SQUARE) | +| 3 | Y (TRIANGLE) | +| 4 | BACK (SELECT) | +| 5 | GUIDE (Middle/Manufacturer Button) | +| 6 | START | +| 7 | LEFTSTICK | +| 8 | RIGHTSTICK | +| 9 | LEFTSHOULDER | +| 10 | RIGHTSHOULDER | +| 11 | DPAD_UP | +| 12 | DPAD_DOWN | +| 13 | DPAD_LEFT | +| 14 | DPAD_RIGHT | +| 15 | MISC1 (Depends on the controller manufacturer, but is usually at a similar location on the controller as back/start) | +| 16 | PADDLE1 (Upper left, facing the back of the controller if present) | +| 17 | PADDLE2 (Upper right, facing the back of the controller if present) | +| 18 | PADDLE3 (Lower left, facing the back of the controller if present) | +| 19 | PADDLE4 (Lower right, facing the back of the controller if present) | +| 20 | TOUCHPAD (If present. Button status only) | + +| Index | Axis | +| - | - | +| 0 | LEFTX | +| 1 | LEFTY | +| 2 | RIGHTX | +| 3 | RIGHTY | +| 4 | TRIGGERLEFT | +| 5 | TRIGGERRIGHT | + +For joy_node run `ros2 run joy joy_node` in one terminal and `ros2 topic echo /joy` in another. Pressing buttons and moving sticks can be used to determine at which location they appear in "sensor_msgs/msg/Joy". ## Supported Hardware -This node should work with any joystick that is supported by SDL. +This nodes should work with any joystick or game controller that is supported by SDL. ## Published Topics @@ -16,7 +55,7 @@ This node should work with any joystick that is supported by SDL. ## Parameters * device_id (int, default: 0) - * The joystick device to use. + * The joystick device to use. `ros2 run joy joy_enumerate_devices` wil list the attached devices. * device_name (string, default: "") * The joystick name to use. This can be useful when multiple different joysticks are attached. If both device_name and device_id are specified, device_name takes precedence. @@ -33,6 +72,6 @@ This node should work with any joystick that is supported by SDL. * coalesce_interval_ms (int, default: 1) * The number of milliseconds to wait after an axis event before publishing a message. Since the kernel sends an event for every change, this can significantly reduce the number of messages published. Setting it to 0 disables this behavior. The default of 1 ms is a good compromise between message delays and number of messages published. -## Technical note about interfacing with the joystick on Linux +## Technical note about interfacing with joysticks and game controllers on Linux On Linux there are two different ways to interface with a joystick. The distinction only makes a difference when attempting to pass through the device into a container or virtual machine. The first interface is via the joystick driver subsystem, which generally shows up as a device in /dev/input/js0 (or other numbers at the end). This is the way that the "joy_linux" package accesses the joystick. The second way to interface is through the generic event subsystem, which generally shows up as /dev/input/event7 (or other numbers at the end). This is the way that SDL (and hence this "joy" package) accesses the joysticks. diff --git a/joy/include/joy/game_controller.hpp b/joy/include/joy/game_controller.hpp new file mode 100644 index 00000000..c58a718d --- /dev/null +++ b/joy/include/joy/game_controller.hpp @@ -0,0 +1,94 @@ +/* + * Copyright (c) 2020, Open Source Robotics Foundation. + * Copyright (c) 2023, CSIRO Data61. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + * * Neither the name of the copyright holder 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. + */ + +#ifndef JOY__GAME_CONTROLLER_HPP_ +#define JOY__GAME_CONTROLLER_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace joy +{ + +class GameController final : public rclcpp::Node +{ +public: + explicit GameController(const rclcpp::NodeOptions & options); + GameController(GameController && c) = delete; + GameController & operator=(GameController && c) = delete; + GameController(const GameController & c) = delete; + GameController & operator=(const GameController & c) = delete; + + ~GameController() override; + +private: + void eventThread(); + bool handleControllerAxis(const SDL_ControllerAxisEvent & e); + bool handleControllerButtonDown(const SDL_ControllerButtonEvent & e); + bool handleControllerButtonUp(const SDL_ControllerButtonEvent & e); + void handleControllerDeviceAdded(const SDL_ControllerDeviceEvent & e); + void handleControllerDeviceRemoved(const SDL_ControllerDeviceEvent & e); + float convertRawAxisValueToROS(int16_t val); + void feedbackCb(const std::shared_ptr msg); + + int dev_id_{0}; + + SDL_GameController * game_controller_{nullptr}; + SDL_JoystickID joystick_instance_id_{0}; + double scaled_deadzone_{0.0}; + double unscaled_deadzone_{0.0}; + double scale_{0.0}; + double autorepeat_rate_{0.0}; + int autorepeat_interval_ms_{0}; + bool sticky_buttons_{false}; + bool publish_soon_{false}; + rclcpp::Time publish_soon_time_; + int coalesce_interval_ms_{0}; + std::string dev_name_; + std::thread event_thread_; + std::shared_future future_; + std::promise exit_signal_; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr feedback_sub_; + + sensor_msgs::msg::Joy joy_msg_; +}; + +} // namespace joy + +#endif // JOY__GAME_CONTROLLER_HPP_ diff --git a/joy/package.xml b/joy/package.xml index 646bb25e..22a64287 100644 --- a/joy/package.xml +++ b/joy/package.xml @@ -4,12 +4,9 @@ joy 3.1.0 - - The joy package contains joy_node, a node that interfaces a - generic joystick to ROS 2. This node publishes a "Joy" - message, which contains the current state of each one of the - joystick's buttons and axes. - + The joy package contains joy_node, a node that interfaces a generic joystick to ROS + 2. This node publishes a "Joy" message, which contains the current state of each one of the + joystick's buttons and axes. Chris Lalancette BSD @@ -21,6 +18,7 @@ Kevin Watts Blaise Gassend Jonathan Bohren + Rod Taylor ament_cmake_ros diff --git a/joy/src/game_controller.cpp b/joy/src/game_controller.cpp new file mode 100644 index 00000000..2e931282 --- /dev/null +++ b/joy/src/game_controller.cpp @@ -0,0 +1,420 @@ +/* + * Copyright (c) 2020, Open Source Robotics Foundation. + * Copyright (c) 2023, CSIRO Data61. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + * * Neither the name of the copyright holder 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 +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include "joy/game_controller.hpp" + +namespace joy +{ + +GameController::GameController(const rclcpp::NodeOptions & options) +: rclcpp::Node("game_controller_node", options) +{ + dev_id_ = static_cast(this->declare_parameter("device_id", 0)); + + dev_name_ = this->declare_parameter("device_name", std::string("")); + + // The user specifies the deadzone to us in the range of 0.0 to 1.0. Later on + // we'll convert that to the range of 0 to 32767. Note also that negatives + // are not allowed, as this is a +/- value. + scaled_deadzone_ = this->declare_parameter("deadzone", 0.05); + if (scaled_deadzone_ < 0.0 || scaled_deadzone_ > 1.0) { + throw std::runtime_error("Deadzone must be between 0.0 and 1.0"); + } + unscaled_deadzone_ = 32767.0 * scaled_deadzone_; + // According to the SDL documentation, this always returns a value between + // -32768 and 32767. However, we want to report a value between -1.0 and 1.0, + // hence the "scale" dividing by 32767. Also note that SDL returns the axes + // with "forward" and "left" as negative. This is opposite to the ROS + // conventionof "forward" and "left" as positive, so we invert the axes here + // as well. Finally, we take into account the amount of deadzone so we truly + // do get value between -1.0 and 1.0 (and not -deadzone to +deadzone). + scale_ = static_cast(-1.0 / (1.0 - scaled_deadzone_) / 32767.0); + + autorepeat_rate_ = this->declare_parameter("autorepeat_rate", 20.0); + if (autorepeat_rate_ < 0.0) { + throw std::runtime_error("Autorepeat rate must be >= 0.0"); + } else if (autorepeat_rate_ > 1000.0) { + throw std::runtime_error("Autorepeat rate must be <= 1000.0"); + } else if (autorepeat_rate_ > 0.0) { + autorepeat_interval_ms_ = static_cast(1000.0 / autorepeat_rate_); + } else { + // If the autorepeat rate is set to 0, the user doesn't want us to + // publish unless an event happens. We still wake up every 200 + // milliseconds to check if we need to quit. + autorepeat_interval_ms_ = 200; + } + + sticky_buttons_ = this->declare_parameter("sticky_buttons", false); + + coalesce_interval_ms_ = static_cast(this->declare_parameter("coalesce_interval_ms", 1)); + if (coalesce_interval_ms_ < 0) { + throw std::runtime_error("coalesce_interval_ms must be positive"); + } + // Make sure to initialize publish_soon_time regardless of whether we are going + // to use it; this ensures that we are always using the correct time source. + publish_soon_time_ = this->now(); + + pub_ = create_publisher("joy", 10); + + feedback_sub_ = this->create_subscription( + "joy/set_feedback", rclcpp::QoS(10), + std::bind(&GameController::feedbackCb, this, std::placeholders::_1)); + + future_ = exit_signal_.get_future(); + + // In theory we could do this with just a timer, which would simplify the code + // a bit. But then we couldn't react to "immediate" events, so we stick with + // the thread. + event_thread_ = std::thread(&GameController::eventThread, this); + + joy_msg_.buttons.resize(SDL_CONTROLLER_BUTTON_MAX); + + joy_msg_.axes.resize(SDL_CONTROLLER_AXIS_MAX); + + if (SDL_Init(SDL_INIT_GAMECONTROLLER) < 0) { + throw std::runtime_error("SDL could not be initialized: " + std::string(SDL_GetError())); + } +} + +GameController::~GameController() +{ + exit_signal_.set_value(); + event_thread_.join(); + if (game_controller_ != nullptr) { + SDL_GameControllerClose(game_controller_); + } + SDL_Quit(); +} + +void GameController::feedbackCb(const std::shared_ptr msg) +{ + if (msg->type != sensor_msgs::msg::JoyFeedback::TYPE_RUMBLE) { + // We only support rumble + return; + } + + if (msg->id != 0) { + // There can be only one (rumble) + // TODO(Rod Taylor): Support high and low frequency rumble channels. + return; + } + + if (msg->intensity < 0.0 || msg->intensity > 1.0) { + // We only accept intensities between 0 and 1. + return; + } + + if (game_controller_ != nullptr) { + // We purposely ignore the return value; if it fails, what can we do? + uint16_t intensity = static_cast(msg->intensity * 0xFFFF); + SDL_GameControllerRumble(game_controller_, intensity, intensity, 1000); + } +} + +float GameController::convertRawAxisValueToROS(int16_t val) +{ + // SDL reports axis values between -32768 and 32767. To make sure + // we report out scaled value between -1.0 and 1.0, we add one to + // the value iff it is exactly -32768. This makes all of the math + // below work properly. + if (val == -32768) { + val = -32767; + } + + // Note that we do all of the math in double space below. This ensures + // that the values stay between -1.0 and 1.0. + double double_val = static_cast(val); + // Apply the deadzone semantic here. This allows the deadzone + // to be "smooth". + if (double_val > unscaled_deadzone_) { + double_val -= unscaled_deadzone_; + } else if (double_val < -unscaled_deadzone_) { + double_val += unscaled_deadzone_; + } else { + double_val = 0.0; + } + + return static_cast(double_val * scale_); +} + +bool GameController::handleControllerAxis(const SDL_ControllerAxisEvent & e) +{ + bool publish = false; + + if (e.which != joystick_instance_id_) { + return publish; + } + + if (e.axis >= SDL_CONTROLLER_AXIS_MAX) { + RCLCPP_WARN(get_logger(), "Saw axes too large for this device, ignoring"); + return publish; + } + + float last_axis_value = joy_msg_.axes.at(e.axis); + joy_msg_.axes.at(e.axis) = convertRawAxisValueToROS(e.value); + if (last_axis_value != joy_msg_.axes.at(e.axis)) { + if (coalesce_interval_ms_ > 0 && !publish_soon_) { + publish_soon_ = true; + publish_soon_time_ = this->now(); + } else { + rclcpp::Duration time_since_publish_soon = this->now() - publish_soon_time_; + if (time_since_publish_soon.nanoseconds() >= RCL_MS_TO_NS(coalesce_interval_ms_)) { + publish = true; + publish_soon_ = false; + } + } + } + // else no change, so don't publish + + return publish; +} + +bool GameController::handleControllerButtonDown(const SDL_ControllerButtonEvent & e) +{ + bool publish = false; + + if (e.which != joystick_instance_id_) { + return publish; + } + + if (e.button >= SDL_CONTROLLER_BUTTON_MAX) { + RCLCPP_WARN(get_logger(), "Saw button too large for this device, ignoring"); + return publish; + } + + if (sticky_buttons_) { + // For sticky buttons, invert 0 -> 1 or 1 -> 0 + joy_msg_.buttons.at(e.button) = 1 - joy_msg_.buttons.at(e.button); + } else { + joy_msg_.buttons.at(e.button) = 1; + } + publish = true; + + return publish; +} + +bool GameController::handleControllerButtonUp(const SDL_ControllerButtonEvent & e) +{ + bool publish = false; + + if (e.which != joystick_instance_id_) { + return publish; + } + + if (e.button >= SDL_CONTROLLER_BUTTON_MAX) { + RCLCPP_WARN(get_logger(), "Saw button too large for this device, ignoring"); + return publish; + } + + if (!sticky_buttons_) { + joy_msg_.buttons.at(e.button) = 0; + publish = true; + } + return publish; +} + +void GameController::handleControllerDeviceAdded(const SDL_ControllerDeviceEvent & e) +{ + int num_joysticks = SDL_NumJoysticks(); + if (num_joysticks < 0) { + RCLCPP_WARN(get_logger(), "Failed to get the number of game controllers: %s", SDL_GetError()); + return; + } + bool matching_device_found = false; + for (int i = 0; i < num_joysticks; ++i) { + const char * name = SDL_JoystickNameForIndex(i); + if (name == nullptr) { + RCLCPP_WARN(get_logger(), "Could not get game controller name: %s", SDL_GetError()); + continue; + } + RCLCPP_INFO(get_logger(), "Controller Found: device_id=%i, device_name=%s", i, name); + if (std::string(name) == dev_name_) { + if (!dev_name_.empty()) { + // We found it! + matching_device_found = true; + dev_id_ = i; + break; + } + } + } + if (!dev_name_.empty() && !matching_device_found) { + RCLCPP_WARN( + get_logger(), "Could not get game controller with name %s: %s", + dev_name_.c_str(), SDL_GetError()); + return; + } + + if (e.which != dev_id_) { + // ignore device that don't match the dev_id_ specified + return; + } + + game_controller_ = SDL_GameControllerOpen(dev_id_); + if (game_controller_ == nullptr) { + RCLCPP_WARN(get_logger(), "Unable to open game controller %d: %s", dev_id_, SDL_GetError()); + return; + } + + // We need to hold onto this so that we can properly remove it on a + // remove event. + joystick_instance_id_ = SDL_JoystickGetDeviceInstanceID(dev_id_); + if (joystick_instance_id_ < 0) { + RCLCPP_WARN(get_logger(), "Failed to get instance ID for game controller: %s", SDL_GetError()); + SDL_GameControllerClose(game_controller_); + game_controller_ = nullptr; + return; + } + + // Get the initial state for each of the axes + for (int i = 0; i < SDL_CONTROLLER_AXIS_MAX; ++i) { + int16_t state = + SDL_GameControllerGetAxis(game_controller_, static_cast(i)); + joy_msg_.axes.at(i) = convertRawAxisValueToROS(state); + } + + const char * has_rumble_string = "No"; + if (SDL_GameControllerHasRumble(game_controller_)) { + has_rumble_string = "Yes"; + } + + RCLCPP_INFO( + get_logger(), "Opened game controller: %s, deadzone: %f, rumble: %s", + SDL_GameControllerName(game_controller_), scaled_deadzone_, has_rumble_string); +} + +void GameController::handleControllerDeviceRemoved(const SDL_ControllerDeviceEvent & e) +{ + if (e.which != joystick_instance_id_) { + return; + } + if (game_controller_ != nullptr) { + RCLCPP_INFO( + get_logger(), "Game controller Removed: %s.", + SDL_GameControllerName(game_controller_)); + SDL_GameControllerClose(game_controller_); + game_controller_ = nullptr; + } +} + +void GameController::eventThread() +{ + std::future_status status; + rclcpp::Time last_publish = this->now(); + + do{ + bool should_publish = false; + SDL_Event e; + int wait_time_ms = autorepeat_interval_ms_; + if (publish_soon_) { + wait_time_ms = std::min(wait_time_ms, coalesce_interval_ms_); + } + int success = SDL_WaitEventTimeout(&e, wait_time_ms); + if (success == 1) { + // Succeeded getting an event + switch (e.type) { + case SDL_CONTROLLERAXISMOTION: { + should_publish = handleControllerAxis(e.caxis); + break; + } + case SDL_CONTROLLERBUTTONDOWN: { + should_publish = handleControllerButtonDown(e.cbutton); + break; + } + case SDL_CONTROLLERBUTTONUP: { + should_publish = handleControllerButtonUp(e.cbutton); + break; + } + case SDL_CONTROLLERDEVICEADDED: { + handleControllerDeviceAdded(e.cdevice); + break; + } + case SDL_CONTROLLERDEVICEREMOVED: { + handleControllerDeviceRemoved(e.cdevice); + break; + } + case SDL_JOYAXISMOTION: // Ignore joystick events, they are duplicates of CONTROLLERDEVICE. + case SDL_JOYBALLMOTION: + case SDL_JOYHATMOTION: + case SDL_JOYBUTTONDOWN: + case SDL_JOYBUTTONUP: + case SDL_JOYDEVICEADDED: + case SDL_JOYDEVICEREMOVED: { + break; + } + default: { + RCLCPP_INFO(get_logger(), "Unknown event type %d", e.type); + break; + } + } + } else { + // We didn't succeed, either because of a failure or because of a timeout. + // If we are autorepeating and enough time has passed, set should_publish. + rclcpp::Time now = this->now(); + rclcpp::Duration diff_since_last_publish = now - last_publish; + if ((autorepeat_rate_ > 0.0 && + RCL_NS_TO_MS(diff_since_last_publish.nanoseconds()) >= autorepeat_interval_ms_) || + publish_soon_) + { + last_publish = now; + should_publish = true; + publish_soon_ = false; + } + } + + if (game_controller_ != nullptr && should_publish) { + joy_msg_.header.frame_id = "joy"; + joy_msg_.header.stamp = this->now(); + + pub_->publish(joy_msg_); + } + + status = future_.wait_for(std::chrono::seconds(0)); + } while (status == std::future_status::timeout); +} + +} // namespace joy + +RCLCPP_COMPONENTS_REGISTER_NODE(joy::GameController) diff --git a/joy/src/joy_enumerate_devices.cpp b/joy/src/joy_enumerate_devices.cpp index 4c1ab3cb..a3c07c93 100644 --- a/joy/src/joy_enumerate_devices.cpp +++ b/joy/src/joy_enumerate_devices.cpp @@ -1,5 +1,6 @@ /* * Copyright (c) 2020, Open Source Robotics Foundation. + * Copyright (c) 2023, CSIRO Data61. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -34,20 +35,48 @@ #include #include -int main(int argc, char ** argv) +int main() { - (void)argc; - (void)argv; - - if (SDL_Init(SDL_INIT_JOYSTICK | SDL_INIT_HAPTIC) < 0) { + // SDL_INIT_GAMECONTROLLER implies SDL_INIT_JOYSTICK + if (SDL_Init(SDL_INIT_GAMECONTROLLER | SDL_INIT_HAPTIC) < 0) { fprintf(stderr, "SDL could not be initialized: %s\n", SDL_GetError()); return 1; } - fprintf(stdout, "Joystick Device ID : Joystick Device Name\n"); - fprintf(stdout, "-----------------------------------------\n"); + + fprintf( + stdout, + "ID : GUID : GamePad : Mapped : Joystick Device Name\n"); + fprintf( + stdout, + "-------------------------------------------------------------------------------\n"); for (int i = 0; i < SDL_NumJoysticks(); ++i) { - fprintf(stdout, "%18d : %s\n", i, SDL_JoystickNameForIndex(i)); + SDL_JoystickGUID joystick_guid; + const char * joystick_name = "Unknown"; + const char * has_mapping_string = "false"; + const char * is_gamepad = "false"; + + if (SDL_IsGameController(i)) { + SDL_GameController * controller = SDL_GameControllerOpen(i); + joystick_guid = SDL_JoystickGetGUID(SDL_GameControllerGetJoystick(controller)); + joystick_name = SDL_GameControllerName(controller); + if (nullptr != SDL_GameControllerMapping(controller)) { + has_mapping_string = "true"; + } + is_gamepad = "true"; + + } else { + joystick_name = SDL_JoystickNameForIndex(i); + joystick_guid = SDL_JoystickGetDeviceGUID(i); + } + + char guid_string[33]; + SDL_JoystickGetGUIDString(joystick_guid, guid_string, sizeof(guid_string)); + + fprintf( + stdout, "%2d : %32s : %7s : %6s : %s\n", i, guid_string, is_gamepad, has_mapping_string, + joystick_name); } + SDL_Quit(); return 0; diff --git a/joy/udev/99-xbox-wireless-controller.rules b/joy/udev/99-xbox-wireless-controller.rules new file mode 100644 index 00000000..576ee94b --- /dev/null +++ b/joy/udev/99-xbox-wireless-controller.rules @@ -0,0 +1 @@ +KERNEL=="event[0-9]*", SUBSYSTEM=="input", ATTRS{name}=="Xbox Wireless Controller", ACTION=="add", SYMLINK+="xbox_wireless_controller", MODE="666"