├── cybergear_driver_msgs ├── msg │ ├── SetpointStamped.msg │ ├── SetpointWithGain.msg │ ├── Setpoint.msg │ └── SetpointWithGainStamped.msg ├── package.xml ├── LICENSE └── CMakeLists.txt ├── cybergear_servo_description ├── meshes │ ├── cybergear_body.stl │ └── cybergear_table.stl ├── urdf │ ├── cybergear_servo.urdf.xacro │ └── cybergear_servo.xacro ├── package.xml ├── LICENSE ├── CMakeLists.txt ├── launch │ └── demo_cybergear_servo.launch.py └── rviz │ └── demo.rviz ├── cybergear_driver_core ├── cmake │ ├── cybergear_driver_coreConfigVersion.cmake.in │ └── cybergear_driver_coreConfig.cmake.in ├── package.xml ├── LICENSE ├── include │ └── cybergear_driver_core │ │ ├── cybergear_driver_core.hpp │ │ ├── cybergear_packet_param.hpp │ │ ├── scaled_float_byte_converter.hpp │ │ ├── protocol_constant.hpp │ │ ├── bounded_float_byte_converter.hpp │ │ ├── cybergear_frame_id.hpp │ │ └── cybergear_packet.hpp └── CMakeLists.txt ├── .gitignore ├── README.md ├── cybergear_maintenance_tools ├── package.xml ├── LICENSE ├── src │ ├── search_cybergear_id_node_parameters.yaml │ ├── change_cybergear_id_node_parameters.yaml │ ├── change_cybergear_id_node.cpp │ └── search_cybergear_id_node.cpp └── CMakeLists.txt ├── cybergear_socketcan_driver ├── LICENSE ├── package.xml ├── include │ └── cybergear_socketcan_driver │ │ ├── single_joint_trajectory_point.hpp │ │ └── single_joint_trajectory_points.hpp ├── CMakeLists.txt ├── README.md ├── src │ ├── cybergear_velocity_driver_node.cpp │ ├── cybergear_torque_driver_node.cpp │ ├── cybergear_position_driver_node.cpp │ ├── cybergear_socketcan_driver_node_params.yaml │ ├── cybergear_default_driver_node.cpp │ ├── cybergear_socketcan_driver_node.hpp │ ├── single_joint_trajectory_points.cpp │ └── cybergear_socketcan_driver_node.cpp └── node_parameters.md └── LICENSE /cybergear_driver_msgs/msg/SetpointStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | Setpoint point 3 | -------------------------------------------------------------------------------- /cybergear_driver_msgs/msg/SetpointWithGain.msg: -------------------------------------------------------------------------------- 1 | Setpoint point 2 | float32 kp 3 | float32 kd 4 | -------------------------------------------------------------------------------- /cybergear_driver_msgs/msg/Setpoint.msg: -------------------------------------------------------------------------------- 1 | float32 position 2 | float32 velocity 3 | float32 effort 4 | -------------------------------------------------------------------------------- /cybergear_driver_msgs/msg/SetpointWithGainStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | SetpointWithGain point_with_gain 3 | -------------------------------------------------------------------------------- /cybergear_servo_description/meshes/cybergear_body.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NaokiTakahashi12/cybergear_ros2/HEAD/cybergear_servo_description/meshes/cybergear_body.stl -------------------------------------------------------------------------------- /cybergear_servo_description/meshes/cybergear_table.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NaokiTakahashi12/cybergear_ros2/HEAD/cybergear_servo_description/meshes/cybergear_table.stl -------------------------------------------------------------------------------- /cybergear_driver_core/cmake/cybergear_driver_coreConfigVersion.cmake.in: -------------------------------------------------------------------------------- 1 | set(PACKAGE_VERSION "@cybergear_driver_core_VERSION@") 2 | 3 | # Check whether the requested PACKAGE_FIND_VERSION is compatible 4 | if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") 5 | set(PACKAGE_VERSION_COMPATIBLE FALSE) 6 | else() 7 | set(PACKAGE_VERSION_COMPATIBLE TRUE) 8 | if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}") 9 | set(PACKAGE_VERSION_EXACT TRUE) 10 | endif() 11 | endif() 12 | -------------------------------------------------------------------------------- /cybergear_servo_description/urdf/cybergear_servo.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /cybergear_driver_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cybergear_driver_core 5 | 0.0.0 6 | TODO: Package description 7 | Naoki Takahashi 8 | MIT 9 | 10 | 11 | cmake 12 | 13 | 14 | -------------------------------------------------------------------------------- /cybergear_driver_core/cmake/cybergear_driver_coreConfig.cmake.in: -------------------------------------------------------------------------------- 1 | # - Config file for the cybergear_driver_core package 2 | # It defines the following variables 3 | # cybergear_driver_core_INCLUDE_DIRS - include directories for FooBar 4 | # cybergear_driver_core_LIBRARIES - libraries to link against 5 | # cybergear_driver_core_EXECUTABLE - the bar executable 6 | 7 | # set(cybergear_driver_core_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@") 8 | 9 | # Our library dependencies (contains definitions for IMPORTED targets) 10 | # include("${cybergear_driver_core_DIR}/export_cybergear_driver_core.cmake") 11 | 12 | # These are IMPORTED targets created by cybergear_driver_coreTargets.cmake 13 | 14 | include(${CMAKE_CURRENT_LIST_DIR}/cybergear_driver_coreTargets.cmake) 15 | 16 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # cybergear_ros2 2 | 3 | This repository is for controlling CyberGear servo motors with ROS 2. 4 | Currently, it includes the following packages: 5 | 6 | + [cybergear_driver_core](https://github.com/NaokiTakahashi12/cybergear_ros2/tree/main/cybergear_driver_core) 7 | 8 | This package provides manipulation of CyberGear's CAN frames independently of ROS 2. 9 | 10 | + [cybergear_driver_msgs](https://github.com/NaokiTakahashi12/cybergear_ros2/tree/main/cybergear_driver_msgs) 11 | 12 | This package provides messages used by the cybergear_socketcan_driver. 13 | 14 | + [cybergear_socketcan_driver](https://github.com/NaokiTakahashi12/cybergear_ros2/tree/main/cybergear_socketcan_driver) 15 | 16 | This package provides functionality to operate CyberGear using CAN frame messages. 17 | 18 | -------------------------------------------------------------------------------- /cybergear_servo_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cybergear_servo_description 5 | 0.0.0 6 | TODO: Package description 7 | Naoki Takahashi 8 | MIT 9 | 10 | ament_cmake 11 | 12 | robot_state_publisher 13 | joint_state_publisher 14 | 15 | ament_lint_auto 16 | ament_lint_common 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /cybergear_driver_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cybergear_driver_msgs 5 | 0.0.1 6 | Messages for the CyberGear driver using SocketCAN 7 | Naoki Takahashi 8 | MIT 9 | ament_cmake 10 | ament_cmake_auto 11 | rosidl_default_generators 12 | builtin_interfaces 13 | std_msgs 14 | ament_lint_auto 15 | ament_lint_common 16 | rosidl_interface_packages 17 | 18 | ament_cmake 19 | 20 | 21 | -------------------------------------------------------------------------------- /cybergear_maintenance_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cybergear_maintenance_tools 5 | 0.0.0 6 | TODO: Package description 7 | Naoki Takahashi 8 | MIT 9 | 10 | ament_cmake 11 | ament_cmake_auto 12 | 13 | std_msgs 14 | can_msgs 15 | rclcpp 16 | rclcpp_components 17 | 18 | generate_parameter_library 19 | cybergear_driver_core 20 | 21 | ros2_socketcan 22 | 23 | ament_lint_auto 24 | ament_lint_common 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /cybergear_driver_core/LICENSE: -------------------------------------------------------------------------------- 1 | Permission is hereby granted, free of charge, to any person obtaining a copy 2 | of this software and associated documentation files (the "Software"), to deal 3 | in the Software without restriction, including without limitation the rights 4 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 5 | copies of the Software, and to permit persons to whom the Software is 6 | furnished to do so, subject to the following conditions: 7 | 8 | The above copyright notice and this permission notice shall be included in 9 | all copies or substantial portions of the Software. 10 | 11 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 12 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 13 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 14 | THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 15 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17 | THE SOFTWARE. 18 | -------------------------------------------------------------------------------- /cybergear_driver_msgs/LICENSE: -------------------------------------------------------------------------------- 1 | Permission is hereby granted, free of charge, to any person obtaining a copy 2 | of this software and associated documentation files (the "Software"), to deal 3 | in the Software without restriction, including without limitation the rights 4 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 5 | copies of the Software, and to permit persons to whom the Software is 6 | furnished to do so, subject to the following conditions: 7 | 8 | The above copyright notice and this permission notice shall be included in 9 | all copies or substantial portions of the Software. 10 | 11 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 12 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 13 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 14 | THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 15 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17 | THE SOFTWARE. 18 | -------------------------------------------------------------------------------- /cybergear_socketcan_driver/LICENSE: -------------------------------------------------------------------------------- 1 | Permission is hereby granted, free of charge, to any person obtaining a copy 2 | of this software and associated documentation files (the "Software"), to deal 3 | in the Software without restriction, including without limitation the rights 4 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 5 | copies of the Software, and to permit persons to whom the Software is 6 | furnished to do so, subject to the following conditions: 7 | 8 | The above copyright notice and this permission notice shall be included in 9 | all copies or substantial portions of the Software. 10 | 11 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 12 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 13 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 14 | THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 15 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17 | THE SOFTWARE. 18 | -------------------------------------------------------------------------------- /cybergear_maintenance_tools/LICENSE: -------------------------------------------------------------------------------- 1 | Permission is hereby granted, free of charge, to any person obtaining a copy 2 | of this software and associated documentation files (the "Software"), to deal 3 | in the Software without restriction, including without limitation the rights 4 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 5 | copies of the Software, and to permit persons to whom the Software is 6 | furnished to do so, subject to the following conditions: 7 | 8 | The above copyright notice and this permission notice shall be included in 9 | all copies or substantial portions of the Software. 10 | 11 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 12 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 13 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 14 | THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 15 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17 | THE SOFTWARE. 18 | -------------------------------------------------------------------------------- /cybergear_servo_description/LICENSE: -------------------------------------------------------------------------------- 1 | Permission is hereby granted, free of charge, to any person obtaining a copy 2 | of this software and associated documentation files (the "Software"), to deal 3 | in the Software without restriction, including without limitation the rights 4 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 5 | copies of the Software, and to permit persons to whom the Software is 6 | furnished to do so, subject to the following conditions: 7 | 8 | The above copyright notice and this permission notice shall be included in 9 | all copies or substantial portions of the Software. 10 | 11 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 12 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 13 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 14 | THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 15 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17 | THE SOFTWARE. 18 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Naoki Takahashi 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /cybergear_servo_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cybergear_servo_description) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | # uncomment the following section in order to fill in 11 | # further dependencies manually. 12 | # find_package( REQUIRED) 13 | 14 | install( 15 | DIRECTORY 16 | meshes 17 | urdf 18 | launch 19 | rviz 20 | DESTINATION "share/${PROJECT_NAME}" 21 | ) 22 | if(BUILD_TESTING) 23 | find_package(ament_lint_auto REQUIRED) 24 | # the following line skips the linter which checks for copyrights 25 | # comment the line when a copyright and license is added to all source files 26 | # set(ament_cmake_copyright_FOUND TRUE) 27 | # the following line skips cpplint (only works in a git repo) 28 | # comment the line when this package is in a git repo and when 29 | # a copyright and license is added to all source files 30 | # set(ament_cmake_cpplint_FOUND TRUE) 31 | ament_lint_auto_find_test_dependencies() 32 | endif() 33 | 34 | ament_package() 35 | -------------------------------------------------------------------------------- /cybergear_driver_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cybergear_driver_msgs) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_auto REQUIRED) 11 | 12 | ament_auto_find_build_dependencies() 13 | 14 | rosidl_generate_interfaces(${PROJECT_NAME} 15 | msg/Setpoint.msg 16 | msg/SetpointStamped.msg 17 | msg/SetpointWithGain.msg 18 | msg/SetpointWithGainStamped.msg 19 | DEPENDENCIES 20 | builtin_interfaces 21 | std_msgs 22 | ) 23 | 24 | ament_export_dependencies(rosidl_default_runtime) 25 | 26 | if(BUILD_TESTING) 27 | find_package(ament_lint_auto REQUIRED) 28 | # the following line skips the linter which checks for copyrights 29 | # comment the line when a copyright and license is added to all source files 30 | set(ament_cmake_copyright_FOUND TRUE) 31 | # the following line skips cpplint (only works in a git repo) 32 | # comment the line when this package is in a git repo and when 33 | # a copyright and license is added to all source files 34 | set(ament_cmake_cpplint_FOUND TRUE) 35 | ament_lint_auto_find_test_dependencies() 36 | endif() 37 | 38 | ament_package() 39 | -------------------------------------------------------------------------------- /cybergear_socketcan_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cybergear_socketcan_driver 5 | 0.0.1 6 | CyberGear driver using SocketCAN 7 | Naoki Takahashi 8 | MIT 9 | ament_cmake 10 | ament_cmake_auto 11 | builtin_interfaces 12 | std_msgs 13 | sensor_msgs 14 | trajectory_msgs 15 | diagnostic_msgs 16 | can_msgs 17 | cybergear_driver_msgs 18 | std_srvs 19 | rclcpp 20 | rclcpp_components 21 | diagnostic_updater 22 | generate_parameter_library 23 | cybergear_driver_core 24 | ros2_socketcan 25 | ament_lint_auto 26 | ament_lint_common 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /cybergear_driver_core/include/cybergear_driver_core/cybergear_driver_core.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #pragma once 24 | 25 | #include "bounded_float_byte_converter.hpp" 26 | #include "cybergear_frame_id.hpp" 27 | #include "cybergear_packet.hpp" 28 | #include "cybergear_packet_param.hpp" 29 | #include "protocol_constant.hpp" 30 | #include "scaled_float_byte_converter.hpp" 31 | -------------------------------------------------------------------------------- /cybergear_maintenance_tools/src/search_cybergear_id_node_parameters.yaml: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2024 Naoki Takahashi 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | search_cybergear_id_node: 23 | primary_id: 24 | type: int 25 | default_value: 0 26 | validation: 27 | bounds<>: [0, 255] 28 | send_frequency: 29 | type: double 30 | default_value: 64.0 31 | validation: 32 | gt<>: 0.0 33 | wait_recive_can_frame: 34 | type: double 35 | default_value: 0.1 36 | description: Wait recive CAN msg (seconds) 37 | validation: 38 | gt<>: 0.0 39 | -------------------------------------------------------------------------------- /cybergear_socketcan_driver/include/cybergear_socketcan_driver/single_joint_trajectory_point.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | namespace cybergear_socketcan_driver 28 | { 29 | struct SingleJointTrajectoryPoint 30 | { 31 | SingleJointTrajectoryPoint() 32 | : position(0.0F), 33 | velocity(0.0F), 34 | acceleration(0.0F), 35 | effort(0.0F) 36 | 37 | {} 38 | 39 | float position; 40 | float velocity; 41 | float acceleration; 42 | float effort; 43 | builtin_interfaces::msg::Duration time_from_start; 44 | }; 45 | } // namespace cybergear_socketcan_driver 46 | -------------------------------------------------------------------------------- /cybergear_maintenance_tools/src/change_cybergear_id_node_parameters.yaml: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2024 Naoki Takahashi 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | change_cybergear_id_node: 23 | primary_id: 24 | type: int 25 | default_value: 0 26 | validation: 27 | bounds<>: [0, 127] 28 | device_id: 29 | type: int 30 | default_value: 127 31 | validation: 32 | bounds<>: [0, 127] 33 | target_id: 34 | type: int 35 | default_value: 127 36 | validation: 37 | bounds<>: [0, 127] 38 | send_frequency: 39 | type: double 40 | default_value: 64.0 41 | validation: 42 | gt<>: 0.0 43 | wait_recive_can_frame: 44 | type: double 45 | default_value: 0.1 46 | description: Wait recive CAN msg (seconds) 47 | validation: 48 | gt<>: 0.0 49 | -------------------------------------------------------------------------------- /cybergear_servo_description/urdf/cybergear_servo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /cybergear_driver_core/include/cybergear_driver_core/cybergear_packet_param.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #pragma once 24 | 25 | namespace cybergear_driver_core 26 | { 27 | struct CybergearPacketParam 28 | { 29 | int device_id, primary_id; 30 | float max_position, min_position; //!< Angular position [rad] 31 | float max_velocity, min_velocity; //!< Angular velocity [rad/s] 32 | float max_effort, min_effort; //!< Effort [N/m] 33 | float max_gain_kp, min_gain_kp; 34 | float max_gain_kd, min_gain_kd; 35 | float max_current, min_current; //!< Motor current [A] 36 | float temperature_scale; 37 | 38 | CybergearPacketParam() 39 | : device_id(127), 40 | primary_id(0), 41 | max_position(12.56637061), 42 | min_position(-12.56637061), 43 | max_velocity(30.0), 44 | min_velocity(-30.0), 45 | max_effort(12.0), 46 | min_effort(-12.0), 47 | max_gain_kp(500.0), 48 | min_gain_kp(0.0), 49 | max_gain_kd(5.0), 50 | min_gain_kd(0.0), 51 | max_current(23.0), 52 | min_current(-23.0), 53 | temperature_scale(0.1) 54 | {} 55 | }; 56 | } // namespace cybergear_driver_core 57 | -------------------------------------------------------------------------------- /cybergear_maintenance_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cybergear_maintenance_tools) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | # uncomment the following section in order to fill in 11 | # further dependencies manually. 12 | # find_package( REQUIRED) 13 | find_package(ament_cmake_auto REQUIRED) 14 | find_package(cybergear_driver_core REQUIRED) 15 | 16 | ament_auto_find_build_dependencies() 17 | 18 | generate_parameter_library(search_cybergear_id_node_parameters 19 | src/search_cybergear_id_node_parameters.yaml 20 | ) 21 | generate_parameter_library(change_cybergear_id_node_parameters 22 | src/change_cybergear_id_node_parameters.yaml 23 | ) 24 | 25 | ament_auto_add_library(${PROJECT_NAME}_components SHARED 26 | src/search_cybergear_id_node.cpp 27 | src/change_cybergear_id_node.cpp 28 | ) 29 | target_link_libraries(${PROJECT_NAME}_components 30 | search_cybergear_id_node_parameters 31 | cybergear_driver_core 32 | change_cybergear_id_node_parameters 33 | ) 34 | 35 | rclcpp_components_register_node(${PROJECT_NAME}_components 36 | PLUGIN "cybergear_maintenance_tools::SearchCybergearIdNode" 37 | EXECUTABLE search_cybergear_id_node 38 | ) 39 | rclcpp_components_register_node(${PROJECT_NAME}_components 40 | PLUGIN "cybergear_maintenance_tools::ChangeCybergearIdNode" 41 | EXECUTABLE change_cybergear_id_node 42 | ) 43 | 44 | install( 45 | TARGETS 46 | ${PROJECT_NAME}_components 47 | ARCHIVE DESTINATION lib 48 | LIBRARY DESTINATION lib 49 | RUNTIME DESTINATION lib/${PROJECT_NAME} 50 | ) 51 | 52 | if(BUILD_TESTING) 53 | find_package(ament_lint_auto REQUIRED) 54 | # the following line skips the linter which checks for copyrights 55 | # comment the line when a copyright and license is added to all source files 56 | # set(ament_cmake_copyright_FOUND TRUE) 57 | # the following line skips cpplint (only works in a git repo) 58 | # comment the line when this package is in a git repo and when 59 | # a copyright and license is added to all source files 60 | # set(ament_cmake_cpplint_FOUND TRUE) 61 | ament_lint_auto_find_test_dependencies() 62 | endif() 63 | 64 | ament_package() 65 | -------------------------------------------------------------------------------- /cybergear_driver_core/include/cybergear_driver_core/scaled_float_byte_converter.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | namespace cybergear_driver_core 29 | { 30 | // TODO(Naoki Takahashi) endian support 31 | class ScaledFloatByteConverter 32 | { 33 | public: 34 | explicit ScaledFloatByteConverter(const float scale) 35 | : scale_(scale) 36 | {} 37 | 38 | ~ScaledFloatByteConverter() {} 39 | 40 | void setScale(const float scale) 41 | { 42 | scale_ = scale; 43 | } 44 | 45 | uint16_t toDoubleByte(const float value) 46 | { 47 | return static_cast(value / scale_); 48 | } 49 | 50 | std::array toTwoBytes(const float value) 51 | { 52 | const uint16_t scaled_double_byte = toDoubleByte(value); 53 | return { 54 | static_cast((scaled_double_byte & 0xff00) >> 8), 55 | static_cast(scaled_double_byte & 0x00ff)}; 56 | } 57 | 58 | template 59 | float toFloat(const std::array & data, const unsigned int offset) const 60 | { 61 | const uint16_t raw_data = data[0 + offset] << 8 | data[1 + offset]; 62 | return scale_ * static_cast(raw_data); 63 | } 64 | 65 | private: 66 | float scale_; 67 | }; 68 | } // namespace cybergear_driver_core 69 | -------------------------------------------------------------------------------- /cybergear_socketcan_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cybergear_socketcan_driver 3 | VERSION 0.0.0 4 | LANGUAGES 5 | CXX 6 | ) 7 | 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | # find dependencies 13 | find_package(ament_cmake REQUIRED) 14 | find_package(ament_cmake_auto REQUIRED) 15 | find_package(cybergear_driver_core REQUIRED) 16 | 17 | ament_auto_find_build_dependencies() 18 | 19 | generate_parameter_library(cybergear_socketcan_driver_node_params 20 | src/cybergear_socketcan_driver_node_params.yaml 21 | ) 22 | 23 | ament_auto_add_library(${PROJECT_NAME}_components SHARED 24 | src/single_joint_trajectory_points.cpp 25 | src/cybergear_socketcan_driver_node.cpp 26 | src/cybergear_position_driver_node.cpp 27 | src/cybergear_velocity_driver_node.cpp 28 | src/cybergear_torque_driver_node.cpp 29 | src/cybergear_default_driver_node.cpp 30 | ) 31 | target_compile_features(${PROJECT_NAME}_components 32 | PUBLIC 33 | cxx_std_20 34 | ) 35 | target_link_libraries(${PROJECT_NAME}_components 36 | cybergear_socketcan_driver_node_params 37 | cybergear_driver_core 38 | ) 39 | rclcpp_components_register_node(${PROJECT_NAME}_components 40 | PLUGIN "cybergear_socketcan_driver::CybergearSocketCanDriverNode" 41 | EXECUTABLE cybergear_socketcan_driver_node 42 | ) 43 | rclcpp_components_register_node(${PROJECT_NAME}_components 44 | PLUGIN "cybergear_socketcan_driver::CybergearPositionDriverNode" 45 | EXECUTABLE cybergear_position_driver_node 46 | ) 47 | rclcpp_components_register_node(${PROJECT_NAME}_components 48 | PLUGIN "cybergear_socketcan_driver::CybergearVelocityDriverNode" 49 | EXECUTABLE cybergear_velocity_driver_node 50 | ) 51 | rclcpp_components_register_node(${PROJECT_NAME}_components 52 | PLUGIN "cybergear_socketcan_driver::CybergearTorqueDriverNode" 53 | EXECUTABLE cybergear_torque_driver_node 54 | ) 55 | rclcpp_components_register_node(${PROJECT_NAME}_components 56 | PLUGIN "cybergear_socketcan_driver::CybergearDefaultDriverNode" 57 | EXECUTABLE cybergear_default_driver_node 58 | ) 59 | 60 | install( 61 | TARGETS 62 | ${PROJECT_NAME}_components 63 | ARCHIVE DESTINATION lib 64 | LIBRARY DESTINATION lib 65 | RUNTIME DESTINATION lib/${PROJECT_NAME} 66 | ) 67 | 68 | if(BUILD_TESTING) 69 | find_package(ament_lint_auto REQUIRED) 70 | # the following line skips the linter which checks for copyrights 71 | # comment the line when a copyright and license is added to all source files 72 | # set(ament_cmake_copyright_FOUND TRUE) 73 | # the following line skips cpplint (only works in a git repo) 74 | # comment the line when this package is in a git repo and when 75 | # a copyright and license is added to all source files 76 | # set(ament_cmake_cpplint_FOUND TRUE) 77 | ament_lint_auto_find_test_dependencies() 78 | endif() 79 | 80 | ament_auto_package() 81 | -------------------------------------------------------------------------------- /cybergear_driver_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.15) 2 | 3 | set(cybergear_driver_core_MAJOR_VERSION 0) 4 | set(cybergear_driver_core_MINOR_VERSION 0) 5 | set(cybergear_driver_core_PATCH_VERSION 0) 6 | set(cybergear_driver_core_VERSION 7 | ${cybergear_driver_core_MAJOR_VERSION}.${cybergear_driver_core_MINOR_VERSION}.${cybergear_driver_core_PATCH_VERSION}) 8 | 9 | project(cybergear_driver_core 10 | VERSION ${cybergear_driver_core_VERSION} 11 | LANGUAGES 12 | CXX 13 | ) 14 | 15 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 16 | add_compile_options(-Wall -Wextra -Wpedantic) 17 | endif() 18 | 19 | # uncomment the following section in order to fill in 20 | # further dependencies manually. 21 | # find_package( REQUIRED) 22 | 23 | set(CMAKE_INCLUDE_CURRENT_DIR_IN_INTERFACE ON) 24 | 25 | set(public_headers 26 | include/cybergear_driver_core/cybergear_packet_param.hpp 27 | include/cybergear_driver_core/cybergear_frame_id.hpp 28 | include/cybergear_driver_core/cybergear_driver_core.hpp 29 | include/cybergear_driver_core/protocol_constant.hpp 30 | include/cybergear_driver_core/bounded_float_byte_converter.hpp 31 | include/cybergear_driver_core/cybergear_packet.hpp 32 | include/cybergear_driver_core/scaled_float_byte_converter.hpp 33 | ) 34 | add_library(${PROJECT_NAME} INTERFACE) 35 | set_target_properties(${PROJECT_NAME} 36 | PROPERTIES 37 | PUBLIC_HEADER "${public_headers}" 38 | ) 39 | target_include_directories(${PROJECT_NAME} 40 | INTERFACE 41 | $ 42 | $ 43 | ) 44 | target_compile_features(${PROJECT_NAME} 45 | INTERFACE 46 | cxx_std_20 47 | ) 48 | 49 | include(GNUInstallDirs) 50 | include(CMakePackageConfigHelpers) 51 | 52 | export( 53 | TARGETS ${PROJECT_NAME} 54 | FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}Targets.cmake 55 | ) 56 | install( 57 | TARGETS 58 | ${PROJECT_NAME} 59 | EXPORT ${PROJECT_NAME}Targets 60 | PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME} 61 | ) 62 | install( 63 | EXPORT ${PROJECT_NAME}Targets 64 | DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME}/cmake 65 | ) 66 | 67 | configure_package_config_file( 68 | ${CMAKE_CURRENT_SOURCE_DIR}/cmake/${PROJECT_NAME}Config.cmake.in 69 | ${CMAKE_CURRENT_BINARY_DIR}/cmake/${PROJECT_NAME}Config.cmake 70 | INSTALL_DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME}/cmake 71 | ) 72 | configure_package_config_file( 73 | ${CMAKE_CURRENT_SOURCE_DIR}/cmake/${PROJECT_NAME}ConfigVersion.cmake.in 74 | ${CMAKE_CURRENT_BINARY_DIR}/cmake/${PROJECT_NAME}ConfigVersion.cmake 75 | INSTALL_DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME}/cmake 76 | ) 77 | install( 78 | FILES 79 | ${CMAKE_CURRENT_BINARY_DIR}/cmake/${PROJECT_NAME}Config.cmake 80 | ${CMAKE_CURRENT_BINARY_DIR}/cmake/${PROJECT_NAME}ConfigVersion.cmake 81 | DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME}/cmake 82 | ) 83 | 84 | set(CPACK_PACKAGE_NAME ${PROJECT_NAME}) 85 | set(CPACK_PACKAGE_VERSION ${PROJECT_VERSION}) 86 | set(CPACK_RESOURCE_FILE_LICENSE ${CMAKE_CURRENT_SOURCE_DIR}/LICENSE) 87 | 88 | include(CPack) 89 | 90 | -------------------------------------------------------------------------------- /cybergear_socketcan_driver/include/cybergear_socketcan_driver/single_joint_trajectory_points.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "single_joint_trajectory_point.hpp" 35 | 36 | namespace cybergear_socketcan_driver 37 | { 38 | class SingleJointTrajectoryPoints 39 | { 40 | public: 41 | using UniquePtr = std::unique_ptr; 42 | using SharedPtr = std::shared_ptr; 43 | 44 | using Points = std::vector; 45 | 46 | SingleJointTrajectoryPoints(); 47 | SingleJointTrajectoryPoints(const SingleJointTrajectoryPoints &); 48 | SingleJointTrajectoryPoints(SingleJointTrajectoryPoints &&) = delete; 49 | ~SingleJointTrajectoryPoints(); 50 | 51 | void reset(); 52 | 53 | void initTrajectoryPoint(const sensor_msgs::msg::JointState &); 54 | void load(const std::string & joint_name, const trajectory_msgs::msg::JointTrajectory &); 55 | 56 | float getLerpPosition(const builtin_interfaces::msg::Time &) const; 57 | float getLerpVelocity(const builtin_interfaces::msg::Time &) const; 58 | float getLerpEffort(const builtin_interfaces::msg::Time &) const; 59 | 60 | const Points & points() const; 61 | 62 | SingleJointTrajectoryPoints & operator=(const SingleJointTrajectoryPoints &); 63 | SingleJointTrajectoryPoints & operator=(SingleJointTrajectoryPoints &&) = delete; 64 | 65 | private: 66 | SingleJointTrajectoryPoint start_trajectory_point_; 67 | Points trajectory_points_; 68 | 69 | rclcpp::Time start_trajectory_time_; 70 | std::vector trajectory_durations_from_recived_; 71 | 72 | static unsigned int getJointIndexFromJointNames( 73 | const std::string &, const std::vector &); 74 | }; 75 | } // namespace cybergear_socketcan_driver 76 | -------------------------------------------------------------------------------- /cybergear_driver_core/include/cybergear_driver_core/protocol_constant.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | namespace cybergear_driver_core 28 | { 29 | namespace commands 30 | { 31 | constexpr uint8_t INFO = 0; 32 | constexpr uint8_t COMMAND = 1; 33 | constexpr uint8_t FEEDBACK = 2; 34 | constexpr uint8_t ENABLE_TORQUE = 3; 35 | constexpr uint8_t RESET_TORQUE = 4; 36 | constexpr uint8_t ZEROING = 6; 37 | constexpr uint8_t CHANGE_DEVICE_ID = 7; 38 | constexpr uint8_t READ_PARAMETER = 17; 39 | constexpr uint8_t WRITE_PARAMETER = 18; 40 | constexpr uint8_t FAULT_FEEDBACK = 21; 41 | constexpr uint8_t CHANGE_BAUDRATE = 22; 42 | } // namespace commands 43 | 44 | namespace feedback_mode_state 45 | { 46 | constexpr uint8_t RESET = 0; 47 | constexpr uint8_t CALI = 1; 48 | constexpr uint8_t RUN = 2; 49 | } // namespace feedback_mode_state 50 | 51 | namespace status_offset 52 | { 53 | constexpr uint8_t DEVICE_ID = 0; 54 | constexpr uint8_t FAULT = 8; 55 | constexpr uint8_t MODE = 2; 56 | } // namespace status_offset 57 | 58 | // TODO(Naoki Takahashi) address constant header 59 | namespace ram_parameters 60 | { 61 | constexpr uint16_t RUN_MODE = 0x7005; 62 | constexpr uint16_t IQ_REF = 0x7006; 63 | constexpr uint16_t SPEED_REF = 0x700a; 64 | constexpr uint16_t LIMIT_TORQUE = 0x700b; 65 | constexpr uint16_t CURRENT_KP = 0x7010; 66 | constexpr uint16_t CURRENT_KI = 0x7011; 67 | constexpr uint16_t CURRENT_FILTER_GAIN = 0x7014; 68 | constexpr uint16_t DEST_POSITION_REF = 0x7016; 69 | constexpr uint16_t LIMIT_SPEED = 0x7017; 70 | constexpr uint16_t LIMIT_CURRENT = 0x7018; 71 | constexpr uint16_t MECH_POSISION = 0x7019; 72 | constexpr uint16_t IQF = 0x701a; 73 | constexpr uint16_t MECH_VELOCITY = 0x701b; 74 | constexpr uint16_t VBUS = 0x701c; 75 | constexpr uint16_t ROTATION_COUNT = 0x701d; 76 | constexpr uint16_t POSITION_KP = 0x701e; 77 | constexpr uint16_t SPEED_KP = 0x701f; 78 | constexpr uint16_t SPEED_KI = 0x7020; 79 | } // namespace ram_parameters 80 | 81 | namespace run_modes 82 | { 83 | constexpr uint8_t OPERATION = 0; 84 | constexpr uint8_t POSITION = 1; 85 | constexpr uint8_t SPEED = 2; 86 | constexpr uint8_t CURRENT = 3; 87 | } // namespace run_modes 88 | } // namespace cybergear_driver_core 89 | -------------------------------------------------------------------------------- /cybergear_socketcan_driver/README.md: -------------------------------------------------------------------------------- 1 | # cybergear_socketcan_driver 2 | 3 | This package provides functionality to control CyberGear using CAN frame messages. 4 | 5 | The control inputs are compatible with the following: 6 | + Position control 7 | + Velocity control 8 | + Torque control (Current control) 9 | 10 | 11 | ## Basic Usage 12 | 13 | To execute position control, run the following command: 14 | 15 | ```bash 16 | ros2 run cybergear_socketcan_driver cybergear_position_driver_node 17 | ``` 18 | 19 | When launched, it listens for topics as follows: 20 | 21 | ```bash 22 | /cybergear_position_driver 23 | Subscribers: 24 | /from_can_bus: can_msgs/msg/Frame 25 | /joint_trajectory: trajectory_msgs/msg/JointTrajectory 26 | /parameter_events: rcl_interfaces/msg/ParameterEvent 27 | Publishers: 28 | /cybergear_position_driver/joint_state: sensor_msgs/msg/JointState 29 | /cybergear_position_driver/temperature: sensor_msgs/msg/Temperature 30 | /diagnostics: diagnostic_msgs/msg/DiagnosticArray 31 | /parameter_events: rcl_interfaces/msg/ParameterEvent 32 | /rosout: rcl_interfaces/msg/Log 33 | /to_can_bus: can_msgs/msg/Frame 34 | Service Servers: 35 | /cybergear_position_driver/describe_parameters: rcl_interfaces/srv/DescribeParameters 36 | /cybergear_position_driver/enable_torque: std_srvs/srv/SetBool 37 | /cybergear_position_driver/get_parameter_types: rcl_interfaces/srv/GetParameterTypes 38 | /cybergear_position_driver/get_parameters: rcl_interfaces/srv/GetParameters 39 | /cybergear_position_driver/list_parameters: rcl_interfaces/srv/ListParameters 40 | /cybergear_position_driver/set_parameters: rcl_interfaces/srv/SetParameters 41 | /cybergear_position_driver/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically 42 | /cybergear_position_driver/zero_position: std_srvs/srv/Trigger 43 | ``` 44 | 45 | Calling the `~/enable_torque` service toggles the torque of the CyberGear on or off. 46 | Sending a `/joint_trajectory` topic executes position control. 47 | Please refer to the parameters for the joint names at this time. 48 | 49 | 50 | ## Environment 51 | The intended operating environment on the following: 52 | 53 | ```mermaid 54 | graph RL; 55 | subgraph linux 56 | cybergear_position_driver([cybergear_position_driver]) 57 | socket_can_receiver([socket_can_receiver]) 58 | socket_can_sender([socket_can_sender]) 59 | can0((can0)) 60 | end 61 | subgraph devices 62 | cybergear[CyberGear] 63 | can_to_usb[CAN to USB] 64 | end 65 | cybergear ---|CAN| can_to_usb 66 | can_to_usb ---|USB| can0 67 | socket_can_sender --> can0 68 | can0 --> socket_can_receiver 69 | cybergear_position_driver -->|to_can_bus| socket_can_sender 70 | socket_can_receiver -->|from_can_bus| cybergear_position_driver 71 | ``` 72 | 73 | 74 | ## Dependencies 75 | 76 | In addition to the ROS 2 core packages, this package depends on the following: 77 | 78 | + [generate_parameter_library](https://github.com/PickNikRobotics/generate_parameter_library) 79 | + [ros2_socketcan](https://github.com/autowarefoundation/ros2_socketcan) 80 | 81 | 82 | ## Control Mode 83 | 84 | The command values transmitted to CyberGear for each node are shown below. 85 | 86 | + `cybergear_default_driver_node` 87 | 88 | position, velocity, effort 89 | 90 | + `cybergear_position_driver_node` 91 | 92 | position 93 | 94 | + `cybergear_velocity_driver_node` 95 | 96 | velocity 97 | 98 | + `cybergear_torque_driver_node` 99 | 100 | effort 101 | 102 | 103 | ## Parameters 104 | 105 | Please refer to [node_parameters.md](node_parameters.md) for the parameters. 106 | 107 | -------------------------------------------------------------------------------- /cybergear_driver_core/include/cybergear_driver_core/bounded_float_byte_converter.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace cybergear_driver_core 33 | { 34 | // TODO(Naoki Takahashi) endian support 35 | class BoundedFloatByteConverter 36 | { 37 | public: 38 | explicit BoundedFloatByteConverter(const float max, const float min) 39 | { 40 | setRange(max, min); 41 | } 42 | 43 | ~BoundedFloatByteConverter() {} 44 | 45 | void setRange(const float max, const float min) 46 | { 47 | max_ = max; 48 | min_ = min; 49 | updateRange(); 50 | } 51 | 52 | float toClampedFloat(const float value) const 53 | { 54 | return std::max(min_, std::min(max_, value)); 55 | } 56 | 57 | uint16_t toDoubleByte(const float value) const 58 | { 59 | const float clamped_value = toClampedFloat(value); 60 | return static_cast((clamped_value - min_) * byte_scale_); 61 | } 62 | 63 | std::array toTwoBytes(const float value) const 64 | { 65 | const uint16_t scaled_double_byte = toDoubleByte(value); 66 | return { 67 | static_cast((scaled_double_byte & 0xff00) >> 8), 68 | static_cast(scaled_double_byte & 0x00ff)}; 69 | } 70 | 71 | std::array toFourBytes(const float value) const 72 | { 73 | constexpr size_t kArrayLength = 4; 74 | const float clamped_float = toClampedFloat(value); 75 | std::array raw_bytes; 76 | std::memcpy(raw_bytes.data(), &clamped_float, sizeof(uint8_t) * kArrayLength); 77 | return raw_bytes; 78 | } 79 | 80 | template 81 | float toFloat(const std::array & data, const unsigned int offset) const 82 | { 83 | const uint16_t raw_data = data[0 + offset] << 8 | data[1 + offset]; 84 | return float_scale_ * static_cast(raw_data) + min_; 85 | } 86 | 87 | private: 88 | float max_, min_; 89 | float float_range_; 90 | float float_scale_, byte_scale_; 91 | 92 | void updateRange() 93 | { 94 | float_range_ = max_ - min_; 95 | float_scale_ = float_range_ / static_cast(0xffff); 96 | byte_scale_ = static_cast(0xffff) / float_range_; 97 | 98 | if (float_range_ <= 0) { 99 | throw std::invalid_argument("Illigal float range: ZERO or NEGATIVE"); 100 | } 101 | } 102 | }; 103 | } // namespace cybergear_driver_core 104 | -------------------------------------------------------------------------------- /cybergear_socketcan_driver/src/cybergear_velocity_driver_node.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include "cybergear_socketcan_driver_node.hpp" 29 | 30 | namespace cybergear_socketcan_driver 31 | { 32 | class CybergearVelocityDriverNode : public CybergearSocketCanDriverNode 33 | { 34 | public: 35 | CybergearVelocityDriverNode() = delete; 36 | explicit CybergearVelocityDriverNode(const rclcpp::NodeOptions &); 37 | CybergearVelocityDriverNode(const CybergearVelocityDriverNode &) = delete; 38 | CybergearVelocityDriverNode(CybergearVelocityDriverNode &&) = delete; 39 | ~CybergearVelocityDriverNode() override; 40 | 41 | CybergearVelocityDriverNode & operator=(const CybergearVelocityDriverNode &) = delete; 42 | CybergearVelocityDriverNode & operator=(CybergearVelocityDriverNode &&) = delete; 43 | 44 | protected: 45 | void sendCanFrameFromTrajectoryCallback( 46 | can_msgs::msg::Frame &, const SingleJointTrajectoryPoints &) final; 47 | void sendCanFrameFromSetpointCallback( 48 | can_msgs::msg::Frame &, const cybergear_driver_msgs::msg::SetpointStamped &) final; 49 | void sendChangeRunModeCallback(can_msgs::msg::Frame &) final; 50 | }; 51 | 52 | CybergearVelocityDriverNode::CybergearVelocityDriverNode(const rclcpp::NodeOptions & node_options) 53 | : CybergearSocketCanDriverNode("cybergear_velocity_driver", node_options) 54 | {} 55 | 56 | CybergearVelocityDriverNode::~CybergearVelocityDriverNode() {} 57 | 58 | void CybergearVelocityDriverNode::sendCanFrameFromTrajectoryCallback( 59 | can_msgs::msg::Frame & msg, const SingleJointTrajectoryPoints & single_joint_trajectory) 60 | { 61 | float velocity = 0.0F; 62 | 63 | if (!single_joint_trajectory.points().empty()) { 64 | velocity = single_joint_trajectory.getLerpVelocity(this->get_clock()->now()); 65 | } 66 | const auto can_frame = this->packet().createVelocityCommand(velocity); 67 | std::copy(can_frame.data.cbegin(), can_frame.data.cend(), msg.data.begin()); 68 | msg.id = can_frame.id; 69 | } 70 | 71 | void CybergearVelocityDriverNode::sendCanFrameFromSetpointCallback( 72 | can_msgs::msg::Frame & msg, const cybergear_driver_msgs::msg::SetpointStamped & setpoint_msg) 73 | { 74 | const auto can_frame = this->packet().createVelocityCommand(setpoint_msg.point.velocity); 75 | std::copy(can_frame.data.cbegin(), can_frame.data.cend(), msg.data.begin()); 76 | msg.id = can_frame.id; 77 | } 78 | 79 | void CybergearVelocityDriverNode::sendChangeRunModeCallback(can_msgs::msg::Frame & msg) 80 | { 81 | const auto can_frame = this->packet().createChangeToVelocityModeCommand(); 82 | std::copy(can_frame.data.cbegin(), can_frame.data.cend(), msg.data.begin()); 83 | msg.id = can_frame.id; 84 | } 85 | } // namespace cybergear_socketcan_driver 86 | 87 | RCLCPP_COMPONENTS_REGISTER_NODE(cybergear_socketcan_driver::CybergearVelocityDriverNode) 88 | -------------------------------------------------------------------------------- /cybergear_socketcan_driver/src/cybergear_torque_driver_node.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include "cybergear_socketcan_driver_node.hpp" 29 | 30 | namespace cybergear_socketcan_driver 31 | { 32 | class CybergearTorqueDriverNode : public CybergearSocketCanDriverNode 33 | { 34 | public: 35 | CybergearTorqueDriverNode() = delete; 36 | explicit CybergearTorqueDriverNode(const rclcpp::NodeOptions &); 37 | CybergearTorqueDriverNode(const CybergearTorqueDriverNode &) = delete; 38 | CybergearTorqueDriverNode(CybergearTorqueDriverNode &&) = delete; 39 | ~CybergearTorqueDriverNode() override; 40 | 41 | CybergearTorqueDriverNode & operator=(const CybergearTorqueDriverNode &) = delete; 42 | CybergearTorqueDriverNode & operator=(CybergearTorqueDriverNode &&) = delete; 43 | 44 | protected: 45 | void sendCanFrameFromTrajectoryCallback( 46 | can_msgs::msg::Frame &, const SingleJointTrajectoryPoints &) final; 47 | void sendCanFrameFromSetpointCallback( 48 | can_msgs::msg::Frame &, const cybergear_driver_msgs::msg::SetpointStamped &) final; 49 | void sendChangeRunModeCallback(can_msgs::msg::Frame &) final; 50 | 51 | private: 52 | float getDestCurrent(float dest_torque); 53 | }; 54 | 55 | CybergearTorqueDriverNode::CybergearTorqueDriverNode(const rclcpp::NodeOptions & node_options) 56 | : CybergearSocketCanDriverNode("cybergear_torque_driver", node_options) 57 | {} 58 | 59 | CybergearTorqueDriverNode::~CybergearTorqueDriverNode() {} 60 | 61 | void CybergearTorqueDriverNode::sendCanFrameFromTrajectoryCallback( 62 | can_msgs::msg::Frame & msg, const SingleJointTrajectoryPoints & single_joint_trajectory) 63 | { 64 | float effort = 0.0F; 65 | 66 | if (!single_joint_trajectory.points().empty()) { 67 | effort = single_joint_trajectory.getLerpEffort(this->get_clock()->now()); 68 | } 69 | const auto can_frame = this->packet().createCurrentCommand(getDestCurrent(effort)); 70 | std::copy(can_frame.data.cbegin(), can_frame.data.cend(), msg.data.begin()); 71 | msg.id = can_frame.id; 72 | } 73 | 74 | void CybergearTorqueDriverNode::sendCanFrameFromSetpointCallback( 75 | can_msgs::msg::Frame & msg, const cybergear_driver_msgs::msg::SetpointStamped & setpoint_msg) 76 | { 77 | const auto can_frame = 78 | this->packet().createCurrentCommand(getDestCurrent(setpoint_msg.point.effort)); 79 | std::copy(can_frame.data.cbegin(), can_frame.data.cend(), msg.data.begin()); 80 | msg.id = can_frame.id; 81 | } 82 | 83 | void CybergearTorqueDriverNode::sendChangeRunModeCallback(can_msgs::msg::Frame & msg) 84 | { 85 | const auto can_frame = this->packet().createChangeToCurrentModeCommand(); 86 | std::copy(can_frame.data.cbegin(), can_frame.data.cend(), msg.data.begin()); 87 | msg.id = can_frame.id; 88 | } 89 | 90 | float CybergearTorqueDriverNode::getDestCurrent(const float dest_torque) 91 | { 92 | const auto torque_constant = static_cast(this->params().torque_constant); 93 | 94 | if (0 == dest_torque) { 95 | return 0.0F; 96 | } 97 | if (0 == torque_constant) { 98 | return 0.0F; 99 | } 100 | return dest_torque / torque_constant; 101 | } 102 | } // namespace cybergear_socketcan_driver 103 | 104 | RCLCPP_COMPONENTS_REGISTER_NODE(cybergear_socketcan_driver::CybergearTorqueDriverNode) 105 | -------------------------------------------------------------------------------- /cybergear_socketcan_driver/src/cybergear_position_driver_node.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "cybergear_socketcan_driver_node.hpp" 32 | 33 | namespace cybergear_socketcan_driver 34 | { 35 | class CybergearPositionDriverNode : public CybergearSocketCanDriverNode 36 | { 37 | public: 38 | CybergearPositionDriverNode() = delete; 39 | explicit CybergearPositionDriverNode(const rclcpp::NodeOptions &); 40 | CybergearPositionDriverNode(const CybergearPositionDriverNode &) = delete; 41 | CybergearPositionDriverNode(CybergearPositionDriverNode &&) = delete; 42 | ~CybergearPositionDriverNode() override; 43 | 44 | CybergearPositionDriverNode & operator=(const CybergearPositionDriverNode &) = delete; 45 | CybergearPositionDriverNode & operator=(CybergearPositionDriverNode &&) = delete; 46 | 47 | protected: 48 | void procFeedbackJointStateCallback(const sensor_msgs::msg::JointState &) final; 49 | void sendCanFrameFromTrajectoryCallback( 50 | can_msgs::msg::Frame &, const SingleJointTrajectoryPoints &) final; 51 | void sendCanFrameFromSetpointCallback( 52 | can_msgs::msg::Frame &, const cybergear_driver_msgs::msg::SetpointStamped &) final; 53 | void sendChangeRunModeCallback(can_msgs::msg::Frame &) final; 54 | 55 | private: 56 | float last_sense_anguler_position_; 57 | }; 58 | 59 | CybergearPositionDriverNode::CybergearPositionDriverNode(const rclcpp::NodeOptions & node_options) 60 | : CybergearSocketCanDriverNode("cybergear_position_driver", node_options), 61 | last_sense_anguler_position_(0) 62 | {} 63 | 64 | CybergearPositionDriverNode::~CybergearPositionDriverNode() {} 65 | 66 | void CybergearPositionDriverNode::procFeedbackJointStateCallback( 67 | const sensor_msgs::msg::JointState & msg) 68 | { 69 | if (msg.position.empty()) { 70 | return; 71 | } 72 | last_sense_anguler_position_ = static_cast(msg.position[0]); 73 | } 74 | 75 | void CybergearPositionDriverNode::sendCanFrameFromTrajectoryCallback( 76 | can_msgs::msg::Frame & msg, const SingleJointTrajectoryPoints & single_joint_trajectory) 77 | { 78 | float position = NAN; 79 | 80 | if (!single_joint_trajectory.points().empty()) { 81 | position = single_joint_trajectory.getLerpPosition(this->get_clock()->now()); 82 | } else { 83 | position = last_sense_anguler_position_; 84 | } 85 | const auto can_frame = this->packet().createPositionCommand(position); 86 | std::copy(can_frame.data.cbegin(), can_frame.data.cend(), msg.data.begin()); 87 | msg.id = can_frame.id; 88 | } 89 | 90 | void CybergearPositionDriverNode::sendCanFrameFromSetpointCallback( 91 | can_msgs::msg::Frame & msg, const cybergear_driver_msgs::msg::SetpointStamped & setpoint_msg) 92 | { 93 | const auto can_frame = this->packet().createPositionCommand(setpoint_msg.point.position); 94 | std::copy(can_frame.data.cbegin(), can_frame.data.cend(), msg.data.begin()); 95 | msg.id = can_frame.id; 96 | } 97 | 98 | void CybergearPositionDriverNode::sendChangeRunModeCallback(can_msgs::msg::Frame & msg) 99 | { 100 | const auto can_frame = this->packet().createChangeToPositionModeCommand(); 101 | std::copy(can_frame.data.cbegin(), can_frame.data.cend(), msg.data.begin()); 102 | msg.id = can_frame.id; 103 | } 104 | } // namespace cybergear_socketcan_driver 105 | 106 | RCLCPP_COMPONENTS_REGISTER_NODE(cybergear_socketcan_driver::CybergearPositionDriverNode) 107 | -------------------------------------------------------------------------------- /cybergear_socketcan_driver/node_parameters.md: -------------------------------------------------------------------------------- 1 | # Cybergear Socketcan Driver Node Parameters 2 | 3 | Default Config 4 | ```yaml 5 | cybergear_socketcan_driver_node: 6 | ros__parameters: 7 | anguler_effort: 8 | max: 12.0 9 | min: -12.0 10 | anguler_position: 11 | max: 12.56637061 12 | min: -12.56637061 13 | anguler_velocity: 14 | max: 30.0 15 | min: -30.0 16 | command_topic_type: trajectory 17 | device_id: 127.0 18 | joint_name: cybergear 19 | pid_gain: 20 | kd: 0.0475 21 | kp: 0.3 22 | pid_gain_range: 23 | kd: 24 | max: 5.0 25 | min: 0.0 26 | kp: 27 | max: 500.0 28 | min: 0.0 29 | primary_id: 0.0 30 | send_frequency: 16.0 31 | temperature: 32 | scale: 0.1 33 | torque_constant: 0.87 34 | update_param_frequency: 1.0 35 | 36 | ``` 37 | 38 | ## primary_id 39 | 40 | Identifier parameter for distinguishing the sender in CAN communication 41 | 42 | * Type: `int` 43 | * Default Value: 0 44 | 45 | *Constraints:* 46 | - parameter must be within bounds 0 47 | 48 | ## device_id 49 | 50 | Identifier parameter for specifying the recipient in CAN communication (CyberGear ID) 51 | 52 | * Type: `int` 53 | * Default Value: 127 54 | 55 | *Constraints:* 56 | - parameter must be within bounds 0 57 | 58 | ## send_frequency 59 | 60 | Transmission interval parameter for CAN frame (Hz) 61 | 62 | * Type: `double` 63 | * Default Value: 16.0 64 | * Read only: True 65 | 66 | *Constraints:* 67 | - greater than 0.0 68 | 69 | ## update_param_frequency 70 | 71 | Cycle for reflecting parameter changes during node execution (Hz) 72 | 73 | * Type: `double` 74 | * Default Value: 1.0 75 | * Read only: True 76 | 77 | *Constraints:* 78 | - greater than 0.0 79 | 80 | ## joint_name 81 | 82 | Joint name parameter used in JointTrajectory and JointState 83 | 84 | * Type: `string` 85 | * Default Value: "cybergear" 86 | * Read only: True 87 | 88 | *Constraints:* 89 | - parameter is not empty 90 | 91 | ## command_topic_type 92 | 93 | Parameter for selecting either JointTrajectory or single target 94 | 95 | * Type: `string` 96 | * Default Value: "trajectory" 97 | * Read only: True 98 | 99 | *Constraints:* 100 | - one of the specified values: ['trajectory', 'setpoint'] 101 | - parameter is not empty 102 | 103 | ## pid_gain.kp 104 | 105 | Proportional gain parameter for PID control 106 | 107 | * Type: `double` 108 | * Default Value: 0.3 109 | 110 | ## pid_gain.kd 111 | 112 | Derivative gain parameter for PID control 113 | 114 | * Type: `double` 115 | * Default Value: 0.0475 116 | 117 | ## torque_constant 118 | 119 | Torque constant used in current control 120 | 121 | * Type: `double` 122 | * Default Value: 0.87 123 | * Read only: True 124 | 125 | ## temperature.scale 126 | 127 | DO NOT CHANGE; CyberGear communication data conversion factor to Celsius 128 | 129 | * Type: `double` 130 | * Default Value: 0.1 131 | * Read only: True 132 | 133 | ## anguler_position.max 134 | 135 | DO NOT CHANGE; Upper limit of position during CyberGear communication 136 | 137 | * Type: `double` 138 | * Default Value: 12.56637061 139 | * Read only: True 140 | 141 | ## anguler_position.min 142 | 143 | DO NOT CHANGE; Lower limit of position during CyberGear communication 144 | 145 | * Type: `double` 146 | * Default Value: -12.56637061 147 | * Read only: True 148 | 149 | ## anguler_velocity.max 150 | 151 | DO NOT CHANGE; Upper limit of velocity during CyberGear communication 152 | 153 | * Type: `double` 154 | * Default Value: 30.0 155 | * Read only: True 156 | 157 | ## anguler_velocity.min 158 | 159 | DO NOT CHANGE; Lower limit of velocity during CyberGear communication 160 | 161 | * Type: `double` 162 | * Default Value: -30.0 163 | * Read only: True 164 | 165 | ## anguler_effort.max 166 | 167 | DO NOT CHANGE; Upper limit of effort during CyberGear communication 168 | 169 | * Type: `double` 170 | * Default Value: 12.0 171 | * Read only: True 172 | 173 | ## anguler_effort.min 174 | 175 | DO NOT CHANGE; Lower limit of effort during CyberGear communication 176 | 177 | * Type: `double` 178 | * Default Value: -12.0 179 | * Read only: True 180 | 181 | ## pid_gain_range.kp.max 182 | 183 | DO NOT CHANGE; Upper limit of proportional gain during CyberGear communication 184 | 185 | * Type: `double` 186 | * Default Value: 500.0 187 | * Read only: True 188 | 189 | ## pid_gain_range.kp.min 190 | 191 | DO NOT CHANGE; Lower limit of proportional gain during CyberGear communication 192 | 193 | * Type: `double` 194 | * Default Value: 0.0 195 | * Read only: True 196 | 197 | ## pid_gain_range.kd.max 198 | 199 | DO NOT CHANGE; Upper limit of derivative gain during CyberGear communication 200 | 201 | * Type: `double` 202 | * Default Value: 5.0 203 | * Read only: True 204 | 205 | ## pid_gain_range.kd.min 206 | 207 | DO NOT CHANGE; Lower limit of derivative gain during CyberGear communication 208 | 209 | * Type: `double` 210 | * Default Value: 0.0 211 | * Read only: True 212 | 213 | -------------------------------------------------------------------------------- /cybergear_servo_description/launch/demo_cybergear_servo.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S python3 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2024 Naoki Takahashi 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | import os 26 | 27 | from ament_index_python.packages import get_package_share_directory 28 | 29 | import launch 30 | import launch_ros 31 | 32 | 33 | def generate_launch_description(): 34 | return launch.LaunchDescription( 35 | generate_declare_launch_arguments() 36 | + generate_launch_nodes() 37 | ) 38 | 39 | 40 | def generate_declare_launch_arguments(): 41 | this_pkg_share_dir = get_package_share_directory('cybergear_servo_description') 42 | return [ 43 | launch.actions.DeclareLaunchArgument( 44 | 'robot_model_path', 45 | default_value=[os.path.join(this_pkg_share_dir, 'urdf')], 46 | ), 47 | launch.actions.DeclareLaunchArgument( 48 | 'robot_model_file', 49 | default_value=['cybergear_servo.urdf.xacro'], 50 | ), 51 | launch.actions.DeclareLaunchArgument( 52 | 'use_joint_state_publisher_gui', 53 | default_value=['false'], 54 | ), 55 | launch.actions.DeclareLaunchArgument( 56 | 'use_rviz', 57 | default_value=['true'], 58 | ), 59 | launch.actions.DeclareLaunchArgument( 60 | 'rviz_config_file', 61 | default_value=[os.path.join(this_pkg_share_dir, 'rviz', 'demo.rviz')], 62 | condition=launch.conditions.IfCondition( 63 | launch.substitutions.LaunchConfiguration('use_rviz') 64 | ) 65 | ), 66 | ] 67 | 68 | 69 | def generate_launch_nodes(): 70 | output = 'screen' 71 | urdf_file = launch.substitutions.PathJoinSubstitution([ 72 | launch.substitutions.LaunchConfiguration('robot_model_path'), 73 | launch.substitutions.LaunchConfiguration('robot_model_file') 74 | ]) 75 | robot_description = { 76 | 'robot_description': launch.substitutions.Command([ 77 | 'xacro', 78 | ' ', 79 | urdf_file 80 | ]) 81 | } 82 | return [ 83 | launch.actions.GroupAction(actions=[ 84 | launch_ros.actions.Node( 85 | package='robot_state_publisher', 86 | executable='robot_state_publisher', 87 | name='cybergear_servo_state_publisher', 88 | output=output, 89 | parameters=[robot_description], 90 | ), 91 | launch_ros.actions.Node( 92 | package='joint_state_publisher', 93 | executable='joint_state_publisher', 94 | name='cybergear_servo_joint_state_publisher', 95 | output=output, 96 | condition=launch.conditions.UnlessCondition( 97 | launch.substitutions.LaunchConfiguration('use_joint_state_publisher_gui') 98 | ) 99 | ), 100 | launch_ros.actions.Node( 101 | package='joint_state_publisher_gui', 102 | executable='joint_state_publisher_gui', 103 | name='cybergear_servo_joint_state_publisher_gui', 104 | output=output, 105 | condition=launch.conditions.IfCondition( 106 | launch.substitutions.LaunchConfiguration('use_joint_state_publisher_gui') 107 | ) 108 | ), 109 | launch_ros.actions.Node( 110 | package='rviz2', 111 | executable='rviz2', 112 | name='cybergear_servo_rviz2', 113 | output=output, 114 | arguments=[ 115 | '-d', launch.substitutions.LaunchConfiguration('rviz_config_file') 116 | ], 117 | condition=launch.conditions.IfCondition( 118 | launch.substitutions.LaunchConfiguration('use_rviz') 119 | ) 120 | ) 121 | ]) 122 | ] 123 | -------------------------------------------------------------------------------- /cybergear_socketcan_driver/src/cybergear_socketcan_driver_node_params.yaml: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2024 Naoki Takahashi 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | cybergear_socketcan_driver_node: 24 | primary_id: 25 | description: Identifier parameter for distinguishing the sender in CAN communication 26 | type: int 27 | default_value: 0 28 | validation: 29 | bounds<>: [0, 127] 30 | device_id: 31 | description: Identifier parameter for specifying the recipient in CAN communication (CyberGear ID) 32 | type: int 33 | default_value: 127 34 | validation: 35 | bounds<>: [0, 127] 36 | send_frequency: 37 | description: Transmission interval parameter for CAN frame (Hz) 38 | type: double 39 | default_value: 16.0 40 | read_only: true 41 | validation: 42 | gt<>: 0.0 43 | update_param_frequency: 44 | description: Cycle for reflecting parameter changes during node execution (Hz) 45 | type: double 46 | default_value: 1.0 47 | read_only: true 48 | validation: 49 | gt<>: 0.0 50 | joint_name: 51 | description: Joint name parameter used in JointTrajectory and JointState 52 | type: string 53 | default_value: cybergear 54 | read_only: true 55 | validation: 56 | not_empty<>: 57 | command_topic_type: 58 | description: Parameter for selecting either JointTrajectory or single target 59 | type: string 60 | default_value: trajectory 61 | read_only: true 62 | validation: 63 | one_of<>: [[trajectory, setpoint]] 64 | not_empty<>: 65 | # TODO 66 | #wait_power_on: 67 | # type: bool 68 | # default_value: false 69 | pid_gain: 70 | kp: 71 | description: Proportional gain parameter for PID control 72 | type: double 73 | default_value: 0.3 74 | kd: 75 | description: Derivative gain parameter for PID control 76 | type: double 77 | default_value: 0.0475 78 | torque_constant: 79 | description: Torque constant used in current control 80 | type: double 81 | default_value: 0.87 82 | read_only: true 83 | temperature: 84 | scale: 85 | description: DO NOT CHANGE; CyberGear communication data conversion factor to Celsius 86 | type: double 87 | default_value: 0.1 88 | read_only: true 89 | anguler_position: 90 | max: 91 | description: DO NOT CHANGE; Upper limit of position during CyberGear communication 92 | type: double 93 | default_value: 12.56637061 94 | read_only: true 95 | min: 96 | description: DO NOT CHANGE; Lower limit of position during CyberGear communication 97 | type: double 98 | default_value: -12.56637061 99 | read_only: true 100 | anguler_velocity: 101 | max: 102 | description: DO NOT CHANGE; Upper limit of velocity during CyberGear communication 103 | type: double 104 | default_value: 30.0 105 | read_only: true 106 | min: 107 | description: DO NOT CHANGE; Lower limit of velocity during CyberGear communication 108 | type: double 109 | default_value: -30.0 110 | read_only: true 111 | anguler_effort: 112 | max: 113 | description: DO NOT CHANGE; Upper limit of effort during CyberGear communication 114 | type: double 115 | default_value: 12.0 116 | read_only: true 117 | min: 118 | description: DO NOT CHANGE; Lower limit of effort during CyberGear communication 119 | type: double 120 | default_value: -12.0 121 | read_only: true 122 | pid_gain_range: 123 | kp: 124 | max: 125 | description: DO NOT CHANGE; Upper limit of proportional gain during CyberGear communication 126 | type: double 127 | default_value: 500.0 128 | read_only: true 129 | min: 130 | description: DO NOT CHANGE; Lower limit of proportional gain during CyberGear communication 131 | type: double 132 | default_value: 0.0 133 | read_only: true 134 | kd: 135 | max: 136 | description: DO NOT CHANGE; Upper limit of derivative gain during CyberGear communication 137 | type: double 138 | default_value: 5.0 139 | read_only: true 140 | min: 141 | description: DO NOT CHANGE; Lower limit of derivative gain during CyberGear communication 142 | type: double 143 | default_value: 0.0 144 | read_only: true 145 | -------------------------------------------------------------------------------- /cybergear_socketcan_driver/src/cybergear_default_driver_node.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "cybergear_socketcan_driver_node.hpp" 30 | 31 | namespace cybergear_socketcan_driver 32 | { 33 | class CybergearDefaultDriverNode : public CybergearSocketCanDriverNode 34 | { 35 | public: 36 | CybergearDefaultDriverNode() = delete; 37 | explicit CybergearDefaultDriverNode(const rclcpp::NodeOptions &); 38 | CybergearDefaultDriverNode(const CybergearDefaultDriverNode &) = delete; 39 | CybergearDefaultDriverNode(CybergearDefaultDriverNode &&) = delete; 40 | ~CybergearDefaultDriverNode() override; 41 | 42 | CybergearDefaultDriverNode & operator=(const CybergearDefaultDriverNode &) = delete; 43 | CybergearDefaultDriverNode & operator=(CybergearDefaultDriverNode &&) = delete; 44 | 45 | protected: 46 | void procFeedbackJointStateCallback(const sensor_msgs::msg::JointState &) final; 47 | void sendCanFrameFromTrajectoryCallback( 48 | can_msgs::msg::Frame &, const SingleJointTrajectoryPoints &) final; 49 | void sendCanFrameFromSetpointCallback( 50 | can_msgs::msg::Frame &, const cybergear_driver_msgs::msg::SetpointStamped &) final; 51 | void sendChangeRunModeCallback(can_msgs::msg::Frame &) final; 52 | 53 | private: 54 | float last_sense_anguler_position_; 55 | }; 56 | 57 | CybergearDefaultDriverNode::CybergearDefaultDriverNode(const rclcpp::NodeOptions & node_options) 58 | : CybergearSocketCanDriverNode("cybergear_default_driver", node_options), 59 | last_sense_anguler_position_(0.0F) 60 | {} 61 | 62 | CybergearDefaultDriverNode::~CybergearDefaultDriverNode() {} 63 | 64 | void CybergearDefaultDriverNode::procFeedbackJointStateCallback( 65 | const sensor_msgs::msg::JointState & msg) 66 | { 67 | if (msg.position.empty()) { 68 | return; 69 | } 70 | last_sense_anguler_position_ = static_cast(msg.position[0]); 71 | } 72 | 73 | void CybergearDefaultDriverNode::sendCanFrameFromTrajectoryCallback( 74 | can_msgs::msg::Frame & msg, const SingleJointTrajectoryPoints & single_joint_trajectory) 75 | { 76 | cybergear_driver_core::MoveParam move_param; 77 | 78 | if (!single_joint_trajectory.points().empty()) { 79 | move_param.position = single_joint_trajectory.getLerpPosition(this->get_clock()->now()); 80 | move_param.velocity = single_joint_trajectory.getLerpVelocity(this->get_clock()->now()); 81 | move_param.effort = single_joint_trajectory.getLerpEffort(this->get_clock()->now()); 82 | } else { 83 | move_param.position = last_sense_anguler_position_; 84 | move_param.velocity = 0.0F; 85 | move_param.effort = 0.0F; 86 | } 87 | move_param.kp = static_cast(this->params().pid_gain.kp); 88 | move_param.kd = static_cast(this->params().pid_gain.kd); 89 | 90 | const auto can_frame = this->packet().createMoveCommand(move_param); 91 | std::copy(can_frame.data.cbegin(), can_frame.data.cend(), msg.data.begin()); 92 | msg.id = can_frame.id; 93 | } 94 | 95 | void CybergearDefaultDriverNode::sendCanFrameFromSetpointCallback( 96 | can_msgs::msg::Frame & msg, const cybergear_driver_msgs::msg::SetpointStamped & setpoint_msg) 97 | { 98 | cybergear_driver_core::MoveParam move_param; 99 | 100 | move_param.position = setpoint_msg.point.position; 101 | move_param.velocity = setpoint_msg.point.velocity; 102 | move_param.effort = setpoint_msg.point.effort; 103 | move_param.kp = static_cast(this->params().pid_gain.kp); 104 | move_param.kd = static_cast(this->params().pid_gain.kd); 105 | 106 | const auto can_frame = this->packet().createMoveCommand(move_param); 107 | std::copy(can_frame.data.cbegin(), can_frame.data.cend(), msg.data.begin()); 108 | msg.id = can_frame.id; 109 | } 110 | 111 | void CybergearDefaultDriverNode::sendChangeRunModeCallback(can_msgs::msg::Frame & msg) 112 | { 113 | const auto can_frame = this->packet().createChangeToOperationModeCommand(); 114 | std::copy(can_frame.data.cbegin(), can_frame.data.cend(), msg.data.begin()); 115 | msg.id = can_frame.id; 116 | } 117 | } // namespace cybergear_socketcan_driver 118 | 119 | RCLCPP_COMPONENTS_REGISTER_NODE(cybergear_socketcan_driver::CybergearDefaultDriverNode) 120 | -------------------------------------------------------------------------------- /cybergear_driver_core/include/cybergear_driver_core/cybergear_frame_id.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | #include "protocol_constant.hpp" 28 | 29 | namespace cybergear_driver_core 30 | { 31 | class CybergearFrameId 32 | { 33 | private: 34 | static constexpr uint32_t FRAME_TYPE_MASK = 0xff000000; 35 | static constexpr uint32_t DEVICE_ID_MASK = 0x0000ff00; 36 | static constexpr uint32_t FEEDBACK_ERROR_MASK = 0x003f0000; 37 | static constexpr uint32_t FEEDBACK_MODE_STATE_MASK = 0x00c00000; 38 | 39 | static constexpr uint8_t FRAME_TYPE_OFFSET = 24; 40 | static constexpr uint8_t MODE_STATE_OFFSET = 22; 41 | static constexpr uint8_t DEVICE_ID_OFFSET = 8; 42 | 43 | public: 44 | explicit CybergearFrameId(const uint8_t device_id, const uint8_t primary_id) 45 | : device_id_(device_id), 46 | primary_id_(primary_id) 47 | { 48 | if (device_id == primary_id) { 49 | throw std::invalid_argument("Please set different id: device vs primary(host)"); 50 | } 51 | } 52 | 53 | ~CybergearFrameId() {} 54 | 55 | bool isInfo(const uint32_t id) const 56 | { 57 | const uint8_t frame_type_id = (id & FRAME_TYPE_MASK) >> FRAME_TYPE_OFFSET; 58 | return frame_type_id == commands::INFO; 59 | } 60 | 61 | bool isFeedback(const uint32_t id) const 62 | { 63 | const uint8_t frame_type_id = (id & FRAME_TYPE_MASK) >> FRAME_TYPE_OFFSET; 64 | return frame_type_id == commands::FEEDBACK; 65 | } 66 | 67 | bool isFault(const uint32_t id) const 68 | { 69 | const uint8_t frame_type_id = (id & FRAME_TYPE_MASK) >> FRAME_TYPE_OFFSET; 70 | return frame_type_id == commands::FAULT_FEEDBACK; 71 | } 72 | 73 | uint8_t getFrameId(const uint32_t id) const 74 | { 75 | return (id & DEVICE_ID_MASK) >> DEVICE_ID_OFFSET; 76 | } 77 | 78 | bool isDevice(const uint32_t id) const 79 | { 80 | return getFrameId(id) == device_id_; 81 | } 82 | 83 | bool hasError(const uint32_t id) const 84 | { 85 | return (id & FEEDBACK_ERROR_MASK) > 0; 86 | } 87 | 88 | bool isResetMode(const uint32_t id) const 89 | { 90 | const uint8_t mode = (id & FEEDBACK_MODE_STATE_MASK) >> MODE_STATE_OFFSET; 91 | return mode == feedback_mode_state::RESET; 92 | } 93 | 94 | bool isCaliMode(const uint32_t id) const 95 | { 96 | const uint8_t mode = (id & FEEDBACK_MODE_STATE_MASK) >> MODE_STATE_OFFSET; 97 | return mode == feedback_mode_state::CALI; 98 | } 99 | 100 | bool isRunningMode(const uint32_t id) const 101 | { 102 | const uint8_t mode = (id & FEEDBACK_MODE_STATE_MASK) >> MODE_STATE_OFFSET; 103 | return mode == feedback_mode_state::RUN; 104 | } 105 | 106 | uint32_t getInfoId(const uint8_t device_id) const 107 | { 108 | return frameId(device_id, commands::INFO, primary_id_); 109 | } 110 | 111 | uint32_t getInfoId() const 112 | { 113 | return getInfoId(device_id_); 114 | } 115 | 116 | uint32_t getCommandId(const uint16_t effort_limit) const 117 | { 118 | return frameId(device_id_, commands::COMMAND, effort_limit); 119 | } 120 | 121 | uint32_t getFeedbackId() const 122 | { 123 | return frameId(device_id_, commands::FEEDBACK, primary_id_); 124 | } 125 | 126 | uint32_t getEnableTorqueId() const 127 | { 128 | return frameId(device_id_, commands::ENABLE_TORQUE, primary_id_); 129 | } 130 | 131 | uint32_t getResetTorqueId() const 132 | { 133 | return frameId(device_id_, commands::RESET_TORQUE, primary_id_); 134 | } 135 | 136 | uint32_t getZeroPositionId() const 137 | { 138 | return frameId(device_id_, commands::ZEROING, primary_id_); 139 | } 140 | 141 | uint32_t getChangeDeviceId(const uint8_t change_id) const 142 | { 143 | return frameId(device_id_, commands::CHANGE_DEVICE_ID, change_id << 8 | primary_id_); 144 | } 145 | 146 | uint32_t getReadParameterId() const 147 | { 148 | return frameId(device_id_, commands::READ_PARAMETER, primary_id_); 149 | } 150 | 151 | uint32_t getWriteParameterId() const 152 | { 153 | return frameId(device_id_, commands::WRITE_PARAMETER, primary_id_); 154 | } 155 | 156 | private: 157 | const uint8_t device_id_; 158 | const uint8_t primary_id_; 159 | 160 | uint32_t frameId(const uint8_t dev_id, const uint8_t cmd_id, const uint16_t opt) const 161 | { 162 | return static_cast(cmd_id << 24 | opt << 8 | dev_id); 163 | } 164 | }; 165 | } // namespace cybergear_driver_core 166 | -------------------------------------------------------------------------------- /cybergear_maintenance_tools/src/change_cybergear_id_node.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | namespace cybergear_maintenance_tools 35 | { 36 | class ChangeCybergearIdNode : public rclcpp::Node 37 | { 38 | public: 39 | ChangeCybergearIdNode() = delete; 40 | explicit ChangeCybergearIdNode(const rclcpp::NodeOptions & options); 41 | ChangeCybergearIdNode(const ChangeCybergearIdNode &) = delete; 42 | ChangeCybergearIdNode(ChangeCybergearIdNode &&) = delete; 43 | ~ChangeCybergearIdNode() override; 44 | 45 | ChangeCybergearIdNode & operator=(const ChangeCybergearIdNode &) = delete; 46 | ChangeCybergearIdNode & operator=(ChangeCybergearIdNode &&) = delete; 47 | 48 | private: 49 | rclcpp::Publisher::SharedPtr can_frame_publisher_; 50 | 51 | rclcpp::Subscription::SharedPtr can_frame_subscriber_; 52 | 53 | rclcpp::TimerBase::SharedPtr send_can_frame_timer_; 54 | 55 | std::unique_ptr param_listener_; 56 | std::unique_ptr params_; 57 | 58 | std::unique_ptr packet_; 59 | 60 | void subscribeCanFrameCallback(const can_msgs::msg::Frame::ConstSharedPtr &); 61 | void sendCanFrameTimerCallback(); 62 | }; 63 | 64 | ChangeCybergearIdNode::ChangeCybergearIdNode(const rclcpp::NodeOptions & options) 65 | : rclcpp::Node("change_cybergear_id", options), 66 | can_frame_publisher_(nullptr), 67 | can_frame_subscriber_(nullptr), 68 | send_can_frame_timer_(nullptr), 69 | param_listener_(nullptr), 70 | params_(nullptr), 71 | packet_(nullptr) 72 | { 73 | RCLCPP_INFO_STREAM(this->get_logger(), "Start " << this->get_name()); 74 | 75 | param_listener_ = std::make_unique( 76 | this->get_node_parameters_interface()); 77 | params_ = std::make_unique(param_listener_->get_params()); 78 | 79 | cybergear_driver_core::CybergearPacketParam packet_param; 80 | packet_param.primary_id = static_cast(params_->primary_id); 81 | packet_param.device_id = static_cast(params_->device_id); 82 | packet_ = std::make_unique(packet_param); 83 | 84 | can_frame_publisher_ = this->create_publisher("to_can_bus", 3); 85 | 86 | can_frame_subscriber_ = this->create_subscription( 87 | "from_can_bus", 88 | 3, 89 | std::bind(&ChangeCybergearIdNode::subscribeCanFrameCallback, this, std::placeholders::_1)); 90 | 91 | const auto send_duration_milliseconds = static_cast(1e3 / params_->send_frequency); 92 | 93 | send_can_frame_timer_ = this->create_wall_timer( 94 | std::chrono::milliseconds(send_duration_milliseconds), 95 | std::bind(&ChangeCybergearIdNode::sendCanFrameTimerCallback, this)); 96 | } 97 | 98 | ChangeCybergearIdNode::~ChangeCybergearIdNode() 99 | { 100 | RCLCPP_INFO_STREAM(this->get_logger(), "Finish " << this->get_name()); 101 | } 102 | 103 | void ChangeCybergearIdNode::subscribeCanFrameCallback( 104 | const can_msgs::msg::Frame::ConstSharedPtr & msg) 105 | { 106 | const unsigned int device_id = packet_->frameId().getFrameId(msg->id); 107 | if (device_id == params_->primary_id) { 108 | return; 109 | } 110 | if (!packet_->frameId().isInfo(msg->id)) { 111 | return; 112 | } 113 | RCLCPP_INFO_STREAM(this->get_logger(), "Found new CyberGear device: " << device_id); 114 | } 115 | 116 | void ChangeCybergearIdNode::sendCanFrameTimerCallback() 117 | { 118 | constexpr uint8_t kDlc = 8; 119 | static unsigned int callback_counter = 0; 120 | 121 | if (callback_counter == 0) { 122 | auto msg = std::make_unique(); 123 | msg->header.stamp = this->get_clock()->now(); 124 | msg->is_rtr = false; 125 | msg->is_extended = true; 126 | msg->is_error = false; 127 | msg->dlc = kDlc; 128 | msg->id = packet_->frameId().getChangeDeviceId(params_->target_id); 129 | can_frame_publisher_->publish(std::move(msg)); 130 | } else { 131 | const double send_duration_seconds = 1 / params_->send_frequency; 132 | const unsigned int wait_count = callback_counter; 133 | const double wait_seconds = wait_count * send_duration_seconds; 134 | if (wait_seconds > params_->wait_recive_can_frame) { 135 | rclcpp::shutdown(); 136 | } 137 | } 138 | callback_counter++; 139 | } 140 | } // namespace cybergear_maintenance_tools 141 | 142 | RCLCPP_COMPONENTS_REGISTER_NODE(cybergear_maintenance_tools::ChangeCybergearIdNode) 143 | -------------------------------------------------------------------------------- /cybergear_maintenance_tools/src/search_cybergear_id_node.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | namespace cybergear_maintenance_tools 35 | { 36 | class SearchCybergearIdNode : public rclcpp::Node 37 | { 38 | public: 39 | SearchCybergearIdNode() = delete; 40 | explicit SearchCybergearIdNode(const rclcpp::NodeOptions & options); 41 | SearchCybergearIdNode(const SearchCybergearIdNode &) = delete; 42 | SearchCybergearIdNode(SearchCybergearIdNode &&) = delete; 43 | ~SearchCybergearIdNode() override; 44 | 45 | SearchCybergearIdNode & operator=(const SearchCybergearIdNode &) = delete; 46 | SearchCybergearIdNode & operator=(SearchCybergearIdNode &&) = delete; 47 | 48 | private: 49 | static constexpr unsigned int kMinId = 0; 50 | static constexpr unsigned int kMaxId = 255; 51 | 52 | rclcpp::Publisher::SharedPtr can_frame_publisher_; 53 | 54 | rclcpp::Subscription::SharedPtr can_frame_subscriber_; 55 | 56 | rclcpp::TimerBase::SharedPtr send_can_frame_timer_; 57 | 58 | std::unique_ptr param_listener_; 59 | std::unique_ptr params_; 60 | 61 | std::unique_ptr packet_; 62 | 63 | void subscribeCanFrameCallback(const can_msgs::msg::Frame::ConstSharedPtr &); 64 | void sendCanFrameTimerCallback(); 65 | }; 66 | 67 | SearchCybergearIdNode::SearchCybergearIdNode(const rclcpp::NodeOptions & options) 68 | : rclcpp::Node("search_cybergear_id", options), 69 | can_frame_publisher_(nullptr), 70 | can_frame_subscriber_(nullptr), 71 | send_can_frame_timer_(nullptr), 72 | param_listener_(nullptr), 73 | params_(nullptr), 74 | packet_(nullptr) 75 | { 76 | RCLCPP_INFO_STREAM(this->get_logger(), "Start " << this->get_name()); 77 | 78 | param_listener_ = std::make_unique( 79 | this->get_node_parameters_interface()); 80 | params_ = std::make_unique(param_listener_->get_params()); 81 | 82 | cybergear_driver_core::CybergearPacketParam packet_param; 83 | packet_param.primary_id = static_cast(params_->primary_id); 84 | packet_ = std::make_unique(packet_param); 85 | 86 | can_frame_publisher_ = this->create_publisher("to_can_bus", 3); 87 | 88 | can_frame_subscriber_ = this->create_subscription( 89 | "from_can_bus", 90 | 3, 91 | std::bind(&SearchCybergearIdNode::subscribeCanFrameCallback, this, std::placeholders::_1)); 92 | 93 | const auto send_duration_milliseconds = static_cast(1e3 / params_->send_frequency); 94 | 95 | send_can_frame_timer_ = this->create_wall_timer( 96 | std::chrono::milliseconds(send_duration_milliseconds), 97 | std::bind(&SearchCybergearIdNode::sendCanFrameTimerCallback, this)); 98 | } 99 | 100 | SearchCybergearIdNode::~SearchCybergearIdNode() 101 | { 102 | RCLCPP_INFO_STREAM(this->get_logger(), "Finish " << this->get_name()); 103 | } 104 | 105 | void SearchCybergearIdNode::subscribeCanFrameCallback( 106 | const can_msgs::msg::Frame::ConstSharedPtr & msg) 107 | { 108 | const unsigned int device_id = packet_->frameId().getFrameId(msg->id); 109 | if (device_id == params_->primary_id) { 110 | return; 111 | } 112 | if (!packet_->frameId().isInfo(msg->id)) { 113 | return; 114 | } 115 | RCLCPP_INFO_STREAM(this->get_logger(), "Found CyberGear device: " << device_id); 116 | } 117 | 118 | void SearchCybergearIdNode::sendCanFrameTimerCallback() 119 | { 120 | constexpr uint8_t kDlc = 8; 121 | static unsigned int callback_counter = 0; 122 | 123 | if (callback_counter < kMaxId) { 124 | auto msg = std::make_unique(); 125 | msg->header.stamp = this->get_clock()->now(); 126 | msg->is_rtr = false; 127 | msg->is_extended = true; 128 | msg->is_error = false; 129 | msg->dlc = kDlc; 130 | msg->id = packet_->frameId().getInfoId(callback_counter); 131 | can_frame_publisher_->publish(std::move(msg)); 132 | } else { 133 | const double send_duration_seconds = 1 / params_->send_frequency; 134 | const unsigned int wait_count = callback_counter - kMaxId; 135 | const double wait_seconds = wait_count * send_duration_seconds; 136 | if (wait_seconds > params_->wait_recive_can_frame) { 137 | rclcpp::shutdown(); 138 | } 139 | } 140 | callback_counter++; 141 | } 142 | } // namespace cybergear_maintenance_tools 143 | 144 | RCLCPP_COMPONENTS_REGISTER_NODE(cybergear_maintenance_tools::SearchCybergearIdNode) 145 | -------------------------------------------------------------------------------- /cybergear_socketcan_driver/src/cybergear_socketcan_driver_node.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace cybergear_socketcan_driver 44 | { 45 | 46 | class CybergearSocketCanDriverNode : public rclcpp::Node 47 | { 48 | public: 49 | CybergearSocketCanDriverNode() = delete; 50 | explicit CybergearSocketCanDriverNode(const std::string & node_name, const rclcpp::NodeOptions &); 51 | explicit CybergearSocketCanDriverNode(const rclcpp::NodeOptions &); 52 | CybergearSocketCanDriverNode(const CybergearSocketCanDriverNode &) = delete; 53 | CybergearSocketCanDriverNode(CybergearSocketCanDriverNode &&) = delete; 54 | ~CybergearSocketCanDriverNode() override; 55 | 56 | CybergearSocketCanDriverNode & operator=(const CybergearSocketCanDriverNode &) = delete; 57 | CybergearSocketCanDriverNode & operator=(CybergearSocketCanDriverNode &&) = delete; 58 | 59 | protected: 60 | cybergear_driver_core::CybergearPacket & packet(); 61 | cybergear_socketcan_driver_node::Params & params(); 62 | 63 | virtual void procFeedbackPacketCallback(const can_msgs::msg::Frame &); 64 | virtual void procFeedbackJointStateCallback(const sensor_msgs::msg::JointState &); 65 | virtual void procFeedbackTemperatureCallabck(const sensor_msgs::msg::Temperature &); 66 | 67 | virtual void sendCanFrameFromTrajectoryCallback( 68 | can_msgs::msg::Frame &, const SingleJointTrajectoryPoints &); 69 | virtual void sendCanFrameFromSetpointCallback( 70 | can_msgs::msg::Frame &, const cybergear_driver_msgs::msg::SetpointStamped &); 71 | virtual void sendChangeRunModeCallback(can_msgs::msg::Frame &); 72 | 73 | private: 74 | bool recived_can_msg_; 75 | unsigned int no_response_can_msg_counter_; 76 | 77 | std::unique_ptr packet_; 78 | 79 | sensor_msgs::msg::JointState::UniquePtr last_subscribe_joint_state_; 80 | can_msgs::msg::Frame::ConstSharedPtr last_subscribe_can_frame_; 81 | cybergear_driver_msgs::msg::SetpointStamped::ConstSharedPtr last_subscribe_setpoint_; 82 | 83 | rclcpp::Subscription::SharedPtr can_frame_subscriber_; 84 | rclcpp::Subscription::SharedPtr 85 | joint_trajectory_subscriber_; 86 | rclcpp::Subscription::SharedPtr 87 | joint_setpoint_subscriber_; 88 | 89 | rclcpp::Publisher::SharedPtr can_frame_publisher_; 90 | rclcpp::Publisher::SharedPtr joint_state_publisher_; 91 | rclcpp::Publisher::SharedPtr joint_temperature_publisher_; 92 | 93 | rclcpp::TimerBase::SharedPtr send_can_frame_timer_; 94 | rclcpp::TimerBase::SharedPtr update_parameter_timer_; 95 | 96 | rclcpp::Service::SharedPtr enable_torque_service_; 97 | rclcpp::Service::SharedPtr zero_position_service_; 98 | 99 | std::unique_ptr diagnostic_updater_; 100 | 101 | std::unique_ptr param_listener_; 102 | std::unique_ptr params_; 103 | 104 | SingleJointTrajectoryPoints::SharedPtr single_joint_trajectory_; 105 | 106 | void subscribeCanFrameCallback(const can_msgs::msg::Frame::ConstSharedPtr &); 107 | void subscribeJointTrajectoryCallback( 108 | const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr &); 109 | void subscribeJointSetpointCallback( 110 | const cybergear_driver_msgs::msg::SetpointStamped::ConstSharedPtr &); 111 | void sendCanFrameTimerCallback(); 112 | void updateParameterTimerCallback(); 113 | void enableTorqueServiceCallback( 114 | const std_srvs::srv::SetBool::Request::ConstSharedPtr & request, 115 | const std_srvs::srv::SetBool::Response::SharedPtr & response); 116 | void zeroPositionServiceCallback( 117 | const std_srvs::srv::Trigger::Request::ConstSharedPtr & request, 118 | const std_srvs::srv::Trigger::Response::ConstSharedPtr & response); 119 | 120 | void canFrameDiagnosricsCallback(diagnostic_updater::DiagnosticStatusWrapper &); 121 | 122 | void procFeedbackPacket(const can_msgs::msg::Frame &); 123 | 124 | void setDefaultCanFrame(can_msgs::msg::Frame::UniquePtr &); 125 | 126 | void sendChangeRunMode(); 127 | void sendEnableTorque(); 128 | void sendResetTorque(); 129 | void sendFeedbackRequst(); 130 | void sendZeroPosition(); 131 | }; 132 | } // namespace cybergear_socketcan_driver 133 | -------------------------------------------------------------------------------- /cybergear_servo_description/rviz/demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | - /RobotModel1 11 | - /RobotModel1/Mass Properties1 12 | Splitter Ratio: 0.5 13 | Tree Height: 555 14 | - Class: rviz_common/Selection 15 | Name: Selection 16 | - Class: rviz_common/Tool Properties 17 | Expanded: 18 | - /2D Goal Pose1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz_common/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz_common/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz_default_plugins/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.029999999329447746 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Class: rviz_default_plugins/TF 54 | Enabled: true 55 | Frame Timeout: 15 56 | Frames: 57 | All Enabled: true 58 | base_link: 59 | Value: true 60 | demo_cybergear_link: 61 | Value: true 62 | demo_cybergear_table_link: 63 | Value: true 64 | Marker Scale: 0.25 65 | Name: TF 66 | Show Arrows: true 67 | Show Axes: true 68 | Show Names: true 69 | Tree: 70 | base_link: 71 | demo_cybergear_link: 72 | demo_cybergear_table_link: 73 | {} 74 | Update Interval: 0 75 | Value: true 76 | - Alpha: 1 77 | Class: rviz_default_plugins/RobotModel 78 | Collision Enabled: false 79 | Description File: "" 80 | Description Source: Topic 81 | Description Topic: 82 | Depth: 5 83 | Durability Policy: Volatile 84 | History Policy: Keep Last 85 | Reliability Policy: Reliable 86 | Value: /robot_description 87 | Enabled: true 88 | Links: 89 | All Links Enabled: true 90 | Expand Joint Details: false 91 | Expand Link Details: false 92 | Expand Tree: false 93 | Link Tree Style: Links in Alphabetic Order 94 | base_link: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | demo_cybergear_link: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | demo_cybergear_table_link: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | Mass Properties: 109 | Inertia: false 110 | Mass: false 111 | Name: RobotModel 112 | TF Prefix: "" 113 | Update Interval: 0 114 | Value: true 115 | Visual Enabled: true 116 | Enabled: true 117 | Global Options: 118 | Background Color: 48; 48; 48 119 | Fixed Frame: base_link 120 | Frame Rate: 30 121 | Name: root 122 | Tools: 123 | - Class: rviz_default_plugins/Interact 124 | Hide Inactive Objects: true 125 | - Class: rviz_default_plugins/MoveCamera 126 | - Class: rviz_default_plugins/Select 127 | - Class: rviz_default_plugins/FocusCamera 128 | - Class: rviz_default_plugins/Measure 129 | Line color: 128; 128; 0 130 | - Class: rviz_default_plugins/SetInitialPose 131 | Covariance x: 0.25 132 | Covariance y: 0.25 133 | Covariance yaw: 0.06853891909122467 134 | Topic: 135 | Depth: 5 136 | Durability Policy: Volatile 137 | History Policy: Keep Last 138 | Reliability Policy: Reliable 139 | Value: /initialpose 140 | - Class: rviz_default_plugins/SetGoal 141 | Topic: 142 | Depth: 5 143 | Durability Policy: Volatile 144 | History Policy: Keep Last 145 | Reliability Policy: Reliable 146 | Value: /goal_pose 147 | - Class: rviz_default_plugins/PublishPoint 148 | Single click: true 149 | Topic: 150 | Depth: 5 151 | Durability Policy: Volatile 152 | History Policy: Keep Last 153 | Reliability Policy: Reliable 154 | Value: /clicked_point 155 | Transformation: 156 | Current: 157 | Class: rviz_default_plugins/TF 158 | Value: true 159 | Views: 160 | Current: 161 | Class: rviz_default_plugins/Orbit 162 | Distance: 0.4256269931793213 163 | Enable Stereo Rendering: 164 | Stereo Eye Separation: 0.05999999865889549 165 | Stereo Focal Distance: 1 166 | Swap Stereo Eyes: false 167 | Value: false 168 | Focal Point: 169 | X: 0 170 | Y: 0 171 | Z: 0 172 | Focal Shape Fixed Size: false 173 | Focal Shape Size: 0.05000000074505806 174 | Invert Z Axis: false 175 | Name: Current View 176 | Near Clip Distance: 0.009999999776482582 177 | Pitch: 1.0197969675064087 178 | Target Frame: 179 | Value: Orbit (rviz) 180 | Yaw: 1.1053972244262695 181 | Saved: ~ 182 | Window Geometry: 183 | Displays: 184 | collapsed: false 185 | Height: 846 186 | Hide Left Dock: false 187 | Hide Right Dock: true 188 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 189 | Selection: 190 | collapsed: false 191 | Time: 192 | collapsed: false 193 | Tool Properties: 194 | collapsed: false 195 | Views: 196 | collapsed: true 197 | Width: 1200 198 | X: 60 199 | Y: 60 200 | -------------------------------------------------------------------------------- /cybergear_driver_core/include/cybergear_driver_core/cybergear_packet.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | #include "bounded_float_byte_converter.hpp" 29 | #include "cybergear_frame_id.hpp" 30 | #include "cybergear_packet_param.hpp" 31 | #include "protocol_constant.hpp" 32 | #include "scaled_float_byte_converter.hpp" 33 | 34 | namespace cybergear_driver_core 35 | { 36 | struct MoveParam 37 | { 38 | MoveParam() 39 | : position(0.0), 40 | velocity(0.0), 41 | effort(0.0), 42 | kp(0.0), 43 | kd(0.0) 44 | {} 45 | 46 | float position; 47 | float velocity; 48 | float effort; 49 | float kp; 50 | float kd; 51 | }; 52 | 53 | using CanData = std::array; 54 | 55 | struct CanFrame 56 | { 57 | uint32_t id; 58 | CanData data; 59 | }; 60 | 61 | class CybergearPacket 62 | { 63 | public: 64 | explicit CybergearPacket(const CybergearPacketParam & param) 65 | : frame_id_(param.device_id, param.primary_id), 66 | anguler_position_converter_(param.max_position, param.min_position), 67 | anguler_velocity_converter_(param.max_velocity, param.min_velocity), 68 | anguler_effort_converter_(param.max_effort, param.min_effort), 69 | pid_kp_converter_(param.max_gain_kp, param.min_gain_kp), 70 | pid_kd_converter_(param.max_gain_kd, param.min_gain_kd), 71 | motor_current_converter_(param.max_current, param.min_current), 72 | temperature_converter_(param.temperature_scale) { 73 | } 74 | 75 | ~CybergearPacket() {} 76 | 77 | CybergearFrameId frameId() 78 | { 79 | return frame_id_; 80 | } 81 | 82 | CanFrame createGetParamCommand(const uint16_t param_index) 83 | { 84 | CanFrame can_frame; 85 | can_frame.id = frame_id_.getReadParameterId(); 86 | return can_frame; 87 | } 88 | 89 | CanFrame createMoveCommand(const MoveParam & param) 90 | { 91 | CanFrame can_frame; 92 | 93 | const auto cmd_pos = anguler_position_converter_.toTwoBytes(param.position); 94 | const auto cmd_vel = anguler_velocity_converter_.toTwoBytes(param.velocity); 95 | const auto cmd_effort = anguler_effort_converter_.toDoubleByte(param.effort); 96 | const auto kp_gain = pid_kp_converter_.toTwoBytes(param.kp); 97 | const auto kd_gain = pid_kd_converter_.toTwoBytes(param.kd); 98 | 99 | std::copy(cmd_pos.cbegin(), cmd_pos.cend(), can_frame.data.begin()); 100 | std::copy(cmd_vel.cbegin(), cmd_vel.cend(), can_frame.data.begin() + 2); 101 | std::copy(kp_gain.cbegin(), kp_gain.cend(), can_frame.data.begin() + 4); 102 | std::copy(kd_gain.cbegin(), kd_gain.cend(), can_frame.data.begin() + 6); 103 | 104 | can_frame.id = frame_id_.getCommandId(cmd_effort); 105 | 106 | return can_frame; 107 | } 108 | 109 | CanFrame createWriteParameter(const uint16_t index, const std::array & param) 110 | { 111 | CanFrame can_frame; 112 | 113 | can_frame.data[0] = index & 0x00ff; 114 | can_frame.data[1] = index >> 8; 115 | std::copy(param.cbegin(), param.cend(), can_frame.data.begin() + 4); 116 | 117 | can_frame.id = frame_id_.getWriteParameterId(); 118 | 119 | return can_frame; 120 | } 121 | 122 | CanFrame createChangeRunMode(const uint8_t mode_id) 123 | { 124 | std::array param; 125 | param[0] = mode_id; 126 | return createWriteParameter(ram_parameters::RUN_MODE, param); 127 | } 128 | 129 | CanFrame createPositionWithGainCommand( 130 | const float position, const float kp, const float kd) 131 | { 132 | MoveParam param; 133 | param.position = position; 134 | param.kp = kp; 135 | param.kd = kd; 136 | return createMoveCommand(param); 137 | } 138 | 139 | CanFrame createZeroPosition() 140 | { 141 | CanFrame can_frame; 142 | can_frame.data[0] = 1; 143 | can_frame.id = frame_id_.getZeroPositionId(); 144 | return can_frame; 145 | } 146 | 147 | CanFrame createPositionCommand(const float position) 148 | { 149 | const auto param = anguler_position_converter_.toFourBytes(position); 150 | return createWriteParameter(ram_parameters::DEST_POSITION_REF, param); 151 | } 152 | 153 | CanFrame createVelocityCommand(const float velocity) 154 | { 155 | const auto param = anguler_velocity_converter_.toFourBytes(velocity); 156 | return createWriteParameter(ram_parameters::SPEED_REF, param); 157 | } 158 | 159 | CanFrame createCurrentCommand(const float current) 160 | { 161 | const auto param = motor_current_converter_.toFourBytes(current); 162 | return createWriteParameter(ram_parameters::IQ_REF, param); 163 | } 164 | 165 | CanFrame createChangeToOperationModeCommand() 166 | { 167 | return createChangeRunMode(run_modes::OPERATION); 168 | } 169 | 170 | CanFrame createChangeToPositionModeCommand() 171 | { 172 | return createChangeRunMode(run_modes::POSITION); 173 | } 174 | 175 | CanFrame createChangeToVelocityModeCommand() 176 | { 177 | return createChangeRunMode(run_modes::SPEED); 178 | } 179 | 180 | CanFrame createChangeToCurrentModeCommand() 181 | { 182 | return createChangeRunMode(run_modes::CURRENT); 183 | } 184 | 185 | float parsePosition(const CanData & data) const 186 | { 187 | return anguler_position_converter_.toFloat<8>(data, 0); 188 | } 189 | 190 | float parseVelocity(const CanData & data) const 191 | { 192 | return anguler_velocity_converter_.toFloat<8>(data, 2); 193 | } 194 | 195 | float parseEffort(const CanData & data) const 196 | { 197 | return anguler_effort_converter_.toFloat<8>(data, 4); 198 | } 199 | 200 | float parseTemperature(const CanData & data) const 201 | { 202 | return temperature_converter_.toFloat<8>(data, 6); 203 | } 204 | 205 | private: 206 | CybergearFrameId frame_id_; 207 | BoundedFloatByteConverter anguler_position_converter_; 208 | BoundedFloatByteConverter anguler_velocity_converter_; 209 | BoundedFloatByteConverter anguler_effort_converter_; 210 | BoundedFloatByteConverter pid_kp_converter_; 211 | BoundedFloatByteConverter pid_kd_converter_; 212 | BoundedFloatByteConverter motor_current_converter_; 213 | ScaledFloatByteConverter temperature_converter_; 214 | }; 215 | } // namespace cybergear_driver_core 216 | -------------------------------------------------------------------------------- /cybergear_socketcan_driver/src/single_joint_trajectory_points.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace cybergear_socketcan_driver 33 | { 34 | SingleJointTrajectoryPoints::SingleJointTrajectoryPoints() {} 35 | 36 | SingleJointTrajectoryPoints::SingleJointTrajectoryPoints( 37 | const SingleJointTrajectoryPoints & joint_trajectory) 38 | { 39 | if (this != &joint_trajectory) { 40 | this->trajectory_points_ = joint_trajectory.trajectory_points_; 41 | } 42 | } 43 | 44 | SingleJointTrajectoryPoints::~SingleJointTrajectoryPoints() {} 45 | 46 | void SingleJointTrajectoryPoints::reset() 47 | { 48 | trajectory_points_.clear(); 49 | } 50 | 51 | void SingleJointTrajectoryPoints::initTrajectoryPoint( 52 | const sensor_msgs::msg::JointState & joint_state) 53 | { 54 | start_trajectory_point_.position = static_cast(joint_state.position[0]); 55 | start_trajectory_point_.velocity = static_cast(joint_state.velocity[0]); 56 | start_trajectory_point_.effort = static_cast(joint_state.effort[0]); 57 | } 58 | 59 | void SingleJointTrajectoryPoints::load( 60 | const std::string & joint_name, const trajectory_msgs::msg::JointTrajectory & joint_trajectory) 61 | { 62 | const unsigned int joint_index = 63 | getJointIndexFromJointNames(joint_name, joint_trajectory.joint_names); 64 | const unsigned int joint_trajectory_points_size = joint_trajectory.points.size(); 65 | 66 | if (joint_index >= joint_trajectory.joint_names.size()) { 67 | return; 68 | } 69 | trajectory_durations_from_recived_.resize(joint_trajectory_points_size); 70 | trajectory_points_.resize(joint_trajectory_points_size); 71 | int point_index = 0; 72 | 73 | for (const auto & point : joint_trajectory.points) { 74 | if (point.positions.size() > joint_index) { 75 | trajectory_points_[point_index].position = static_cast(point.positions[joint_index]); 76 | } 77 | if (point.velocities.size() > joint_index) { 78 | trajectory_points_[point_index].velocity = static_cast(point.velocities[joint_index]); 79 | } 80 | if (point.accelerations.size() > joint_index) { 81 | trajectory_points_[point_index].acceleration = 82 | static_cast(point.accelerations[joint_index]); 83 | } 84 | if (point.effort.size() > joint_index) { 85 | trajectory_points_[point_index].effort = static_cast(point.effort[joint_index]); 86 | } 87 | trajectory_points_[point_index].time_from_start = point.time_from_start; 88 | point_index++; 89 | } 90 | 91 | for (unsigned int i = 0; i < joint_trajectory_points_size; ++i) { 92 | trajectory_durations_from_recived_[i] = 0.0; 93 | for (unsigned int j = 0; j <= i; ++j) { 94 | rclcpp::Duration duration(joint_trajectory.points[j].time_from_start); 95 | trajectory_durations_from_recived_[i] += static_cast(duration.seconds()); 96 | } 97 | } 98 | start_trajectory_time_ = rclcpp::Time(joint_trajectory.header.stamp); 99 | } 100 | 101 | float SingleJointTrajectoryPoints::getLerpPosition(const builtin_interfaces::msg::Time & time) const 102 | { 103 | if (trajectory_durations_from_recived_.empty()) { 104 | return 0.0F; 105 | } 106 | rclcpp::Time control_time(time); 107 | const float time_from_recived = 108 | static_cast((control_time - start_trajectory_time_).seconds()); 109 | 110 | if (trajectory_durations_from_recived_.back() < time_from_recived) { 111 | return trajectory_points_.back().position; 112 | } 113 | unsigned int point_index = 0; 114 | 115 | for (const auto & duration : trajectory_durations_from_recived_) { 116 | if (duration > time_from_recived) { 117 | break; 118 | } 119 | point_index++; 120 | } 121 | float start_point = NAN; 122 | 123 | if (point_index == 0) { 124 | start_point = start_trajectory_point_.position; 125 | } else { 126 | start_point = trajectory_points_[point_index - 1].position; 127 | } 128 | const float dest_delta = trajectory_points_[point_index].position - start_point; 129 | const float point_duration = 130 | static_cast(rclcpp::Duration(trajectory_points_[point_index].time_from_start).seconds()); 131 | float normalized_duration = NAN; 132 | 133 | if (point_duration == 0.0) { 134 | normalized_duration = 1.0F; 135 | } else { 136 | const float time_left = trajectory_durations_from_recived_[point_index] - time_from_recived; 137 | normalized_duration = 1.0F - (time_left) / point_duration; 138 | } 139 | return normalized_duration * dest_delta + start_point; 140 | } 141 | 142 | float SingleJointTrajectoryPoints::getLerpVelocity(const builtin_interfaces::msg::Time & time) const 143 | { 144 | if (trajectory_durations_from_recived_.empty()) { 145 | return 0.0F; 146 | } 147 | rclcpp::Time control_time(time); 148 | const float time_from_recived = 149 | static_cast((control_time - start_trajectory_time_).seconds()); 150 | 151 | if (trajectory_durations_from_recived_.back() < time_from_recived) { 152 | return trajectory_points_.back().velocity; 153 | } 154 | unsigned int point_index = 0; 155 | 156 | for (const auto & duration : trajectory_durations_from_recived_) { 157 | if (duration > time_from_recived) { 158 | break; 159 | } 160 | point_index++; 161 | } 162 | float start_point = NAN; 163 | 164 | if (point_index == 0) { 165 | start_point = start_trajectory_point_.velocity; 166 | } else { 167 | start_point = trajectory_points_[point_index - 1].velocity; 168 | } 169 | const float dest_delta = trajectory_points_[point_index].velocity - start_point; 170 | const float point_duration = 171 | static_cast(rclcpp::Duration(trajectory_points_[point_index].time_from_start).seconds()); 172 | float normalized_duration = NAN; 173 | 174 | if (point_duration == 0.0) { 175 | normalized_duration = 1.0F; 176 | } else { 177 | const float time_left = trajectory_durations_from_recived_[point_index] - time_from_recived; 178 | normalized_duration = 1.0F - (time_left) / point_duration; 179 | } 180 | return normalized_duration * dest_delta + start_point; 181 | } 182 | 183 | float SingleJointTrajectoryPoints::getLerpEffort(const builtin_interfaces::msg::Time & time) const 184 | { 185 | if (trajectory_durations_from_recived_.empty()) { 186 | return 0.0F; 187 | } 188 | rclcpp::Time control_time(time); 189 | const float time_from_recived = 190 | static_cast((control_time - start_trajectory_time_).seconds()); 191 | 192 | if (trajectory_durations_from_recived_.back() < time_from_recived) { 193 | return trajectory_points_.back().effort; 194 | } 195 | unsigned int point_index = 0; 196 | 197 | for (const auto & duration : trajectory_durations_from_recived_) { 198 | if (duration > time_from_recived) { 199 | break; 200 | } 201 | point_index++; 202 | } 203 | float start_point = NAN; 204 | 205 | if (point_index == 0) { 206 | start_point = start_trajectory_point_.effort; 207 | } else { 208 | start_point = trajectory_points_[point_index - 1].effort; 209 | } 210 | const float dest_delta = trajectory_points_[point_index].effort - start_point; 211 | const float point_duration = 212 | static_cast(rclcpp::Duration(trajectory_points_[point_index].time_from_start).seconds()); 213 | float normalized_duration = NAN; 214 | 215 | if (point_duration == 0.0) { 216 | normalized_duration = 1.0F; 217 | } else { 218 | const float time_left = trajectory_durations_from_recived_[point_index] - time_from_recived; 219 | normalized_duration = 1.0F - (time_left) / point_duration; 220 | } 221 | return normalized_duration * dest_delta + start_point; 222 | } 223 | 224 | const SingleJointTrajectoryPoints::Points & SingleJointTrajectoryPoints::points() const 225 | { 226 | return trajectory_points_; 227 | } 228 | 229 | SingleJointTrajectoryPoints & SingleJointTrajectoryPoints::operator=( 230 | const SingleJointTrajectoryPoints & joint_trajectory) 231 | { 232 | if (this != &joint_trajectory) { 233 | this->trajectory_points_ = joint_trajectory.trajectory_points_; 234 | } 235 | return *this; 236 | } 237 | 238 | unsigned int SingleJointTrajectoryPoints::getJointIndexFromJointNames( 239 | const std::string & joint_name, const std::vector & joint_names) 240 | { 241 | unsigned int joint_index = 0; 242 | 243 | for (const auto & name : joint_names) { 244 | if (joint_name == name) { 245 | return joint_index; 246 | } 247 | joint_index++; 248 | } 249 | return joint_names.size(); 250 | } 251 | } // namespace cybergear_socketcan_driver 252 | -------------------------------------------------------------------------------- /cybergear_socketcan_driver/src/cybergear_socketcan_driver_node.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Naoki Takahashi 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include "cybergear_socketcan_driver_node.hpp" 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | namespace cybergear_socketcan_driver 46 | { 47 | CybergearSocketCanDriverNode::CybergearSocketCanDriverNode( 48 | const std::string & node_name, const rclcpp::NodeOptions & node_options) 49 | : rclcpp::Node(node_name, rclcpp::NodeOptions(node_options).use_intra_process_comms(true)), 50 | recived_can_msg_(false), 51 | no_response_can_msg_counter_(0), 52 | packet_(nullptr), 53 | last_subscribe_joint_state_(nullptr), 54 | last_subscribe_can_frame_(nullptr), 55 | last_subscribe_setpoint_(nullptr), 56 | can_frame_subscriber_(nullptr), 57 | joint_trajectory_subscriber_(nullptr), 58 | joint_setpoint_subscriber_(nullptr), 59 | can_frame_publisher_(nullptr), 60 | joint_state_publisher_(nullptr), 61 | joint_temperature_publisher_(nullptr), 62 | send_can_frame_timer_(nullptr), 63 | update_parameter_timer_(nullptr), 64 | enable_torque_service_(nullptr), 65 | zero_position_service_(nullptr), 66 | diagnostic_updater_(nullptr), 67 | param_listener_(nullptr), 68 | params_(nullptr), 69 | single_joint_trajectory_(nullptr) 70 | { 71 | RCLCPP_INFO_STREAM(this->get_logger(), "Start " << this->get_name()); 72 | RCLCPP_WARN(this->get_logger(), "THIS NODE IS UNDER DEVELOPMENT"); 73 | 74 | param_listener_ = std::make_unique( 75 | this->get_node_parameters_interface()); 76 | params_ = 77 | std::make_unique(param_listener_->get_params()); 78 | 79 | cybergear_driver_core::CybergearPacketParam packet_param; 80 | packet_param.device_id = static_cast(params_->device_id); 81 | packet_param.primary_id = static_cast(params_->primary_id); 82 | packet_param.max_position = static_cast(params_->anguler_position.max); 83 | packet_param.min_position = static_cast(params_->anguler_position.min); 84 | packet_param.max_velocity = static_cast(params_->anguler_velocity.max); 85 | packet_param.min_velocity = static_cast(params_->anguler_velocity.min); 86 | packet_param.max_effort = static_cast(params_->anguler_effort.max); 87 | packet_param.min_effort = static_cast(params_->anguler_effort.min); 88 | packet_param.max_gain_kp = static_cast(params_->pid_gain_range.kp.max); 89 | packet_param.min_gain_kp = static_cast(params_->pid_gain_range.kp.min); 90 | packet_param.max_gain_kd = static_cast(params_->pid_gain_range.kd.max); 91 | packet_param.min_gain_kd = static_cast(params_->pid_gain_range.kd.min); 92 | packet_param.temperature_scale = static_cast(params_->temperature.scale); 93 | packet_ = std::make_unique(packet_param); 94 | 95 | can_frame_publisher_ = this->create_publisher("to_can_bus", 3); 96 | joint_state_publisher_ = this->create_publisher("~/joint_state", 3); 97 | joint_temperature_publisher_ = 98 | this->create_publisher("~/temperature", 3); 99 | 100 | diagnostic_updater_ = std::make_unique( 101 | this->get_node_base_interface(), 102 | this->get_node_clock_interface(), 103 | this->get_node_logging_interface(), 104 | this->get_node_parameters_interface(), 105 | this->get_node_timers_interface(), 106 | this->get_node_topics_interface()); 107 | diagnostic_updater_->setHardwareID("CyberGear-" + std::to_string(params_->device_id)); 108 | diagnostic_updater_->add( 109 | "CyberGear Status", 110 | std::bind( 111 | &CybergearSocketCanDriverNode::canFrameDiagnosricsCallback, this, std::placeholders::_1)); 112 | 113 | can_frame_subscriber_ = this->create_subscription( 114 | "from_can_bus", 115 | 3, 116 | std::bind( 117 | &CybergearSocketCanDriverNode::subscribeCanFrameCallback, this, std::placeholders::_1)); 118 | 119 | if (params_->command_topic_type == "trajectory") { 120 | joint_trajectory_subscriber_ = this->create_subscription( 121 | "joint_trajectory", 122 | 3, 123 | std::bind( 124 | &CybergearSocketCanDriverNode::subscribeJointTrajectoryCallback, 125 | this, 126 | std::placeholders::_1)); 127 | } else if (params_->command_topic_type == "setpoint") { 128 | joint_setpoint_subscriber_ = 129 | this->create_subscription( 130 | "~/joint_setpoint", 131 | 2, 132 | std::bind( 133 | &CybergearSocketCanDriverNode::subscribeJointSetpointCallback, 134 | this, 135 | std::placeholders::_1)); 136 | } 137 | 138 | const auto send_duration_milliseconds = static_cast(1e3 / params_->send_frequency); 139 | const auto update_param_duration_milliseconds = 140 | static_cast(1e3 / params_->update_param_frequency); 141 | 142 | send_can_frame_timer_ = this->create_wall_timer( 143 | std::chrono::milliseconds(send_duration_milliseconds), 144 | std::bind(&CybergearSocketCanDriverNode::sendCanFrameTimerCallback, this)); 145 | update_parameter_timer_ = this->create_wall_timer( 146 | std::chrono::milliseconds(update_param_duration_milliseconds), 147 | std::bind(&CybergearSocketCanDriverNode::updateParameterTimerCallback, this)); 148 | 149 | enable_torque_service_ = this->create_service( 150 | "~/enable_torque", 151 | std::bind( 152 | &CybergearSocketCanDriverNode::enableTorqueServiceCallback, 153 | this, 154 | std::placeholders::_1, 155 | std::placeholders::_2)); 156 | zero_position_service_ = this->create_service( 157 | "~/zero_position", 158 | std::bind( 159 | &CybergearSocketCanDriverNode::zeroPositionServiceCallback, 160 | this, 161 | std::placeholders::_1, 162 | std::placeholders::_2)); 163 | } 164 | 165 | CybergearSocketCanDriverNode::CybergearSocketCanDriverNode(const rclcpp::NodeOptions & node_options) 166 | : CybergearSocketCanDriverNode("cybergear_socketcan_driver", node_options) 167 | {} 168 | 169 | CybergearSocketCanDriverNode::~CybergearSocketCanDriverNode() 170 | { 171 | RCLCPP_INFO_STREAM(this->get_logger(), "Finish " << this->get_name()); 172 | } 173 | 174 | cybergear_driver_core::CybergearPacket & CybergearSocketCanDriverNode::packet() 175 | { 176 | return *packet_; 177 | } 178 | 179 | cybergear_socketcan_driver_node::Params & CybergearSocketCanDriverNode::params() 180 | { 181 | return *params_; 182 | } 183 | 184 | void CybergearSocketCanDriverNode::procFeedbackPacketCallback(const can_msgs::msg::Frame &) {} 185 | 186 | void CybergearSocketCanDriverNode::procFeedbackJointStateCallback( 187 | const sensor_msgs::msg::JointState &) 188 | {} 189 | 190 | void CybergearSocketCanDriverNode::procFeedbackTemperatureCallabck( 191 | const sensor_msgs::msg::Temperature &) 192 | {} 193 | 194 | void CybergearSocketCanDriverNode::sendCanFrameFromTrajectoryCallback( 195 | can_msgs::msg::Frame &, const SingleJointTrajectoryPoints &) 196 | {} 197 | 198 | void CybergearSocketCanDriverNode::sendCanFrameFromSetpointCallback( 199 | can_msgs::msg::Frame &, const cybergear_driver_msgs::msg::SetpointStamped &) 200 | {} 201 | 202 | void CybergearSocketCanDriverNode::sendChangeRunModeCallback(can_msgs::msg::Frame & msg) 203 | { 204 | const auto can_frame = packet_->createChangeToOperationModeCommand(); 205 | std::copy(can_frame.data.cbegin(), can_frame.data.cend(), msg.data.begin()); 206 | msg.id = can_frame.id; 207 | } 208 | 209 | // TODO(Naoki Takahashi) parse read ram parameter 210 | void CybergearSocketCanDriverNode::subscribeCanFrameCallback( 211 | const can_msgs::msg::Frame::ConstSharedPtr & msg) 212 | { 213 | if (!packet_->frameId().isDevice(msg->id)) { 214 | return; 215 | } 216 | // TODO(Naoki Takahashi) params_->wait_power_on 217 | 218 | last_subscribe_can_frame_ = msg; 219 | 220 | if (packet_->frameId().isFault(msg->id)) { 221 | RCLCPP_ERROR(this->get_logger(), "Detect fault state from cybergear"); 222 | std::string can_frame_data; 223 | for (const auto & d : msg->data) { 224 | can_frame_data += std::to_string(d) + ", "; 225 | } 226 | RCLCPP_ERROR_STREAM(this->get_logger(), "data: " << can_frame_data); 227 | rclcpp::shutdown(); 228 | return; 229 | } 230 | if (packet_->frameId().isFeedback(msg->id)) { 231 | procFeedbackPacket(*msg); 232 | } 233 | recived_can_msg_ = true; 234 | } 235 | 236 | void CybergearSocketCanDriverNode::subscribeJointTrajectoryCallback( 237 | const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr & msg) 238 | { 239 | bool has_this_joint_cmd = false; 240 | int cmd_index = 0; 241 | 242 | for (const auto & joint_name : msg->joint_names) { 243 | if (joint_name == this->params().joint_name) { 244 | has_this_joint_cmd = true; 245 | break; 246 | } 247 | cmd_index++; 248 | } 249 | if (!has_this_joint_cmd) { 250 | return; 251 | } 252 | if (msg->points.size() <= static_cast(cmd_index)) { 253 | return; 254 | } 255 | single_joint_trajectory_.reset(); 256 | single_joint_trajectory_ = std::make_shared(); 257 | if (single_joint_trajectory_) { 258 | single_joint_trajectory_->load(this->params().joint_name, *msg); 259 | if (last_subscribe_joint_state_) { 260 | single_joint_trajectory_->initTrajectoryPoint(*last_subscribe_joint_state_); 261 | } 262 | } 263 | } 264 | 265 | void CybergearSocketCanDriverNode::subscribeJointSetpointCallback( 266 | const cybergear_driver_msgs::msg::SetpointStamped::ConstSharedPtr & msg) 267 | { 268 | last_subscribe_setpoint_ = msg; 269 | } 270 | 271 | void CybergearSocketCanDriverNode::sendCanFrameTimerCallback() 272 | { 273 | static int wait_state_print_cycle_counter = 0; 274 | 275 | if (!recived_can_msg_) { 276 | if (wait_state_print_cycle_counter > params_->send_frequency) { 277 | RCLCPP_INFO(this->get_logger(), "Waiting for feedback message from the CAN bus.."); 278 | wait_state_print_cycle_counter = 0; 279 | } 280 | sendFeedbackRequst(); 281 | wait_state_print_cycle_counter++; 282 | no_response_can_msg_counter_++; 283 | return; 284 | } 285 | auto msg = std::make_unique(); 286 | setDefaultCanFrame(msg); 287 | 288 | if (last_subscribe_setpoint_) { 289 | sendCanFrameFromSetpointCallback(*msg, *last_subscribe_setpoint_); 290 | } else if (single_joint_trajectory_) { 291 | sendCanFrameFromTrajectoryCallback(*msg, *single_joint_trajectory_); 292 | } else { 293 | msg->id = packet_->frameId().getFeedbackId(); 294 | } 295 | can_frame_publisher_->publish(std::move(msg)); 296 | recived_can_msg_ = false; 297 | } 298 | 299 | void CybergearSocketCanDriverNode::updateParameterTimerCallback() 300 | { 301 | if (param_listener_->is_old(*params_)) { 302 | param_listener_->refresh_dynamic_parameters(); 303 | *params_ = param_listener_->get_params(); 304 | RCLCPP_INFO(this->get_logger(), "Changed dynamic parameters"); 305 | } 306 | } 307 | 308 | // TODO(Naoki Takahashi) wait for result 309 | void CybergearSocketCanDriverNode::enableTorqueServiceCallback( 310 | const std_srvs::srv::SetBool::Request::ConstSharedPtr & request, 311 | const std_srvs::srv::SetBool::Response::SharedPtr & response) 312 | { 313 | RCLCPP_INFO(this->get_logger(), "Calling enableTorqueServiceCallback"); 314 | 315 | // Prevent abrupt behavior during torque switching 316 | if (last_subscribe_setpoint_) { 317 | last_subscribe_setpoint_.reset(); 318 | } 319 | if (single_joint_trajectory_) { 320 | single_joint_trajectory_.reset(); 321 | } 322 | sendResetTorque(); 323 | 324 | sendChangeRunMode(); 325 | 326 | if (request->data) { 327 | sendEnableTorque(); 328 | response->message = "Sent enable torque message"; 329 | } else { 330 | sendResetTorque(); 331 | response->message = "Sent reset torque message"; 332 | } 333 | response->success = true; 334 | RCLCPP_INFO_STREAM(this->get_logger(), response->message); 335 | } 336 | 337 | void CybergearSocketCanDriverNode::zeroPositionServiceCallback( 338 | const std_srvs::srv::Trigger::Request::ConstSharedPtr &, 339 | const std_srvs::srv::Trigger::Response::ConstSharedPtr &) 340 | { 341 | RCLCPP_INFO(this->get_logger(), "Calling zeroPositionServiceCallback"); 342 | sendZeroPosition(); 343 | } 344 | 345 | // TODO(Naoki Takahashi): more information 346 | void CybergearSocketCanDriverNode::canFrameDiagnosricsCallback( 347 | diagnostic_updater::DiagnosticStatusWrapper & diag_status) 348 | { 349 | const auto no_response_can_msg_counter = no_response_can_msg_counter_; 350 | no_response_can_msg_counter_ = 0; 351 | 352 | if (!last_subscribe_can_frame_) { 353 | diag_status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Not recived CAN frame"); 354 | return; 355 | } 356 | std::string control_mode; 357 | 358 | if (packet_->frameId().isResetMode(last_subscribe_can_frame_->id)) { 359 | control_mode = "reset"; 360 | } else if (packet_->frameId().isCaliMode(last_subscribe_can_frame_->id)) { 361 | control_mode = "cali"; 362 | } else if (packet_->frameId().isRunningMode(last_subscribe_can_frame_->id)) { 363 | control_mode = "running"; 364 | } else { 365 | control_mode = "unknown"; 366 | } 367 | diag_status.add("Control mode", control_mode); 368 | diag_status.add("Raw ID", last_subscribe_can_frame_->id); 369 | diag_status.add("No response", no_response_can_msg_counter); 370 | diag_status.add("Joint trajectory subscribed", (single_joint_trajectory_ != nullptr)); 371 | 372 | if (packet_->frameId().hasError(last_subscribe_can_frame_->id)) { 373 | diag_status.summary( 374 | diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Recived error from CyberGear"); 375 | } else if (no_response_can_msg_counter > 0) { 376 | diag_status.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No response CyberGear"); 377 | } else { 378 | diag_status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "No problem"); 379 | } 380 | last_subscribe_can_frame_.reset(); 381 | } 382 | 383 | void CybergearSocketCanDriverNode::procFeedbackPacket(const can_msgs::msg::Frame & msg) 384 | { 385 | if (packet_->frameId().hasError(msg.id)) { 386 | RCLCPP_WARN(this->get_logger(), "Detect error state from cybergear"); 387 | } 388 | std_msgs::msg::Header header_msg; 389 | header_msg.stamp = this->get_clock()->now(); 390 | header_msg.frame_id = params_->joint_name; 391 | 392 | if (joint_state_publisher_) { 393 | auto joint_state_msg = std::make_unique(); 394 | 395 | joint_state_msg->header = header_msg; 396 | joint_state_msg->name.push_back(params_->joint_name); 397 | joint_state_msg->position.push_back(packet_->parsePosition(msg.data)); 398 | joint_state_msg->velocity.push_back(packet_->parseVelocity(msg.data)); 399 | joint_state_msg->effort.push_back(packet_->parseEffort(msg.data)); 400 | 401 | if (!last_subscribe_joint_state_) { 402 | last_subscribe_joint_state_ = std::make_unique(); 403 | } 404 | *last_subscribe_joint_state_ = *joint_state_msg; 405 | 406 | procFeedbackJointStateCallback(*joint_state_msg); 407 | 408 | joint_state_publisher_->publish(std::move(joint_state_msg)); 409 | } 410 | if (joint_temperature_publisher_) { 411 | auto temperature_msg = std::make_unique(); 412 | 413 | temperature_msg->header = header_msg; 414 | temperature_msg->temperature = packet_->parseTemperature(msg.data); 415 | temperature_msg->variance = 0.0; // unknown 416 | 417 | procFeedbackTemperatureCallabck(*temperature_msg); 418 | 419 | joint_temperature_publisher_->publish(std::move(temperature_msg)); 420 | } 421 | procFeedbackPacketCallback(msg); 422 | } 423 | 424 | void CybergearSocketCanDriverNode::setDefaultCanFrame(can_msgs::msg::Frame::UniquePtr & msg) 425 | { 426 | constexpr uint8_t kDlc = 8; 427 | // TODO(Naoki Takahashi) params_->wait_power_on 428 | if (!msg) { 429 | return; 430 | } 431 | msg->header.stamp = this->get_clock()->now(); 432 | msg->header.frame_id = params_->joint_name; 433 | msg->is_rtr = false; 434 | msg->is_extended = true; 435 | msg->is_error = false; 436 | msg->dlc = kDlc; 437 | } 438 | 439 | void CybergearSocketCanDriverNode::sendChangeRunMode() 440 | { 441 | auto msg = std::make_unique(); 442 | setDefaultCanFrame(msg); 443 | sendChangeRunModeCallback(*msg); 444 | if (msg->id == 0) { 445 | return; 446 | } 447 | can_frame_publisher_->publish(std::move(msg)); 448 | } 449 | 450 | void CybergearSocketCanDriverNode::sendEnableTorque() 451 | { 452 | auto msg = std::make_unique(); 453 | setDefaultCanFrame(msg); 454 | msg->id = packet_->frameId().getEnableTorqueId(); 455 | can_frame_publisher_->publish(std::move(msg)); 456 | } 457 | 458 | void CybergearSocketCanDriverNode::sendResetTorque() 459 | { 460 | auto msg = std::make_unique(); 461 | setDefaultCanFrame(msg); 462 | msg->id = packet_->frameId().getResetTorqueId(); 463 | can_frame_publisher_->publish(std::move(msg)); 464 | } 465 | 466 | void CybergearSocketCanDriverNode::sendFeedbackRequst() 467 | { 468 | auto msg = std::make_unique(); 469 | setDefaultCanFrame(msg); 470 | msg->id = packet_->frameId().getFeedbackId(); 471 | can_frame_publisher_->publish(std::move(msg)); 472 | } 473 | 474 | void CybergearSocketCanDriverNode::sendZeroPosition() 475 | { 476 | auto msg = std::make_unique(); 477 | setDefaultCanFrame(msg); 478 | const auto can_frame = packet_->createZeroPosition(); 479 | std::copy(can_frame.data.cbegin(), can_frame.data.cend(), msg->data.begin()); 480 | msg->id = can_frame.id; 481 | can_frame_publisher_->publish(std::move(msg)); 482 | } 483 | } // namespace cybergear_socketcan_driver 484 | 485 | RCLCPP_COMPONENTS_REGISTER_NODE(cybergear_socketcan_driver::CybergearSocketCanDriverNode) 486 | --------------------------------------------------------------------------------