diff --git a/actuators_rppico_hardware_interface/CMakeLists.txt b/actuators_rppico_hardware_interface/CMakeLists.txt new file mode 100644 index 0000000..6500ee7 --- /dev/null +++ b/actuators_rppico_hardware_interface/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 3.16) +project(actuators_rppico_h_i LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra ) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + + +## COMPILE +add_library( + actuators_rppico_h_i + SHARED + hardware/actuators_rppico_h_i.cpp +) +target_compile_features(actuators_rppico_h_i PUBLIC cxx_std_17) +target_include_directories(actuators_rppico_h_i PUBLIC +$ +$ +) +ament_target_dependencies( + actuators_rppico_h_i PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +target_link_libraries(actuators_rppico_h_i PUBLIC serial) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "DIFFDRIVE_ARDUINO_BUILDING_DLL") + +# Export hardware plugins +pluginlib_export_plugin_description_file(hardware_interface actuators_rppico_h_i.xml) + +# INSTALL +install( + DIRECTORY hardware/include/ + DESTINATION include/actuators_rppico_h_i +) +install( + DIRECTORY description/launch description/ros2_control description/urdf description/rviz + DESTINATION share/actuators_rppico_h_i +) +install( + DIRECTORY bringup/launch bringup/config + DESTINATION share/actuators_rppico_h_i +) +install(TARGETS actuators_rppico_h_i + EXPORT export_actuators_rppico_h_i + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) +endif() + +## EXPORTS +ament_export_targets(export_actuators_rppico_h_i HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/actuators_rppico_hardware_interface/README.md b/actuators_rppico_hardware_interface/README.md new file mode 100644 index 0000000..c96e320 --- /dev/null +++ b/actuators_rppico_hardware_interface/README.md @@ -0,0 +1,23 @@ +# diffdrive_arduino + +This node is designed to provide a ros2_control hardware interface for an Arduino running firmware from `ros_arduino_bridge`. +It is designed to be used with a `diff_drive_controller` from `ros2_control`. +It is expected to communicate via serial and to have two motors, each with velocity control and position/velocity feedback. + + + + +It is based on the diffbot example from [ros2_control demos](https://github.com/ros-controls/ros2_control_demos/tree/master/example_2). + +For a tutorial on how to develop a hardware interface like this, check out the video below: + +https://youtu.be/J02jEKawE5U + + + +## To Do + +- [ ] Document changes from earlier versions +- [ ] Document usage and parameters +- [ ] Clean up remaining connections to original demo code +- [ ] Add license etc (in the meantime, do whatever you want with it) \ No newline at end of file diff --git a/actuators_rppico_hardware_interface/actuators_rppico_h_i.xml b/actuators_rppico_hardware_interface/actuators_rppico_h_i.xml new file mode 100644 index 0000000..97fbd1e --- /dev/null +++ b/actuators_rppico_hardware_interface/actuators_rppico_h_i.xml @@ -0,0 +1,10 @@ + + + + Hardware interface to commuicate with the raspberry pico that control the actuators (mainly servos) + + + + diff --git a/actuators_rppico_hardware_interface/bringup/config/diffbot_controllers.yaml b/actuators_rppico_hardware_interface/bringup/config/diffbot_controllers.yaml new file mode 100644 index 0000000..d45de1b --- /dev/null +++ b/actuators_rppico_hardware_interface/bringup/config/diffbot_controllers.yaml @@ -0,0 +1,57 @@ +controller_manager: + ros__parameters: + update_rate: 10 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + diffbot_base_controller: + type: diff_drive_controller/DiffDriveController + +diffbot_base_controller: + ros__parameters: + left_wheel_names: ["left_wheel_joint"] + right_wheel_names: ["right_wheel_joint"] + + wheel_separation: 0.10 + #wheels_per_side: 1 # actually 2, but both are controlled by 1 signal + wheel_radius: 0.015 + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + publish_rate: 50.0 + odom_frame_id: odom + base_frame_id: base_link + pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + + open_loop: true + enable_odom_tf: true + + cmd_vel_timeout: 0.5 + #publish_limited_velocity: true + use_stamped_vel: false + #velocity_rolling_window_size: 10 + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear.x.has_velocity_limits: true + linear.x.has_acceleration_limits: true + linear.x.has_jerk_limits: false + linear.x.max_velocity: 1.0 + linear.x.min_velocity: -1.0 + linear.x.max_acceleration: 1.0 + linear.x.max_jerk: 0.0 + linear.x.min_jerk: 0.0 + + angular.z.has_velocity_limits: true + angular.z.has_acceleration_limits: true + angular.z.has_jerk_limits: false + angular.z.max_velocity: 1.0 + angular.z.min_velocity: -1.0 + angular.z.max_acceleration: 1.0 + angular.z.min_acceleration: -1.0 + angular.z.max_jerk: 0.0 + angular.z.min_jerk: 0.0 diff --git a/actuators_rppico_hardware_interface/bringup/launch/diffbot.launch.py b/actuators_rppico_hardware_interface/bringup/launch/diffbot.launch.py new file mode 100644 index 0000000..9c761c4 --- /dev/null +++ b/actuators_rppico_hardware_interface/bringup/launch/diffbot.launch.py @@ -0,0 +1,107 @@ +# Copyright 2020 ros2_control Development Team +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("diffdrive_arduino"), "urdf", "diffbot.urdf.xacro"] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("diffdrive_arduino"), + "config", + "diffbot_controllers.yaml", + ] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("diffdrive_arduino"), "rviz", "diffbot.rviz"] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + remappings=[ + ("/diff_drive_controller/cmd_vel_unstamped", "/cmd_vel"), + ], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["diffbot_base_controller", "--controller-manager", "/controller_manager"], + ) + + # Delay rviz start after `joint_state_broadcaster` + delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[rviz_node], + ) + ) + + # Delay start of robot_controller after `joint_state_broadcaster` + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[robot_controller_spawner], + ) + ) + + nodes = [ + control_node, + robot_state_pub_node, + joint_state_broadcaster_spawner, + delay_rviz_after_joint_state_broadcaster_spawner, + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + ] + + return LaunchDescription(nodes) diff --git a/actuators_rppico_hardware_interface/description/launch/view_robot.launch.py b/actuators_rppico_hardware_interface/description/launch/view_robot.launch.py new file mode 100644 index 0000000..716f8b5 --- /dev/null +++ b/actuators_rppico_hardware_interface/description/launch/view_robot.launch.py @@ -0,0 +1,99 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="diffdrive_arduino", + description="Description package with robot URDF/xacro files. Usually the argument \ + is not set, it enables use of a custom description.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="diffbot.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for \ + multi-robot setup. If changed than also joint names in the controllers' configuration \ + have to be updated.", + ) + ) + + # Initialize Arguments + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + prefix = LaunchConfiguration("prefix") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", description_file] + ), + " ", + "prefix:=", + prefix, + ] + ) + robot_description = {"robot_description": robot_description_content} + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "rviz", "diffbot_view.rviz"] + ) + + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + ) + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + nodes = [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/actuators_rppico_hardware_interface/description/ros2_control/diffbot.ros2_control.xacro b/actuators_rppico_hardware_interface/description/ros2_control/diffbot.ros2_control.xacro new file mode 100644 index 0000000..ffd5723 --- /dev/null +++ b/actuators_rppico_hardware_interface/description/ros2_control/diffbot.ros2_control.xacro @@ -0,0 +1,35 @@ + + + + + + + + diffdrive_arduino/DiffDriveArduinoHardware + left_wheel_joint + right_wheel_joint + 30 + /dev/serial/by-path/platform-fd500000.pcie-pci-0000:01:00.0-usb-0:1.3:1.0-port0 + 57600 + 1000 + 3436 + 20 + 12 + 0 + 50 + + + + + + + + + + + + + + + + diff --git a/actuators_rppico_hardware_interface/description/rviz/diffbot.rviz b/actuators_rppico_hardware_interface/description/rviz/diffbot.rviz new file mode 100644 index 0000000..365d572 --- /dev/null +++ b/actuators_rppico_hardware_interface/description/rviz/diffbot.rviz @@ -0,0 +1,172 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 1112 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_frontal_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_rear_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.359799385070801 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.05434183403849602 + Y: 0.6973574757575989 + Z: -0.00023954140488058329 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.48539823293685913 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.0053997039794921875 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1383 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000004fcfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000044000004fc000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004fcfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000044000004fc000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000784000004fc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 28 diff --git a/actuators_rppico_hardware_interface/description/rviz/diffbot_view.rviz b/actuators_rppico_hardware_interface/description/rviz/diffbot_view.rviz new file mode 100644 index 0000000..d887a81 --- /dev/null +++ b/actuators_rppico_hardware_interface/description/rviz/diffbot_view.rviz @@ -0,0 +1,172 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 1112 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_frontal_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_rear_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.359799385070801 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.05434183403849602 + Y: 0.6973574757575989 + Z: -0.00023954140488058329 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.48539823293685913 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.0053997039794921875 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1383 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000004fcfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000044000004fc000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004fcfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000044000004fc000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000784000004fc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 28 diff --git a/actuators_rppico_hardware_interface/description/urdf/diffbot.materials.xacro b/actuators_rppico_hardware_interface/description/urdf/diffbot.materials.xacro new file mode 100644 index 0000000..035bf58 --- /dev/null +++ b/actuators_rppico_hardware_interface/description/urdf/diffbot.materials.xacro @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/actuators_rppico_hardware_interface/description/urdf/diffbot.urdf.xacro b/actuators_rppico_hardware_interface/description/urdf/diffbot.urdf.xacro new file mode 100644 index 0000000..6c14440 --- /dev/null +++ b/actuators_rppico_hardware_interface/description/urdf/diffbot.urdf.xacro @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + diff --git a/actuators_rppico_hardware_interface/description/urdf/diffbot_description.urdf.xacro b/actuators_rppico_hardware_interface/description/urdf/diffbot_description.urdf.xacro new file mode 100644 index 0000000..03aa349 --- /dev/null +++ b/actuators_rppico_hardware_interface/description/urdf/diffbot_description.urdf.xacro @@ -0,0 +1,184 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/actuators_rppico_hardware_interface/doc/diffbot.png b/actuators_rppico_hardware_interface/doc/diffbot.png new file mode 100644 index 0000000..1c4fc24 Binary files /dev/null and b/actuators_rppico_hardware_interface/doc/diffbot.png differ diff --git a/actuators_rppico_hardware_interface/doc/userdoc.rst b/actuators_rppico_hardware_interface/doc/userdoc.rst new file mode 100644 index 0000000..0207e0c --- /dev/null +++ b/actuators_rppico_hardware_interface/doc/userdoc.rst @@ -0,0 +1,121 @@ +.. _ros2_control_demos_example_2_userdoc: + +********* +DiffBot +********* + +*DiffBot*, or ''Differential Mobile Robot'', is a simple mobile base with differential drive. +The robot is basically a box moving according to differential drive kinematics. + +For *example_2*, the hardware interface plugin is implemented having only one interface. + +- The communication is done using proprietary API to communicate with the robot control box. +- Data for all joints is exchanged at once. + +The *DiffBot* URDF files can be found in ``description/urdf`` folder. + +Tutorial steps +-------------------------- + +1. To check that *DiffBot* description is working properly use following launch commands + + .. code-block:: shell + + ros2 launch diffdrive_arduino view_robot.launch.py + + .. warning:: + Getting the following output in terminal is OK: ``Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist``. + This happens because ``joint_state_publisher_gui`` node need some time to start. + + .. image:: diffbot.png + :width: 400 + :alt: Differential Mobile Robot + +2. To start *DiffBot* example open a terminal, source your ROS2-workspace and execute its launch file with + + .. code-block:: shell + + ros2 launch diffdrive_arduino diffbot.launch.py + + The launch file loads and starts the robot hardware, controllers and opens *RViz*. + In the starting terminal you will see a lot of output from the hardware implementation showing its internal states. + This excessive printing is only added for demonstration. In general, printing to the terminal should be avoided as much as possible in a hardware interface implementation. + + If you can see an orange box in *RViz* everything has started properly. + Still, to be sure, let's introspect the control system before moving *DiffBot*. + +3. Check if the hardware interface loaded properly, by opening another terminal and executing + + .. code-block:: shell + + ros2 control list_hardware_interfaces + + You should get + + .. code-block:: shell + + command interfaces + left_wheel_joint/velocity [available] [claimed] + right_wheel_joint/velocity [available] [claimed] + state interfaces + left_wheel_joint/position + left_wheel_joint/velocity + right_wheel_joint/position + right_wheel_joint/velocity + + The ``[claimed]`` marker on command interfaces means that a controller has access to command *DiffBot*. + +4. Check if controllers are running + + .. code-block:: shell + + ros2 control list_controllers + + You should get + + .. code-block:: shell + + diffbot_base_controller[diff_drive_controller/DiffDriveController] active + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + +5. If everything is fine, now you can send a command to *Diff Drive Controller* using ROS 2 CLI interface: + + .. code-block:: shell + + ros2 topic pub --rate 30 /diffbot_base_controller/cmd_vel_unstamped geometry_msgs/msg/Twist "linear: + x: 0.7 + y: 0.0 + z: 0.0 + angular: + x: 0.0 + y: 0.0 + z: 1.0" + + You should now see an orange box circling in *RViz*. + Also, you should see changing states in the terminal where launch file is started. + + .. code-block:: shell + + [DiffDriveArduinoHardware]: Got command 43.33333 for 'left_wheel_joint'! + [DiffDriveArduinoHardware]: Got command 50.00000 for 'right_wheel_joint'! + +Files used for this demos +######################### + + - Launch file: `diffbot.launch.py `__ + - Controllers yaml: `diffbot_controllers.yaml `__ + - URDF file: `diffbot.urdf.xacro `__ + + + Description: `diffbot_description.urdf.xacro `__ + + ``ros2_control`` tag: `diffbot.ros2_control.xacro `__ + + - RViz configuration: `diffbot.rviz `__ + + - Hardware interface plugin: `diffbot_system.cpp `__ + + +Controllers from this demo +########################## + +- ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ +- ``Diff Drive Controller`` (`ros2_controllers repository `__): `doc `__ diff --git a/actuators_rppico_hardware_interface/hardware/actuators_rppico_h_i.cpp b/actuators_rppico_hardware_interface/hardware/actuators_rppico_h_i.cpp new file mode 100644 index 0000000..ceddfee --- /dev/null +++ b/actuators_rppico_hardware_interface/hardware/actuators_rppico_h_i.cpp @@ -0,0 +1,207 @@ +#include "actuators_rppico_h_i/actuators_rppico_h_i.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace actuators_rppico_h_i +{ +hardware_interface::CallbackReturn ActuatorsRpPicoHardware::on_init( + const hardware_interface::HardwareInfo & info) +{ + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + number_of_servos = 0; + for (int i = 1; i <= 12; i++) { + std::string param_name = "servo" + std::to_string(i) + "_name"; + if (info_.hardware_parameters.count(param_name) > 0) { + cfg_.servo_names[i] = info_.hardware_parameters[param_name]; + number_of_servos++; + } + } + + RCLCPP_INFO(rclcpp::get_logger("ActuatorsRpPicoHardware"), "%i servos found", number_of_servos); + + + + cfg_.loop_rate = std::stof(info_.hardware_parameters["loop_rate"]); + RCLCPP_INFO(rclcpp::get_logger("ActuatorsRpPicoHardware"), "Setting params from URDF"); + + cfg_.device = info_.hardware_parameters["device"]; + cfg_.baud_rate = std::stoi(info_.hardware_parameters["baudrate"]); + cfg_.timeout_ms = std::stoi(info_.hardware_parameters["timeout_ms"]); + + + + for (int i = 1; i <= 12; i++) { + if (cfg_.servo_names[i] != "") { + servos_[i].setup(cfg_.servo_names[i], i); + } + } + + RCLCPP_INFO(rclcpp::get_logger("ActuatorsRpPicoHardware"), "Setting up hardware interfaces"); + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + RCLCPP_INFO( + rclcpp::get_logger("ActuatorsRpPicoHardware"), "Loading joint '%s'", joint.name.c_str()); + + // servos have exactly one states and one command interface on each joint + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("ActuatorsRpPicoHardware"), + "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("ActuatorsRpPicoHardware"), + "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("ActuatorsRpPicoHardware"), + "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("ActuatorsRpPicoHardware"), + "Joint '%s' have '%s' as first state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + + + } + RCLCPP_INFO(rclcpp::get_logger("ActuatorsRpPicoHardware"), "Successfully set up hardware interfaces"); + return hardware_interface::CallbackReturn::SUCCESS; +} +} +std::vector ActuatorsRpPicoHardware::export_state_interfaces() +{ + std::vector state_interfaces; + RCLCPP_INFO(rclcpp::get_logger("ActuatorsRpPicoHardware"), "Exporting state interfaces"); + + for (int i = 1; i <= 12; i++) { + if (cfg_.servo_names[i] != "") { + state_interfaces.emplace_back(hardware_interface::StateInterface( + cfg_.servo_names[i], hardware_interface::HW_IF_POSITION, &servos_[i].angle)); + } + } + + return state_interfaces; +} + +std::vector ActuatorsRpPicoHardware::export_command_interfaces() +{ + std::vector command_interfaces; + for (int i = 1; i <= 12; i++) { + if (cfg_.servo_names[i] != "") { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + cfg_.servo_names[i], hardware_interface::HW_IF_POSITION, &servos_[i].cmd)); + } + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn ActuatorsRpPicoHardware::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("ActuatorsRpPicoHardware"), "Configuring ...please wait..."); + if (comms_.connected()) + { + comms_.disconnect(); + } + comms_.connect(cfg_.device, cfg_.baud_rate, cfg_.timeout_ms); + RCLCPP_INFO(rclcpp::get_logger("ActuatorsRpPicoHardware"), "Successfully configured!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn ActuatorsRpPicoHardware::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("ActuatorsRpPicoHardware"), "Cleaning up ...please wait..."); + if (comms_.connected()) + { + comms_.disconnect(); + } + RCLCPP_INFO(rclcpp::get_logger("ActuatorsRpPicoHardware"), "Successfully cleaned up!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + + +hardware_interface::CallbackReturn ActuatorsRpPicoHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("ActuatorsRpPicoHardware"), "Activating ...please wait..."); + if (!comms_.connected()) + { + return hardware_interface::CallbackReturn::ERROR; + } + + + RCLCPP_INFO(rclcpp::get_logger("ActuatorsRpPicoHardware"), "Successfully activated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn ActuatorsRpPicoHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("ActuatorsRpPicoHardware"), "Deactivating ...please wait..."); + RCLCPP_INFO(rclcpp::get_logger("ActuatorsRpPicoHardware"), "Successfully deactivated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::return_type ActuatorsRpPicoHardware::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +{ + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type actuators_rppico_h_i ::ActuatorsRpPicoHardware::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + if (!comms_.connected()) + { + return hardware_interface::return_type::ERROR; + } + + for (int i = 1; i <= 12; i++) { + if (cfg_.servo_names[i] != "") { + comms_.set_servo_angle(i, servos_[i].cmd); + } + } + + return hardware_interface::return_type::OK; +} + + +} // namespace actuators_rppico_h_i +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + actuators_rppico_h_i::ActuatorsRpPicoHardware, hardware_interface::SystemInterface) diff --git a/actuators_rppico_hardware_interface/hardware/include/actuators_rppico_h_i/actuators_rppico_h_i.hpp b/actuators_rppico_hardware_interface/hardware/include/actuators_rppico_h_i/actuators_rppico_h_i.hpp new file mode 100644 index 0000000..05f9f63 --- /dev/null +++ b/actuators_rppico_hardware_interface/hardware/include/actuators_rppico_h_i/actuators_rppico_h_i.hpp @@ -0,0 +1,107 @@ +// Copyright 2021 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ACTUATORS_RPPICO_H_I__OMNIBOT_SYSTEM_HPP_ +#define ACTUATORS_RPPICO_H_I__OMNIBOT_SYSTEM_HPP_ + +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "actuators_rppico_h_i/visibility_control.h" + +#include "actuators_rppico_h_i/rppico_comms.hpp" +#include "actuators_rppico_h_i/servo.hpp" + +namespace actuators_rppico_h_i +{ +class ActuatorsRpPicoHardware : public hardware_interface::SystemInterface +{ + +struct Config +{ + std::array servo_names = {}; + + float loop_rate = 0.0; + + std::string device = ""; + int baud_rate = 0; + int timeout_ms = 0; + +}; + + +public: + + int number_of_servos = 0; + + RCLCPP_SHARED_PTR_DEFINITIONS(ActuatorsRpPicoHardware); + + ACTUATORS_RPPICO_H_I_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + ACTUATORS_RPPICO_H_I_PUBLIC + std::vector export_state_interfaces() override; + + ACTUATORS_RPPICO_H_I_PUBLIC + std::vector export_command_interfaces() override; + + ACTUATORS_RPPICO_H_I_PUBLIC + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + ACTUATORS_RPPICO_H_I_PUBLIC + hardware_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + + ACTUATORS_RPPICO_H_I_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + ACTUATORS_RPPICO_H_I_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + ACTUATORS_RPPICO_H_I_PUBLIC + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + ACTUATORS_RPPICO_H_I_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + + RpPicoComs comms_; + Config cfg_; + + std::array servos_; + +}; + +} // namespace ACTUATORS_RPPICO_H_I + +#endif // ACTUATORS_RPPICO_H_I__DIFFBOT_SYSTEM_HPP_ diff --git a/actuators_rppico_hardware_interface/hardware/include/actuators_rppico_h_i/rppico_comms.hpp b/actuators_rppico_hardware_interface/hardware/include/actuators_rppico_h_i/rppico_comms.hpp new file mode 100644 index 0000000..8e4386c --- /dev/null +++ b/actuators_rppico_hardware_interface/hardware/include/actuators_rppico_h_i/rppico_comms.hpp @@ -0,0 +1,136 @@ +#ifndef ACTUATORS_RPPICO_H_I__OMNIBOT_COMMS_HPP_ +#define ACTUATORS_RPPICO_H_I__OMNIBOT_COMMS_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + + + +LibSerial::BaudRate convert_baud_rate(int baud_rate) +{ + // Just handle some common baud rates + switch (baud_rate) + { + case 1200: return LibSerial::BaudRate::BAUD_1200; + case 1800: return LibSerial::BaudRate::BAUD_1800; + case 2400: return LibSerial::BaudRate::BAUD_2400; + case 4800: return LibSerial::BaudRate::BAUD_4800; + case 9600: return LibSerial::BaudRate::BAUD_9600; + case 19200: return LibSerial::BaudRate::BAUD_19200; + case 38400: return LibSerial::BaudRate::BAUD_38400; + case 57600: return LibSerial::BaudRate::BAUD_57600; + case 115200: return LibSerial::BaudRate::BAUD_115200; + case 230400: return LibSerial::BaudRate::BAUD_230400; + default: + std::cout << "Error! Baud rate " << baud_rate << " not supported! Default to 57600" << std::endl; + return LibSerial::BaudRate::BAUD_57600; + } +} + +class RpPicoComs +{ + +public: + + RpPicoComs() = default; + + void connect(const std::string &serial_device, int32_t baud_rate, int32_t timeout_ms) + { + std::cout << "Connecting to serial device: " << serial_device << " at baud rate: " << baud_rate << std::endl; + timeout_ms_ = timeout_ms; + serial_conn_.Open(serial_device); + serial_conn_.SetBaudRate(convert_baud_rate(baud_rate)); + } + + void disconnect() + { + serial_conn_.Close(); + } + + bool connected() const + { + return serial_conn_.IsOpen(); + } + + + std::string send_msg(const std::string &msg_to_send, bool print_output = false) + { + serial_conn_.FlushIOBuffers(); // Just in case + serial_conn_.Write(msg_to_send); + + std::string response = ""; + //skip the response part for now + /* + try + { + // Responses end with \r\n so we will read up to (and including) the \n. + serial_conn_.ReadLine(response, '\n', timeout_ms_); + } + catch (const LibSerial::ReadTimeout&) + { + std::cerr << "The ReadByte() call has timed out." << std::endl ; + } + */ + if (print_output) + { + std::cout << "Sent: " << msg_to_send << " Recv: " << response << std::endl; + } + + + return response; + } + + + void send_empty_msg() + { + std::string response = send_msg("\r"); + } + + void read_encoder_values(int &val_1, int &val_2,int &val_3, int &val_4) + { + std::string response = send_msg("e\r"); + + std::string delimiter = " "; + size_t del_pos = response.find(delimiter); + std::string token_1 = response.substr(0, del_pos); + std::string token_2 = response.substr(del_pos + delimiter.length()); + std::string token_3 = response.substr(del_pos + delimiter.length()); + std::string token_4 = response.substr(del_pos + delimiter.length()); + + val_1 = std::atoi(token_1.c_str()); + val_2 = std::atoi(token_2.c_str()); + val_3 = std::atoi(token_3.c_str()); + val_4 = std::atoi(token_4.c_str()); + + } + void set_motor_values(int val_1, int val_2, int val_3, int val_4) + { + std::stringstream ss; + ss << "m " << val_1 << " " << val_2 << " " << val_3 << " " << val_4 << "\r"; + send_msg(ss.str()); + } + + void set_pid_values(int k_p, int k_d, int k_i, int k_o) + { + std::stringstream ss; + ss << "u " << k_p << ":" << k_d << ":" << k_i << ":" << k_o << "\r"; + send_msg(ss.str()); + } + + void set_servo_angle(int servo_num, int angle) + { + std::stringstream ss; + ss << "s " << servo_num << " " << angle << "\r"; + send_msg(ss.str()); + } + + +private: + LibSerial::SerialPort serial_conn_; + int timeout_ms_; +}; + +#endif // ACTUATORS_RPPICO_H_I__OMNIBOT_SYSTEM_HPP_ \ No newline at end of file diff --git a/actuators_rppico_hardware_interface/hardware/include/actuators_rppico_h_i/servo.hpp b/actuators_rppico_hardware_interface/hardware/include/actuators_rppico_h_i/servo.hpp new file mode 100644 index 0000000..10fb14e --- /dev/null +++ b/actuators_rppico_hardware_interface/hardware/include/actuators_rppico_h_i/servo.hpp @@ -0,0 +1,37 @@ +#ifndef ACTUATOR_SERVO_HPP +#define ACTUATOR_SERVO_HPP + +#include +#include + + +class Servo +{ + public: + + std::string name = ""; + int enc = 0; + double cmd = 0; + double angle = 0; + + Servo() = default; + + Servo(const std::string &servo_name, int servo_number) + { + setup(servo_name, servo_number); + } + + + void setup(const std::string &servo_name, int servo_number) + { + name = servo_name; + servo_number = servo_number; + } + + + private: + int servo_number = 0; +}; + + +#endif // DIFFDRIVE_ARDUINO_WHEEL_HPP diff --git a/actuators_rppico_hardware_interface/hardware/include/actuators_rppico_h_i/visibility_control.h b/actuators_rppico_hardware_interface/hardware/include/actuators_rppico_h_i/visibility_control.h new file mode 100644 index 0000000..4dd4098 --- /dev/null +++ b/actuators_rppico_hardware_interface/hardware/include/actuators_rppico_h_i/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2021 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef ACTUATORS_RPPICO_H_I__VISIBILITY_CONTROL_H_ +#define ACTUATORS_RPPICO_H_I__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ACTUATORS_RPPICO_H_I_EXPORT __attribute__((dllexport)) +#define ACTUATORS_RPPICO_H_I_IMPORT __attribute__((dllimport)) +#else +#define ACTUATORS_RPPICO_H_I_EXPORT __declspec(dllexport) +#define ACTUATORS_RPPICO_H_I_IMPORT __declspec(dllimport) +#endif +#ifdef ACTUATORS_RPPICO_H_I_BUILDING_DLL +#define ACTUATORS_RPPICO_H_I_PUBLIC ACTUATORS_RPPICO_H_I_EXPORT +#else +#define ACTUATORS_RPPICO_H_I_PUBLIC ACTUATORS_RPPICO_H_I_IMPORT +#endif +#define ACTUATORS_RPPICO_H_I_PUBLIC_TYPE ACTUATORS_RPPICO_H_I_PUBLIC +#define ACTUATORS_RPPICO_H_I_LOCAL +#else +#define ACTUATORS_RPPICO_H_I_EXPORT __attribute__((visibility("default"))) +#define ACTUATORS_RPPICO_H_I_IMPORT +#if __GNUC__ >= 4 +#define ACTUATORS_RPPICO_H_I_PUBLIC __attribute__((visibility("default"))) +#define ACTUATORS_RPPICO_H_I_LOCAL __attribute__((visibility("hidden"))) +#else +#define ACTUATORS_RPPICO_H_I_PUBLIC +#define ACTUATORS_RPPICO_H_I_LOCAL +#endif +#define ACTUATORS_RPPICO_H_I_PUBLIC_TYPE +#endif + +#endif // ACTUATORS_RPPICO_H_I__VISIBILITY_CONTROL_H_ diff --git a/actuators_rppico_hardware_interface/package.xml b/actuators_rppico_hardware_interface/package.xml new file mode 100644 index 0000000..3d43d0d --- /dev/null +++ b/actuators_rppico_hardware_interface/package.xml @@ -0,0 +1,34 @@ + + + + actuators_rppico_h_i + 0.0.0 + hardware interface for ezbot's actuators + + Vincent Belpois + + + Apache-2.0 + + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + libserial-dev + + ros2_controllers_test_nodes + joint_state_broadcaster + diff_drive_controller + ros2controlcli + controller_manager + rviz2 + robot_state_publisher + + ament_cmake_gtest + + + ament_cmake + + diff --git a/omnidrive_rppico/hardware/omnibot_pico_system.cpp b/omnidrive_rppico/hardware/omnibot_pico_system.cpp index 4c48e47..f0079d7 100644 --- a/omnidrive_rppico/hardware/omnibot_pico_system.cpp +++ b/omnidrive_rppico/hardware/omnibot_pico_system.cpp @@ -103,9 +103,9 @@ hardware_interface::CallbackReturn OmniDriveRpPicoHardware::on_init( joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); return hardware_interface::CallbackReturn::ERROR; } - } RCLCPP_INFO(rclcpp::get_logger("OmniDriveRpPicoHardware"), "Successfully set up hardware interfaces"); return hardware_interface::CallbackReturn::SUCCESS; + } } std::vector OmniDriveRpPicoHardware::export_state_interfaces() diff --git a/rpi-pico-actuators/src/main.cpp b/rpi-pico-actuators/src/main.cpp index c182962..15229ca 100644 --- a/rpi-pico-actuators/src/main.cpp +++ b/rpi-pico-actuators/src/main.cpp @@ -98,8 +98,8 @@ void loop() { //run speed each motor - while(Serial.available() > 0){ - chr = Serial.read(); + while(Serial2.available() > 0){ + chr = Serial2.read(); // if the chr is a carriage return if (chr == '\r'){ diff --git a/src/ezbot_robot/config/omnidirectional_controller.yaml b/src/ezbot_robot/config/omnidirectional_controller.yaml index dfbfc62..86fff07 100644 --- a/src/ezbot_robot/config/omnidirectional_controller.yaml +++ b/src/ezbot_robot/config/omnidirectional_controller.yaml @@ -9,6 +9,11 @@ controller_manager: omnidirectional_controller: type: omnidirectional_controllers/OmnidirectionalController + actuator_controller: + type: position_controllers/JointGroupPositionController + + + omnidirectional_controller: ros__parameters: wheel_names: @@ -17,7 +22,7 @@ omnidirectional_controller: - roue_arriere_joint - roue_droite_joint - robot_radius: 0.15 + robot_radius: 0.19 wheel_radius: 0.03 publish_rate: 30.0 @@ -46,4 +51,7 @@ omnidirectional_controller: angular_min_acceleration: -2.0 - +actuator_controller: + ros__parameters: + joints: + - renverseurDePano diff --git a/src/ezbot_robot/description/ros2_control.xacro b/src/ezbot_robot/description/ros2_control.xacro index 61f5c4d..f2afa99 100644 --- a/src/ezbot_robot/description/ros2_control.xacro +++ b/src/ezbot_robot/description/ros2_control.xacro @@ -54,4 +54,26 @@ + + + + + actuators_rppico_h_i/ActuatorsRpPicoHardware + renverseurDePano + 30 + /dev/ttyAMA2 + 115200 + 10000 + + + + + + 0 + 180 + + + + + \ No newline at end of file diff --git a/src/ezbot_robot/launch/real_robot.launch.py b/src/ezbot_robot/launch/real_robot.launch.py index 21ac4e7..36b156a 100644 --- a/src/ezbot_robot/launch/real_robot.launch.py +++ b/src/ezbot_robot/launch/real_robot.launch.py @@ -86,6 +86,10 @@ def generate_launch_description(): actions=[joint_broad_spawner], ) + actuators_spawner = Node( + package='controller_manager', + + ) imu_node = Node( package='ros_qwiic_icm_20948', executable='ros_qwiic_icm_20948',