diff --git a/patch/dependencies.yaml b/patch/dependencies.yaml index 8d34af42f..92807dbc5 100644 --- a/patch/dependencies.yaml +++ b/patch/dependencies.yaml @@ -137,3 +137,5 @@ resource_retriever: add_run: ["libcurl"] plotjuggler: add_host: ["${{ 'elfutils' if linux }}", "protobuf"] +joy: + add_host: ["sdl2"] \ No newline at end of file diff --git a/patch/ros-noetic-joy.patch b/patch/ros-noetic-joy.patch new file mode 100644 index 000000000..d44de3a53 --- /dev/null +++ b/patch/ros-noetic-joy.patch @@ -0,0 +1,401 @@ +diff --git a/joy/CMakeLists.txt b/joy/CMakeLists.txt +index d06f9b0..4311bee 100644 +--- a/joy/CMakeLists.txt ++++ b/joy/CMakeLists.txt +@@ -4,33 +4,61 @@ project(joy) + # Load catkin and all dependencies required for this package + set(CATKIN_DEPS roscpp diagnostic_updater sensor_msgs roslint) + find_package(catkin REQUIRED ${CATKIN_DEPS}) +- + roslint_cpp() +- + catkin_package(DEPENDS sensor_msgs) + +-# Look for +-include(CheckIncludeFiles) +-check_include_files(linux/joystick.h HAVE_LINUX_JOYSTICK_H) ++include_directories(msg/cpp ${catkin_INCLUDE_DIRS}) + + if(CATKIN_ENABLE_TESTING) + roslint_add_test() + endif() + +-if(HAVE_LINUX_JOYSTICK_H) +- include_directories(msg/cpp ${catkin_INCLUDE_DIRS}) ++if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux") ++ # Look for ++ include(CheckIncludeFiles) ++ check_include_files(linux/joystick.h HAVE_LINUX_JOYSTICK_H) ++endif() ++ ++if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux" AND HAVE_LINUX_JOYSTICK_H) + add_executable(joy_node src/joy_node.cpp) + target_link_libraries(joy_node ${catkin_LIBRARIES}) +- +-# Install targets +- install(TARGETS joy_node +- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +- RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + else() +- message("Warning: no ; won't build joy node") ++ find_package(SDL2 REQUIRED) ++ add_executable(joy_node src/joy_node_sdl.cpp) ++ ++ set_target_properties(SDL2::SDL2 PROPERTIES ++ MAP_IMPORTED_CONFIG_MINSIZEREL Release ++ MAP_IMPORTED_CONFIG_RELWITHDEBINFO Release ++ ) ++ ++ set_target_properties(SDL2::SDL2main PROPERTIES ++ MAP_IMPORTED_CONFIG_MINSIZEREL Release ++ MAP_IMPORTED_CONFIG_RELWITHDEBINFO Release ++ ) ++ ++ target_link_libraries(joy_node ++ ${catkin_LIBRARIES} ++ SDL2::SDL2 SDL2::SDL2main ++ ) ++ ++ file(DOWNLOAD ++ https://raw.githubusercontent.com/mdqinc/SDL_GameControllerDB/master/gamecontrollerdb.txt ++ ${PROJECT_SOURCE_DIR}/launch/gamecontrollerdb.txt ++ SHOW_PROGRESS ++ ) ++ ++ install(DIRECTORY launch/ ++ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch ++ FILES_MATCHING PATTERN "*.launch" ++ FILES_MATCHING PATTERN "gamecontrollerdb.txt" ++ ) + endif() + +-install(DIRECTORY migration_rules scripts config launch ++install(TARGETS joy_node ++ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ++ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ++ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) ++ ++install(DIRECTORY migration_rules + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +- USE_SOURCE_PERMISSIONS) ++ ) +\ No newline at end of file +diff --git a/joy/launch/joy.launch b/joy/launch/joy.launch +new file mode 100644 +index 0000000..d51a4fb +--- /dev/null ++++ b/joy/launch/joy.launch +@@ -0,0 +1,6 @@ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/joy/package.xml b/joy/package.xml +index 2b1d8e0..891fc1b 100644 +--- a/joy/package.xml ++++ b/joy/package.xml +@@ -28,6 +28,8 @@ + joystick + roscpp + sensor_msgs ++ ++ sdl2 + + rosbag + +diff --git a/joy/src/joy_node_sdl.cpp b/joy/src/joy_node_sdl.cpp +new file mode 100644 +index 0000000..c55a397 +--- /dev/null ++++ b/joy/src/joy_node_sdl.cpp +@@ -0,0 +1,285 @@ ++/* ++The MIT License (MIT) ++ ++Copyright (c) 2016 ++ ++Permission is hereby granted, free of charge, to any person obtaining a copy ++of this software and associated documentation files (the "Software"), to deal ++in the Software without restriction, including without limitation the rights ++to use, copy, modify, merge, publish, distribute, sublicense, and/or sell ++copies of the Software, and to permit persons to whom the Software is ++furnished to do so, subject to the following conditions: ++ ++The above copyright notice and this permission notice shall be included in all ++copies or substantial portions of the Software. ++ ++THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR ++IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, ++FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE ++AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER ++LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, ++OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE ++SOFTWARE. ++*/ ++ ++#include "ros/ros.h" ++#include ++#include ++#include ++ ++ ++#include "SDL.h" ++#include "SDL_gamecontroller.h" ++ ++ ++class Joystick ++{ ++ ros::NodeHandle _joyNodeHandle; ++ ros::NodeHandle _joyNodeHandlePrivate; ++ bool _sticky_buttons; ++ bool _default_trig_val; ++ bool _ackermann; ++ double _deadzone; ++ double _autorepeat_rate; // in Hz. 0 for no repeat. ++ double _coalesce_interval; // Defaults to 100 Hz rate limit. ++ double _unscaled_deadzone; ++ int _pub_count; ++ int _vid_param; ++ int _pid_param; ++ int _vid_current; ++ int _pid_current; ++ ros::Publisher _joystickPublisher; ++ std::string _mappingsFile; ++ ++ SDL_Joystick* _gameController = nullptr; ++ int16_t _gameControllerIndex; ++ ++ public: ++ Joystick() ++ : _joyNodeHandle() ++ , _gameControllerIndex(-1) ++ , _joyNodeHandlePrivate("~") ++ {} ++ ++ ///\brief Opens joystick port, reads from port and publishes while node is active ++ int main(int argc, char **argv) ++ { ++ _joyNodeHandlePrivate.param("vid", _vid_param, 0); ++ _joyNodeHandlePrivate.param("pid", _pid_param, 0); ++ _joyNodeHandlePrivate.param("deadzone", _deadzone, 0.05); ++ _joyNodeHandlePrivate.param("autorepeat_rate", _autorepeat_rate, 0); ++ _joyNodeHandlePrivate.param("coalesce_interval", _coalesce_interval, 0.001); ++ _joyNodeHandlePrivate.param("default_trig_val", _default_trig_val,false); ++ _joyNodeHandlePrivate.param("sticky_buttons", _sticky_buttons, false); ++ std::string defaultMappings = ros::package::getPath("joy") + "/launch/gamecontrollerdb.txt"; ++ _joyNodeHandlePrivate.param("mappings", _mappingsFile, defaultMappings); ++ ++ if (_coalesce_interval < 0) ++ { ++ ROS_WARN("joy_node: coalesce_interval (%f) less than 0, setting to 0.", _coalesce_interval); ++ _coalesce_interval = 0; ++ } ++ ++ if (_coalesce_interval != 0 && (_autorepeat_rate > 1 / _coalesce_interval)) ++ { ++ ROS_WARN("joy_node: autorepeat_rate (%f Hz) > 1/coalesce_interval (%f Hz) does not make sense. Timing behavior is not well defined.", _autorepeat_rate, 1/_coalesce_interval); ++ } ++ ++ if (_deadzone >= 1) ++ { ++ ROS_WARN("joy_node: deadzone greater than 1 was requested. The semantics of deadzone have changed. It is now related to the range [-1:1] instead of [-32767:32767]. For now I am dividing your deadzone by 32767, but this behavior is deprecated so you need to update your launch file."); ++ _deadzone /= 32767; ++ } ++ ++ if (_deadzone > 0.9) ++ { ++ ROS_WARN("joy_node: deadzone (%f) greater than 0.9, setting it to 0.9", _deadzone); ++ _deadzone = 0.9; ++ } ++ ++ if (_deadzone < 0) ++ { ++ ROS_WARN("joy_node: deadzone_ (%f) less than 0, setting to 0.", _deadzone); ++ _deadzone = 0; ++ } ++ ++ if (_autorepeat_rate < 0) ++ { ++ ROS_WARN("joy_node: autorepeat_rate (%f) less than 0, setting to 0.", _autorepeat_rate); ++ _autorepeat_rate = 0; ++ } ++ ++ _joystickPublisher = _joyNodeHandle.advertise("joy", 1); ++ ++ ++ // Parameter conversions ++ double autorepeat_interval = 1 / _autorepeat_rate; ++ double scale = -1. / (1. - _deadzone) / 32767.; ++ _unscaled_deadzone = 32767. * _deadzone; ++ ++ // Rate is measured in Hz ++ ros::Rate loop_rate(100); ++ ros::Rate nojoy_rate(1); ++ sensor_msgs::Joy joy_msg; ++ sensor_msgs::Joy last_published_joy_msg; // used for sticky buttons option ++ sensor_msgs::Joy sticky_buttons_joy_msg; // used for sticky buttons option ++ ++ joy_msg.buttons.resize(SDL_CONTROLLER_BUTTON_MAX, 0); ++ last_published_joy_msg.buttons.resize(SDL_CONTROLLER_BUTTON_MAX, 0); ++ sticky_buttons_joy_msg.buttons.resize(SDL_CONTROLLER_BUTTON_MAX, 0); ++ ++ joy_msg.axes.resize(SDL_CONTROLLER_AXIS_MAX, 0); ++ last_published_joy_msg.axes.resize(SDL_CONTROLLER_AXIS_MAX, 0); ++ sticky_buttons_joy_msg.axes.resize(SDL_CONTROLLER_AXIS_MAX, 0); ++ ++ //Initialize SDL ++ if (SDL_Init(SDL_INIT_JOYSTICK | SDL_INIT_GAMECONTROLLER) < 0) ++ { ++ ROS_ERROR( "SDL could not initialize! SDL Error: %s\n", SDL_GetError() ); ++ ++ // todo - is this a recoverable error? ++ return 0; ++ } ++ ++ if (!_mappingsFile.empty()) ++ { ++ if (SDL_GameControllerAddMappingsFromFile(_mappingsFile.c_str()) < 0) ++ { ++ ROS_WARN( "SDL could not initialize mapping file.\n%s\n SDL Error: %s\n", _mappingsFile.c_str(), SDL_GetError() ); ++ // not fatal. ++ } ++ } ++ ++ while (ros::ok()) ++ { ++ ++ if (_gameController == nullptr) ++ { ++ SDL_JoystickUpdate(); ++ int nJoysticks = SDL_NumJoysticks(); ++ for (int i = 0; i < nJoysticks; i++) ++ { ++ if (SDL_IsGameController(i)) ++ { ++ _gameController = SDL_JoystickOpen(i); ++ if (_gameController == nullptr) ++ { ++ ROS_ERROR( "SDL reported a game controller, but would not open it! SDL Error: %s\n", SDL_GetError() ); ++ } ++ else ++ { ++ _gameControllerIndex = i; ++ break; ++ } ++ } ++ } ++ } ++ ++ if (_gameController != nullptr) ++ { ++ SDL_Event e; ++ while (SDL_PollEvent(&e)) ++ { ++ switch (e.type) ++ { ++ case SDL_JOYAXISMOTION: ++ { ++ //Motion on controller 0 ++ if (e.jaxis.which == _gameControllerIndex) ++ { ++ float value = e.jaxis.value; ++ if (value > _unscaled_deadzone) ++ { ++ value -= _unscaled_deadzone; ++ } ++ else if (value < -_unscaled_deadzone) ++ { ++ value += _unscaled_deadzone; ++ } ++ else ++ { ++ value = 0; ++ } ++ ++ joy_msg.axes[e.jaxis.axis] = value * scale; ++ } ++ } ++ break; ++ ++ case SDL_JOYBUTTONDOWN: ++ { ++ joy_msg.buttons[e.jbutton.button] = 1.0; ++ } ++ break; ++ case SDL_JOYBUTTONUP: ++ { ++ joy_msg.buttons[e.jbutton.button] = 0.0; ++ } ++ break; ++ ++ case SDL_JOYDEVICEREMOVED: ++ { ++ if (e.jaxis.which == _gameControllerIndex) ++ { ++ _gameControllerIndex = -1; ++ SDL_JoystickClose(_gameController); ++ _gameController = nullptr; ++ } ++ } ++ break; ++ } ++ } ++ ++ joy_msg.header.stamp = ros::Time().now(); ++ _joystickPublisher.publish(joy_msg); ++ ++ SDL_JoystickUpdate(); ++ } ++ ++ ros::spinOnce(); ++ ++ if (_gameController != nullptr) ++ { ++ loop_rate.sleep(); ++ } ++ else ++ { ++ nojoy_rate.sleep(); ++ } ++ } ++ ++ if (_gameController != nullptr) ++ { ++ SDL_JoystickClose( _gameController ); ++ _gameController = nullptr; ++ } ++ ++ return 0; ++ } ++ uint32_t getScaledDeadzone(uint32_t val) ++ { ++ if (val > _unscaled_deadzone) ++ { ++ val -= _unscaled_deadzone; ++ } ++ else if (val < -_unscaled_deadzone) ++ { ++ val += _unscaled_deadzone; ++ } ++ else ++ { ++ val = 0; ++ } ++ return val; ++ } ++ ++}; ++ ++ ++int main(int argc, char **argv) ++{ ++ ros::init(argc, argv, "joy_node"); ++ ++ Joystick j; ++ return j.main(argc, argv); ++} +\ No newline at end of file