From 4b4361783dd2ac3b91a61fc9f09615e411ec4e82 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Tue, 20 Jun 2023 17:42:14 -0400 Subject: [PATCH 01/37] first commit; mostly file name changes --- example_11/CMakeLists.txt | 73 ++++++ example_11/README.md | 6 + .../config/carlikebot_controllers.yaml | 57 +++++ .../bringup/launch/carlikebot.launch.py | 122 ++++++++++ .../description/launch/view_robot.launch.py | 99 ++++++++ .../carlikebot.ros2_control.xacro | 26 +++ example_11/description/rviz/carlikebot.rviz | 172 ++++++++++++++ .../description/rviz/carlikebot_view.rviz | 172 ++++++++++++++ .../urdf/carlikebot.materials.xacro | 40 ++++ .../description/urdf/carlikebot.urdf.xacro | 19 ++ .../urdf/carlikebot_description.urdf.xacro | 184 +++++++++++++++ example_11/doc/carlikebot.png | Bin 0 -> 19716 bytes example_11/doc/userdoc.rst | 123 ++++++++++ example_11/hardware/carlikebot_system.cpp | 220 ++++++++++++++++++ .../carlikebot_system.hpp | 81 +++++++ .../visibility_control.h | 56 +++++ example_11/package.xml | 40 ++++ example_11/ros2_control_demo_example_11.xml | 9 + ros2_control_demos/package.xml | 1 + 19 files changed, 1500 insertions(+) create mode 100644 example_11/CMakeLists.txt create mode 100644 example_11/README.md create mode 100644 example_11/bringup/config/carlikebot_controllers.yaml create mode 100644 example_11/bringup/launch/carlikebot.launch.py create mode 100644 example_11/description/launch/view_robot.launch.py create mode 100644 example_11/description/ros2_control/carlikebot.ros2_control.xacro create mode 100644 example_11/description/rviz/carlikebot.rviz create mode 100644 example_11/description/rviz/carlikebot_view.rviz create mode 100644 example_11/description/urdf/carlikebot.materials.xacro create mode 100644 example_11/description/urdf/carlikebot.urdf.xacro create mode 100644 example_11/description/urdf/carlikebot_description.urdf.xacro create mode 100644 example_11/doc/carlikebot.png create mode 100644 example_11/doc/userdoc.rst create mode 100644 example_11/hardware/carlikebot_system.cpp create mode 100644 example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp create mode 100644 example_11/hardware/include/ros2_control_demo_example_11/visibility_control.h create mode 100644 example_11/package.xml create mode 100644 example_11/ros2_control_demo_example_11.xml diff --git a/example_11/CMakeLists.txt b/example_11/CMakeLists.txt new file mode 100644 index 000000000..129560ed2 --- /dev/null +++ b/example_11/CMakeLists.txt @@ -0,0 +1,73 @@ +cmake_minimum_required(VERSION 3.16) +project(ros2_control_demo_example_11 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( + ros2_control_demo_example_11 + SHARED + hardware/carlikebot_system.cpp +) +target_compile_features(ros2_control_demo_example_11 PUBLIC cxx_std_17) +target_include_directories(ros2_control_demo_example_11 PUBLIC +$ +$ +) +ament_target_dependencies( + ros2_control_demo_example_11 PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +# 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 "ROS2_CONTROL_DEMO_EXAMPLE_11_BUILDING_DLL") + +# Export hardware plugins +pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_11.xml) + +# INSTALL +install( + DIRECTORY hardware/include/ + DESTINATION include/ros2_control_demo_example_11 +) +install( + DIRECTORY description/launch description/ros2_control description/urdf description/rviz + DESTINATION share/ros2_control_demo_example_11 +) +install( + DIRECTORY bringup/launch bringup/config + DESTINATION share/ros2_control_demo_example_11 +) +install(TARGETS ros2_control_demo_example_11 + EXPORT export_ros2_control_demo_example_11 + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) +endif() + +## EXPORTS +ament_export_targets(export_ros2_control_demo_example_11 HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/example_11/README.md b/example_11/README.md new file mode 100644 index 000000000..841051c0b --- /dev/null +++ b/example_11/README.md @@ -0,0 +1,6 @@ +# ros2_control_demo_example_11 + + *CarlikeBot*, or ''Carlike Mobile Robot'', is a simple mobile base with [ackermann drive](https://en.wikipedia.org/wiki/Ackermann_steering_geometry). + The robot has two wheels in the front that steer the robot and two wheels in the back that power the robot forward and backwards. + +Find the documentation in [doc/userdoc.rst](doc/userdoc.rst) or on [control.ros.org](https://control.ros.org/master/doc/ros2_control_demos/example_11/doc/userdoc.html). diff --git a/example_11/bringup/config/carlikebot_controllers.yaml b/example_11/bringup/config/carlikebot_controllers.yaml new file mode 100644 index 000000000..d45de1b44 --- /dev/null +++ b/example_11/bringup/config/carlikebot_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/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py new file mode 100644 index 000000000..c9cba5f10 --- /dev/null +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -0,0 +1,122 @@ +# 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 DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "start_rviz", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ) + ) + + # Initialize Arguments + start_rviz = LaunchConfiguration("start_rviz") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_2"), "urdf", "diffbot.urdf.xacro"] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_2"), + "config", + "diffbot_controllers.yaml", + ] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_2"), "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], + condition=IfCondition(start_rviz), + ) + + 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(declared_arguments + nodes) diff --git a/example_11/description/launch/view_robot.launch.py b/example_11/description/launch/view_robot.launch.py new file mode 100644 index 000000000..badd807c2 --- /dev/null +++ b/example_11/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="ros2_control_demo_example_2", + 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/example_11/description/ros2_control/carlikebot.ros2_control.xacro b/example_11/description/ros2_control/carlikebot.ros2_control.xacro new file mode 100644 index 000000000..e1f184d5e --- /dev/null +++ b/example_11/description/ros2_control/carlikebot.ros2_control.xacro @@ -0,0 +1,26 @@ + + + + + + + + ros2_control_demo_example_2/DiffBotSystemHardware + 0 + 3.0 + + + + + + + + + + + + + + + + diff --git a/example_11/description/rviz/carlikebot.rviz b/example_11/description/rviz/carlikebot.rviz new file mode 100644 index 000000000..365d5724e --- /dev/null +++ b/example_11/description/rviz/carlikebot.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/example_11/description/rviz/carlikebot_view.rviz b/example_11/description/rviz/carlikebot_view.rviz new file mode 100644 index 000000000..d887a8193 --- /dev/null +++ b/example_11/description/rviz/carlikebot_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/example_11/description/urdf/carlikebot.materials.xacro b/example_11/description/urdf/carlikebot.materials.xacro new file mode 100644 index 000000000..035bf5822 --- /dev/null +++ b/example_11/description/urdf/carlikebot.materials.xacro @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_11/description/urdf/carlikebot.urdf.xacro b/example_11/description/urdf/carlikebot.urdf.xacro new file mode 100644 index 000000000..a2858e1b2 --- /dev/null +++ b/example_11/description/urdf/carlikebot.urdf.xacro @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + diff --git a/example_11/description/urdf/carlikebot_description.urdf.xacro b/example_11/description/urdf/carlikebot_description.urdf.xacro new file mode 100644 index 000000000..03aa3497f --- /dev/null +++ b/example_11/description/urdf/carlikebot_description.urdf.xacro @@ -0,0 +1,184 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_11/doc/carlikebot.png b/example_11/doc/carlikebot.png new file mode 100644 index 0000000000000000000000000000000000000000..1c4fc2476dba992c2984f32d8225fadf4cb9eba2 GIT binary patch literal 19716 zcmeIac|4SD^e}!?O0-N8k+sbzA*n23vW+ARO17x%Eh3R@BV@}o)lkSfh>#~s3t5t~ zRg9vL$P!`5n%(bQqv!d)zt8*E`|taC=kxS*+;iR6b@p?vbB@9c^@;1a1i3H_TX$4P z(+I;>w!)vSob2#~VMSUCf33N!V|E3@)>ooGY*BmGZ^bY%?5O4ulWPx$+AXeFcwU$3 zFT)#k9!=U&$Fn!UH&12fn`}wXJY#0f4(~?eEBiWZe%jaf`p{?C^Id+fNZlQk_5DBz z{jA4vrA*q8N0r;ROAMa9W`_y9M?-lP`Rjs?JbZyk*~BOGsjp(goi|I>S)48%^vxX_ z8d?eXJa`gQU;uxU<#>o}7^V}T0O%9ADoOCnNQ?rQQ`ud4;d|atIy_1x2yp3NSnS#UB$W#MEdHwi&$(arN3X!7mYG*6Nnml-Qtv>khvzIfr zzc*T{FsQrRSe23$^qh)RfV9&-Rg`%gUm!11wr#03#3GaKM(t9#%4>L1&A(>z1^n`MZ zeSXWhhuT1sF|q=^4%!ppo!qNd*ZFO@TA4gm@QJiJ(GX{pD>8xsqMl+>sQwj1}#+<=olfr(tM)P_`SD2rK6qe%d{*~XRnyyY!WX&T+P)MdPd@?gG2uP*rEGDM{O8sT z(<@`-{)*H23s+mKQi4SGNc;QbcgLMQ-)+|)+t*T}RG2v3;ppqXld@0ar@M4r5W_v_ zy>G;0I zKNZtu<&+Md=m?|zZw4qdSNkPR(T!1hZaehvIz+Nv=8A@6DL(ftLz_2a+H zRL74wbG^K-L=-qUdN7XY>i1b-9KUE6)g0)L*;b(AHrVQ*g>|gUr?hiSmx+hOf#p2GUuFxklLwG;u z(qIoDJ?B}oDr?((^}5I4*`FDSuewF` zKWl%#aInq)@$jE}LH-If&H5URBjbA~Omm)m9u#%Cq*`-#(*)us9U zOQ*KfI=-A$Tk2aX?$~5Pd};H(PHyx5=oPk_7Zz=cxVy2- zk+LR6R(O-Nn_SQt=Y-6r7>%d8AB^kXFT}bWI+QrQn&W8ZmoFNeiT;at=B5DAP`a6X z#gM4@rgsd`odxcAOJ2pA8{48{C1^1gb&2&ZCMj8Tdf7eue6P_j+4sw`r!KrODJf0p z6)&RQQ&3V_^tr`up-fWUNQ1^bvo;hwS ze9jTqiBASgc~|EeWg_nT9S~aDoagk4E#F(E%`t${|2TL^$Cl~f)Ru46sCUEdX+v;h zcyWJkV{ZO^zhL2|&8z%tBF3Yn7x&!qGE1kf*~C*;-O^sWUXN0k*m5Wg*l|!lcnvK! zV%E$#RNCx!T*rKRhlW?HveNXi(+_jqnPumPzRHREeVzXECZ0LBVE#Pr1HGAbEX7}U9 zYe$Kz*F2Cl#<2Ab&fEe3yE)F7*)C<++`sH^Id7@1@p4|S`B_| z3a?e?KkL^YGQT!vI``nJw~WZhkbA0!W%DSxyY>}3*ZXfdp7hjXKB4cJf3L?cnYP+b z2i66e&8@NH+v|QutA>6o_G*A$n3|F!msMd=dLYlfUko2^&5!y0^a6vG+Ts-TXO*JD zJ#rtZ0Z*KEo=tz#BcAjKc*pp9Ppd}8mDh-xxlN+ma2qAGGHmeql=0%qZ8PTm>o-MK!Wydk4obR(>F)6y8L94a`v@#_%nYz;!fQJ4nx^UA9`%8pU6;*v zP`ZYTM@qMC%A0`3KMThP4c+nHNO<_xN7pH{jMaXf9Mwz3(Q_f)iJtVx$6AR z3Y=9fLC9_!uVR4ICm-%jCn!&T)kKGO`2NvPGjh5ZAoydm7=I;6+twXN-M(wXk(Qn! zO(8pmHhtf%g08W`--1|CMI`e+0#;R>kUz-aY!c(;CJS=7xwEoO-&Xn5%<%Chk zQVy>43=MZ`G> zZL#KIJjLUcM)xjBfdTMrY@+{qOSuu&LLnxiz#9Df4T(l`B#&Gac*8Mgv4| z@^%fA%NlClTG0c^jd@d>@Q=(d=9pXE(Td5CqJ_>$cH-%`s@nTwAU`sS$u-%{h-XCF z=L;;fTo5VzzP9f`z%+5K+-~Z{SQoa6nWX4?HGkSv7{Qrq3sHjE_@a%}bfLHm{4DAB z(6v9qf0#}g1H+vBT|d)e1ghokgXWUyyA)|f-f zjwwM<3zKmVnCBtR?e6iTEk2cve-T&x@UhsidTaK=%FxUVZYvx4GzwjN_0CJvFXGCB zi(m6?d!Y2;IksI^lV6EfjbdJHwj;CB$|AJ9N1Qw=kiy*_jw5onoH({yPBnUFen#?w zc*+IXOQ+e3?|>%1G$g(!dQ!2<_Sf|iTFrj!dX!-427{Yj8QUaoWtyCenJbBq;dTW9 zJy@^de&(OdcO-hCSXth|XvMGU@@s*l^m?4t3zIQd6Vtu2H?rThzj6@=zKU7p;Vbv) z3N@YJ?$Li*C1<)j3a6eX*Abq?ESo&PbhO&^99i<829C3WyhLSbX|C{xn6I9f{)vgE z>4iUPoZ=?C#~A^Fk?$h->meD0*j-;n&UJs4yH^vtXP?YniPWghYIqa|kG>cQ)Cuhp@(f|xip)zJ+QU~0 z@Kul5+)*w84=ku!=w~-j&1kQ(W z(5(M1|I00PLg{(I_>oGIO&3_U*xYh_2%6~KHq3WUDqn$XM}4aSHkNkU+C_fKmXyBz zi|e75kG8J7TkgZgJzvtet;9U@o&`H%@F@1D1r{zo87xgz-X3W;d`yT#`pg&(3$_iF zP6b9O0S5>_fRAQSGyqjMl)l-ot{qA@R039cCh4N8>GO+ zVat@j+`+))bcBMxb+MC_$KkZo&OQYxYj zSi3nG3SC^dE;%bXKkrf4?L>;qV6G8;UZ$v!9iC@P1wK-@Gt~J5BgAf>JuvueX}*2X zVf4t-w3b;OVt(~@4R`U8U4BeQBho?lv=-Nt3oo))#IPeGIDlbnw%2JSQ^kVq61PO` zvYMhI>?}ynszI9M8~Zf*!^JN;eJAN18E9r7Lj6d^;tL zTg9Ru+-L{9II)bArp|>2xUG_GDj8kX6{-oEDcL|8L0Q8+Iir%KuA^Lga(7g}Z`=p_ z!lK3tBEh)W5B61_!bf;9-D;c_Ue=Z7x^AnTa(H5t?oO^W$e+y$%`es`b`Nz+HWub# zhUS2BoI-8o+r+~YU2n%DMwDh%6QjICbt1e{cxYK$r<|J+1o3;C-=ZS3OlstH@CDhB)?6=GDA%5#k zi@nO%Y0N8UFgVrp%0+^$q(O!<(+xalt(AFs7kED-4B)~D7(u0g57o3 zRj7)P__n-<#eoV+5K9BHH`)bAhTiP2O@T%3$Q5u&ag(0SHKa({>J}CmW#yRlv%7C1 z-gAH$DRfk;h1!o8de<&gT9&M!Bn?`X?&6kZR~44`BX7Meek-8)x{{IXRLW6ZSfkY< zdpYze{#Z+ON_ko(D=1&$MrLugHK3Jx$uv<#~8~JWN*yBC4na?$3O%YIZrI5+QRd4 zq~Mk#8=hT7f*?!IjIpGxTM@FQPrg=L!`+T#V;d7e2VP=cCXbu)Sl3|)i?Znxtm6nZ zy?bf}Q@D57wYr*s%@A%NVLT9jN1{c}emVW>nuyf_rOp>Ox2(7ydrb&gk68Xq!LycG zIZe)IAXZJhPn{YK%`-HwsX{Ek!XJ4j1H;N6yo+QV%6qzt+e*_+>=q6&0i)?Q^A+vb zY^Pz$GUPfXYQUxYl?ZQ4H|rX=)vo8}&&;h}o_e;~?uQ%HCmtm(+WiAwG>InV{!8{H ze(S2??u~Ym{~U33KiB=^^T!n!c_^c)*}1TA>b2Ib?bca#XPnbZJb5wltwE8LW8vwq z%%{4Z4t1Z&Lp5MO+BD9p@j$%zyY}1Rd6nW&5dIywtlT^`NYkEuMa$F`tWjJIt5&Bm zRSWQO2cU*aXc0&`mL8hdi(aC|O6lJD|50%aA_#_;0iR+Ay7kw!O|3>R9tC|o%96>S z(H@oBaHt2`*(P(_Rw}TR5b=6bcph5$OB2bfx)^EErNW_pk_Ut;EJ>4<8-Gye7t?bM zbyY(v%|JnR*ny|phOhw*Bs9NI^@Vbz-a{!aTj~HTd99`4e(0b`y6-lO9~HZ`PUhx@ z;MOuYATj}}*^bE(ueAsVArHYgAnH+lwFTHP-bg8~o*U@(WH_NG7(uQqti2K-415}LOrLD-{lxX6?Cy&IMVGng6`Xr#2|cQ zv#>;W=CPzzYJ zp$+1OHUw($w_F%iYjU;-sCtU1YT;SOX#eK3ErR{6J#8aKxLKV-@IvU>1_Pp*Ea(P6 zEeJ}j2M-g3NHhZDFw{o}WM=-26J$3dVGc}($f8R37@~#aW&U@DpD3#xb# zw!qp^mvv14Q8>nC+sFJ2_@V?Y*#Rh$kFlUkbF0jzW+`{J7c|8FvnpXmU48=CORC565{}(2ra;{ zOoW*3-v9WG7Y1DLG8*n|Mmu2dohMlQrfx$|;~@e1`E!#Qub39n?_SUb+aTE8a2trn z7M)|r$4a)uwgV1iBZc}D^aWCULm$2(jAcuPw*Puf!YYJ)@aA=lC@r=;BqIDSI9E;| zLEyy2=0Lxb96>VG9H8HXWH1pbKSJCULE908%!aCrpkeR~R3d^(L4&Er2%;d!8bPNK zbRP7ZdJItNi3g?XF(@g$ba^A;JBWdh0Covwyy_Ip%K;cp-I|9d+(8US21Iy`AY?#< zQ3RPe03?o(j{$KINC*=6H?72Yqkvwj7K;%{<%kjQAsRQZJ)(q0)pHbnq`$_{P%G zl(?|ePL@Y8JPjn57XU*DAYGf3Xnkv$_)J=6GW z5LZ#mFY4Ue7?^d8`KLN77~BNL1dMo^MKtGBlDludGG7JEYk-T6*ildYgfj1WpZEL% zin*)q9LTaIVMV~m9o>y7j(JrBFGeqYxsqbJJJ&t{>|g>^N%o1;D#momcvzxoV032< zXe!wcHh*_l$Me!|r7`FJPdrk&6#Cx_2X-CIU<>JBM9P}Grd4N>xv~4`f%%U>N4+s; zHBcs82w;DCtSw&IxA%P(;Dx)Vz(ue;_9)lCd0vT`kEcEGI?&_5kp(taDM4Ystl0k2 zspbpUNl>vZ;qdNGzvg*!+=uL;Y#X?JX>jJ@u~DF4qF0V_c8dqcAg9*>)=dP{*hpmS zKjL{Qb8Va{-8)5j9fQNh7F3C!GsS&S`0h4Ts41hZ+}_oI)6|o zF`>!%eBFru1z_ZM5HjZx676e>l%Tq!{M6!(2zJ?3uDnS@D@k4F6CReR%>D47GaOBR z8mkCnV{kXcAAoJXBxteO_;le3NB72;E}D%G7S!MHx#rCmD-nNm!tr2DVH!rGDsUMp z6!Zagy4J6;oDDeSL3Co;OU^dWGCP~mln>>R+^8)rWTfVHN@9dgUHk2 zwv1=jBE8#Z&ZkmTBp-HZ|xzuEiZtX3lkB}Ez9Zi5H0uv6Bm)4*(5{!&%_ITJh=U|Li>vJSPKe8?IOpOjrL%pIWnS)BO}(+H|DQ)4-LM%eWt5p>m#O1g$=Ot zO&nMO3qc7#NJ+e*LRX868voypwZqXM`C2>&?G-pxXb#BALlQmrn zL;*E{)YUIULC$ceT+M)Ej;9IdfLR#s=<3Hy8FEx-v)?}f#)fW|L1sx@w|+7fyJp9Z zv^tP{6xSh>H`M9sr=hKVTZY&Uf}*DI5RL7rJOt^~6DO}e#bX@1DE9XOU$1@1laI|l zrV#UwIVG}VFC{2HZ*x4xX5?D0H%U2=Fr6R;bD>#3Hn&ICW|O1eTb3qst1{#HFuh;} zb1ry}k+U}vk9}Z2QB5J{?!C^2)oKZ)9pW~DFT!|;0u!Y5Q?C}o6tYU0sUee zQIZ|)D+Xl;u8-^|Fsyo)EA4I`VKasdsDfjaGqc$TChYF z*a&%UD4puUm%)xvjl1x?_t4h14;8r5&LOf;`9|#@Km_@NfMW<>8y=`2xzGCDo9r)y z>K-91luj4}(P5oNU3l3OXeU&8(u23$dVUYMwLXH74QtY)oT-iUh2YDFnD=1NgbylV zdje?~<_3N*{2fqDw!{$w^6)2s0#v8LkPOhw$pjFImG?j@NdJcr)!3T(tv%=4(K#s$ z52GkzN4upA+e@aUsYk%%)_~JW7q{<_v29@t-32|p0e*mH)j#Je(PSL32#S%gcX1mt zA4@W{Z&U@24}wEORXl7JMCk`O*iS=*DUKto;n1pq+Ew%$aA9XvqZ&W_AR3+ z4F(s;6>yZ;6_Js6SPb89f^dVyd+hdvfUpZF!lb|c51vrC`lS~MnaDf= z_K$`A#a4nfWP%6B;BPclW9R|fWr7=&>tQUaw>~2Lf(!*YRr1(fEjFDy@bb>9^2J~@ ztPHKT-_he&OP5TeDmSJJ2a_9tl4>-hQ>z6obUSi{UIk?jrX&r0l%tA~4g~>I37Log zqXkpvJadq_3B%IQjJ{*$QnCPn7PzJnBrO+479Sa0`nFzNUN91;PJhbZ-r*>3{+x}4 zI=(c%wmr9O`Z;cKZxAG(LAb|F&J%_?&ras}3AJCkoi3cY8RG%Q5MDD)hKElM7kt0P zV{h@%r}27*xs}uHOz|Lm<};%4zRl!RAiQPF(cjf~MvR%Cq@a}7Zxi|Md-c8ppmH`k z5P5l<4b9xiY|9T!&;w&5c(8BIFYv~0!b{K>LxOR2O5j_`3>e~Q8VG;DWoV9;9Up*Y z5gaGL`J_Skv#q0liMV}Q=824O5bnMF*_GfTR=^>6aS)QlVq%V`I;Z%nPel@3X@hkk z;w$$>y>^S$S`DV11f?xEFLE{pp{2T8Z}w?GN-SZ-+Ofb&R3L{I}M6UAAM@>?e@! zvyD0X`%1X5R0 zxd89nXV0F!^S^^nN~rqGo%UHQ{b!|1VuGLlghEltt5@H~4Ych8cC$=qLRKt&VEO~C zLseyAvA>G@IGt~JSbLb_SoG?ZR63;wP(c~dOPY?8`$|`7y#?#<)=$Ijl|Ufa*@4f% zs8ERiVr)4Nii(P6tt*_}9p>JB##PKV20VyOng&)s*mo5^?^kSGYsB6URwFCPe<=NC zbK~c!2Sugs4Mic-eq~n<`tb=TZBHK_8S(%9di6bJ%NlU@v}j1t3aoBrs@Z-vzsL-1 zi^#IrKtJ^MGaI}y_4ebh-uh!77Z(?so1+DPO~4E2&PqMD07W`gJOlhBUxiVhUYrt&WpU!v9_Z_9~ySxf=W~dBBr*29AjZ=FwiqPVW~ml)$}}x^UUas!zabGY49pdS}}6IW#z~}?2y#| zmHqD=M81OBVXIr4pB?$3r{AD_#8-Q%m5 zp7*PbkB|R}joWXq_$%bx!mp*cnNMnw9xq0h?Q>{EO@DA@>q{RR3hhEbXJzI4l%|2& zPAMg2W$)f^H8nNASf94%F7+Qj+S}WsGY=i&SJ}^%_QC#2#`s|cg;Qo`Ub7RwK0PvU zX)n6EFguyJIPplh-fQljnm_!xeMxlb_L9TkVwV5xqu*6aQ*W3bIzFh*v>S$qG=ouG z>ea9IS9vH0@nZ`p1mUc7B)!k9UIFVxDIdnK{|nK{ydrpXtumfi9&pyMl1Rjav_0D$q}A zYfGG;8TDTBt%}c&D{E_MiEp2O#AL<~)OCd5oF(edCcMYMr0e!VaZ53Q<64Qd{ zrh?3);MJ>##cpO$j3DpJxqQ^Ye|3jTIFY@87?VJLE#4P&_<6 zH8?{;LL?+4s;a8QgSxxBKYjXCQc@BV6Qdw;#Rmj*`JWdrrU#m`T*?*>7SZIn(in+< zhTDt1#_b1Zn9u3kwfk`DUt3zjE)9v4mR%{>6#ac^RMvmqB~rVa9=wpBpFf%A7;W@z zUs;ZnAWd6R+PBQm4HQ1i%*u-C<!`dS^7`N(z-a4y& zg(?v@rQR}z9?FPdy_Z=vB``IBv?R(7q4f1*+9FHOO1cS7@{O4S~v7oHxA+D7D z`}hAD=_vCYIX%HOQnuvB`J=b)KA3P$c}s1I=@ZMabb${aKPEhUXjzOnb{=dBJ$*9u zt+n;~)R5VPGh*lDKvSpTbswMkg+CoK($d=XE;7{h$|91FKRuiOW#F%*sOTihAyoDD z!R!iG-Y(GgF{mqw3ADSq6Zj&FF6Ua8iEP%=;LOU(0vaPl7Jk3k;PU;|%jf9h36gos zU^1DwiX4mQDShwj(TAvZw$x{4Y;L4%LtlGEj;3JLQ4P+(y42OIkG0=SFX$vu)c12y z8GV&{M8 zGB*=7IM1Iy??3%UR7qb_H+7`s&uRHV{Leka z$L!crAuC3Ge85A#eCu32DTM#747EP^vn^cqk80ApCu3h`;I_Z?b1zrFU+q0o!r$(=j&ae+O0HE-AwpY;D>Wh_LO9I*;>C-0 zcDs+fb`p2iZ%Jtz`|dqb*wrtQ=)dkc;OaU$#cb>FUs?c%`0d*__&;dvrS_u2k`m$LhXoby@W^>F zj_%k8-Ua% zEe#8yb>d*Ydt0a7+}%H4IB3CrxZ8a|I|XgavrgzSjHA@~O|>}W^nEM3I{0RT+O210 z-^%>`&!69;#Pz1O+J%r5G_ha=nJJ+(=gAXQzj=im`cjkiLeH8}ICjZ4R zR$>=F$a9H99GKL5hS}K!>=uAa1FxpLuv5VmJ4j`ldRqLHHZ zY7ZlFxY1Ez-xGwyCR+_GfRaEChr*+TQ3Zl z`p-T0->Uus91u`$xSAa=5lBN9yd`kPkK#s?^G@t-hh_|YgOhbs(`%R5k z-Co-3XC+_2Q5PH>jKW~wd%rXWW8#&Me2`&Qlew$B`HoHMFRt``9Pg{o@5qfSw1@eG z(0cY0U6A+pC(~)>xw(e~z2o8xwAb$d&ijEK(R5bxtA)g$bI)`xEGj8!zi`k%Uq8kB zyYHWN%Sr|mCjv%)&80yxtz3*%Ugxu8Z2Yd`++5;2=P{UY5`WwylvH0wgggyC!{1BV zzUKl%uJq~4cj=T#6Fj!0ny5pD;0wK{$=ND&bY04iS7v|OJv*2$mP-D7_nw<#M(r_8 z;j!~hcKfQREu3;~2DI(uM@us^fj$mnTA{OT^T*ZrHQXlLdO{pY&!ISW(qe9Yynff8 zy@wy4e)j!gOw5{#uJV5Ea=Q88I@>xrZjYbNxpsK}{&RDVP~xN>aOHYD>z9w-4q;7) z3G9z}_Jp1JgfiuR{kmZ5?zHwV9tjBv>tA$U0XG6Rp(0x3HU9JCNBub!hWz&UA{aTL z8l8r`*j=eU<@P(DcwoQgq1IgC$h8L&p6`79LxUkthe*lzm>NXW4q%tnkvtFAZs`nk z5$JYc;MDV54zqHc&?RYzN;B%ay^>h>EYow`zdjkpCHB?R@wnIA~sI(F!3T4VG)sCr%oJu zZ1(5(@88qY3f*apY{<%v&+%e%njJ1sLc(@ML8QNO<;v1*?$Ydd;?g{5V~+A8|Cjc2 z4A>Xqm)hJ={=P%Cv~XybC2~AQcQm_0zk;P@PQUv49eql#4xZ%YUhO_WF?(uhX$g6F z|4hS&xI>AmzD4=@SFT-C_$Z!!!OP3b-(L+1AYyEM86Hq|(A|3{Sb7C`Wcc_jE&Ku9 zyA<5`%lfI8yZZzyiBEhs?6o-WB^`U!)YMc|&F|rq88k%nPCD^tgwwI!sL$&6lP|cq z_)j*OGCZ57L|Q9_;UglV?SB*3n(xT@<81{0&n<6l)1?XKTxvQ;!FV#E5yG3-520Yh ze`&64aj@)Cdp@K!5DuVQ@d_y6Jmcr*mk}kaV)fFkx4$2+PaNK%J*QtWHs&_4Py~Le znb)3r7Zt%-svkY1e&1CySw0W$q_dNQ6wB7{C%k=pgxVF9l(@tT&N{evgX+jwmkz0F zRy8u}Rp+E)j1$8@cW3BZk%el$u%7_OV7cp%=JQ0z$SI*Oz)6nOkX)GLR{K4a^=aEF zH|5jkC{pcRsZv~qYg>lqHUsNy8#UbLV&dW=(gg~KTfmXFNPD}x%j<8w_&)O`o$M(pjqOC7m@GoOZM9#WIz!?fU&uY}L6NxgNtvFZR|H^|8}mHKe_Plm`g_a%C$>;5X|Q zYpn>^_PBhxVSEwNB!~J074NAbh*QkA=iNfvxKmt_LlV4On~(X(TAMlJAOxO<)qOY#^9w=a1HEvS0M)!c z*qk%?CAZ^+E4_PLBb`p4`XEgQR-#XPI|Qe;Z{KdoT@J{1R3j0D4Rx6@Br7?ta(6_B z{6etwKH+~aA`1&0OAFTXZ>`bg7Ya)>58|if!@jfbE-oW+E~QRoO72(SI)6_>JP58{ zqXwn2s|x0(d^lOvO_jPDHKuGX8# zR?!9EPwRe%kpAR^qQtV7F9p;Ul$Gykv55t;N|ATn6SPF_4n}BGZr`R4>Xc1)MNmRw zKm6qMK2l5^)4o{r=+UDUJW?1c`Xwpk{nN0sG%zp_t|yC~ok_}f);}oq?yZeV_~Ti{ zP+RzA0A~-RU{tf%t8C$7fg0}TloS-EX9sgZD6>C8M4t`c;z|XN9I>R7qu&meF+4n6 z>bGD{0-prqd(|cH*E1qVM!O+BM_Kz!XW#~PPEp?pG{`MV^z`(!hMPu++WZ+8mt3RF z_aA46S7C~sTMrUSwanz}0wcQM;jme?d6q6lMf*e@rNh_ z_k-KteHQ0uKmn}F{8W^cEiz4_qoa)!uV6MSfsqtDpX546d$>(HI6&wtU6_JPdJ9BY zNTZ)j_5%#ry8GPp^mKoJ|Ip9{C#R!YyqH_nAs6zi+C}gP1zmO~Gq0{sI#1Zsv55GxGB4_HFAuO_=o*v$0UB~-sQv1hEn`K1 z+#~&+IzLACfZo1dAardpVQ5+J`iK@@f)np8H|jAZCdeUZOE|1#XF<`h&kGh508l>* ziUBB;1;J=RIUen}1)bNxf)W9`j3DUUaLKNO4jpzEDvXuO0}B`xr%!y#BYj4gDAh!<_$y5W=RMvTk@Ia%I*SR%H$3vcMWJluh5`2lxf zUIP=e0yd-3C5$j&LJVkxHY+B^4Uij*e$=MGzzoce==QMs`509dvP~((ifUefrV-iI zVgPlrpj`k}r${+Ni;u7hW`xfmN~m`5iy&uNM|H!t>?~*wpcEJw!q5a#02+(L#E1ah zU_sUZZ9`Ovc>?r}h5T)Rd|1$pb{1k?O4LzU|Pn*-xb7{F5%mn#xZiJTa=31$mg&Z0F(p|Nxm2K`s@vtg);WD5gHFdlY} zwvnpJZ3Q!NU>iCyV6H41O_dZ(*#jHWpvpjv9Y<4t7(oDz`Gt6q)B|)oA-c&)N?dxZ zw>06=uduufXd`1QU3nh}qD>tinejhZg`j@4@F6RD#43T5x2f*ch$bNB&FoX@`5Qo5c$kWhfv;0q)u~Bdt zx1ghg*~mYpFI$U^EkG=5v4PR0ay(KN8$Ey;Sdb4u(2;>(3xUx$414!TiL3IjJ?p>- z4=w_+Wd}@Sy?~iFjL-$79{XQh1}|Kk*CHlko503=o0gd@E8c}~V0j9?SdunD0&(F1 zT#P!|ku^S84-kAJYw;5#3b2CK5gu zA0cSD5+m%cB;h5|wmSFit?=7fQ2YR%2EQx;ok?&NG&F>Tp(!L98ok17!KKhxa<`2f z!1E>|ve4KO??V;@?t_;lOIxTW2P4hGpv0xV?=L}A2-&>H|D7ts2+_djV3=~k(Abe7 z{JH{kT4C(S5XXXK09wg{U;v5g4bc1&v%Su>TL-t&FlR;53hD~ZSr_&dez-rbXEvyO zc68XuwNfx+@s$Hc&lOl>&t|xti8r~hMY?->IEfp>4m48Ka~fF=hLBc3H{7`$&ikyd z%ryyJJj{{o!MppOwtHArhaGfv6VSg4&^N=kfWxq_T0+6GTY!s60$q5MbFoiuMu1|% zF9vMB4600?dLVbBQ4v^6xCgt+qg@MhmAQI#x5G3zh8>4vlSRxvBW7F#I7|qHm*mw- z!bfZb&MPGg2j5_AF4~2sZL?$8y{rTGG*MnvWVmuQ()&mc-Y(YmAq8~Y`y-%Uv#G+t zI_u$_hP_ODB@2m?sw(=D0|%W4Vc?+7~7}Nv z6|-5v#1GxocK^%Zj9Us^Z7ifeRFb|rE;HB$@H_nz32w-tmh*Hsf^30V{49Jd3+Vrx z6hxD4;MSS=?6K+;M3mSDCVrm9qiPUrJ!@&8Bk_j?i(-W?ywBc$IedAg=wJmu5(b&> zMEv1`_*uTDfc{&Q1t|g)f*5*Pkcod)f&_p)`b4>b9^nYex+Axc#5tLGE66J`v2YKX zU)siYuTm{F__rOd@1QhFB1OO*6G|g~az6mZhCQt$%>vE$d!<3+2~x(|?wW`@ZjQuX z!H7F}*h4yXBNQtztTtIV&G)vHGq@~_D@8cX72!mPDWJ=r|4*N3N0oKsmh~BX+em2? zLkAkFGVwV`pOd&+NwNRK;!YUvWy(U*gl*nPbgGoMS_fX7pjAnF+=m$Zxtg}|>J=oo z-D$#UXA!rOPPdYrqqJeH5yLWQ9<<#mc^L@Jj3^J` z1u~r^+sHiaX${O2Xg z52`lz+Ocq*b0T_0BIP9TwUSPEB6dn3R#Iv7e^!}vybEv5lB&>gp^INy6e+V7#h za_z{n8Y|rMDDeEN#tKm$1?O3)k_zbYf;a0FN|h0}G#QjBd?YI;BZx5_Jy|T=*+7{> z0T|;-7G~-QGUEFzCNw}Im5fVB=veqRipQD1EKUifN3vLqzb8qLX0e#}m~yKWVlknn zfPV3>BQ}&`ItKn_v7t26@jQz)kCTKih9DLvZEIy{vo;^tMtLUoubUw3^~ttSVJU~8 z6LFB`kPIc6j&E6S55P!O6pQOk1@uW*WIzx|3glRHUTmN=S0c9$_ThK{aX4vbD?=&u zKQ|$^!L@*$rBqX)^t8W9oTSp___8r{IPV*>yd;(MTU;<&2(jZ6CAylm+hL}o>t8J{ zIIe6S{a1_S^_9(WEJXbri4n5@x#Ip15Bbx}u9E!3kr>EQBM2dpDBr`{8!0#cCpr)? zSNWSPZd~bdBs&!eO2ZN*2}(jO(iw6kjp4TvmLX8(`B3@#UK zWt?Nl;`dHw|FT^iP8L=PV!hN?$(T3dvx+W;WC;5$EIMe<;%m8JE8`(+|5HbmV_Dja z4LT9WQG~-ffp^PR@s*%l6oHtIwK)=>vvlsC7!UXD$S7?#F#GwOG(8hs7#qNVGNKe_ z<&Yv^b7Az{m}NIXUZ`Xg{>NeRf}TJQN-8H zsa#faow&HH#+ZKdRiUehP%+&yV{MiKtsE#2W;rNaR+WeRvW;K?Mdyj4%jpCmwX~Il zB#v=0jw!Q(MH1esfX+Hy_miK{aWi+bVdP#2ih@h0zOZB{SNM&hgbdFx+rDl2ZQW*j zJ?I9UmQ;-}Ez!yPH2ut~YN?yY4m z#F|D7F!8!rz@8)tIx0UWje9a zD<;c3lO@(W5s}ej33VNmbmS)Nmh9li*gJ|8lx<-)kq=$n|LV)B?FK@}|4rGjQ`_Ir z?jUO_&zgF+O!`+FQ{~v@_>G+|EcuV5R~OFxMbP`zxBrF1Hn+e0Cy{?XasMUK&8&@r z?#q~M!DC@KZ`?cH&J`-ZX%>~fF; zGlPZOk!BGX?u6fWf;v2gu|s7Mu6j@z!7xs!m7v55rui^27^MVVM88jjkufM{Vi?>1 rZ7)psK_QOe0;DA=xBj0pW0B*>ch2ISyfts(ee9@~zUC8kn;ZWh)^F|4 literal 0 HcmV?d00001 diff --git a/example_11/doc/userdoc.rst b/example_11/doc/userdoc.rst new file mode 100644 index 000000000..b22486380 --- /dev/null +++ b/example_11/doc/userdoc.rst @@ -0,0 +1,123 @@ +:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_11/doc/userdoc.rst + +.. _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 ros2_control_demo_example_2 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 ros2_control_demo_example_2 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 + + [DiffBotSystemHardware]: Got command 43.33333 for 'left_wheel_joint'! + [DiffBotSystemHardware]: 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/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp new file mode 100644 index 000000000..478819573 --- /dev/null +++ b/example_11/hardware/carlikebot_system.cpp @@ -0,0 +1,220 @@ +// 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. + +#include "ros2_control_demo_example_2/diffbot_system.hpp" + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace ros2_control_demo_example_2 +{ +hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( + const hardware_interface::HardwareInfo & info) +{ + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + hw_start_sec_ = std::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); + hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); + // END: This part here is for exemplary purposes - Please do not copy to your production code + hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + // DiffBotSystem has exactly two states and one command interface on each joint + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("DiffBotSystemHardware"), + "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_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger("DiffBotSystemHardware"), + "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 2) + { + RCLCPP_FATAL( + rclcpp::get_logger("DiffBotSystemHardware"), + "Joint '%s' has %zu state interface. 2 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("DiffBotSystemHardware"), + "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; + } + + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger("DiffBotSystemHardware"), + "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector DiffBotSystemHardware::export_state_interfaces() +{ + std::vector state_interfaces; + for (auto i = 0u; i < info_.joints.size(); i++) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i])); + } + + return state_interfaces; +} + +std::vector DiffBotSystemHardware::export_command_interfaces() +{ + std::vector command_interfaces; + for (auto i = 0u; i < info_.joints.size(); i++) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn DiffBotSystemHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Activating ...please wait..."); + + for (auto i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("DiffBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code + + // set some default values + for (auto i = 0u; i < hw_positions_.size(); i++) + { + if (std::isnan(hw_positions_[i])) + { + hw_positions_[i] = 0; + hw_velocities_[i] = 0; + hw_commands_[i] = 0; + } + } + + RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Successfully activated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DiffBotSystemHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Deactivating ...please wait..."); + + for (auto i = 0; i < hw_stop_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("DiffBotSystemHardware"), "%.1f seconds left...", hw_stop_sec_ - i); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code + + RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Successfully deactivated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::return_type DiffBotSystemHardware::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + for (std::size_t i = 0; i < hw_velocities_.size(); i++) + { + // Simulate DiffBot wheels's movement as a first-order system + // Update the joint status: this is a revolute joint without any limit. + // Simply integrates + hw_positions_[i] = hw_positions_[1] + period.seconds() * hw_velocities_[i]; + + RCLCPP_INFO( + rclcpp::get_logger("DiffBotSystemHardware"), + "Got position state %.5f and velocity state %.5f for '%s'!", hw_positions_[i], + hw_velocities_[i], info_.joints[i].name.c_str()); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type ros2_control_demo_example_2 ::DiffBotSystemHardware::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Writing..."); + + for (auto i = 0u; i < hw_commands_.size(); i++) + { + // Simulate sending commands to the hardware + RCLCPP_INFO( + rclcpp::get_logger("DiffBotSystemHardware"), "Got command %.5f for '%s'!", hw_commands_[i], + info_.joints[i].name.c_str()); + + hw_velocities_[i] = hw_commands_[i]; + } + RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Joints successfully written!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::return_type::OK; +} + +} // namespace ros2_control_demo_example_2 + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_example_2::DiffBotSystemHardware, hardware_interface::SystemInterface) diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp new file mode 100644 index 000000000..13490e5e0 --- /dev/null +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -0,0 +1,81 @@ +// 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 ROS2_CONTROL_DEMO_EXAMPLE_2__DIFFBOT_SYSTEM_HPP_ +#define ROS2_CONTROL_DEMO_EXAMPLE_2__DIFFBOT_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 "ros2_control_demo_example_2/visibility_control.h" + +namespace ros2_control_demo_example_2 +{ +class DiffBotSystemHardware : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(DiffBotSystemHardware); + + ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC + std::vector export_state_interfaces() override; + + ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC + std::vector export_command_interfaces() override; + + ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + // Parameters for the DiffBot simulation + double hw_start_sec_; + double hw_stop_sec_; + + // Store the command for the simulated robot + std::vector hw_commands_; + std::vector hw_positions_; + std::vector hw_velocities_; +}; + +} // namespace ros2_control_demo_example_2 + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_2__DIFFBOT_SYSTEM_HPP_ diff --git a/example_11/hardware/include/ros2_control_demo_example_11/visibility_control.h b/example_11/hardware/include/ros2_control_demo_example_11/visibility_control.h new file mode 100644 index 000000000..568aa9017 --- /dev/null +++ b/example_11/hardware/include/ros2_control_demo_example_11/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 ROS2_CONTROL_DEMO_EXAMPLE_2__VISIBILITY_CONTROL_H_ +#define ROS2_CONTROL_DEMO_EXAMPLE_2__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 ROS2_CONTROL_DEMO_EXAMPLE_2_EXPORT __attribute__((dllexport)) +#define ROS2_CONTROL_DEMO_EXAMPLE_2_IMPORT __attribute__((dllimport)) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_2_EXPORT __declspec(dllexport) +#define ROS2_CONTROL_DEMO_EXAMPLE_2_IMPORT __declspec(dllimport) +#endif +#ifdef ROS2_CONTROL_DEMO_EXAMPLE_2_BUILDING_DLL +#define ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_2_EXPORT +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_2_IMPORT +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC_TYPE ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_2_LOCAL +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_2_EXPORT __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_2_IMPORT +#if __GNUC__ >= 4 +#define ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_2_LOCAL __attribute__((visibility("hidden"))) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_2_LOCAL +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC_TYPE +#endif + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_2__VISIBILITY_CONTROL_H_ diff --git a/example_11/package.xml b/example_11/package.xml new file mode 100644 index 000000000..10828b63f --- /dev/null +++ b/example_11/package.xml @@ -0,0 +1,40 @@ + + + + ros2_control_demo_example_11 + 0.0.0 + Demo package of `ros2_control` hardware for a carlike robot with two wheels in front that steer and two wheels in the back that drive. + + Dr.-Ing. Denis Štogl + Bence Magyar + Christoph Froehlich + + Reza Kermani + + Apache-2.0 + + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + ackermann_steering_controller + bicycle_steering_controller + controller_manager + joint_state_broadcaster + joint_state_publisher_gui + robot_state_publisher + ros2_controllers_test_nodes + ros2controlcli + ros2launch + rviz2 + xacro + + ament_cmake_gtest + + + ament_cmake + + diff --git a/example_11/ros2_control_demo_example_11.xml b/example_11/ros2_control_demo_example_11.xml new file mode 100644 index 000000000..40fac739e --- /dev/null +++ b/example_11/ros2_control_demo_example_11.xml @@ -0,0 +1,9 @@ + + + + The ros2_control CarlikeBot example using a system hardware interface-type. It uses velocity command and state interface for the drive motor and position command and state interface for the steering. The example is the starting point to implement a hardware interface for ackermann and bicycle drive robots. + + + diff --git a/ros2_control_demos/package.xml b/ros2_control_demos/package.xml index f5fbf4a9a..603939229 100644 --- a/ros2_control_demos/package.xml +++ b/ros2_control_demos/package.xml @@ -22,6 +22,7 @@ ros2_control_demo_example_6 ros2_control_demo_example_8 ros2_control_demo_example_9 + ros2_control_demo_example_11 ament_cmake From 1fedade78a1e2046bba8f1369a9cfe441c8143f4 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Tue, 20 Jun 2023 17:46:15 -0400 Subject: [PATCH 02/37] claiming example 11 --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index d9de60bd8..aec578d5b 100644 --- a/README.md +++ b/README.md @@ -50,6 +50,8 @@ The following examples are part of this demo repository: Demonstrates how to switch between simulation and hardware. +* Example 11: ["Carlike Robot using steering controllers library"](example_11) + ## Getting started The repository is structured into `example_XY` folders that fully contained packages with names `ros2_control_demos_example_XY`. From 92f4ebfaa2b7effcfa77ed9f52cc3b425a80c8a7 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Tue, 20 Jun 2023 18:02:49 -0400 Subject: [PATCH 03/37] reserving example 11 --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 0498731d5..34abb6125 100644 --- a/README.md +++ b/README.md @@ -50,6 +50,8 @@ The following examples are part of this demo repository: Demonstrates how to switch between simulation and hardware. +* Example 11: "Carlike Robot using steering controller library derivatives (ackermann and bicycle) (tba.)" + ## Getting started The repository is structured into `example_XY` folders that fully contained packages with names `ros2_control_demos_example_XY`. From c3648042dbf0ea30bd373c82d01e236e229ff885 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Tue, 27 Jun 2023 17:36:50 +0000 Subject: [PATCH 04/37] double controller setup --- .../config/carlikebot_controllers.yaml | 133 ++++++++++++------ .../bringup/launch/carlikebot.launch.py | 34 +++-- 2 files changed, 118 insertions(+), 49 deletions(-) diff --git a/example_11/bringup/config/carlikebot_controllers.yaml b/example_11/bringup/config/carlikebot_controllers.yaml index d45de1b44..04a8ad8d7 100644 --- a/example_11/bringup/config/carlikebot_controllers.yaml +++ b/example_11/bringup/config/carlikebot_controllers.yaml @@ -5,53 +5,104 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - diffbot_base_controller: - type: diff_drive_controller/DiffDriveController + ackermann_steering_controller: + type: ackermann_steering_controller/AckermannSteeringController -diffbot_base_controller: - ros__parameters: - left_wheel_names: ["left_wheel_joint"] - right_wheel_names: ["right_wheel_joint"] + bicycle_steering_controller: + type: bicycle_steering_controller/BicycleSteeringController - 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 +ackermann_steering_controller: + ros__parameters: - publish_rate: 50.0 + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: false + rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_names: [left_steering_hinge_joint, right_steering_hinge_joint] 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 + wheelbase: 0.325 + front_wheel_track: 0.2 + rear_wheel_track: 0.2 + front_wheels_radius: 0.05 + rear_wheels_radius: 0.05 + +bicycle_steering_controller: + ros__parameters: + + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false 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 + rear_wheels_names: [rear_wheel_joint] + front_wheels_names: [steering_axis_joint] + + odom_frame_id: odom + base_frame_id: base_link + enable_odom_tf: true + + + wheelbase: 0.325 + front_wheel_radius: 0.05 + rear_wheel_radius: 0.05 + + + + + + +# 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/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index c9cba5f10..3b32e358e 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -14,7 +14,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, RegisterEventHandler -from launch.conditions import IfCondition +from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnProcessExit from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration @@ -30,11 +30,17 @@ def generate_launch_description(): "start_rviz", default_value="true", description="Start RViz2 automatically with this launch file.", + ), + DeclareLaunchArgument( + "sim", + default_value="true", + description="Whether to start controllers for simulation or real hardware." ) ) # Initialize Arguments start_rviz = LaunchConfiguration("start_rviz") + sim = LaunchConfiguration("sim") # Get URDF via xacro robot_description_content = Command( @@ -42,7 +48,7 @@ def generate_launch_description(): PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_2"), "urdf", "diffbot.urdf.xacro"] + [FindPackageShare("ros2_control_demo_example_11"), "urdf", "carlikebot.urdf.xacro"] ), ] ) @@ -50,13 +56,13 @@ def generate_launch_description(): robot_controllers = PathJoinSubstitution( [ - FindPackageShare("ros2_control_demo_example_2"), + FindPackageShare("ros2_control_demo_example_11"), "config", - "diffbot_controllers.yaml", + "carlikebot_controllers.yaml", ] ) rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_2"), "rviz", "diffbot.rviz"] + [FindPackageShare("ros2_control_demo_example_11"), "rviz", "carlikebot.rviz"] ) control_node = Node( @@ -65,14 +71,25 @@ def generate_launch_description(): parameters=[robot_description, robot_controllers], output="both", ) - robot_state_pub_node = Node( + robot_state_pub_ackermann_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + remappings=[ + ("/ackermann_steering_controller/reference_unstamped", "/cmd_vel"), + ], + condition=IfCondition(sim), + ) + robot_state_pub_bicycle_node = Node( package="robot_state_publisher", executable="robot_state_publisher", output="both", parameters=[robot_description], remappings=[ - ("/diff_drive_controller/cmd_vel_unstamped", "/cmd_vel"), + ("/bicycle_steering_controller/reference_unstamped", "/cmd_vel"), ], + condition=UnlessCondition(sim), ) rviz_node = Node( package="rviz2", @@ -113,7 +130,8 @@ def generate_launch_description(): nodes = [ control_node, - robot_state_pub_node, + robot_state_pub_ackermann_node, + robot_state_pub_bicycle_node, joint_state_broadcaster_spawner, delay_rviz_after_joint_state_broadcaster_spawner, delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, From b55b4a61525bec9b0953f415e6383c38cc294c82 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Tue, 27 Jun 2023 17:38:58 +0000 Subject: [PATCH 05/37] format --- .../config/carlikebot_controllers.yaml | 53 ------------------- 1 file changed, 53 deletions(-) diff --git a/example_11/bringup/config/carlikebot_controllers.yaml b/example_11/bringup/config/carlikebot_controllers.yaml index 04a8ad8d7..4143011da 100644 --- a/example_11/bringup/config/carlikebot_controllers.yaml +++ b/example_11/bringup/config/carlikebot_controllers.yaml @@ -53,56 +53,3 @@ bicycle_steering_controller: wheelbase: 0.325 front_wheel_radius: 0.05 rear_wheel_radius: 0.05 - - - - - - -# 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 From a6b0dcf231d97e6a4641c8ca5179818de801548e Mon Sep 17 00:00:00 2001 From: ARK3r Date: Tue, 27 Jun 2023 18:05:28 -0400 Subject: [PATCH 06/37] added controller specific control descriptions --- .../description/launch/view_robot.launch.py | 8 +-- ...> carlikebot_ackermann.ros2_control.xacro} | 4 +- .../carlikebot_bicycle.ros2_control.xacro | 26 +++++++++ .../description/urdf/carlikebot.urdf.xacro | 24 +++++---- .../urdf/carlikebot_description.urdf.xacro | 2 +- example_11/hardware/carlikebot_system.cpp | 54 +++++++++---------- .../carlikebot_system.hpp | 30 +++++------ .../visibility_control.h | 38 ++++++------- 8 files changed, 110 insertions(+), 76 deletions(-) rename example_11/description/ros2_control/{carlikebot.ros2_control.xacro => carlikebot_ackermann.ros2_control.xacro} (83%) create mode 100644 example_11/description/ros2_control/carlikebot_bicycle.ros2_control.xacro diff --git a/example_11/description/launch/view_robot.launch.py b/example_11/description/launch/view_robot.launch.py index badd807c2..141bad973 100644 --- a/example_11/description/launch/view_robot.launch.py +++ b/example_11/description/launch/view_robot.launch.py @@ -26,7 +26,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "description_package", - default_value="ros2_control_demo_example_2", + default_value="ros2_control_demo_example_11", description="Description package with robot URDF/xacro files. Usually the argument \ is not set, it enables use of a custom description.", ) @@ -34,7 +34,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "description_file", - default_value="diffbot.urdf.xacro", + default_value="carlikebot.urdf.xacro", description="URDF/XACRO description file with the robot.", ) ) @@ -64,12 +64,14 @@ def generate_launch_description(): " ", "prefix:=", prefix, + " ", + "sim:=true", ] ) robot_description = {"robot_description": robot_description_content} rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", "diffbot_view.rviz"] + [FindPackageShare(description_package), "rviz", "carlikebot_view.rviz"] ) joint_state_publisher_node = Node( diff --git a/example_11/description/ros2_control/carlikebot.ros2_control.xacro b/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro similarity index 83% rename from example_11/description/ros2_control/carlikebot.ros2_control.xacro rename to example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro index e1f184d5e..d2bc6ae31 100644 --- a/example_11/description/ros2_control/carlikebot.ros2_control.xacro +++ b/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro @@ -1,11 +1,11 @@ - + - ros2_control_demo_example_2/DiffBotSystemHardware + ros2_control_demo_example_11/CarlikeBotSystemHardware 0 3.0 diff --git a/example_11/description/ros2_control/carlikebot_bicycle.ros2_control.xacro b/example_11/description/ros2_control/carlikebot_bicycle.ros2_control.xacro new file mode 100644 index 000000000..d2bc6ae31 --- /dev/null +++ b/example_11/description/ros2_control/carlikebot_bicycle.ros2_control.xacro @@ -0,0 +1,26 @@ + + + + + + + + ros2_control_demo_example_11/CarlikeBotSystemHardware + 0 + 3.0 + + + + + + + + + + + + + + + + diff --git a/example_11/description/urdf/carlikebot.urdf.xacro b/example_11/description/urdf/carlikebot.urdf.xacro index a2858e1b2..cd6901543 100644 --- a/example_11/description/urdf/carlikebot.urdf.xacro +++ b/example_11/description/urdf/carlikebot.urdf.xacro @@ -1,19 +1,25 @@ - - + + + - + - + - - + + + + + + + - + - + diff --git a/example_11/description/urdf/carlikebot_description.urdf.xacro b/example_11/description/urdf/carlikebot_description.urdf.xacro index 03aa3497f..29b4cbd3e 100644 --- a/example_11/description/urdf/carlikebot_description.urdf.xacro +++ b/example_11/description/urdf/carlikebot_description.urdf.xacro @@ -1,7 +1,7 @@ - + diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index 478819573..a89fef4bd 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ros2_control_demo_example_2/diffbot_system.hpp" +#include "ros2_control_demo_example_11/carlikebot_system.hpp" #include #include @@ -24,9 +24,9 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" -namespace ros2_control_demo_example_2 +namespace ros2_control_demo_example_11 { -hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( +hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( const hardware_interface::HardwareInfo & info) { if ( @@ -46,11 +46,11 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( for (const hardware_interface::ComponentInfo & joint : info_.joints) { - // DiffBotSystem has exactly two states and one command interface on each joint + // CarlikeBotSystem has exactly two states and one command interface on each joint if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("DiffBotSystemHardware"), + rclcpp::get_logger("CarlikeBotSystemHardware"), "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; @@ -59,7 +59,7 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) { RCLCPP_FATAL( - rclcpp::get_logger("DiffBotSystemHardware"), + rclcpp::get_logger("CarlikeBotSystemHardware"), "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); return hardware_interface::CallbackReturn::ERROR; @@ -68,7 +68,7 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( if (joint.state_interfaces.size() != 2) { RCLCPP_FATAL( - rclcpp::get_logger("DiffBotSystemHardware"), + rclcpp::get_logger("CarlikeBotSystemHardware"), "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), joint.state_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; @@ -77,7 +77,7 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("DiffBotSystemHardware"), + rclcpp::get_logger("CarlikeBotSystemHardware"), "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; @@ -86,7 +86,7 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { RCLCPP_FATAL( - rclcpp::get_logger("DiffBotSystemHardware"), + rclcpp::get_logger("CarlikeBotSystemHardware"), "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(), joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); return hardware_interface::CallbackReturn::ERROR; @@ -96,7 +96,7 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( return hardware_interface::CallbackReturn::SUCCESS; } -std::vector DiffBotSystemHardware::export_state_interfaces() +std::vector CarlikeBotSystemHardware::export_state_interfaces() { std::vector state_interfaces; for (auto i = 0u; i < info_.joints.size(); i++) @@ -110,7 +110,7 @@ std::vector DiffBotSystemHardware::export_st return state_interfaces; } -std::vector DiffBotSystemHardware::export_command_interfaces() +std::vector CarlikeBotSystemHardware::export_command_interfaces() { std::vector command_interfaces; for (auto i = 0u; i < info_.joints.size(); i++) @@ -122,17 +122,17 @@ std::vector DiffBotSystemHardware::export_ return command_interfaces; } -hardware_interface::CallbackReturn DiffBotSystemHardware::on_activate( +hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Activating ...please wait..."); + RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Activating ...please wait..."); for (auto i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( - rclcpp::get_logger("DiffBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i); + rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -147,31 +147,31 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_activate( } } - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Successfully activated!"); + RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn DiffBotSystemHardware::on_deactivate( +hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Deactivating ...please wait..."); + RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Deactivating ...please wait..."); for (auto i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( - rclcpp::get_logger("DiffBotSystemHardware"), "%.1f seconds left...", hw_stop_sec_ - i); + rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_stop_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Successfully deactivated!"); + RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully deactivated!"); return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::return_type DiffBotSystemHardware::read( +hardware_interface::return_type CarlikeBotSystemHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code @@ -183,7 +183,7 @@ hardware_interface::return_type DiffBotSystemHardware::read( hw_positions_[i] = hw_positions_[1] + period.seconds() * hw_velocities_[i]; RCLCPP_INFO( - rclcpp::get_logger("DiffBotSystemHardware"), + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got position state %.5f and velocity state %.5f for '%s'!", hw_positions_[i], hw_velocities_[i], info_.joints[i].name.c_str()); } @@ -192,29 +192,29 @@ hardware_interface::return_type DiffBotSystemHardware::read( return hardware_interface::return_type::OK; } -hardware_interface::return_type ros2_control_demo_example_2 ::DiffBotSystemHardware::write( +hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Writing..."); + RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Writing..."); for (auto i = 0u; i < hw_commands_.size(); i++) { // Simulate sending commands to the hardware RCLCPP_INFO( - rclcpp::get_logger("DiffBotSystemHardware"), "Got command %.5f for '%s'!", hw_commands_[i], + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'!", hw_commands_[i], info_.joints[i].name.c_str()); hw_velocities_[i] = hw_commands_[i]; } - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Joints successfully written!"); + RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Joints successfully written!"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; } -} // namespace ros2_control_demo_example_2 +} // namespace ros2_control_demo_example_11 #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - ros2_control_demo_example_2::DiffBotSystemHardware, hardware_interface::SystemInterface) + ros2_control_demo_example_11::CarlikeBotSystemHardware, hardware_interface::SystemInterface) diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp index 13490e5e0..4772eb1de 100644 --- a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ROS2_CONTROL_DEMO_EXAMPLE_2__DIFFBOT_SYSTEM_HPP_ -#define ROS2_CONTROL_DEMO_EXAMPLE_2__DIFFBOT_SYSTEM_HPP_ +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_11__CARLIKEBOT_SYSTEM_HPP_ +#define ROS2_CONTROL_DEMO_EXAMPLE_11__CARLIKEBOT_SYSTEM_HPP_ #include #include @@ -30,38 +30,38 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "ros2_control_demo_example_2/visibility_control.h" +#include "ros2_control_demo_example_11/visibility_control.h" -namespace ros2_control_demo_example_2 +namespace ros2_control_demo_example_11 { -class DiffBotSystemHardware : public hardware_interface::SystemInterface +class CarlikeBotSystemHardware : public hardware_interface::SystemInterface { public: - RCLCPP_SHARED_PTR_DEFINITIONS(DiffBotSystemHardware); + RCLCPP_SHARED_PTR_DEFINITIONS(CarlikeBotSystemHardware); - ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC std::vector export_state_interfaces() override; - ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC std::vector export_command_interfaces() override; - ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC hardware_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC hardware_interface::return_type read( const rclcpp::Time & time, const rclcpp::Duration & period) override; - ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; @@ -76,6 +76,6 @@ class DiffBotSystemHardware : public hardware_interface::SystemInterface std::vector hw_velocities_; }; -} // namespace ros2_control_demo_example_2 +} // namespace ros2_control_demo_example_11 -#endif // ROS2_CONTROL_DEMO_EXAMPLE_2__DIFFBOT_SYSTEM_HPP_ +#endif // ROS2_CONTROL_DEMO_EXAMPLE_11__CARLIKEBOT_SYSTEM_HPP_ diff --git a/example_11/hardware/include/ros2_control_demo_example_11/visibility_control.h b/example_11/hardware/include/ros2_control_demo_example_11/visibility_control.h index 568aa9017..dff3344de 100644 --- a/example_11/hardware/include/ros2_control_demo_example_11/visibility_control.h +++ b/example_11/hardware/include/ros2_control_demo_example_11/visibility_control.h @@ -19,38 +19,38 @@ * library cannot have, but the consuming code must have inorder to link. */ -#ifndef ROS2_CONTROL_DEMO_EXAMPLE_2__VISIBILITY_CONTROL_H_ -#define ROS2_CONTROL_DEMO_EXAMPLE_2__VISIBILITY_CONTROL_H_ +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_11__VISIBILITY_CONTROL_H_ +#define ROS2_CONTROL_DEMO_EXAMPLE_11__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 ROS2_CONTROL_DEMO_EXAMPLE_2_EXPORT __attribute__((dllexport)) -#define ROS2_CONTROL_DEMO_EXAMPLE_2_IMPORT __attribute__((dllimport)) +#define ROS2_CONTROL_DEMO_EXAMPLE_11_EXPORT __attribute__((dllexport)) +#define ROS2_CONTROL_DEMO_EXAMPLE_11_IMPORT __attribute__((dllimport)) #else -#define ROS2_CONTROL_DEMO_EXAMPLE_2_EXPORT __declspec(dllexport) -#define ROS2_CONTROL_DEMO_EXAMPLE_2_IMPORT __declspec(dllimport) +#define ROS2_CONTROL_DEMO_EXAMPLE_11_EXPORT __declspec(dllexport) +#define ROS2_CONTROL_DEMO_EXAMPLE_11_IMPORT __declspec(dllimport) #endif -#ifdef ROS2_CONTROL_DEMO_EXAMPLE_2_BUILDING_DLL -#define ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_2_EXPORT +#ifdef ROS2_CONTROL_DEMO_EXAMPLE_11_BUILDING_DLL +#define ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_11_EXPORT #else -#define ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_2_IMPORT +#define ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_11_IMPORT #endif -#define ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC_TYPE ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC -#define ROS2_CONTROL_DEMO_EXAMPLE_2_LOCAL +#define ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC_TYPE ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_11_LOCAL #else -#define ROS2_CONTROL_DEMO_EXAMPLE_2_EXPORT __attribute__((visibility("default"))) -#define ROS2_CONTROL_DEMO_EXAMPLE_2_IMPORT +#define ROS2_CONTROL_DEMO_EXAMPLE_11_EXPORT __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_11_IMPORT #if __GNUC__ >= 4 -#define ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC __attribute__((visibility("default"))) -#define ROS2_CONTROL_DEMO_EXAMPLE_2_LOCAL __attribute__((visibility("hidden"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_11_LOCAL __attribute__((visibility("hidden"))) #else -#define ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC -#define ROS2_CONTROL_DEMO_EXAMPLE_2_LOCAL +#define ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_11_LOCAL #endif -#define ROS2_CONTROL_DEMO_EXAMPLE_2_PUBLIC_TYPE +#define ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC_TYPE #endif -#endif // ROS2_CONTROL_DEMO_EXAMPLE_2__VISIBILITY_CONTROL_H_ +#endif // ROS2_CONTROL_DEMO_EXAMPLE_11__VISIBILITY_CONTROL_H_ From b7bdcf27c4ba0f4fdae02f869a355e6991986ee7 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Wed, 28 Jun 2023 15:05:49 -0400 Subject: [PATCH 07/37] it runs --- .../bringup/launch/carlikebot.launch.py | 25 ++++++++++++++++--- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index 3b32e358e..e6029c448 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -30,7 +30,9 @@ def generate_launch_description(): "start_rviz", default_value="true", description="Start RViz2 automatically with this launch file.", - ), + ) + ) + declared_arguments.append( DeclareLaunchArgument( "sim", default_value="true", @@ -50,6 +52,8 @@ def generate_launch_description(): PathJoinSubstitution( [FindPackageShare("ros2_control_demo_example_11"), "urdf", "carlikebot.urdf.xacro"] ), + " sim:=", + sim, ] ) robot_description = {"robot_description": robot_description_content} @@ -106,10 +110,23 @@ def generate_launch_description(): arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) - robot_controller_spawner = Node( + # robot_controller_spawner = Node( + # package="controller_manager", + # executable="spawner", + # arguments=["diffbot_base_controller", "--controller-manager", "/controller_manager"], + # ) + + robot_ackermann_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["ackermann_steering_controller", "--controller-manager", "/controller_manager"], + condition=IfCondition(sim), + ) + robot_bicycle_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["diffbot_base_controller", "--controller-manager", "/controller_manager"], + arguments=["bicycle_steering_controller", "--controller-manager", "/controller_manager"], + condition=UnlessCondition(sim), ) # Delay rviz start after `joint_state_broadcaster` @@ -124,7 +141,7 @@ def generate_launch_description(): delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( event_handler=OnProcessExit( target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], + on_exit=[robot_ackermann_controller_spawner, robot_bicycle_controller_spawner], ) ) From 255178ea0ba462b49bccf7c38f54cbe1f7385549 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Wed, 28 Jun 2023 15:09:09 -0400 Subject: [PATCH 08/37] format --- example_11/bringup/launch/carlikebot.launch.py | 4 ++-- example_11/hardware/carlikebot_system.cpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index e6029c448..52cd53b0a 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): DeclareLaunchArgument( "sim", default_value="true", - description="Whether to start controllers for simulation or real hardware." + description="Whether to start controllers for simulation or real hardware.", ) ) @@ -115,7 +115,7 @@ def generate_launch_description(): # executable="spawner", # arguments=["diffbot_base_controller", "--controller-manager", "/controller_manager"], # ) - + robot_ackermann_controller_spawner = Node( package="controller_manager", executable="spawner", diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index a89fef4bd..108fa886a 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -110,7 +110,8 @@ std::vector CarlikeBotSystemHardware::export return state_interfaces; } -std::vector CarlikeBotSystemHardware::export_command_interfaces() +std::vector +CarlikeBotSystemHardware::export_command_interfaces() { std::vector command_interfaces; for (auto i = 0u; i < info_.joints.size(); i++) From 6e37b75287069b740c71f19bf402eb87f30cd339 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Thu, 29 Jun 2023 11:30:57 -0400 Subject: [PATCH 09/37] almost --- .../carlikebot_ackermann.ros2_control.xacro | 17 +- .../carlikebot_bicycle.ros2_control.xacro | 11 +- .../description/urdf/carlikebot.urdf.xacro | 6 +- ...arlikebot_ackermann_description.urdf.xacro | 214 +++++++++++++++++ ...carlikebot_bicycle_description.urdf.xacro} | 0 example_11/hardware/carlikebot_system.cpp | 216 +++++++++++------- .../carlikebot_system.hpp | 19 +- 7 files changed, 378 insertions(+), 105 deletions(-) create mode 100644 example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro rename example_11/description/urdf/{carlikebot_description.urdf.xacro => carlikebot_bicycle_description.urdf.xacro} (100%) diff --git a/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro b/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro index d2bc6ae31..8cdcee0ef 100644 --- a/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro +++ b/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro @@ -8,19 +8,26 @@ ros2_control_demo_example_11/CarlikeBotSystemHardware 0 3.0 + 1 - - + + + + + + + + + - + - - + \ No newline at end of file diff --git a/example_11/description/ros2_control/carlikebot_bicycle.ros2_control.xacro b/example_11/description/ros2_control/carlikebot_bicycle.ros2_control.xacro index d2bc6ae31..3cc0de60b 100644 --- a/example_11/description/ros2_control/carlikebot_bicycle.ros2_control.xacro +++ b/example_11/description/ros2_control/carlikebot_bicycle.ros2_control.xacro @@ -6,17 +6,14 @@ ros2_control_demo_example_11/CarlikeBotSystemHardware - 0 - 3.0 + 0 - - + + - - + - diff --git a/example_11/description/urdf/carlikebot.urdf.xacro b/example_11/description/urdf/carlikebot.urdf.xacro index cd6901543..4a0454025 100644 --- a/example_11/description/urdf/carlikebot.urdf.xacro +++ b/example_11/description/urdf/carlikebot.urdf.xacro @@ -4,16 +4,18 @@ - - + + + + diff --git a/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro b/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro new file mode 100644 index 000000000..0657851b6 --- /dev/null +++ b/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro @@ -0,0 +1,214 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/example_11/description/urdf/carlikebot_description.urdf.xacro b/example_11/description/urdf/carlikebot_bicycle_description.urdf.xacro similarity index 100% rename from example_11/description/urdf/carlikebot_description.urdf.xacro rename to example_11/description/urdf/carlikebot_bicycle_description.urdf.xacro diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index 108fa886a..98ad780c5 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -36,17 +36,35 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( return hardware_interface::CallbackReturn::ERROR; } - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - hw_start_sec_ = std::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); - hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); - // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + m_running_simulation = std::stod(info_.hardware_parameters["is_simulation"]); + if (m_running_simulation && info_.joints.size() != 4) + { + RCLCPP_ERROR( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "CarlikeBotSystemHardware::on_init() - Failed to initialize, " + "because the number of joints %ld is not 4 for simulation.", + info_.joints.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (!m_running_simulation && info_.joints.size() != 2) + { + RCLCPP_ERROR( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "CarlikeBotSystemHardware::on_init() - Failed to initialize, " + "because the number of joints %ld is not 2 for physical.", + info_.joints.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "CarlikeBotSystemHardware is %s.", m_running_simulation ? "running in simulation" : "running on hardware"); + + // All joints should have one state interface and one command interface for (const hardware_interface::ComponentInfo & joint : info_.joints) { - // CarlikeBotSystem has exactly two states and one command interface on each joint if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( @@ -56,41 +74,35 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( return hardware_interface::CallbackReturn::ERROR; } - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) - { - RCLCPP_FATAL( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); - return hardware_interface::CallbackReturn::ERROR; - } - - if (joint.state_interfaces.size() != 2) + if (joint.state_interfaces.size() != 1) { RCLCPP_FATAL( rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), + "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("CarlikeBotSystemHardware"), - "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; - } - - if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) - { - RCLCPP_FATAL( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(), - joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); - return hardware_interface::CallbackReturn::ERROR; - } + // The steering joints are controlled by position and the drive joints by velocity + // Running in simulation which means we have individual control of two front steering wheels and two rear drive wheels + if (m_running_simulation) + { + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + hw_start_sec_ = std::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); + hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + // hw_positions_.resize(2, std::numeric_limits::quiet_NaN()); + // hw_velocities_.resize(2, std::numeric_limits::quiet_NaN()); + // hw_commands_.resize(4, std::numeric_limits::quiet_NaN()); + } + // Running on hardware which means we have a single front steering and a single rear drive motor + else if (!m_running_simulation) + { + // hw_positions_.resize(1, std::numeric_limits::quiet_NaN()); + // hw_velocities_.resize(1, std::numeric_limits::quiet_NaN()); + // hw_commands_.resize(2, std::numeric_limits::quiet_NaN()); } return hardware_interface::CallbackReturn::SUCCESS; @@ -101,10 +113,27 @@ std::vector CarlikeBotSystemHardware::export std::vector state_interfaces; for (auto i = 0u; i < info_.joints.size(); i++) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i])); + bool is_steering_joint = info_.joints[i].name.find("steering") != std::string::npos; + + RCLCPP_DEBUG( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Joint '%s' has state interfaces: '%s', '%s'.", info_.joints[i].name.c_str(), + info_.joints[i].state_interfaces[0].name.c_str(), + info_.joints[i].state_interfaces[1].name.c_str() + ); + + if (is_steering_joint) + { + hw_positions_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits::quiet_NaN())); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_.at(info_.joints[i].name))); + } + else + { + hw_velocities_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits::quiet_NaN())); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_.at(info_.joints[i].name))); + } } return state_interfaces; @@ -116,8 +145,13 @@ CarlikeBotSystemHardware::export_command_interfaces() std::vector command_interfaces; for (auto i = 0u; i < info_.joints.size(); i++) { + bool is_steering_joint = info_.joints[i].name.find("steering") != std::string::npos; + + auto hw_if = is_steering_joint ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY; + + hw_commands_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits::quiet_NaN())); command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); + info_.joints[i].name, hw_if, &hw_commands_.at(info_.joints[i].name))); } return command_interfaces; @@ -126,25 +160,31 @@ CarlikeBotSystemHardware::export_command_interfaces() hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Activating ...please wait..."); - - for (auto i = 0; i < hw_start_sec_; i++) + if (m_running_simulation) { - rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( + RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Activating ...please wait..."); + + for (auto i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i); - } - // END: This part here is for exemplary purposes - Please do not copy to your production code + } - // set some default values - for (auto i = 0u; i < hw_positions_.size(); i++) - { - if (std::isnan(hw_positions_[i])) + // iterate through hw_positions_ map and set all second values to 0 + for (auto it = hw_positions_.begin(); it != hw_positions_.end(); ++it) + { + it->second = 0; + } + + for (auto it = hw_velocities_.begin(); it != hw_velocities_.end(); ++it) { - hw_positions_[i] = 0; - hw_velocities_[i] = 0; - hw_commands_[i] = 0; + it->second = 0; + } + + for (auto it = hw_commands_.begin(); it != hw_commands_.end(); ++it) + { + it->second = 0; } } @@ -156,17 +196,19 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Deactivating ...please wait..."); - - for (auto i = 0; i < hw_stop_sec_; i++) + if (m_running_simulation) { - rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_stop_sec_ - i); - } - // END: This part here is for exemplary purposes - Please do not copy to your production code + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Deactivating ...please wait..."); + for (auto i = 0; i < hw_stop_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_stop_sec_ - i); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code + } RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully deactivated!"); return hardware_interface::CallbackReturn::SUCCESS; @@ -175,41 +217,43 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_deactivate( hardware_interface::return_type CarlikeBotSystemHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - for (std::size_t i = 0; i < hw_velocities_.size(); i++) + if (m_running_simulation) { - // Simulate DiffBot wheels's movement as a first-order system - // Update the joint status: this is a revolute joint without any limit. - // Simply integrates - hw_positions_[i] = hw_positions_[1] + period.seconds() * hw_velocities_[i]; - - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Got position state %.5f and velocity state %.5f for '%s'!", hw_positions_[i], - hw_velocities_[i], info_.joints[i].name.c_str()); + // Inside the write method both position and velocity states for the simulation are updated + } + else + { + // Robot specific code to read data from the hardware } - // END: This part here is for exemplary purposes - Please do not copy to your production code - return hardware_interface::return_type::OK; } hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Writing..."); - for (auto i = 0u; i < hw_commands_.size(); i++) + if (m_running_simulation) { - // Simulate sending commands to the hardware - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'!", hw_commands_[i], - info_.joints[i].name.c_str()); + for (auto it = hw_commands_.begin(); it != hw_commands_.end(); ++it) + { + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'!", it->second, + it->first.c_str()); + + if (it->first.find("steering") != std::string::npos) + { + hw_positions_.at(it->first) = it->second; + } + else + { + hw_velocities_.at(it->first) = it->second; + } - hw_velocities_[i] = hw_commands_[i]; + } } + RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Joints successfully written!"); - // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; } diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp index 4772eb1de..6be3332ae 100644 --- a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -66,14 +67,22 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; private: - // Parameters for the DiffBot simulation + // Parameter disnguishing between simulation and physical robot + bool m_running_simulation; + + // Parameters for the CarlikeBot simulation double hw_start_sec_; double hw_stop_sec_; - // Store the command for the simulated robot - std::vector hw_commands_; - std::vector hw_positions_; - std::vector hw_velocities_; + + // Store the command for the CarlikeBot robot + // std::vector hw_commands_; + // std::vector hw_positions_; + // std::vector hw_velocities_; + + std::map hw_commands_; + std::map hw_positions_; + std::map hw_velocities_; }; } // namespace ros2_control_demo_example_11 From 05d9eae7397efeeffdb5e2a5dfab65b432a5371e Mon Sep 17 00:00:00 2001 From: ARK3r Date: Thu, 29 Jun 2023 15:25:50 -0400 Subject: [PATCH 10/37] ackermann almost done --- .../bringup/launch/carlikebot.launch.py | 6 +- .../description/urdf/carlikebot.urdf.xacro | 2 +- example_11/hardware/carlikebot_system.cpp | 201 +++++++++++++++--- .../carlikebot_system.hpp | 15 +- 4 files changed, 183 insertions(+), 41 deletions(-) diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index 52cd53b0a..934bf6362 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -27,7 +27,7 @@ def generate_launch_description(): declared_arguments = [] declared_arguments.append( DeclareLaunchArgument( - "start_rviz", + "gui", default_value="true", description="Start RViz2 automatically with this launch file.", ) @@ -41,7 +41,7 @@ def generate_launch_description(): ) # Initialize Arguments - start_rviz = LaunchConfiguration("start_rviz") + gui = LaunchConfiguration("gui") sim = LaunchConfiguration("sim") # Get URDF via xacro @@ -101,7 +101,7 @@ def generate_launch_description(): name="rviz2", output="log", arguments=["-d", rviz_config_file], - condition=IfCondition(start_rviz), + condition=IfCondition(gui), ) joint_state_broadcaster_spawner = Node( diff --git a/example_11/description/urdf/carlikebot.urdf.xacro b/example_11/description/urdf/carlikebot.urdf.xacro index 4a0454025..42a02b5cf 100644 --- a/example_11/description/urdf/carlikebot.urdf.xacro +++ b/example_11/description/urdf/carlikebot.urdf.xacro @@ -2,7 +2,7 @@ - + diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index 98ad780c5..919b1a4cd 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -38,6 +38,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( m_running_simulation = std::stod(info_.hardware_parameters["is_simulation"]); + // Check if the number of joints is correct based on the mode of operation if (m_running_simulation && info_.joints.size() != 4) { RCLCPP_ERROR( @@ -47,7 +48,6 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( info_.joints.size()); return hardware_interface::CallbackReturn::ERROR; } - if (!m_running_simulation && info_.joints.size() != 2) { RCLCPP_ERROR( @@ -82,10 +82,56 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( joint.state_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } + + // Check that the joints have the correct interfaces + // The steering joints have position interfaces and the drive joints velocity + if (joint.name.find("steering") != std::string::npos) + { + // Steering joints should have a single position command and state interface + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Joint '%s' has %s command interface. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Joint '%s' has %s state interface. '%s' expected.", + joint.name.c_str(), joint.state_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); + } + } + else + { + // Drive joints should have a single velocity command and state interface + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Joint '%s' has %s command interface. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), + hardware_interface::HW_IF_VELOCITY); + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Joint '%s' has %s state interface. '%s' expected.", + joint.name.c_str(), joint.state_interfaces[0].name.c_str(), + hardware_interface::HW_IF_VELOCITY); + } + } + + } - // The steering joints are controlled by position and the drive joints by velocity - // Running in simulation which means we have individual control of two front steering wheels and two rear drive wheels + // Running in simulation: we have individual control of two front steering wheels and two rear drive wheels if (m_running_simulation) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code @@ -93,16 +139,20 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - // hw_positions_.resize(2, std::numeric_limits::quiet_NaN()); - // hw_velocities_.resize(2, std::numeric_limits::quiet_NaN()); - // hw_commands_.resize(4, std::numeric_limits::quiet_NaN()); + // Two steering joints and two drive joints + hw_positions_.resize(2, std::numeric_limits::quiet_NaN()); + hw_velocities_.resize(2, std::numeric_limits::quiet_NaN()); + // Which means 4 commands + hw_commands_.resize(4, std::numeric_limits::quiet_NaN()); } - // Running on hardware which means we have a single front steering and a single rear drive motor + // Running on hardware: we have a single front steering and a single rear drive motor else if (!m_running_simulation) { - // hw_positions_.resize(1, std::numeric_limits::quiet_NaN()); - // hw_velocities_.resize(1, std::numeric_limits::quiet_NaN()); - // hw_commands_.resize(2, std::numeric_limits::quiet_NaN()); + // A single steering joint and a single drive joint + hw_positions_.resize(1, std::numeric_limits::quiet_NaN()); + hw_velocities_.resize(1, std::numeric_limits::quiet_NaN()); + // Which means 2 commands + hw_commands_.resize(2, std::numeric_limits::quiet_NaN()); } return hardware_interface::CallbackReturn::SUCCESS; @@ -111,6 +161,9 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( std::vector CarlikeBotSystemHardware::export_state_interfaces() { std::vector state_interfaces; + + // int commands_idx = 0; + for (auto i = 0u; i < info_.joints.size(); i++) { bool is_steering_joint = info_.joints[i].name.find("steering") != std::string::npos; @@ -124,18 +177,34 @@ std::vector CarlikeBotSystemHardware::export if (is_steering_joint) { - hw_positions_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits::quiet_NaN())); + // hw_positions_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits::quiet_NaN())); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_.at(info_.joints[i].name))); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[0])); + // hw_names_.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_POSITION, commands_idx); + // commands_idx++; } else { - hw_velocities_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits::quiet_NaN())); + // hw_velocities_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits::quiet_NaN())); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_.at(info_.joints[i].name))); + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[0])); + // hw_names_.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, commands_idx); + // commands_idx++; } } + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Exported %zu state interfaces.", state_interfaces.size() + ); + + for (auto s : state_interfaces) + { + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Exported state interface '%s'.", s.get_name().c_str()); + } + return state_interfaces; } @@ -143,17 +212,51 @@ std::vector CarlikeBotSystemHardware::export_command_interfaces() { std::vector command_interfaces; + + int position_commands_counter = 0; + int velocity_commands_counter = 0; + int commands_counter = 0; + for (auto i = 0u; i < info_.joints.size(); i++) { bool is_steering_joint = info_.joints[i].name.find("steering") != std::string::npos; - auto hw_if = is_steering_joint ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY; + if (is_steering_joint) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + commands_counterpart_.emplace_back(commands_counter, hardware_interface::HW_IF_POSITION, position_commands_counter); + position_commands_counter++; + commands_counter++; + } + else + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); + commands_counterpart_.emplace_back(commands_counter, hardware_interface::HW_IF_VELOCITY, velocity_commands_counter); + velocity_commands_counter++; + commands_counter++; + } + } - hw_commands_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits::quiet_NaN())); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hw_if, &hw_commands_.at(info_.joints[i].name))); + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Exported %zu command interfaces.", command_interfaces.size() + ); + + for (int i = 0u; i < commands_positions_idx_.size(); i++) + { + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Exported position joint %d at command interface %d.", commands_positions_idx_[i].first, commands_positions_idx_[i].second); } + for (int i = 0u; i < commands_velocities_idx_.size(); i++) + { + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Exported velocity joint %d at command interface %d.", commands_velocities_idx_[i].first, commands_velocities_idx_[i].second); + } return command_interfaces; } @@ -171,20 +274,19 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i); } - // iterate through hw_positions_ map and set all second values to 0 - for (auto it = hw_positions_.begin(); it != hw_positions_.end(); ++it) + for (auto i = 0u; i < hw_positions_.size(); i++) { - it->second = 0; + hw_positions_[i] = 0.0; } - for (auto it = hw_velocities_.begin(); it != hw_velocities_.end(); ++it) + for (auto i = 0u; i < hw_velocities_.size(); i++) { - it->second = 0; + hw_velocities_[i] = 0.0; } - for (auto it = hw_commands_.begin(); it != hw_commands_.end(); ++it) + for (auto i = 0u; i < hw_commands_.size(); i++) { - it->second = 0; + hw_commands_[i] = 0.0; } } @@ -235,22 +337,59 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH if (m_running_simulation) { - for (auto it = hw_commands_.begin(); it != hw_commands_.end(); ++it) + for (auto i = 0u; i < hw_commands_.size(); i++) { RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'!", it->second, - it->first.c_str()); + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'!", hw_commands_[i], + info_.joints[i].name.c_str()); + + } + + for (auto i = 0u; i < hw_commands_.size(); i++) + { + // Find the tuple in commands_counterpart_ that has the first value of it equal to i + auto it = std::find_if(commands_counterpart_.begin(), commands_counterpart_.end(), + [i](const std::tuple & element) { return std::get<0>(element) == i; }); + + // Get the interface_type from the second value of the tuple + std::string interface_type = std::get<1>(*it); + + // Get the interface specific idx from the third value of the tuple + int interface_idx = std::get<2>(*it); - if (it->first.find("steering") != std::string::npos) + if (interface_type == "position") { - hw_positions_.at(it->first) = it->second; + hw_positions_[interface_idx] = hw_commands_[i]; } else { - hw_velocities_.at(it->first) = it->second; + hw_velocities_[interface_idx] = hw_commands_[i]; } + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully written to joint %d of type %s!", i, interface_type.c_str()); + } + + + + // for (auto it : hw_commands_) + // { + // RCLCPP_INFO( + // rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'!", it.second, + // it.first.c_str()); + + // if + + // if (it.first.find("steering") != std::string::npos) + // { + // hw_positions_.at(it.first) = it.second; + // } + // else + // { + // hw_velocities_.at(it.first) = it.second; + // } + // } } RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Joints successfully written!"); diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp index 6be3332ae..e0ff4a51b 100644 --- a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -76,13 +77,15 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface // Store the command for the CarlikeBot robot - // std::vector hw_commands_; - // std::vector hw_positions_; - // std::vector hw_velocities_; + std::vector hw_commands_; + std::vector hw_positions_; + std::vector hw_velocities_; - std::map hw_commands_; - std::map hw_positions_; - std::map hw_velocities_; + std::vector> commands_counterpart_; + + // std::map hw_commands_; + // std::map hw_positions_; + // std::map hw_velocities_; }; } // namespace ros2_control_demo_example_11 From 25ce02a085c047f64c1d0e1c9a21cf51e77786c2 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Thu, 29 Jun 2023 15:27:29 -0400 Subject: [PATCH 11/37] format --- .../carlikebot_ackermann.ros2_control.xacro | 2 +- ...arlikebot_ackermann_description.urdf.xacro | 18 ++-- example_11/hardware/carlikebot_system.cpp | 95 +++++++++---------- .../carlikebot_system.hpp | 7 +- 4 files changed, 59 insertions(+), 63 deletions(-) diff --git a/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro b/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro index 8cdcee0ef..4f6e44cd0 100644 --- a/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro +++ b/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro @@ -30,4 +30,4 @@ - \ No newline at end of file + diff --git a/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro b/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro index 0657851b6..534bc9135 100644 --- a/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro +++ b/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro @@ -59,7 +59,7 @@ - + @@ -83,7 +83,7 @@ iyy="${wheel_mass / 12.0 * wheel_radius*wheel_radius}" iyz="0.0" izz="${wheel_mass / 12.0 * wheel_radius*wheel_radius}"/> - + @@ -96,14 +96,14 @@ - + - + @@ -111,7 +111,7 @@ - + @@ -120,7 +120,7 @@ iyy="${wheel_mass / 12.0 * wheel_radius*wheel_radius}" iyz="0.0" izz="${wheel_mass / 12.0 * wheel_radius*wheel_radius}"/> - + @@ -129,7 +129,7 @@ - + @@ -137,7 +137,7 @@ - + @@ -211,4 +211,4 @@ - \ No newline at end of file + diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index 919b1a4cd..89daa0d12 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -59,8 +59,8 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( } RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "CarlikeBotSystemHardware is %s.", m_running_simulation ? "running in simulation" : "running on hardware"); + rclcpp::get_logger("CarlikeBotSystemHardware"), "CarlikeBotSystemHardware is %s.", + m_running_simulation ? "running in simulation" : "running on hardware"); // All joints should have one state interface and one command interface for (const hardware_interface::ComponentInfo & joint : info_.joints) @@ -92,18 +92,16 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( { RCLCPP_FATAL( rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %s command interface. '%s' expected.", - joint.name.c_str(), joint.command_interfaces[0].name.c_str(), - hardware_interface::HW_IF_POSITION); + "Joint '%s' has %s command interface. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); } if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %s state interface. '%s' expected.", - joint.name.c_str(), joint.state_interfaces[0].name.c_str(), - hardware_interface::HW_IF_POSITION); + "Joint '%s' has %s state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); } } else @@ -113,25 +111,22 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( { RCLCPP_FATAL( rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %s command interface. '%s' expected.", - joint.name.c_str(), joint.command_interfaces[0].name.c_str(), - hardware_interface::HW_IF_VELOCITY); + "Joint '%s' has %s command interface. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); } if (joint.state_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) { RCLCPP_FATAL( rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %s state interface. '%s' expected.", - joint.name.c_str(), joint.state_interfaces[0].name.c_str(), - hardware_interface::HW_IF_VELOCITY); + "Joint '%s' has %s state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); } } - - } - // Running in simulation: we have individual control of two front steering wheels and two rear drive wheels + // Running in simulation: we have individual control of two front steering wheels and two rear + // drive wheels if (m_running_simulation) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code @@ -161,7 +156,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( std::vector CarlikeBotSystemHardware::export_state_interfaces() { std::vector state_interfaces; - + // int commands_idx = 0; for (auto i = 0u; i < info_.joints.size(); i++) @@ -172,37 +167,37 @@ std::vector CarlikeBotSystemHardware::export rclcpp::get_logger("CarlikeBotSystemHardware"), "Joint '%s' has state interfaces: '%s', '%s'.", info_.joints[i].name.c_str(), info_.joints[i].state_interfaces[0].name.c_str(), - info_.joints[i].state_interfaces[1].name.c_str() - ); + info_.joints[i].state_interfaces[1].name.c_str()); if (is_steering_joint) { - // hw_positions_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits::quiet_NaN())); + // hw_positions_.insert(std::make_pair(info_.joints[i].name, + // std::numeric_limits::quiet_NaN())); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[0])); - // hw_names_.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_POSITION, commands_idx); - // commands_idx++; + // hw_names_.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_POSITION, + // commands_idx); commands_idx++; } else { - // hw_velocities_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits::quiet_NaN())); + // hw_velocities_.insert(std::make_pair(info_.joints[i].name, + // std::numeric_limits::quiet_NaN())); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[0])); - // hw_names_.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, commands_idx); - // commands_idx++; + // hw_names_.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, + // commands_idx); commands_idx++; } } RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Exported %zu state interfaces.", state_interfaces.size() - ); + rclcpp::get_logger("CarlikeBotSystemHardware"), "Exported %zu state interfaces.", + state_interfaces.size()); for (auto s : state_interfaces) { RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Exported state interface '%s'.", s.get_name().c_str()); + rclcpp::get_logger("CarlikeBotSystemHardware"), "Exported state interface '%s'.", + s.get_name().c_str()); } return state_interfaces; @@ -225,7 +220,8 @@ CarlikeBotSystemHardware::export_command_interfaces() { command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); - commands_counterpart_.emplace_back(commands_counter, hardware_interface::HW_IF_POSITION, position_commands_counter); + commands_counterpart_.emplace_back( + commands_counter, hardware_interface::HW_IF_POSITION, position_commands_counter); position_commands_counter++; commands_counter++; } @@ -233,29 +229,31 @@ CarlikeBotSystemHardware::export_command_interfaces() { command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); - commands_counterpart_.emplace_back(commands_counter, hardware_interface::HW_IF_VELOCITY, velocity_commands_counter); + commands_counterpart_.emplace_back( + commands_counter, hardware_interface::HW_IF_VELOCITY, velocity_commands_counter); velocity_commands_counter++; commands_counter++; } } RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Exported %zu command interfaces.", command_interfaces.size() - ); + rclcpp::get_logger("CarlikeBotSystemHardware"), "Exported %zu command interfaces.", + command_interfaces.size()); for (int i = 0u; i < commands_positions_idx_.size(); i++) { RCLCPP_INFO( rclcpp::get_logger("CarlikeBotSystemHardware"), - "Exported position joint %d at command interface %d.", commands_positions_idx_[i].first, commands_positions_idx_[i].second); + "Exported position joint %d at command interface %d.", commands_positions_idx_[i].first, + commands_positions_idx_[i].second); } for (int i = 0u; i < commands_velocities_idx_.size(); i++) { RCLCPP_INFO( rclcpp::get_logger("CarlikeBotSystemHardware"), - "Exported velocity joint %d at command interface %d.", commands_velocities_idx_[i].first, commands_velocities_idx_[i].second); + "Exported velocity joint %d at command interface %d.", commands_velocities_idx_[i].first, + commands_velocities_idx_[i].second); } return command_interfaces; } @@ -271,7 +269,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i); + rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i); } for (auto i = 0u; i < hw_positions_.size(); i++) @@ -340,16 +338,17 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH for (auto i = 0u; i < hw_commands_.size(); i++) { RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'!", hw_commands_[i], - info_.joints[i].name.c_str()); - + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'!", + hw_commands_[i], info_.joints[i].name.c_str()); } for (auto i = 0u; i < hw_commands_.size(); i++) { // Find the tuple in commands_counterpart_ that has the first value of it equal to i - auto it = std::find_if(commands_counterpart_.begin(), commands_counterpart_.end(), - [i](const std::tuple & element) { return std::get<0>(element) == i; }); + auto it = std::find_if( + commands_counterpart_.begin(), commands_counterpart_.end(), + [i](const std::tuple & element) + { return std::get<0>(element) == i; }); // Get the interface_type from the second value of the tuple std::string interface_type = std::get<1>(*it); @@ -367,19 +366,17 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH } RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully written to joint %d of type %s!", i, interface_type.c_str()); - + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Successfully written to joint %d of type %s!", i, interface_type.c_str()); } - - // for (auto it : hw_commands_) // { // RCLCPP_INFO( // rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'!", it.second, // it.first.c_str()); - // if + // if // if (it.first.find("steering") != std::string::npos) // { diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp index e0ff4a51b..ed237553c 100644 --- a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -15,11 +15,11 @@ #ifndef ROS2_CONTROL_DEMO_EXAMPLE_11__CARLIKEBOT_SYSTEM_HPP_ #define ROS2_CONTROL_DEMO_EXAMPLE_11__CARLIKEBOT_SYSTEM_HPP_ +#include #include #include -#include -#include #include +#include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -70,12 +70,11 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface private: // Parameter disnguishing between simulation and physical robot bool m_running_simulation; - + // Parameters for the CarlikeBot simulation double hw_start_sec_; double hw_stop_sec_; - // Store the command for the CarlikeBot robot std::vector hw_commands_; std::vector hw_positions_; From 25a4e97f0005674bce91448639ebcbc0326c5405 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Thu, 29 Jun 2023 16:58:51 -0400 Subject: [PATCH 12/37] inactive ackermann; rear joints missing --- .../config/carlikebot_controllers.yaml | 4 +- .../bringup/launch/carlikebot.launch.py | 6 --- .../carlikebot_ackermann.ros2_control.xacro | 4 +- ...arlikebot_ackermann_description.urdf.xacro | 25 +++++----- example_11/doc/carlikebot.png | Bin 19716 -> 20581 bytes example_11/doc/userdoc.rst | 6 +-- example_11/hardware/carlikebot_system.cpp | 45 ++++++------------ .../carlikebot_system.hpp | 2 +- 8 files changed, 36 insertions(+), 56 deletions(-) diff --git a/example_11/bringup/config/carlikebot_controllers.yaml b/example_11/bringup/config/carlikebot_controllers.yaml index 4143011da..6fb4b0ffe 100644 --- a/example_11/bringup/config/carlikebot_controllers.yaml +++ b/example_11/bringup/config/carlikebot_controllers.yaml @@ -19,10 +19,10 @@ ackermann_steering_controller: front_steering: true open_loop: false velocity_rolling_window_size: 10 - position_feedback: false + position_feedback: true use_stamped_vel: false + front_wheels_names: [right_steering_joint, left_steering_joint] rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] - front_wheels_names: [left_steering_hinge_joint, right_steering_hinge_joint] odom_frame_id: odom base_frame_id: base_link enable_odom_tf: true diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index 934bf6362..41c1cd496 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -110,12 +110,6 @@ def generate_launch_description(): 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"], - # ) - robot_ackermann_controller_spawner = Node( package="controller_manager", executable="spawner", diff --git a/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro b/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro index 4f6e44cd0..5ef7801a4 100644 --- a/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro +++ b/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro @@ -10,11 +10,11 @@ 3.0 1 - + - + diff --git a/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro b/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro index 534bc9135..244d44c2f 100644 --- a/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro +++ b/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro @@ -91,6 +91,7 @@ + @@ -124,19 +125,19 @@ - - + + + - - + @@ -163,17 +164,17 @@ - - - + + + - + - + @@ -200,9 +201,9 @@ - - - + + + diff --git a/example_11/doc/carlikebot.png b/example_11/doc/carlikebot.png index 1c4fc2476dba992c2984f32d8225fadf4cb9eba2..44a32285cb89745a5907fc666a2caa2ce348b7dc 100644 GIT binary patch literal 20581 zcmZ8}c_5Vg_y2oHQxqwoGS-Sp3&}Q?2_=*iDSL%1*~z|)tDRY=QcCX^M20zywCDFuX7%EG}M(hGwx(W2yIq5rF0&lbwq^d zk~XY|Z#q#{7yNIXqk@Xo2Kdi&gGm5<-r;oOl9Q(GH7A!V4yMS=#@5>Oprf&asi}>l zxvkSEeSsWYbP&5p(ZTeJlZCAfuag8yEgpzLO6Z} zuM?_qqF?Rn-aU8s-w}QqajH_mt2^wo{E760$~TTNW_1y&-F@Q43vp4($l#H2SaM5YlywQy~d7{Za*w%YvmYG;hPJIM@2;JkNy0q-Ey50 zxKJyHs$|2zUOIi|jPe_;yC-AszbD83`uf%3TV-p2dtS`1qjZQXUi$;jw7cvzGuzx1 zGs@&mLa1XNo1K`Rkq=jogVd{6uRJ<)5%O0V5!KYJJ(pa$L59M#Kusg9Oj#D%Zy6Rw zq?mV2B$o|DUcoT3KpF_gTAjhCoY9mm9~>7~tiSXDA+5xttk2^Zq*WSlvO~kiVK^=1Wtf#ly1h}d^}6yb+w%kK4m2UMOoCR)Rp*(LNy*7MeT6&T43Qt^ z=Be!A*RrPyS(tt4>fM1T@4`GPe*AEPZP2ku(uWhOOdGWti>4e9?{}^APro)0DNO6Q zqC2)C)UA^#y^Xaag+88lh|J7t=VVw@Z+T%C3cZ-1)8TZJP<3*Bkn=MpghE|Lh;|O2 zm?dLXO@+VqINZCZe>WhYqM~oTne!88cEmfHQ1|5V<97)!w(FYgVu}hA4(!<5bO|6dGu*y z^B|$8mN)k9-J9axuGq0dLv@5p%>0MM!qtSjkzI`ph)va@Per>n2d}C1iP_;fc!R@x zE&+i@{QOrj2N=B3*6fxSU0hF(G}tP9mEY^nRQW4xb*`$_vbT%s9=u{{*;v0GX>evX zF2AlHOg$j8j+-s$tzV2WGhQ$=e?=1PJTn8KTVc)j8MxW**OG{=$o9XVbI-6XtB z!JPJp`NNbapxk5Y5nDu|AO8``!yo3Fdl)d@0(F+NvAWSB-|y*fbFR8T$DDo{ z)cml#=P}%)M301Cn%F3(zbTH6%B?O@DBUPowqG#^kzXW&!(8pO#H@DP{R8`n{Tp!j zJYEa4H}!-0ayYUA=uJ7#(8r*IZ;bcwO#<}{92NNU$&-gpD1Gkjb>zc8iJ)dcWLT7D zK#T|8<2TBbZL=1Rx^rfKw=dWq`C@Zm8PgSf{EIWUaz%o;Q2TaYyc_Qj#gvYDLU?{5 zE1Chd*R|K&zpplOa!U9LUU{pX#_pJ_2zj1uKYnH1F%}4Y#60 zpChs3{5hd^c8fDxD49N3JFz9?{rmaXS|v*@2nAf6jdjmk$@!$%e*EV~BBMefPB0@q zJxPBXqNq3sZgs6<+9XsVg2P7zZR|z}pHUo~w9UEu^#9ax^UQub?bb3JV(yJmir7eW zDBQcfKWfcTFJ-SsVCip7!r#XFgIptHdzq1)q|2W4Qw`O|>$pOhO`k$=Tsw>e%S)m~ z;t|}u?c}M0!p(tL1a!|E8RbERdSQ5ltY7!He+s^*N*8?dcx-4s#Ky-@pU%u~LG6fU zvD&O^2{#Elu2u<|G%YN22|X%@XzMz;9!Zk^I;_jy8vda&WWB$NXqmYv0#+IR^>INw z?CaFXDLR&#-9xADkm=p)*JAvb3p;Ax0-kRSCiShNBJu8??OAC~_sR4VMR$)O^3MiK z<()SPguU(LrHPWwsA?z6-G1>a%J_ztbj2EP?iCLWZC>ogAzj`vO5{y~_0X2Oc7542#g(~lrs}J2`BCg{iNq{~USLl!(BZMpnzUTLqKViE{dPAn~ z&oUt-j~;_1G{Mp=Z1o&vs-xp%j4DD=8e3g=N9rzdm8w8mn71@e zQ(-9E>6e9&da7>pojZ3D)3UOD%N3`Er9Ohh8}gOedOt~f&#>^cd&}$|MBerv2W_{8 zBn>WK&sSPga}`4DNXuKrLfq6pFQZK%aLo!KvgWA~4LX*mdC?np^C`cGZ@dIsNUf;^ z`JG3!#QNxzGbK#rhK`uCmHDk%Ztep~dSoX`7jo6|SobLtQQB_;#4a3agm80b!@+|m zs`Qb4m_D!O6@T&VyrVLA5He?B37Z(E@+tFAV*;>M3^(yy2mPD-?H(I*H1UptrCz27 zoq(Ty)zrMe^lYs`Q&R8rEB4vM*1D8Xf$B``-Z9nm;4IzfYD~nFU%V_K!#*rGolWF^ zpw*{DbI6SK0Z(e&ch89vS&_MMoLeXW`-+wUBxxn>`}WWZ(|#)s!(+vVjet%F0L)L|S}r z9G5C}^pc+5@Nb{MZTL3Y*bRu2i}i5n-ctwEGm5&6!h-u}Y^=2O;Sq z*+h@VJh>TR=FP-bZn2#A^D@7ubI9}*#fQ3d$m!%(*O$xmu7;-jx=-b11f7UvBuR%2 z3;$yZa$pCYDLv_~8Tdil!$hR})RZy^l+HV!Pds*P*y9KyMeeD~fF%1u|4`y9SwCTa zt~)CbJJ{`z$!7=z5G8`a3U^@_V#&fVwIF58u1)&MOM(#p2R`M_rSeCI<%|rL_hx19 zvUtWo4!8vWwrS1X-qh5TFF}u}zh;~Tva++g7N%#rI*S9-($enF+!w{nG*ZI$gi+i3 zOFBBicoxei1@SSi)50kF(ZlsmcQT_Ysr8?Eup|b#cH6d#9_hQ38gQZKe!j#Zr#za2~eY#z6j9m~M_8z)zdqSO`<;yQ%k08-y>`G)Tm74e>7 zBZ+{yY;5EJ3P3KJeCOE5<1W`9pz#YA>V~O1N8r87_UGwY96w8AZoQ?a;QRM5pnlK- z1`tZXM@Hz#0ZW*ks4S>(Jj^))&}@^$ZbTM7#eJv((5=J!s7U`aCpJoL{lrY>e)~7h z^sNMKM7-92V1QZtv_tzjYN!C;5b9@zFCpoI$G?hiv%?|x<69Z0EZgsYe2T$0LB`VEaRFHXA)L-e(1*}4U3zHj9ZY!B z`3k^usetDK%GA}=CblGL?cXTX$`8*x2hpUp3N|eKHCBv0e3myhUIU+7De$~}g(tECGwyUT7QCeP0e`8y1WcmDzkUTbeNr<-kcc^<(X$HtzuZ7Hb^q?^$+bDx=BE?hb+vVKsGzM z0aclPSxzLt53ycW#<;SL0Eke{jmE`G=^c9%%Bl}#rJ?T6V6m7r8hdzrNYwm@L$0Pb zrX%M%SlU85IyxeORFjlTVM>3^0_9&!%e8 zyx$mrkl4=6;amjJY1s}eEDd-p@@a{N#A2GxA%WE@0&wP1QwwY*;m~=l9O}$u~kUPw5EsUhZB_vXL*bs%Om07Aa-v>gJ2+>3FfZC(i zAYR&93E$QE!5WwW0M zjeWAHV;21^Dr!k)SP@m}95%|oH(1{mGCVxo&}#G2iwlusG+}QQk=ZX&NA&@HmHOc` zkG3RgUo5zD;G*}Iiz!RIU_Fhl=lS7rY_~D+_27+G351@~&!6ke#dZ>QER6*J+yN&} zcka+eWS48g4LbULsjT{Rd$o7tCB&#iWI}}AngM>MKWtLo) zX4vmAI1ZQ0qYw);NK8z#SgMDRXV!U!fGCmaoDRFgGIj$%2&p{>S7>~R%sqYyk9`JdPMl#lwB4s0MIMjIQO{Jz#HKr{D~oPzrEK$&rDbJtSWvum%R_dTlaeQo1Hc6cT&>U3>B;qHWxtIgI6SJMLg^ddo4Nv#Q!xPIFR2=E{V#y{)~x*ZyOW*s*O82p@TD>Bdi zHy-+hZF%$hLeBb!bapRI2-b0;+GP@|h~gENP<{RlThPhaxAFlliW8LmI%o8#tIuu` z5Z3`6Wj(5d3ZlraX-XmcqLb!-RiXb{)d0jS)m>vOfD z_!o{;o)pV~bgy_>iEke`(w$Cg+bKaX+SV~JuPBxYf2!#_+RKSI6sIW=PVJd>(stL@`?&^vC zzUvUPV0XWPn-6)<5Ng-5X>QqJGNiBhDY(~d3gk2Rnl@6&NM3H zP|nN0DOu{Ml*46XV}aZL{$0Hldha8@T!X|MkGT_+s(|1yU?rvf=H7|-`oRD;tIfr< zoO@3@gs5rP_VyGci?gFj6B`@D`TeL{gJy^)VynlzpE-Wn8x7{BFH?5@7q0LL79oE8 zpq2wGptP2I%H5cA54C^g| zauG*W@`K%bg3UnZ)>yR<)W{9$4!ftJ0wPj#Vsam}MGNRD{>S{e@+Ro_{|eU{-kWHV zu`>Mg2+ua4si>^Tm6EDz5G-;quLa|jrEwUXCgTRqy@JC;3O9qwmSt~v>wlK18bC|aV^tYEzj`qkGMl;0U|$06CCfsbgceDsvF?{c85^CFZI)FoSwmE8Mq_#uJD<`k(V;S zGcJ|}%|e7X{|L5t?f}=mS2h=C7>VfAnV2S>yIUN@wz(p%6t@gj!?sxp4e}~oTw#VB zbE*zfN1x>uy=sNvYj6m;UluO8Ud&GL0NuXa|9-=a15xe%Un9NdjVFiF2mcHzR0Mze zougCJpsoRb@X{GA)+7oYr8^h zeE2B+iO}m%Whp5&t+s+NWj7(fY99_rkkp+8vl$=wNtq{*M)sV2wF|;Cz4~*hF`yW% zlPBs*=t$R0>=z(mR>Me`mPz$=`;7SUZiH#x=HSk@&|W&`IXv| zGGak~M#i?p#Kd!nbyw}|j;zPyH6;m!v(^t|V`DMqnuYrZfM$VVFvVP(PSzP+Hl+RBL=&=H@1C=5!Pm{seW~7+-`X5_c4c%_*E^4qg+d4uEDAcQxCcM=dUukQ zsbUx$K#W}jqbN?L9ofN=jzU5;GSa%mm3Ous z=cGDOr13@zu23gP)Sm&n{SG`_r%#R?Tlj+fMk$BR)k|}SRu+tA;Hic5lB%?%vUA$q zpbRJFe5uD4DkU;E<*q=1x^w~LgP+NIB1eu?J>Q1Q>mLs765EwaZpFYkz%Q3;9{^mG z3G*q7MC~1W{Os9il?BY(Yw|JB4zZMyE@C=KS&^`dr`;g`jGp zW{^H^kRliD4mW`Eho2u}=S#J+4x0(w(Fv=h`!R|3U z@@+K3#F;W&)X6weIBtrEmVMRTKVJ(D5-bN|&N3bNOh;|FI^L1t^X!8Iv zW|AUj0Td?bZICk0zAn($X}kKepvO5P{_L}a5i<9fb6MJ3yTii*I4LZhX>PNt+AR2_ z?RgG;dZJs#SE#=05AfrpkDKC=;NEUZq`GK?We&g~$vR<~93+e7nE)qKi8%PSw>96X1;}VFx z1h`ZUHlN#(as7Mef`($*@x7U%1zpv#$|B<8uX5;VxMvN-!0p#Sv83&I**=z)@3tXJ z%j4trb%d%but+&pvpf#oo090@?w*GKx8rEmMT!Zfv2ARk42h1{mM-KN2B<7D-{5MWMsA?2KX&_Hd6Lr_0QZF zJK2?%sd@_@`ooyqwG*LtS7y|G3&e-^$8bpug2M)JXnK zzFy1tTpCY1FObYw#RHpZ#)_wV&g>IIjb-oW7UlEiN>VLMeMc38f#gr4QiZyaL#nN} zcWV#_1+6z6`P^d`n7TTj>g~x_TuU-*ahnreS@v1UzdHbKot3U3`Q_T06&E>gr)c~o zH2QN_@>)2Xb$ph;e|MhgQr^zs-Ca8s=P2^FfWzgdp=|BI;9y{Nb@juu3@zu@fcWF6 zsCz*{g5zTyMF7Uxo0@{CA?vG-;YTg#d0oQz;_eU=7spFAX(v!~hmzqf?jiy~DF4hN zXVtN_lJk!fyeturCDWr|NPlbOEDM||+0@6y7r z=HhqnngL6zE$+5+S70XpeE*A%t*l8=bz7s3&_8EOVm?3mz89<}>TxK2dp9_f(oHxh zTZb_VPv>MdpUKSa)T!HR{1_CYTO3eS)%=$)U%uDMtwSwG#~dUoN3D70JU)0Vj>q2p z%{+14vfu6WtkFFq+ovtZodL)FOSiCaFX_p+Y6K1sLxn@vzYnq=V=sj-ZsV{p+edJx zl6XD8@t%x8V4@7wbT6pJofq2~1eLAU7o6RC%*>>$PFW}rnND2x#_LqmvcBa-ocT7u zj}RqqWh2Mo zfUu4*g>0z*noBxc;Cb{d{v!o`HfeI~ln(EcSnYtW=X`3xvl)@HeSCP5bn|5# zqxtpMTOWnI849SXRIp!qu@E#*poWLi^BQ2T-8jHw{w916$XN_>4)b@TvfDIR5%0AD z<=Boc6>~OA$qfYrG%V^y*ZeyR(9#sbb3w}`)K*jRyYY21v60cJvx`Dg6wo_Hw<;hSZg{4V8?_S|`@O`^?EyueRjPxAh_^ZTI6 zcHU0j1a+K-ouGx8TUKoJt)*Q9tKzQ~50|7$;A`X<|1D(y=RbM*FF_zA8-sNR1Oz0W^{$<`o|&P+=98@- zboiOU=|#?$FKPJ$I*BBfG%YrdU1)GKELn4);!bw@XvNt5K&c@5{&d@jy$?OS)UY{FjbU*vR!+_(R2I zGrDT2EiFfr9$r22R%|uwBS19_t6B96k(jYWvpG2{0}GE&(w*7wcElcF>8edWK1p;q zxd%`31_a^i=gcnIa5nz>cn?<6lu|aYHP(T-RRn!&a+l)9f|H+r^(|<1P2&m4YSr(Y zuKQ{u5xnlnSm1973m)I2zA)&j9?f1{v`f~xjO$}K` zVr*8$0lmam+Ep5YrC`AY$ASg-!*Y4vteY8p`;|n$9Gn1F!$-Lv~ zD)Jhy`RW@yOtx;Vvz(vu%Z%1cSjyEXa)UF+=P5?gIfUGAD0Rn}7VO8bz_ zBcJekgu;1pp&%&aaNfODsz}((IfwW)j-|M&O5+bd3e@T9>>)I90`u1I&*Vo>HKdL6 zzi4jb*wuF89?MK|=9M}EThXc2Y>^fp5$`;yP*{dh(2p3g3Z+{ZC5-BI-TBBL6TWbwJq~8bw7#gylJSz>D##@*_zjrIyA8vCo*&dpYWC z;qJ4p#BQvM@lN!f(EMN7dhIy_9IY-l5BT#nY2 zZxYry@gaA+h`jTmlZP6x;M&oD(rWl#$#V3h1<#%f(96U)%IfNKgT}aiMj*r)fO7sh z{{382qmOLTRAP8bu#a5XQ%Gtry>9*x%T9!I*d@(Ks)tz9|L((}2UJpKH{?fBhT}YU z_NVYyjzT^Q6~`9(+1Az;t{o8&u>2^sQO7$YKQ;Q+rzbnzxoej>y6PjNpM^ho^}lG` zcmZKq9Ob;o%H#>+fJ~Yak|J{tz@geAC*cq9p;5iG{6+eGuNW`%4%%a`Q~l>I%GLjL zVFmAN)VcQ6W4z<$x9F?$CPONo^H!q6#?%w(I&oV2fm92C#sChFo4vzni}F@S;1G@*qE?A9hrnZfug|LMp;SeeZk?| zhw~XFoo6vj1O}&;S~+^^XQ}%CY3LN4hVohkz`B98soTP^_dS*ZMk08G7SunS&uliA zJoguetN>_?p*itYkdEAik;=ZlzEB&Dsd%XO@=I+S(D;+Bk|J zAZ}iM{@b^2=ghosO&NiFd(PD2@*7oVH1U~y9`WblXDVZ$ilU*y7^ICE~qOML(PGYcb*GIpn&tuV)48Vre&6_rT?ka6@d-`FV zzccT=k-p@r*Sjl__ow}1cNwtAKsz%sO%d4;R zqo0rF7aS?Y&1rq%E+`aha+s<4oBFC@fec#`nr z>sCIrYKC8)|l>j4|4;ZM0pIWQe){bUjj|W4W8?<_)zg(ye_qgF|I0zP>+u z8K{cZ*X6Grz^}GO*DcR=y;*+eUZ~Gf_5lkyJbkSptBF%>%HS;F;ilj$T#8`9$_wp& zEMPLs*G=yP%vqXdU+*I)?eLiYKHeqBv|kEQP5_k@>;#+&m}0E14i@!VGV@ZR8Pc!i zuNH7g7SJPCj7KuQk(kruaQ=P z3Va{x2J7n9B>xUhHcprVOydOHbQhGSHFE2b$x~P`LOMxDYXW)!dmo_;dg+-wKX zD0ydf~6QSf=I79=f>aXi0jCPk0HZP{8iCz4YESnmz@tjB zPI*1`*Ufw{LbDx&$ul630i`7h61k_d6})` zR+VE2V_+q4e6XUHAqZ7nU+C@YtDB>%()?2-A1qioEI$;IxW^a2)b*zW;eFAsoAz59?=-?8nu?U4d{GC!P9 zRfc+cbWdP0sGpaAFaBA{;e@uA*qTDvt!hN3s7f0D-k3oR!65?B6qI-iGv8VfxL*c* zx@wY|Q8@7DC@BJ9ic>Pfz-)j;g(}0rI&{ome&17XY34inqzy;n#;CZU_*e;D?__88 zxfo~wbXWW>FsVf&?^ifo-|y<1&C>H&ZuMSz%|GiO5FpRh5|A8S{jR_ewvP07N7yNu6@%=-1~vCL2;$UyFLlc3|h7^ClgI7Lp!7U~D>S>so^96ah9WO(`4 z*!zqx65iHkW`g|uFv?=KZqQw z;kc;gMUdNW(v~<+h?_H7DW43Gl>Q-E7ONbBjZhpi%5T2Y1fwU)#s*;fDB1dJ%tL(Q7(kQz+U)2&CIxoI1P1LWzjS(C zWW1N&Rm9`SKf&S4WZ^Ky@l3&8{IoCSAWvb4dYmfaxsTXoSSEg}M=#Cu0y;JzyvO&g_l=KG|U-z^j-)>gsiQ+=Qb^)b4`mjUZ8g4XXsk<0!9T z?J;7b4n;QjXApSo!@c1T2EQ>TR@DTnBbp2p z2I4Y3gunT#_7~w$Ix)4k*;-?v$Kf=rdJtw3hmX?A!Wm&mKcSkS7)cy(+Hy368tnZD zn#E;&py((vXgYN*cT%I_vFfLYOw{^62~!FToUHlIb-BQLJ`;}393oSd4Yru?9|YRz z&jU$ad;wynFZ6XC|IHvs=i>k0zGrWnW+^EwZ;>p>ZmB74pz$~|X5^yNn`J}R0+ifv zEUN0l-k3lpIzYohSDuv>V{>47wry@X`MIjTcUo&rY0h6hupQ_Bzj6TX1*@R`2iJ3htCajWWu$*_I&(NFR4}#Iv|Lv=B^Cwh&WWyBB*&*jh0IABa2uJ8-yoA-J{|4>8Lt6Y!rla=3$H$i}V z#50pPLN>-y8@RC@KOB-n>Mm&G^T_x#K+99DKWtX(aFfcGlMIhbu5) zBMwG#kwA+flOO*}j4%EUbkMjSC?s^PY$a;sex}V-8r}OC{qs|BPjbNc$NYEVNjdbv zqR_BhV`Z6N!fP?2Ww8QC^dPOl3IiTjEi6Q8!@2Ok26zHEICN7n1ei=4G7$a*kfs&u zhdo#ul7Tlx9?jgZ?EVrr+xh#!PycX;*j?71A9!{JC1U_@Y;U#@v}tW&E0EMXCo+m7 z3_YOczVw_F^C;McKS91c+0w5=Y0q)tyt1xQ@AT_IQk zFpOX3R)^ic7jHkHvpbT#cWp}I-{#R7@SaHZtq!OVUW!3Z*h~x7w~&NTBiVF&dyxuG z@*YT?rGhcnVWF`nK#T^!an9`S3Lii}=C|~=qJ!HUoGL9E zbyi$0fn*g;tmX6Xc!{_${GE1_t7ROv8&@*B+Uz?1+y8UTEkF98XKh|Pe5SjYS|oQ; zlk~#5uzj*Yxb)xZ;M7)Iy~}?7*HzE?rWP0PROx1pjBp9F8|G$w2XcO;T$S91O6`es z?F5D3sXrWRz$K5qSxdb|*0kA^?NHuP<3t`5VY`9w+MfBhAl~BevL9^ah4fOtOGim$ zSfQ~cb@YY@mK+j@{nCEQxH3@*0o1xdiq>V@F_tUFo;KZI*p>)X7F2#K{f+O14Eh5B z`-^C{W&}s1lqo>p@uK>$BYo8_I0}Yvj0xMrryQ~XEM6*o2-IrEmM_l|nFtSVVZ|iO zNeMk(i!W;}fl7+h7*Gj3W^B19>F85m$kxPocHFL>5hi8?V1R=IPh<35$RSI!4l5uhyKhBqAysaWx_jVBo4wgFYK9%Z0uUM}VU%>60yBU9 zvo_GCHneog4nTu0WSc`(5|qWx?92k)@a|SK8M$l07$Qtf11Y&d%EA0{`;`3Do=Kn? z#8dco!R;MJ+7PF;zCYPk5cfvF~VP^ z>>O`bSJ!xdb*$#0qc$uos|rs~wy(wY^Yh^?RBuC+8jywUNbUd$IsP+D z=ohhh6Lr?V%w)Uuz(39rhR}g0*Fs2e()i=xyf;*CC_|0^

`Di=bW1FjK##vyV@L zLPDOoEn8*uUZ6JWq$|TChapm$Klrb)Noq{Kt%1Jm1s4;T$w7_f!|U7c-Q=2j(R(G1 z*8YR`t@8e)xcJ5Kv)7?R+;t3|18+kiNnn2ZZv$PHm6f*pssH~P8ab!oLDAr4l6-u4 zw`v){uxhhlE|ke;OQgHIn|Cv)jj-5&9aD1dLR+Jb^kJA*Rr);9>LffA4y`_ffueR! z1?GvN>Ys@f!mQ7u;}pR%tGs{^SKJ^a^bb&_;{#*!^{YwIabECCD0lV_a`u1q_G9xV zC+}bd#~c365gFy3J9R+p6Cl)F^dFl$ekIoc>f#sk;;>d&gZ)C{URZ0c7RJ3ZU^wKh8R25RKCzD#i2Y<<%?0&@pm=*p3vZ;)!g zOfx{Qj2uN``A1AR5Sy~aO8faEFf*6jpgo-}zk`9RsKB;FY&In8Z*vI6gHjCUW$+hV z%*|LG5a}?ao%g%D$+c3ib7Ef)3xjd=Lj#ZZ;!Q3)akxr}EioE1;k;DNY z9y?W#RS@O@0Xr!$JJ@=9<{sFk!_RDsF;zV^<;~O#lVjbNS|fvR!gs6sqGwOYII_)J1693hXQfd8kt_JnoqTwz&1K%` z2!G(__|t1E!v;cQ-x!zzNCCRi3wQ=*2yO;~-DFRdjOaaq*G)p6e!(Vi*5+65K$o2{ zo}PRa(1-yxv%WTduD~pLYkYh>#m3~0q&Lvw(!;`e><(gj@a8K_M!-10yxazajrmKb zz#t#YeZKeScKp#CtdtG2qf!7x*4|h3lwSvL-#R10{k61%r+$H9uKEK+U=Q*Gg$yKD zZbVy|eNijBf9dbkFh-T*)$ynp5`AL=ZA|dS9e*xY8q3aOlvaJmIU@HKR|RxNVRD+H z2zCxLHrzH8(@r8U-7h82ej@LzSam1dv$YQ5+R6NpM1WDq6jCZX_2LlWJ5{nyZ% zYi1T^`&7&BpK8>Bfn@Rw7s*`vT*b{kZuC7%8xYX0-Omt!s-1MR*V3djB z1V}ozOOt<~N4d{RU~$WwU1t=ar$6)g@aAmc4hS3)Ft!OEbEqj)WLRImiOU3Xb-{QV z%dXowz=j+?q}BN;a5_T%yU$YJBM}j&tP9lPh)kGAg;&YoMFdHKEBLXT3(!^BD8(zN zzRiSciM8bb{7~btFrG!&dgYr@cnwVO?$KCC5y) z!#6>N^8|Q7a%=}u2f~8^0!gd*v18#gtewEE9mBR zHm-4BIhY+6ylUVa0WUj07lXI`plI^^QA=Nn5X&p_*Z(HQCd~O*)5I zrixF$C(^4L;T4c7=_~l|fKd8Vin>iY6WnmrSs9n${fT5&XA<*&;yeAR6B1wx zYz{CrL5GsJIg?mmE({==fw!;LjhyJX*L=z_MXI;BlGS=$8sJAfNLK>$N;0lM2)$c)32zCO z*-NjzbmYN+asd+WfV@8O>RirB<=dKT3c(p_KdarpBeKLsBCjcof2Csl+^4{=t8J~z z)U5k`Zd|L^1mAx6bcOlrSA4vVPzP6_^i!D}uYjl032!eSzw!2MmdMW^LnIbi|8J>! zD#SZhY6(86^=~W|G$c4iQ$q4t$9;@eJnnay*F^34&Qvh44tacLHosGbyZ6y()p6(Q z^z8RE#cs(UF|+YSjPpsOuM{rt6^S%BSu4hmKjP}+vp71wYU_QsV5v)9DR`TzBYU4Y zuKg%nXCm*QRaa9kbY$o1V!XWaQsa!~CcUQ_l5mL6^7h^RG>DHn^)%u1-fcz>$j-sd z@UzXVc_m$8|9m>V`ek_sD{&WZF=lec5pde?pP423@6(gTpMaa0T@$ujcL-d5Pp>e# z7k!x6zFi3#oUesB>>Ve+L@|)7w-VbKu&+`QBCty$8gSov9Z(g=z$`J{$_9Hz0KD-J z>&fXR+X#FQe5~HK*u+ryI)E@n&=C*$H6;4<6)VK}IH%yBZAVav(hym2NI5t`*Zuf9 zN;XYXg4H16qNhiO_QxS)mna?XJGk$WG6>Ew|=gJD6g-aSy}~9>7<%tgug%D z#{<`YEX_|H6iJb_sO*GGyv#}z1mSU21lYwCoSb<$0GAfch`@L6=S?c~iN2y z9L`iYgKQ5cIDYj>Q#^i~6ueVp=H}`;b_kKo?huwVA}Wl@0Kmt z1KC!@3`Z&V{PmH8b`napV&^Wvcs!c(OV|HgQ6UJ|N~;X<^5@^^j3`H8 zh;t)6L^52=479Zy#I|#HZU^Do^|5^&@cbpBfn9I2c52q!i0k+M=TPjl|9pScc*h1< zr=JnTZ0s!$=2p`T|6;QBOs&#|gD02~qR=`kGBlmX2GB#CLUejwpxp_)+_ zlV8nXh#9OzX(jk`2Di;KbAr~qV0k?`V+&E5gnfG`I(9mSzECQFaPVuo+qY|vs>Om} zy-m7LGe9$@@68wO5vOFlnI1$2WZVPR+1Fkkd{pKCY~iv~O;oXzE>c$+l1y9Mh1E91 znYrf1nxe*1_frM@&mO}eZ!-uh`{vWT%D}JDoCv4{yJ;)vr`>MA01Hc+TZo3>AISB| z@QhqaikwnJ1=zOa{Zz@d95F>Q3nZX#d&s{*`Hye3E7eFf${fy}#f>TE1=3cL{Oi?T9k@c5>AlXFsgVp)yA~E= zU5=yt#7Ww2c)=Ijw?yZaU>X}c_&2Vd{oVWsTL{UA^$N4Fu(WdE=KLx@GsDS}72~7= z9jQnrZhD8>f1Dh}6vKJPQ)f&|y}Ms0#DU!vW`y_f10;e3Hg+qcWNkm#s>;MaV=FW;kAcbuJ>92dcdC*UXt%koqv^2 z{I^Rp0;Sg+`!eL=_0WnahRdGbi|Jj1WgX&R(25%WhWaD?wg#|)%l!$K7L|1HDK$k7 zvzkr13(LX6k&zkbUg5LW+&ioDO>IxJp3%eKl=$8E%qRGn_rmC0&m*Hxi-!x}F6%Y1 zz+boO3#+WAJ7RSev>BTQx#5~4hnpdFSqMdiLOZ7L^~<)QvUIodvvh@T0tg7*Q#VM( z;_<+gS5pXG;l{P^o@VG9c$qJrgHBBv?B*Q!A?G3NU)9*Z_Ws|0ojVHOanrs#P<~5_ zsgR5OuH~ zgt$RrBukUhH>+u}VVD~*Q5Fvad2|c>`LXsLU}LcO$X4vbli%)y`g)!w?@cKGIK012 zHfyvQ1-t+X=@-I8GUQ#857Q1Ze4SDcXXAvO13LV$R~Y*chURlP`9M@_DeGcdig;t3Vl{w?7`bqXu!2njrJj7@CIZISlw(l zc6J-VYonfFLI_@~q_Z=mt$3U!b|p>hqi-;=3wO}O-V0){oxI>)?Cu>9fYwgF{U%MY zj2y$#>FLre9{S5~*S>HH7N=<(18eXnNRCS@Ux(*-V$Y%OuFzS>Lhiz}PuWAuyoL6d zRaCv8Mc;N;+ATUXn+EHX+=}f_7n4&TyWfcnf{kN>RYBGRFfB<_fb>gRYDvZRNAkDI zo=t0#g_M%}54KA}hHgtqaWN52orCsVzrUwWHeCCH7G@j0*~qP|m|(F099(x`PnJ%i z9l6W{+CqT8;mEnvF|7uJjkwax!o4txq1_IXZVHpG1KUCWMtE8gO-=!toO75TBKKD` zIotR!Ilupv)6)-IUY7b-PC?q8nm6)-p<8J!zn;{$_Qe2~bqwG?MhqK)3XDOTrTesd zFVFzzj z)}z;qU~*)?{DXEQB|$d_et^OQexO1~vYF*v3rT=$;6_rMBehq!umOAck;{LDF4Vye z_Q+*0AgF%e0;ovfvmH&kBkY)T_rQ>&V;N2=Vj5*Vg&D1{2#~s3t5t~ zRg9vL$P!`5n%(bQqv!d)zt8*E`|taC=kxS*+;iR6b@p?vbB@9c^@;1a1i3H_TX$4P z(+I;>w!)vSob2#~VMSUCf33N!V|E3@)>ooGY*BmGZ^bY%?5O4ulWPx$+AXeFcwU$3 zFT)#k9!=U&$Fn!UH&12fn`}wXJY#0f4(~?eEBiWZe%jaf`p{?C^Id+fNZlQk_5DBz z{jA4vrA*q8N0r;ROAMa9W`_y9M?-lP`Rjs?JbZyk*~BOGsjp(goi|I>S)48%^vxX_ z8d?eXJa`gQU;uxU<#>o}7^V}T0O%9ADoOCnNQ?rQQ`ud4;d|atIy_1x2yp3NSnS#UB$W#MEdHwi&$(arN3X!7mYG*6Nnml-Qtv>khvzIfr zzc*T{FsQrRSe23$^qh)RfV9&-Rg`%gUm!11wr#03#3GaKM(t9#%4>L1&A(>z1^n`MZ zeSXWhhuT1sF|q=^4%!ppo!qNd*ZFO@TA4gm@QJiJ(GX{pD>8xsqMl+>sQwj1}#+<=olfr(tM)P_`SD2rK6qe%d{*~XRnyyY!WX&T+P)MdPd@?gG2uP*rEGDM{O8sT z(<@`-{)*H23s+mKQi4SGNc;QbcgLMQ-)+|)+t*T}RG2v3;ppqXld@0ar@M4r5W_v_ zy>G;0I zKNZtu<&+Md=m?|zZw4qdSNkPR(T!1hZaehvIz+Nv=8A@6DL(ftLz_2a+H zRL74wbG^K-L=-qUdN7XY>i1b-9KUE6)g0)L*;b(AHrVQ*g>|gUr?hiSmx+hOf#p2GUuFxklLwG;u z(qIoDJ?B}oDr?((^}5I4*`FDSuewF` zKWl%#aInq)@$jE}LH-If&H5URBjbA~Omm)m9u#%Cq*`-#(*)us9U zOQ*KfI=-A$Tk2aX?$~5Pd};H(PHyx5=oPk_7Zz=cxVy2- zk+LR6R(O-Nn_SQt=Y-6r7>%d8AB^kXFT}bWI+QrQn&W8ZmoFNeiT;at=B5DAP`a6X z#gM4@rgsd`odxcAOJ2pA8{48{C1^1gb&2&ZCMj8Tdf7eue6P_j+4sw`r!KrODJf0p z6)&RQQ&3V_^tr`up-fWUNQ1^bvo;hwS ze9jTqiBASgc~|EeWg_nT9S~aDoagk4E#F(E%`t${|2TL^$Cl~f)Ru46sCUEdX+v;h zcyWJkV{ZO^zhL2|&8z%tBF3Yn7x&!qGE1kf*~C*;-O^sWUXN0k*m5Wg*l|!lcnvK! zV%E$#RNCx!T*rKRhlW?HveNXi(+_jqnPumPzRHREeVzXECZ0LBVE#Pr1HGAbEX7}U9 zYe$Kz*F2Cl#<2Ab&fEe3yE)F7*)C<++`sH^Id7@1@p4|S`B_| z3a?e?KkL^YGQT!vI``nJw~WZhkbA0!W%DSxyY>}3*ZXfdp7hjXKB4cJf3L?cnYP+b z2i66e&8@NH+v|QutA>6o_G*A$n3|F!msMd=dLYlfUko2^&5!y0^a6vG+Ts-TXO*JD zJ#rtZ0Z*KEo=tz#BcAjKc*pp9Ppd}8mDh-xxlN+ma2qAGGHmeql=0%qZ8PTm>o-MK!Wydk4obR(>F)6y8L94a`v@#_%nYz;!fQJ4nx^UA9`%8pU6;*v zP`ZYTM@qMC%A0`3KMThP4c+nHNO<_xN7pH{jMaXf9Mwz3(Q_f)iJtVx$6AR z3Y=9fLC9_!uVR4ICm-%jCn!&T)kKGO`2NvPGjh5ZAoydm7=I;6+twXN-M(wXk(Qn! zO(8pmHhtf%g08W`--1|CMI`e+0#;R>kUz-aY!c(;CJS=7xwEoO-&Xn5%<%Chk zQVy>43=MZ`G> zZL#KIJjLUcM)xjBfdTMrY@+{qOSuu&LLnxiz#9Df4T(l`B#&Gac*8Mgv4| z@^%fA%NlClTG0c^jd@d>@Q=(d=9pXE(Td5CqJ_>$cH-%`s@nTwAU`sS$u-%{h-XCF z=L;;fTo5VzzP9f`z%+5K+-~Z{SQoa6nWX4?HGkSv7{Qrq3sHjE_@a%}bfLHm{4DAB z(6v9qf0#}g1H+vBT|d)e1ghokgXWUyyA)|f-f zjwwM<3zKmVnCBtR?e6iTEk2cve-T&x@UhsidTaK=%FxUVZYvx4GzwjN_0CJvFXGCB zi(m6?d!Y2;IksI^lV6EfjbdJHwj;CB$|AJ9N1Qw=kiy*_jw5onoH({yPBnUFen#?w zc*+IXOQ+e3?|>%1G$g(!dQ!2<_Sf|iTFrj!dX!-427{Yj8QUaoWtyCenJbBq;dTW9 zJy@^de&(OdcO-hCSXth|XvMGU@@s*l^m?4t3zIQd6Vtu2H?rThzj6@=zKU7p;Vbv) z3N@YJ?$Li*C1<)j3a6eX*Abq?ESo&PbhO&^99i<829C3WyhLSbX|C{xn6I9f{)vgE z>4iUPoZ=?C#~A^Fk?$h->meD0*j-;n&UJs4yH^vtXP?YniPWghYIqa|kG>cQ)Cuhp@(f|xip)zJ+QU~0 z@Kul5+)*w84=ku!=w~-j&1kQ(W z(5(M1|I00PLg{(I_>oGIO&3_U*xYh_2%6~KHq3WUDqn$XM}4aSHkNkU+C_fKmXyBz zi|e75kG8J7TkgZgJzvtet;9U@o&`H%@F@1D1r{zo87xgz-X3W;d`yT#`pg&(3$_iF zP6b9O0S5>_fRAQSGyqjMl)l-ot{qA@R039cCh4N8>GO+ zVat@j+`+))bcBMxb+MC_$KkZo&OQYxYj zSi3nG3SC^dE;%bXKkrf4?L>;qV6G8;UZ$v!9iC@P1wK-@Gt~J5BgAf>JuvueX}*2X zVf4t-w3b;OVt(~@4R`U8U4BeQBho?lv=-Nt3oo))#IPeGIDlbnw%2JSQ^kVq61PO` zvYMhI>?}ynszI9M8~Zf*!^JN;eJAN18E9r7Lj6d^;tL zTg9Ru+-L{9II)bArp|>2xUG_GDj8kX6{-oEDcL|8L0Q8+Iir%KuA^Lga(7g}Z`=p_ z!lK3tBEh)W5B61_!bf;9-D;c_Ue=Z7x^AnTa(H5t?oO^W$e+y$%`es`b`Nz+HWub# zhUS2BoI-8o+r+~YU2n%DMwDh%6QjICbt1e{cxYK$r<|J+1o3;C-=ZS3OlstH@CDhB)?6=GDA%5#k zi@nO%Y0N8UFgVrp%0+^$q(O!<(+xalt(AFs7kED-4B)~D7(u0g57o3 zRj7)P__n-<#eoV+5K9BHH`)bAhTiP2O@T%3$Q5u&ag(0SHKa({>J}CmW#yRlv%7C1 z-gAH$DRfk;h1!o8de<&gT9&M!Bn?`X?&6kZR~44`BX7Meek-8)x{{IXRLW6ZSfkY< zdpYze{#Z+ON_ko(D=1&$MrLugHK3Jx$uv<#~8~JWN*yBC4na?$3O%YIZrI5+QRd4 zq~Mk#8=hT7f*?!IjIpGxTM@FQPrg=L!`+T#V;d7e2VP=cCXbu)Sl3|)i?Znxtm6nZ zy?bf}Q@D57wYr*s%@A%NVLT9jN1{c}emVW>nuyf_rOp>Ox2(7ydrb&gk68Xq!LycG zIZe)IAXZJhPn{YK%`-HwsX{Ek!XJ4j1H;N6yo+QV%6qzt+e*_+>=q6&0i)?Q^A+vb zY^Pz$GUPfXYQUxYl?ZQ4H|rX=)vo8}&&;h}o_e;~?uQ%HCmtm(+WiAwG>InV{!8{H ze(S2??u~Ym{~U33KiB=^^T!n!c_^c)*}1TA>b2Ib?bca#XPnbZJb5wltwE8LW8vwq z%%{4Z4t1Z&Lp5MO+BD9p@j$%zyY}1Rd6nW&5dIywtlT^`NYkEuMa$F`tWjJIt5&Bm zRSWQO2cU*aXc0&`mL8hdi(aC|O6lJD|50%aA_#_;0iR+Ay7kw!O|3>R9tC|o%96>S z(H@oBaHt2`*(P(_Rw}TR5b=6bcph5$OB2bfx)^EErNW_pk_Ut;EJ>4<8-Gye7t?bM zbyY(v%|JnR*ny|phOhw*Bs9NI^@Vbz-a{!aTj~HTd99`4e(0b`y6-lO9~HZ`PUhx@ z;MOuYATj}}*^bE(ueAsVArHYgAnH+lwFTHP-bg8~o*U@(WH_NG7(uQqti2K-415}LOrLD-{lxX6?Cy&IMVGng6`Xr#2|cQ zv#>;W=CPzzYJ zp$+1OHUw($w_F%iYjU;-sCtU1YT;SOX#eK3ErR{6J#8aKxLKV-@IvU>1_Pp*Ea(P6 zEeJ}j2M-g3NHhZDFw{o}WM=-26J$3dVGc}($f8R37@~#aW&U@DpD3#xb# zw!qp^mvv14Q8>nC+sFJ2_@V?Y*#Rh$kFlUkbF0jzW+`{J7c|8FvnpXmU48=CORC565{}(2ra;{ zOoW*3-v9WG7Y1DLG8*n|Mmu2dohMlQrfx$|;~@e1`E!#Qub39n?_SUb+aTE8a2trn z7M)|r$4a)uwgV1iBZc}D^aWCULm$2(jAcuPw*Puf!YYJ)@aA=lC@r=;BqIDSI9E;| zLEyy2=0Lxb96>VG9H8HXWH1pbKSJCULE908%!aCrpkeR~R3d^(L4&Er2%;d!8bPNK zbRP7ZdJItNi3g?XF(@g$ba^A;JBWdh0Covwyy_Ip%K;cp-I|9d+(8US21Iy`AY?#< zQ3RPe03?o(j{$KINC*=6H?72Yqkvwj7K;%{<%kjQAsRQZJ)(q0)pHbnq`$_{P%G zl(?|ePL@Y8JPjn57XU*DAYGf3Xnkv$_)J=6GW z5LZ#mFY4Ue7?^d8`KLN77~BNL1dMo^MKtGBlDludGG7JEYk-T6*ildYgfj1WpZEL% zin*)q9LTaIVMV~m9o>y7j(JrBFGeqYxsqbJJJ&t{>|g>^N%o1;D#momcvzxoV032< zXe!wcHh*_l$Me!|r7`FJPdrk&6#Cx_2X-CIU<>JBM9P}Grd4N>xv~4`f%%U>N4+s; zHBcs82w;DCtSw&IxA%P(;Dx)Vz(ue;_9)lCd0vT`kEcEGI?&_5kp(taDM4Ystl0k2 zspbpUNl>vZ;qdNGzvg*!+=uL;Y#X?JX>jJ@u~DF4qF0V_c8dqcAg9*>)=dP{*hpmS zKjL{Qb8Va{-8)5j9fQNh7F3C!GsS&S`0h4Ts41hZ+}_oI)6|o zF`>!%eBFru1z_ZM5HjZx676e>l%Tq!{M6!(2zJ?3uDnS@D@k4F6CReR%>D47GaOBR z8mkCnV{kXcAAoJXBxteO_;le3NB72;E}D%G7S!MHx#rCmD-nNm!tr2DVH!rGDsUMp z6!Zagy4J6;oDDeSL3Co;OU^dWGCP~mln>>R+^8)rWTfVHN@9dgUHk2 zwv1=jBE8#Z&ZkmTBp-HZ|xzuEiZtX3lkB}Ez9Zi5H0uv6Bm)4*(5{!&%_ITJh=U|Li>vJSPKe8?IOpOjrL%pIWnS)BO}(+H|DQ)4-LM%eWt5p>m#O1g$=Ot zO&nMO3qc7#NJ+e*LRX868voypwZqXM`C2>&?G-pxXb#BALlQmrn zL;*E{)YUIULC$ceT+M)Ej;9IdfLR#s=<3Hy8FEx-v)?}f#)fW|L1sx@w|+7fyJp9Z zv^tP{6xSh>H`M9sr=hKVTZY&Uf}*DI5RL7rJOt^~6DO}e#bX@1DE9XOU$1@1laI|l zrV#UwIVG}VFC{2HZ*x4xX5?D0H%U2=Fr6R;bD>#3Hn&ICW|O1eTb3qst1{#HFuh;} zb1ry}k+U}vk9}Z2QB5J{?!C^2)oKZ)9pW~DFT!|;0u!Y5Q?C}o6tYU0sUee zQIZ|)D+Xl;u8-^|Fsyo)EA4I`VKasdsDfjaGqc$TChYF z*a&%UD4puUm%)xvjl1x?_t4h14;8r5&LOf;`9|#@Km_@NfMW<>8y=`2xzGCDo9r)y z>K-91luj4}(P5oNU3l3OXeU&8(u23$dVUYMwLXH74QtY)oT-iUh2YDFnD=1NgbylV zdje?~<_3N*{2fqDw!{$w^6)2s0#v8LkPOhw$pjFImG?j@NdJcr)!3T(tv%=4(K#s$ z52GkzN4upA+e@aUsYk%%)_~JW7q{<_v29@t-32|p0e*mH)j#Je(PSL32#S%gcX1mt zA4@W{Z&U@24}wEORXl7JMCk`O*iS=*DUKto;n1pq+Ew%$aA9XvqZ&W_AR3+ z4F(s;6>yZ;6_Js6SPb89f^dVyd+hdvfUpZF!lb|c51vrC`lS~MnaDf= z_K$`A#a4nfWP%6B;BPclW9R|fWr7=&>tQUaw>~2Lf(!*YRr1(fEjFDy@bb>9^2J~@ ztPHKT-_he&OP5TeDmSJJ2a_9tl4>-hQ>z6obUSi{UIk?jrX&r0l%tA~4g~>I37Log zqXkpvJadq_3B%IQjJ{*$QnCPn7PzJnBrO+479Sa0`nFzNUN91;PJhbZ-r*>3{+x}4 zI=(c%wmr9O`Z;cKZxAG(LAb|F&J%_?&ras}3AJCkoi3cY8RG%Q5MDD)hKElM7kt0P zV{h@%r}27*xs}uHOz|Lm<};%4zRl!RAiQPF(cjf~MvR%Cq@a}7Zxi|Md-c8ppmH`k z5P5l<4b9xiY|9T!&;w&5c(8BIFYv~0!b{K>LxOR2O5j_`3>e~Q8VG;DWoV9;9Up*Y z5gaGL`J_Skv#q0liMV}Q=824O5bnMF*_GfTR=^>6aS)QlVq%V`I;Z%nPel@3X@hkk z;w$$>y>^S$S`DV11f?xEFLE{pp{2T8Z}w?GN-SZ-+Ofb&R3L{I}M6UAAM@>?e@! zvyD0X`%1X5R0 zxd89nXV0F!^S^^nN~rqGo%UHQ{b!|1VuGLlghEltt5@H~4Ych8cC$=qLRKt&VEO~C zLseyAvA>G@IGt~JSbLb_SoG?ZR63;wP(c~dOPY?8`$|`7y#?#<)=$Ijl|Ufa*@4f% zs8ERiVr)4Nii(P6tt*_}9p>JB##PKV20VyOng&)s*mo5^?^kSGYsB6URwFCPe<=NC zbK~c!2Sugs4Mic-eq~n<`tb=TZBHK_8S(%9di6bJ%NlU@v}j1t3aoBrs@Z-vzsL-1 zi^#IrKtJ^MGaI}y_4ebh-uh!77Z(?so1+DPO~4E2&PqMD07W`gJOlhBUxiVhUYrt&WpU!v9_Z_9~ySxf=W~dBBr*29AjZ=FwiqPVW~ml)$}}x^UUas!zabGY49pdS}}6IW#z~}?2y#| zmHqD=M81OBVXIr4pB?$3r{AD_#8-Q%m5 zp7*PbkB|R}joWXq_$%bx!mp*cnNMnw9xq0h?Q>{EO@DA@>q{RR3hhEbXJzI4l%|2& zPAMg2W$)f^H8nNASf94%F7+Qj+S}WsGY=i&SJ}^%_QC#2#`s|cg;Qo`Ub7RwK0PvU zX)n6EFguyJIPplh-fQljnm_!xeMxlb_L9TkVwV5xqu*6aQ*W3bIzFh*v>S$qG=ouG z>ea9IS9vH0@nZ`p1mUc7B)!k9UIFVxDIdnK{|nK{ydrpXtumfi9&pyMl1Rjav_0D$q}A zYfGG;8TDTBt%}c&D{E_MiEp2O#AL<~)OCd5oF(edCcMYMr0e!VaZ53Q<64Qd{ zrh?3);MJ>##cpO$j3DpJxqQ^Ye|3jTIFY@87?VJLE#4P&_<6 zH8?{;LL?+4s;a8QgSxxBKYjXCQc@BV6Qdw;#Rmj*`JWdrrU#m`T*?*>7SZIn(in+< zhTDt1#_b1Zn9u3kwfk`DUt3zjE)9v4mR%{>6#ac^RMvmqB~rVa9=wpBpFf%A7;W@z zUs;ZnAWd6R+PBQm4HQ1i%*u-C<!`dS^7`N(z-a4y& zg(?v@rQR}z9?FPdy_Z=vB``IBv?R(7q4f1*+9FHOO1cS7@{O4S~v7oHxA+D7D z`}hAD=_vCYIX%HOQnuvB`J=b)KA3P$c}s1I=@ZMabb${aKPEhUXjzOnb{=dBJ$*9u zt+n;~)R5VPGh*lDKvSpTbswMkg+CoK($d=XE;7{h$|91FKRuiOW#F%*sOTihAyoDD z!R!iG-Y(GgF{mqw3ADSq6Zj&FF6Ua8iEP%=;LOU(0vaPl7Jk3k;PU;|%jf9h36gos zU^1DwiX4mQDShwj(TAvZw$x{4Y;L4%LtlGEj;3JLQ4P+(y42OIkG0=SFX$vu)c12y z8GV&{M8 zGB*=7IM1Iy??3%UR7qb_H+7`s&uRHV{Leka z$L!crAuC3Ge85A#eCu32DTM#747EP^vn^cqk80ApCu3h`;I_Z?b1zrFU+q0o!r$(=j&ae+O0HE-AwpY;D>Wh_LO9I*;>C-0 zcDs+fb`p2iZ%Jtz`|dqb*wrtQ=)dkc;OaU$#cb>FUs?c%`0d*__&;dvrS_u2k`m$LhXoby@W^>F zj_%k8-Ua% zEe#8yb>d*Ydt0a7+}%H4IB3CrxZ8a|I|XgavrgzSjHA@~O|>}W^nEM3I{0RT+O210 z-^%>`&!69;#Pz1O+J%r5G_ha=nJJ+(=gAXQzj=im`cjkiLeH8}ICjZ4R zR$>=F$a9H99GKL5hS}K!>=uAa1FxpLuv5VmJ4j`ldRqLHHZ zY7ZlFxY1Ez-xGwyCR+_GfRaEChr*+TQ3Zl z`p-T0->Uus91u`$xSAa=5lBN9yd`kPkK#s?^G@t-hh_|YgOhbs(`%R5k z-Co-3XC+_2Q5PH>jKW~wd%rXWW8#&Me2`&Qlew$B`HoHMFRt``9Pg{o@5qfSw1@eG z(0cY0U6A+pC(~)>xw(e~z2o8xwAb$d&ijEK(R5bxtA)g$bI)`xEGj8!zi`k%Uq8kB zyYHWN%Sr|mCjv%)&80yxtz3*%Ugxu8Z2Yd`++5;2=P{UY5`WwylvH0wgggyC!{1BV zzUKl%uJq~4cj=T#6Fj!0ny5pD;0wK{$=ND&bY04iS7v|OJv*2$mP-D7_nw<#M(r_8 z;j!~hcKfQREu3;~2DI(uM@us^fj$mnTA{OT^T*ZrHQXlLdO{pY&!ISW(qe9Yynff8 zy@wy4e)j!gOw5{#uJV5Ea=Q88I@>xrZjYbNxpsK}{&RDVP~xN>aOHYD>z9w-4q;7) z3G9z}_Jp1JgfiuR{kmZ5?zHwV9tjBv>tA$U0XG6Rp(0x3HU9JCNBub!hWz&UA{aTL z8l8r`*j=eU<@P(DcwoQgq1IgC$h8L&p6`79LxUkthe*lzm>NXW4q%tnkvtFAZs`nk z5$JYc;MDV54zqHc&?RYzN;B%ay^>h>EYow`zdjkpCHB?R@wnIA~sI(F!3T4VG)sCr%oJu zZ1(5(@88qY3f*apY{<%v&+%e%njJ1sLc(@ML8QNO<;v1*?$Ydd;?g{5V~+A8|Cjc2 z4A>Xqm)hJ={=P%Cv~XybC2~AQcQm_0zk;P@PQUv49eql#4xZ%YUhO_WF?(uhX$g6F z|4hS&xI>AmzD4=@SFT-C_$Z!!!OP3b-(L+1AYyEM86Hq|(A|3{Sb7C`Wcc_jE&Ku9 zyA<5`%lfI8yZZzyiBEhs?6o-WB^`U!)YMc|&F|rq88k%nPCD^tgwwI!sL$&6lP|cq z_)j*OGCZ57L|Q9_;UglV?SB*3n(xT@<81{0&n<6l)1?XKTxvQ;!FV#E5yG3-520Yh ze`&64aj@)Cdp@K!5DuVQ@d_y6Jmcr*mk}kaV)fFkx4$2+PaNK%J*QtWHs&_4Py~Le znb)3r7Zt%-svkY1e&1CySw0W$q_dNQ6wB7{C%k=pgxVF9l(@tT&N{evgX+jwmkz0F zRy8u}Rp+E)j1$8@cW3BZk%el$u%7_OV7cp%=JQ0z$SI*Oz)6nOkX)GLR{K4a^=aEF zH|5jkC{pcRsZv~qYg>lqHUsNy8#UbLV&dW=(gg~KTfmXFNPD}x%j<8w_&)O`o$M(pjqOC7m@GoOZM9#WIz!?fU&uY}L6NxgNtvFZR|H^|8}mHKe_Plm`g_a%C$>;5X|Q zYpn>^_PBhxVSEwNB!~J074NAbh*QkA=iNfvxKmt_LlV4On~(X(TAMlJAOxO<)qOY#^9w=a1HEvS0M)!c z*qk%?CAZ^+E4_PLBb`p4`XEgQR-#XPI|Qe;Z{KdoT@J{1R3j0D4Rx6@Br7?ta(6_B z{6etwKH+~aA`1&0OAFTXZ>`bg7Ya)>58|if!@jfbE-oW+E~QRoO72(SI)6_>JP58{ zqXwn2s|x0(d^lOvO_jPDHKuGX8# zR?!9EPwRe%kpAR^qQtV7F9p;Ul$Gykv55t;N|ATn6SPF_4n}BGZr`R4>Xc1)MNmRw zKm6qMK2l5^)4o{r=+UDUJW?1c`Xwpk{nN0sG%zp_t|yC~ok_}f);}oq?yZeV_~Ti{ zP+RzA0A~-RU{tf%t8C$7fg0}TloS-EX9sgZD6>C8M4t`c;z|XN9I>R7qu&meF+4n6 z>bGD{0-prqd(|cH*E1qVM!O+BM_Kz!XW#~PPEp?pG{`MV^z`(!hMPu++WZ+8mt3RF z_aA46S7C~sTMrUSwanz}0wcQM;jme?d6q6lMf*e@rNh_ z_k-KteHQ0uKmn}F{8W^cEiz4_qoa)!uV6MSfsqtDpX546d$>(HI6&wtU6_JPdJ9BY zNTZ)j_5%#ry8GPp^mKoJ|Ip9{C#R!YyqH_nAs6zi+C}gP1zmO~Gq0{sI#1Zsv55GxGB4_HFAuO_=o*v$0UB~-sQv1hEn`K1 z+#~&+IzLACfZo1dAardpVQ5+J`iK@@f)np8H|jAZCdeUZOE|1#XF<`h&kGh508l>* ziUBB;1;J=RIUen}1)bNxf)W9`j3DUUaLKNO4jpzEDvXuO0}B`xr%!y#BYj4gDAh!<_$y5W=RMvTk@Ia%I*SR%H$3vcMWJluh5`2lxf zUIP=e0yd-3C5$j&LJVkxHY+B^4Uij*e$=MGzzoce==QMs`509dvP~((ifUefrV-iI zVgPlrpj`k}r${+Ni;u7hW`xfmN~m`5iy&uNM|H!t>?~*wpcEJw!q5a#02+(L#E1ah zU_sUZZ9`Ovc>?r}h5T)Rd|1$pb{1k?O4LzU|Pn*-xb7{F5%mn#xZiJTa=31$mg&Z0F(p|Nxm2K`s@vtg);WD5gHFdlY} zwvnpJZ3Q!NU>iCyV6H41O_dZ(*#jHWpvpjv9Y<4t7(oDz`Gt6q)B|)oA-c&)N?dxZ zw>06=uduufXd`1QU3nh}qD>tinejhZg`j@4@F6RD#43T5x2f*ch$bNB&FoX@`5Qo5c$kWhfv;0q)u~Bdt zx1ghg*~mYpFI$U^EkG=5v4PR0ay(KN8$Ey;Sdb4u(2;>(3xUx$414!TiL3IjJ?p>- z4=w_+Wd}@Sy?~iFjL-$79{XQh1}|Kk*CHlko503=o0gd@E8c}~V0j9?SdunD0&(F1 zT#P!|ku^S84-kAJYw;5#3b2CK5gu zA0cSD5+m%cB;h5|wmSFit?=7fQ2YR%2EQx;ok?&NG&F>Tp(!L98ok17!KKhxa<`2f z!1E>|ve4KO??V;@?t_;lOIxTW2P4hGpv0xV?=L}A2-&>H|D7ts2+_djV3=~k(Abe7 z{JH{kT4C(S5XXXK09wg{U;v5g4bc1&v%Su>TL-t&FlR;53hD~ZSr_&dez-rbXEvyO zc68XuwNfx+@s$Hc&lOl>&t|xti8r~hMY?->IEfp>4m48Ka~fF=hLBc3H{7`$&ikyd z%ryyJJj{{o!MppOwtHArhaGfv6VSg4&^N=kfWxq_T0+6GTY!s60$q5MbFoiuMu1|% zF9vMB4600?dLVbBQ4v^6xCgt+qg@MhmAQI#x5G3zh8>4vlSRxvBW7F#I7|qHm*mw- z!bfZb&MPGg2j5_AF4~2sZL?$8y{rTGG*MnvWVmuQ()&mc-Y(YmAq8~Y`y-%Uv#G+t zI_u$_hP_ODB@2m?sw(=D0|%W4Vc?+7~7}Nv z6|-5v#1GxocK^%Zj9Us^Z7ifeRFb|rE;HB$@H_nz32w-tmh*Hsf^30V{49Jd3+Vrx z6hxD4;MSS=?6K+;M3mSDCVrm9qiPUrJ!@&8Bk_j?i(-W?ywBc$IedAg=wJmu5(b&> zMEv1`_*uTDfc{&Q1t|g)f*5*Pkcod)f&_p)`b4>b9^nYex+Axc#5tLGE66J`v2YKX zU)siYuTm{F__rOd@1QhFB1OO*6G|g~az6mZhCQt$%>vE$d!<3+2~x(|?wW`@ZjQuX z!H7F}*h4yXBNQtztTtIV&G)vHGq@~_D@8cX72!mPDWJ=r|4*N3N0oKsmh~BX+em2? zLkAkFGVwV`pOd&+NwNRK;!YUvWy(U*gl*nPbgGoMS_fX7pjAnF+=m$Zxtg}|>J=oo z-D$#UXA!rOPPdYrqqJeH5yLWQ9<<#mc^L@Jj3^J` z1u~r^+sHiaX${O2Xg z52`lz+Ocq*b0T_0BIP9TwUSPEB6dn3R#Iv7e^!}vybEv5lB&>gp^INy6e+V7#h za_z{n8Y|rMDDeEN#tKm$1?O3)k_zbYf;a0FN|h0}G#QjBd?YI;BZx5_Jy|T=*+7{> z0T|;-7G~-QGUEFzCNw}Im5fVB=veqRipQD1EKUifN3vLqzb8qLX0e#}m~yKWVlknn zfPV3>BQ}&`ItKn_v7t26@jQz)kCTKih9DLvZEIy{vo;^tMtLUoubUw3^~ttSVJU~8 z6LFB`kPIc6j&E6S55P!O6pQOk1@uW*WIzx|3glRHUTmN=S0c9$_ThK{aX4vbD?=&u zKQ|$^!L@*$rBqX)^t8W9oTSp___8r{IPV*>yd;(MTU;<&2(jZ6CAylm+hL}o>t8J{ zIIe6S{a1_S^_9(WEJXbri4n5@x#Ip15Bbx}u9E!3kr>EQBM2dpDBr`{8!0#cCpr)? zSNWSPZd~bdBs&!eO2ZN*2}(jO(iw6kjp4TvmLX8(`B3@#UK zWt?Nl;`dHw|FT^iP8L=PV!hN?$(T3dvx+W;WC;5$EIMe<;%m8JE8`(+|5HbmV_Dja z4LT9WQG~-ffp^PR@s*%l6oHtIwK)=>vvlsC7!UXD$S7?#F#GwOG(8hs7#qNVGNKe_ z<&Yv^b7Az{m}NIXUZ`Xg{>NeRf}TJQN-8H zsa#faow&HH#+ZKdRiUehP%+&yV{MiKtsE#2W;rNaR+WeRvW;K?Mdyj4%jpCmwX~Il zB#v=0jw!Q(MH1esfX+Hy_miK{aWi+bVdP#2ih@h0zOZB{SNM&hgbdFx+rDl2ZQW*j zJ?I9UmQ;-}Ez!yPH2ut~YN?yY4m z#F|D7F!8!rz@8)tIx0UWje9a zD<;c3lO@(W5s}ej33VNmbmS)Nmh9li*gJ|8lx<-)kq=$n|LV)B?FK@}|4rGjQ`_Ir z?jUO_&zgF+O!`+FQ{~v@_>G+|EcuV5R~OFxMbP`zxBrF1Hn+e0Cy{?XasMUK&8&@r z?#q~M!DC@KZ`?cH&J`-ZX%>~fF; zGlPZOk!BGX?u6fWf;v2gu|s7Mu6j@z!7xs!m7v55rui^27^MVVM88jjkufM{Vi?>1 rZ7)psK_QOe0;DA=xBj0pW0B*>ch2ISyfts(ee9@~zUC8kn;ZWh)^F|4 diff --git a/example_11/doc/userdoc.rst b/example_11/doc/userdoc.rst index b22486380..3efe5621e 100644 --- a/example_11/doc/userdoc.rst +++ b/example_11/doc/userdoc.rst @@ -27,11 +27,11 @@ Tutorial steps .. 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. + This happens because ``joint_state_publisher_gui`` node needs some time to start. - .. image:: diffbot.png + .. image:: carlikebot.png :width: 400 - :alt: Differential Mobile Robot + :alt: Carlike Mobile Robot 2. To start *DiffBot* example open a terminal, source your ROS2-workspace and execute its launch file with diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index 89daa0d12..8ac1eb6c9 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -159,33 +159,24 @@ std::vector CarlikeBotSystemHardware::export // int commands_idx = 0; + int positions_idx = 0; + int velocities_idx = 0; + for (auto i = 0u; i < info_.joints.size(); i++) { bool is_steering_joint = info_.joints[i].name.find("steering") != std::string::npos; - RCLCPP_DEBUG( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has state interfaces: '%s', '%s'.", info_.joints[i].name.c_str(), - info_.joints[i].state_interfaces[0].name.c_str(), - info_.joints[i].state_interfaces[1].name.c_str()); - if (is_steering_joint) { - // hw_positions_.insert(std::make_pair(info_.joints[i].name, - // std::numeric_limits::quiet_NaN())); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[0])); - // hw_names_.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_POSITION, - // commands_idx); commands_idx++; + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[positions_idx])); + positions_idx++; } else { - // hw_velocities_.insert(std::make_pair(info_.joints[i].name, - // std::numeric_limits::quiet_NaN())); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[0])); - // hw_names_.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, - // commands_idx); commands_idx++; + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[velocities_idx])); + velocities_idx++; } } @@ -240,21 +231,13 @@ CarlikeBotSystemHardware::export_command_interfaces() rclcpp::get_logger("CarlikeBotSystemHardware"), "Exported %zu command interfaces.", command_interfaces.size()); - for (int i = 0u; i < commands_positions_idx_.size(); i++) + for (auto i = 0u; i < command_interfaces.size(); i++) { RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Exported position joint %d at command interface %d.", commands_positions_idx_[i].first, - commands_positions_idx_[i].second); + rclcpp::get_logger("CarlikeBotSystemHardware"), "Exported command interface '%s'.", + command_interfaces[i].get_name().c_str()); } - for (int i = 0u; i < commands_velocities_idx_.size(); i++) - { - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Exported velocity joint %d at command interface %d.", commands_velocities_idx_[i].first, - commands_velocities_idx_[i].second); - } return command_interfaces; } @@ -338,8 +321,9 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH for (auto i = 0u; i < hw_commands_.size(); i++) { RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'!", - hw_commands_[i], info_.joints[i].name.c_str()); + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Got command %.5f for joint '%s' with type %s!", hw_commands_[i], + info_.joints[i].name.c_str(), info_.joints[i].command_interfaces[0].name.c_str()); } for (auto i = 0u; i < hw_commands_.size(); i++) @@ -367,7 +351,8 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH RCLCPP_INFO( rclcpp::get_logger("CarlikeBotSystemHardware"), - "Successfully written to joint %d of type %s!", i, interface_type.c_str()); + "Successfully written command %.5f to joint %d of type %s!", hw_commands_[i], i, + interface_type.c_str()); } // for (auto it : hw_commands_) diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp index ed237553c..67b96b8c7 100644 --- a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -80,7 +80,7 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface std::vector hw_positions_; std::vector hw_velocities_; - std::vector> commands_counterpart_; + std::vector> commands_counterpart_; // std::map hw_commands_; // std::map hw_positions_; From ff11e76524e53113bb0cfa12554a07edb2de1386 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Fri, 30 Jun 2023 15:10:04 -0400 Subject: [PATCH 13/37] ackermann working --- .../bringup/launch/carlikebot.launch.py | 12 +- .../carlikebot_ackermann.ros2_control.xacro | 2 + ...arlikebot_ackermann_description.urdf.xacro | 16 +- example_11/hardware/carlikebot_system.cpp | 165 ++++++++++-------- 4 files changed, 113 insertions(+), 82 deletions(-) diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index 41c1cd496..d9d1156a8 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -80,9 +80,9 @@ def generate_launch_description(): executable="robot_state_publisher", output="both", parameters=[robot_description], - remappings=[ - ("/ackermann_steering_controller/reference_unstamped", "/cmd_vel"), - ], + # remappings=[ + # ("/ackermann_steering_controller/reference_unstamped", "/cmd_vel"), + # ], condition=IfCondition(sim), ) robot_state_pub_bicycle_node = Node( @@ -90,9 +90,9 @@ def generate_launch_description(): executable="robot_state_publisher", output="both", parameters=[robot_description], - remappings=[ - ("/bicycle_steering_controller/reference_unstamped", "/cmd_vel"), - ], + # remappings=[ + # ("/bicycle_steering_controller/reference_unstamped", "/cmd_vel"), + # ], condition=UnlessCondition(sim), ) rviz_node = Node( diff --git a/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro b/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro index 5ef7801a4..fcbb59014 100644 --- a/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro +++ b/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro @@ -20,10 +20,12 @@ + + diff --git a/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro b/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro index 244d44c2f..2495bf8f0 100644 --- a/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro +++ b/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro @@ -23,14 +23,14 @@ - + - + @@ -89,7 +89,7 @@ - + @@ -127,7 +127,7 @@ - + @@ -167,8 +167,8 @@ - - + + @@ -204,8 +204,8 @@ - - + + diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index 8ac1eb6c9..21ff6e6e6 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -62,38 +62,38 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( rclcpp::get_logger("CarlikeBotSystemHardware"), "CarlikeBotSystemHardware is %s.", m_running_simulation ? "running in simulation" : "running on hardware"); - // All joints should have one state interface and one command interface - for (const hardware_interface::ComponentInfo & joint : info_.joints) + for (const hardware_interfacec::ComponentInfo & joint : info_.joints) { - if (joint.command_interfaces.size() != 1) - { - RCLCPP_FATAL( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); - return hardware_interface::CallbackReturn::ERROR; - } + bool joint_is_steering = info_.joints[i].name.find("steering") != std::string::npos; - if (joint.state_interfaces.size() != 1) + // Steering joints have a position command and state interface + if (joint_is_steering) { - RCLCPP_FATAL( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), - joint.state_interfaces.size()); - return hardware_interface::CallbackReturn::ERROR; - } + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } - // Check that the joints have the correct interfaces - // The steering joints have position interfaces and the drive joints velocity - if (joint.name.find("steering") != std::string::npos) - { - // Steering joints should have a single position command and state interface if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( rclcpp::get_logger("CarlikeBotSystemHardware"), "Joint '%s' has %s command interface. '%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("CarlikeBotSystemHardware"), + "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) @@ -102,25 +102,55 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( rclcpp::get_logger("CarlikeBotSystemHardware"), "Joint '%s' has %s 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; } } else { - // Drive joints should have a single velocity command and state interface + // Drive joints have a velocity command interface and velocity and position state interface + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "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_VELOCITY) { RCLCPP_FATAL( rclcpp::get_logger("CarlikeBotSystemHardware"), "Joint '%s' has %s command interface. '%s' expected.", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 2) + { + RCLCPP_FATAL( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Joint '%s' has %zu state interface. 2 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("CarlikeBotSystemHardware"), + "Joint '%s' has %s 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; } - if (joint.state_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { RCLCPP_FATAL( rclcpp::get_logger("CarlikeBotSystemHardware"), "Joint '%s' has %s state interface. '%s' expected.", joint.name.c_str(), - joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); + joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; } } } @@ -129,26 +159,20 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( // drive wheels if (m_running_simulation) { - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + // // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code hw_start_sec_ = std::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); - // END: This part here is for exemplary purposes - Please do not copy to your production code + // // END: This part here is for exemplary purposes - Please do not copy to your production code - // Two steering joints and two drive joints - hw_positions_.resize(2, std::numeric_limits::quiet_NaN()); - hw_velocities_.resize(2, std::numeric_limits::quiet_NaN()); - // Which means 4 commands - hw_commands_.resize(4, std::numeric_limits::quiet_NaN()); } // Running on hardware: we have a single front steering and a single rear drive motor else if (!m_running_simulation) { - // A single steering joint and a single drive joint - hw_positions_.resize(1, std::numeric_limits::quiet_NaN()); - hw_velocities_.resize(1, std::numeric_limits::quiet_NaN()); - // Which means 2 commands - hw_commands_.resize(2, std::numeric_limits::quiet_NaN()); } + + hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); return hardware_interface::CallbackReturn::SUCCESS; } @@ -164,19 +188,19 @@ std::vector CarlikeBotSystemHardware::export for (auto i = 0u; i < info_.joints.size(); i++) { - bool is_steering_joint = info_.joints[i].name.find("steering") != std::string::npos; + bool joint_is_steering = info_.joints[i].name.find("steering") != std::string::npos; + + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i])); - if (is_steering_joint) + if (!joint_is_steering) { state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[positions_idx])); - positions_idx++; + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i])); } else { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[velocities_idx])); - velocities_idx++; + } } @@ -199,31 +223,19 @@ CarlikeBotSystemHardware::export_command_interfaces() { std::vector command_interfaces; - int position_commands_counter = 0; - int velocity_commands_counter = 0; - int commands_counter = 0; - - for (auto i = 0u; i < info_.joints.size(); i++) + for (uint i = 0; i < info_.joints.size(); i++) { - bool is_steering_joint = info_.joints[i].name.find("steering") != std::string::npos; + bool joint_is_steering = info_.joints[i].name.find("steering") != std::string::npos; - if (is_steering_joint) + if (joint_is_steering) { command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); - commands_counterpart_.emplace_back( - commands_counter, hardware_interface::HW_IF_POSITION, position_commands_counter); - position_commands_counter++; - commands_counter++; } else { command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); - commands_counterpart_.emplace_back( - commands_counter, hardware_interface::HW_IF_VELOCITY, velocity_commands_counter); - velocity_commands_counter++; - commands_counter++; } } @@ -255,18 +267,10 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i); } - for (auto i = 0u; i < hw_positions_.size(); i++) + for (uint i = 0; i < hw_positions_.size(); i++) { hw_positions_[i] = 0.0; - } - - for (auto i = 0u; i < hw_velocities_.size(); i++) - { hw_velocities_[i] = 0.0; - } - - for (auto i = 0u; i < hw_commands_.size(); i++) - { hw_commands_[i] = 0.0; } } @@ -300,6 +304,18 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_deactivate( hardware_interface::return_type CarlikeBotSystemHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { + for (std::size_t i = 0; i < hw_velocities_.size(); ++i) + { + + hw_positions_[i] += hw_velocities_[i] * period.seconds(); + + RCLCPP_INFO( + rclcpp::get_logger("F1TENTHSystemHardware"), + "Got position state %.5f and velocity state %.5f for '%s'", + hw_positions_[i], hw_velocities_[i], + info_.joints[i].name.c_str()); + } + if (m_running_simulation) { // Inside the write method both position and velocity states for the simulation are updated @@ -316,6 +332,19 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH { RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Writing..."); + for (auto i = 0u; i < hw_commands_.size(); ++i) + { + hw_velocities_[i] = hw_commands_[i]; + + RCLCPP_INFO( + rclcpp::get_logger("F1TENTHSystemHardware"), + "Got command %.5f for '%s'", + hw_commands_[0], info_.joints[i].name.c_str()); + } + + return hardware_interface::return_type::OK; + + if (m_running_simulation) { for (auto i = 0u; i < hw_commands_.size(); i++) From 38f7ff4937da9873dbc0c0aef725538bdf958ca0 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Fri, 30 Jun 2023 15:10:25 -0400 Subject: [PATCH 14/37] format smh --- example_11/hardware/carlikebot_system.cpp | 23 +++++++++-------------- 1 file changed, 9 insertions(+), 14 deletions(-) diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index 21ff6e6e6..9d35c426f 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -159,17 +159,17 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( // drive wheels if (m_running_simulation) { - // // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + // // BEGIN: This part here is for exemplary purposes - Please do not copy to your production + // code hw_start_sec_ = std::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); // // END: This part here is for exemplary purposes - Please do not copy to your production code - } // Running on hardware: we have a single front steering and a single rear drive motor else if (!m_running_simulation) { } - + hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); @@ -200,7 +200,6 @@ std::vector CarlikeBotSystemHardware::export } else { - } } @@ -306,14 +305,12 @@ hardware_interface::return_type CarlikeBotSystemHardware::read( { for (std::size_t i = 0; i < hw_velocities_.size(); ++i) { - hw_positions_[i] += hw_velocities_[i] * period.seconds(); - + RCLCPP_INFO( - rclcpp::get_logger("F1TENTHSystemHardware"), - "Got position state %.5f and velocity state %.5f for '%s'", - hw_positions_[i], hw_velocities_[i], - info_.joints[i].name.c_str()); + rclcpp::get_logger("F1TENTHSystemHardware"), + "Got position state %.5f and velocity state %.5f for '%s'", hw_positions_[i], + hw_velocities_[i], info_.joints[i].name.c_str()); } if (m_running_simulation) @@ -337,14 +334,12 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH hw_velocities_[i] = hw_commands_[i]; RCLCPP_INFO( - rclcpp::get_logger("F1TENTHSystemHardware"), - "Got command %.5f for '%s'", - hw_commands_[0], info_.joints[i].name.c_str()); + rclcpp::get_logger("F1TENTHSystemHardware"), "Got command %.5f for '%s'", hw_commands_[0], + info_.joints[i].name.c_str()); } return hardware_interface::return_type::OK; - if (m_running_simulation) { for (auto i = 0u; i < hw_commands_.size(); i++) From adcba1e8ec707ea49c4e283735fe4862b7180fab Mon Sep 17 00:00:00 2001 From: ARK3r Date: Thu, 21 Sep 2023 19:26:44 +0000 Subject: [PATCH 15/37] ackermann works with reference acceptance --- example_11/CMakeLists.txt | 2 +- .../config/carlikebot_controllers.yaml | 23 --- .../bringup/launch/carlikebot.launch.py | 48 ++--- .../description/launch/view_robot.launch.py | 17 +- ...ol.xacro => carlikebot.ros2_control.xacro} | 0 .../carlikebot_bicycle.ros2_control.xacro | 23 --- .../description/urdf/carlikebot.urdf.xacro | 16 +- .../carlikebot_bicycle_description.urdf.xacro | 184 ------------------ example_11/hardware/carlikebot_system.cpp | 139 +++---------- example_11/package.xml | 1 + ros2_control_demo_description/CMakeLists.txt | 5 + .../carlikebot}/rviz/carlikebot.rviz | 0 .../carlikebot}/rviz/carlikebot_view.rviz | 0 .../urdf/carlikebot.materials.xacro | 0 .../carlikebot/urdf/carlikebot.urdf.xacro | 27 +++ .../urdf/carlikebot_description.urdf.xacro | 1 + 16 files changed, 91 insertions(+), 395 deletions(-) rename example_11/description/ros2_control/{carlikebot_ackermann.ros2_control.xacro => carlikebot.ros2_control.xacro} (100%) delete mode 100644 example_11/description/ros2_control/carlikebot_bicycle.ros2_control.xacro delete mode 100644 example_11/description/urdf/carlikebot_bicycle_description.urdf.xacro rename {example_11/description => ros2_control_demo_description/carlikebot}/rviz/carlikebot.rviz (100%) rename {example_11/description => ros2_control_demo_description/carlikebot}/rviz/carlikebot_view.rviz (100%) rename {example_11/description => ros2_control_demo_description/carlikebot}/urdf/carlikebot.materials.xacro (100%) create mode 100644 ros2_control_demo_description/carlikebot/urdf/carlikebot.urdf.xacro rename example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro => ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro (99%) diff --git a/example_11/CMakeLists.txt b/example_11/CMakeLists.txt index 129560ed2..2323ecb5e 100644 --- a/example_11/CMakeLists.txt +++ b/example_11/CMakeLists.txt @@ -49,7 +49,7 @@ install( DESTINATION include/ros2_control_demo_example_11 ) install( - DIRECTORY description/launch description/ros2_control description/urdf description/rviz + DIRECTORY description/launch description/ros2_control description/urdf DESTINATION share/ros2_control_demo_example_11 ) install( diff --git a/example_11/bringup/config/carlikebot_controllers.yaml b/example_11/bringup/config/carlikebot_controllers.yaml index 6fb4b0ffe..c66521458 100644 --- a/example_11/bringup/config/carlikebot_controllers.yaml +++ b/example_11/bringup/config/carlikebot_controllers.yaml @@ -8,9 +8,6 @@ controller_manager: ackermann_steering_controller: type: ackermann_steering_controller/AckermannSteeringController - bicycle_steering_controller: - type: bicycle_steering_controller/BicycleSteeringController - ackermann_steering_controller: ros__parameters: @@ -33,23 +30,3 @@ ackermann_steering_controller: front_wheels_radius: 0.05 rear_wheels_radius: 0.05 -bicycle_steering_controller: - ros__parameters: - - reference_timeout: 2.0 - front_steering: true - open_loop: false - velocity_rolling_window_size: 10 - position_feedback: false - use_stamped_vel: false - rear_wheels_names: [rear_wheel_joint] - front_wheels_names: [steering_axis_joint] - - odom_frame_id: odom - base_frame_id: base_link - enable_odom_tf: true - - - wheelbase: 0.325 - front_wheel_radius: 0.05 - rear_wheel_radius: 0.05 diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index d9d1156a8..561527fbc 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -32,17 +32,9 @@ def generate_launch_description(): description="Start RViz2 automatically with this launch file.", ) ) - declared_arguments.append( - DeclareLaunchArgument( - "sim", - default_value="true", - description="Whether to start controllers for simulation or real hardware.", - ) - ) # Initialize Arguments gui = LaunchConfiguration("gui") - sim = LaunchConfiguration("sim") # Get URDF via xacro robot_description_content = Command( @@ -52,8 +44,6 @@ def generate_launch_description(): PathJoinSubstitution( [FindPackageShare("ros2_control_demo_example_11"), "urdf", "carlikebot.urdf.xacro"] ), - " sim:=", - sim, ] ) robot_description = {"robot_description": robot_description_content} @@ -66,7 +56,7 @@ def generate_launch_description(): ] ) rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_11"), "rviz", "carlikebot.rviz"] + [FindPackageShare("ros2_control_demo_description"), "carlikebot/rviz", "carlikebot.rviz"] ) control_node = Node( @@ -80,20 +70,9 @@ def generate_launch_description(): executable="robot_state_publisher", output="both", parameters=[robot_description], - # remappings=[ - # ("/ackermann_steering_controller/reference_unstamped", "/cmd_vel"), - # ], - condition=IfCondition(sim), - ) - robot_state_pub_bicycle_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - # remappings=[ - # ("/bicycle_steering_controller/reference_unstamped", "/cmd_vel"), - # ], - condition=UnlessCondition(sim), + remappings=[ + ("/ackermann_steering_controller/reference_unstamped", "/cmd_vel"), + ], ) rviz_node = Node( package="rviz2", @@ -114,13 +93,6 @@ def generate_launch_description(): package="controller_manager", executable="spawner", arguments=["ackermann_steering_controller", "--controller-manager", "/controller_manager"], - condition=IfCondition(sim), - ) - robot_bicycle_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["bicycle_steering_controller", "--controller-manager", "/controller_manager"], - condition=UnlessCondition(sim), ) # Delay rviz start after `joint_state_broadcaster` @@ -135,17 +107,25 @@ def generate_launch_description(): delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( event_handler=OnProcessExit( target_action=joint_state_broadcaster_spawner, - on_exit=[robot_ackermann_controller_spawner, robot_bicycle_controller_spawner], + on_exit=[robot_ackermann_controller_spawner], ) ) + + # the steering controller libraries by default publish odometry on a seperate topic than /tf + relay_topic_to_tf_node = Node( + package='topic_tools', + executable='relay', + arguments=['/ackermann_steering_controller/tf_odometry', '/tf'], + output='screen', + ) nodes = [ control_node, robot_state_pub_ackermann_node, - robot_state_pub_bicycle_node, joint_state_broadcaster_spawner, delay_rviz_after_joint_state_broadcaster_spawner, delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + relay_topic_to_tf_node, ] return LaunchDescription(declared_arguments + nodes) diff --git a/example_11/description/launch/view_robot.launch.py b/example_11/description/launch/view_robot.launch.py index 141bad973..a22325d60 100644 --- a/example_11/description/launch/view_robot.launch.py +++ b/example_11/description/launch/view_robot.launch.py @@ -26,7 +26,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "description_package", - default_value="ros2_control_demo_example_11", + default_value="ros2_control_demo_description", description="Description package with robot URDF/xacro files. Usually the argument \ is not set, it enables use of a custom description.", ) @@ -37,6 +37,14 @@ def generate_launch_description(): default_value="carlikebot.urdf.xacro", description="URDF/XACRO description file with the robot.", ) + ), + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start Rviz2 and Joint State Publisher gui automatically \ + with this launch file.", + ) ) declared_arguments.append( DeclareLaunchArgument( @@ -51,6 +59,7 @@ def generate_launch_description(): # Initialize Arguments description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") + gui = LaunchConfiguration("gui") prefix = LaunchConfiguration("prefix") # Get URDF via xacro @@ -59,19 +68,17 @@ def generate_launch_description(): PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( - [FindPackageShare(description_package), "urdf", description_file] + [FindPackageShare("ros2_control_demo_example_11"), "urdf", description_file] ), " ", "prefix:=", prefix, - " ", - "sim:=true", ] ) robot_description = {"robot_description": robot_description_content} rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", "carlikebot_view.rviz"] + [FindPackageShare(description_package), "carlikebot/rviz", "carlikebot_view.rviz"] ) joint_state_publisher_node = Node( diff --git a/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro b/example_11/description/ros2_control/carlikebot.ros2_control.xacro similarity index 100% rename from example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro rename to example_11/description/ros2_control/carlikebot.ros2_control.xacro diff --git a/example_11/description/ros2_control/carlikebot_bicycle.ros2_control.xacro b/example_11/description/ros2_control/carlikebot_bicycle.ros2_control.xacro deleted file mode 100644 index 3cc0de60b..000000000 --- a/example_11/description/ros2_control/carlikebot_bicycle.ros2_control.xacro +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - ros2_control_demo_example_11/CarlikeBotSystemHardware - 0 - - - - - - - - - - - - - - diff --git a/example_11/description/urdf/carlikebot.urdf.xacro b/example_11/description/urdf/carlikebot.urdf.xacro index 42a02b5cf..cf8a85b2e 100644 --- a/example_11/description/urdf/carlikebot.urdf.xacro +++ b/example_11/description/urdf/carlikebot.urdf.xacro @@ -2,22 +2,14 @@ - + - + - - - - - - - - - - + + diff --git a/example_11/description/urdf/carlikebot_bicycle_description.urdf.xacro b/example_11/description/urdf/carlikebot_bicycle_description.urdf.xacro deleted file mode 100644 index 29b4cbd3e..000000000 --- a/example_11/description/urdf/carlikebot_bicycle_description.urdf.xacro +++ /dev/null @@ -1,184 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index 9d35c426f..f1b760d3a 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -36,39 +36,29 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( return hardware_interface::CallbackReturn::ERROR; } - m_running_simulation = std::stod(info_.hardware_parameters["is_simulation"]); - // Check if the number of joints is correct based on the mode of operation - if (m_running_simulation && info_.joints.size() != 4) - { - RCLCPP_ERROR( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "CarlikeBotSystemHardware::on_init() - Failed to initialize, " - "because the number of joints %ld is not 4 for simulation.", - info_.joints.size()); - return hardware_interface::CallbackReturn::ERROR; - } - if (!m_running_simulation && info_.joints.size() != 2) + if (info_.joints.size() != 4) { RCLCPP_ERROR( rclcpp::get_logger("CarlikeBotSystemHardware"), "CarlikeBotSystemHardware::on_init() - Failed to initialize, " - "because the number of joints %ld is not 2 for physical.", + "because the number of joints %ld is not 4.", info_.joints.size()); return hardware_interface::CallbackReturn::ERROR; } - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "CarlikeBotSystemHardware is %s.", - m_running_simulation ? "running in simulation" : "running on hardware"); - - for (const hardware_interfacec::ComponentInfo & joint : info_.joints) + for (const hardware_interface::ComponentInfo & joint : info_.joints) { - bool joint_is_steering = info_.joints[i].name.find("steering") != std::string::npos; + bool joint_is_steering = joint.name.find("steering") != std::string::npos; // Steering joints have a position command and state interface if (joint_is_steering) { + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Joint '%s' is a steering joint.", joint.name.c_str() + ); + if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( @@ -107,6 +97,11 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( } else { + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Joint '%s' is a drive joint.", joint.name.c_str() + ); + // Drive joints have a velocity command interface and velocity and position state interface if (joint.command_interfaces.size() != 1) { @@ -155,20 +150,11 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( } } - // Running in simulation: we have individual control of two front steering wheels and two rear - // drive wheels - if (m_running_simulation) - { - // // BEGIN: This part here is for exemplary purposes - Please do not copy to your production - // code - hw_start_sec_ = std::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); - hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); - // // END: This part here is for exemplary purposes - Please do not copy to your production code - } - // Running on hardware: we have a single front steering and a single rear drive motor - else if (!m_running_simulation) - { - } + // // BEGIN: This part here is for exemplary purposes - Please do not copy to your production + // code + hw_start_sec_ = std::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); + hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); + // // END: This part here is for exemplary purposes - Please do not copy to your production code hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); @@ -181,11 +167,6 @@ std::vector CarlikeBotSystemHardware::export { std::vector state_interfaces; - // int commands_idx = 0; - - int positions_idx = 0; - int velocities_idx = 0; - for (auto i = 0u; i < info_.joints.size(); i++) { bool joint_is_steering = info_.joints[i].name.find("steering") != std::string::npos; @@ -198,9 +179,6 @@ std::vector CarlikeBotSystemHardware::export state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i])); } - else - { - } } RCLCPP_INFO( @@ -303,24 +281,21 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_deactivate( hardware_interface::return_type CarlikeBotSystemHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code for (std::size_t i = 0; i < hw_velocities_.size(); ++i) { + // Simulate DiffBot wheels's movement as a first-order system + // Update the joint status: this is a revolute joint without any limit. + // Simply integrates hw_positions_[i] += hw_velocities_[i] * period.seconds(); RCLCPP_INFO( - rclcpp::get_logger("F1TENTHSystemHardware"), + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got position state %.5f and velocity state %.5f for '%s'", hw_positions_[i], hw_velocities_[i], info_.joints[i].name.c_str()); } + // END: This part here is for exemplary purposes - Please do not copy to your production code - if (m_running_simulation) - { - // Inside the write method both position and velocity states for the simulation are updated - } - else - { - // Robot specific code to read data from the hardware - } return hardware_interface::return_type::OK; } @@ -334,73 +309,11 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH hw_velocities_[i] = hw_commands_[i]; RCLCPP_INFO( - rclcpp::get_logger("F1TENTHSystemHardware"), "Got command %.5f for '%s'", hw_commands_[0], + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'", hw_commands_[0], info_.joints[i].name.c_str()); } return hardware_interface::return_type::OK; - - if (m_running_simulation) - { - for (auto i = 0u; i < hw_commands_.size(); i++) - { - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Got command %.5f for joint '%s' with type %s!", hw_commands_[i], - info_.joints[i].name.c_str(), info_.joints[i].command_interfaces[0].name.c_str()); - } - - for (auto i = 0u; i < hw_commands_.size(); i++) - { - // Find the tuple in commands_counterpart_ that has the first value of it equal to i - auto it = std::find_if( - commands_counterpart_.begin(), commands_counterpart_.end(), - [i](const std::tuple & element) - { return std::get<0>(element) == i; }); - - // Get the interface_type from the second value of the tuple - std::string interface_type = std::get<1>(*it); - - // Get the interface specific idx from the third value of the tuple - int interface_idx = std::get<2>(*it); - - if (interface_type == "position") - { - hw_positions_[interface_idx] = hw_commands_[i]; - } - else - { - hw_velocities_[interface_idx] = hw_commands_[i]; - } - - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Successfully written command %.5f to joint %d of type %s!", hw_commands_[i], i, - interface_type.c_str()); - } - - // for (auto it : hw_commands_) - // { - // RCLCPP_INFO( - // rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'!", it.second, - // it.first.c_str()); - - // if - - // if (it.first.find("steering") != std::string::npos) - // { - // hw_positions_.at(it.first) = it.second; - // } - // else - // { - // hw_velocities_.at(it.first) = it.second; - // } - // } - } - - RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Joints successfully written!"); - - return hardware_interface::return_type::OK; } } // namespace ros2_control_demo_example_11 diff --git a/example_11/package.xml b/example_11/package.xml index 10828b63f..aa8ddb22b 100644 --- a/example_11/package.xml +++ b/example_11/package.xml @@ -19,6 +19,7 @@ pluginlib rclcpp rclcpp_lifecycle + topic_tools ackermann_steering_controller bicycle_steering_controller diff --git a/ros2_control_demo_description/CMakeLists.txt b/ros2_control_demo_description/CMakeLists.txt index b7aedafbf..ebe8c7f1e 100644 --- a/ros2_control_demo_description/CMakeLists.txt +++ b/ros2_control_demo_description/CMakeLists.txt @@ -13,6 +13,11 @@ install( DESTINATION share/${PROJECT_NAME}/diffbot ) +install( + DIRECTORY carlikebot/urdf carlikebot/rviz + DESTINATION share/${PROJECT_NAME}/carlikebot +) + install( DIRECTORY r6bot/meshes r6bot/srdf r6bot/urdf r6bot/rviz DESTINATION share/${PROJECT_NAME}/r6bot diff --git a/example_11/description/rviz/carlikebot.rviz b/ros2_control_demo_description/carlikebot/rviz/carlikebot.rviz similarity index 100% rename from example_11/description/rviz/carlikebot.rviz rename to ros2_control_demo_description/carlikebot/rviz/carlikebot.rviz diff --git a/example_11/description/rviz/carlikebot_view.rviz b/ros2_control_demo_description/carlikebot/rviz/carlikebot_view.rviz similarity index 100% rename from example_11/description/rviz/carlikebot_view.rviz rename to ros2_control_demo_description/carlikebot/rviz/carlikebot_view.rviz diff --git a/example_11/description/urdf/carlikebot.materials.xacro b/ros2_control_demo_description/carlikebot/urdf/carlikebot.materials.xacro similarity index 100% rename from example_11/description/urdf/carlikebot.materials.xacro rename to ros2_control_demo_description/carlikebot/urdf/carlikebot.materials.xacro diff --git a/ros2_control_demo_description/carlikebot/urdf/carlikebot.urdf.xacro b/ros2_control_demo_description/carlikebot/urdf/carlikebot.urdf.xacro new file mode 100644 index 000000000..42a02b5cf --- /dev/null +++ b/ros2_control_demo_description/carlikebot/urdf/carlikebot.urdf.xacro @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro b/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro similarity index 99% rename from example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro rename to ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro index 2495bf8f0..792cb1872 100644 --- a/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro +++ b/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro @@ -13,6 +13,7 @@ + From de2d3c949c294f756c01635235569e2cbaa67d14 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Thu, 21 Sep 2023 19:32:18 +0000 Subject: [PATCH 16/37] bug fixes --- example_11/hardware/carlikebot_system.cpp | 46 ++++++++----------- .../carlikebot_system.hpp | 2 - 2 files changed, 20 insertions(+), 28 deletions(-) diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index f1b760d3a..8fb7ce283 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -233,23 +233,20 @@ CarlikeBotSystemHardware::export_command_interfaces() hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - if (m_running_simulation) - { - RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Activating ...please wait..."); + RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Activating ...please wait..."); - for (auto i = 0; i < hw_start_sec_; i++) - { - rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i); - } + for (auto i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i); + } - for (uint i = 0; i < hw_positions_.size(); i++) - { - hw_positions_[i] = 0.0; - hw_velocities_[i] = 0.0; - hw_commands_[i] = 0.0; - } + for (uint i = 0; i < hw_positions_.size(); i++) + { + hw_positions_[i] = 0.0; + hw_velocities_[i] = 0.0; + hw_commands_[i] = 0.0; } RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully activated!"); @@ -260,19 +257,16 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - if (m_running_simulation) - { - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Deactivating ...please wait..."); + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Deactivating ...please wait..."); - for (auto i = 0; i < hw_stop_sec_; i++) - { - rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_stop_sec_ - i); - } - // END: This part here is for exemplary purposes - Please do not copy to your production code + for (auto i = 0; i < hw_stop_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_stop_sec_ - i); } + // END: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully deactivated!"); return hardware_interface::CallbackReturn::SUCCESS; diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp index 67b96b8c7..7db84b4f5 100644 --- a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -68,8 +68,6 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; private: - // Parameter disnguishing between simulation and physical robot - bool m_running_simulation; // Parameters for the CarlikeBot simulation double hw_start_sec_; From 5b8f57308c5be840962e37f426a017c475310657 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Thu, 21 Sep 2023 19:35:36 +0000 Subject: [PATCH 17/37] formatting --- .../bringup/config/carlikebot_controllers.yaml | 1 - example_11/bringup/launch/carlikebot.launch.py | 12 ++++++------ example_11/hardware/carlikebot_system.cpp | 10 ++++------ .../carlikebot_system.hpp | 1 - 4 files changed, 10 insertions(+), 14 deletions(-) diff --git a/example_11/bringup/config/carlikebot_controllers.yaml b/example_11/bringup/config/carlikebot_controllers.yaml index c66521458..d0bca8c56 100644 --- a/example_11/bringup/config/carlikebot_controllers.yaml +++ b/example_11/bringup/config/carlikebot_controllers.yaml @@ -29,4 +29,3 @@ ackermann_steering_controller: rear_wheel_track: 0.2 front_wheels_radius: 0.05 rear_wheels_radius: 0.05 - diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index 561527fbc..7d7d899d6 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -110,13 +110,13 @@ def generate_launch_description(): on_exit=[robot_ackermann_controller_spawner], ) ) - - # the steering controller libraries by default publish odometry on a seperate topic than /tf + + # the steering controller libraries by default publish odometry on a separate topic than /tf relay_topic_to_tf_node = Node( - package='topic_tools', - executable='relay', - arguments=['/ackermann_steering_controller/tf_odometry', '/tf'], - output='screen', + package="topic_tools", + executable="relay", + arguments=["/ackermann_steering_controller/tf_odometry", "/tf"], + output="screen", ) nodes = [ diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index 8fb7ce283..f728ec181 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -55,9 +55,8 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( if (joint_is_steering) { RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' is a steering joint.", joint.name.c_str() - ); + rclcpp::get_logger("CarlikeBotSystemHardware"), "Joint '%s' is a steering joint.", + joint.name.c_str()); if (joint.command_interfaces.size() != 1) { @@ -98,9 +97,8 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( else { RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' is a drive joint.", joint.name.c_str() - ); + rclcpp::get_logger("CarlikeBotSystemHardware"), "Joint '%s' is a drive joint.", + joint.name.c_str()); // Drive joints have a velocity command interface and velocity and position state interface if (joint.command_interfaces.size() != 1) diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp index 7db84b4f5..55739cf6c 100644 --- a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -68,7 +68,6 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; private: - // Parameters for the CarlikeBot simulation double hw_start_sec_; double hw_stop_sec_; From cdaafef276a23e7b5d059ecca3801f8ec85906b6 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Thu, 21 Sep 2023 20:31:01 +0000 Subject: [PATCH 18/37] docs and clean up --- .github/workflows/ci-coverage-build.yml | 1 + .github/workflows/ci-ros-lint.yml | 1 + doc/index.rst | 4 + .../bringup/launch/carlikebot.launch.py | 2 +- example_11/doc/userdoc.rst | 77 +++++++++++-------- example_11/hardware/carlikebot_system.cpp | 2 +- ros2_control_demos/package.xml | 1 + 7 files changed, 52 insertions(+), 36 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index b1a543f0c..4b64730e2 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -34,6 +34,7 @@ jobs: ros2_control_demo_example_7 ros2_control_demo_example_8 ros2_control_demo_example_9 + ros2_control_demo_example_11 ros2_control_demo_example_12 vcs-repo-file-url: | diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 9c6607bb7..2eb79f36f 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -29,6 +29,7 @@ jobs: ros2_control_demo_example_7 ros2_control_demo_example_8 ros2_control_demo_example_9 + ros2_control_demo_example_11 ros2_control_demo_example_12 ament_lint_100: diff --git a/doc/index.rst b/doc/index.rst index c3af543bf..20eafd07a 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -67,6 +67,9 @@ Example 8: "Using transmissions" Example 9: "Gazebo Classic" Demonstrates how to switch between simulation and hardware. +Example 11: "CarlikeBot" + *CarlikeBot* with an Ackermann steering controller would be similiar to how a car would be controlled. + Example 12: "Controller chaining" The example shows a simple chainable controller and its integration to form a controller chain to control the joints of *RRBot*. @@ -259,4 +262,5 @@ Examples Example 7: Full tutorial with a 6DOF robot <../example_7/doc/userdoc.rst> Example 8: Using transmissions <../example_8/doc/userdoc.rst> Example 9: Gazebo classic <../example_9/doc/userdoc.rst> + Example 11: CarlikeBot <../example_11/doc/userdoc.rst> Example 12: Controller chaining <../example_12/doc/userdoc.rst> diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index 7d7d899d6..10cc6d5d6 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -14,7 +14,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, RegisterEventHandler -from launch.conditions import IfCondition, UnlessCondition +from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration diff --git a/example_11/doc/userdoc.rst b/example_11/doc/userdoc.rst index 3efe5621e..39ad3dbd8 100644 --- a/example_11/doc/userdoc.rst +++ b/example_11/doc/userdoc.rst @@ -1,29 +1,30 @@ :github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_11/doc/userdoc.rst -.. _ros2_control_demos_example_2_userdoc: +.. _ros2_control_demos_example_11_userdoc: ********* -DiffBot +CarlikeBot ********* -*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. +*CarlikeBot* is a simple mobile base with ackermann drive. -For *example_2*, the hardware interface plugin is implemented having only one interface. +This example shows how to use the ackermann steering controller, which is a sub-design of the steering controller library. * 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. +The *CarlikeBot* URDF files can be found in ``description/urdf`` folder. + +.. include:: ../../doc/run_from_docker.rst Tutorial steps -------------------------- -1. To check that *DiffBot* description is working properly use following launch commands +1. To check that *CarlikeBot* description is working properly use following launch commands .. code-block:: shell - ros2 launch ros2_control_demo_example_2 view_robot.launch.py + ros2 launch ros2_control_demo_example_11 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``. @@ -31,20 +32,20 @@ Tutorial steps .. image:: carlikebot.png :width: 400 - :alt: Carlike Mobile Robot + :alt: Ackermann Mobile Robot -2. To start *DiffBot* example open a terminal, source your ROS2-workspace and execute its launch file with +2. To start *CarlikeBot* example open a terminal, source your ROS2-workspace and execute its launch file with .. code-block:: shell - ros2 launch ros2_control_demo_example_2 diffbot.launch.py + ros2 launch ros2_control_demo_example_11 carlikebot.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*. + Still, to be sure, let's introspect the control system before moving *CarlikeBot*. 3. Check if the hardware interface loaded properly, by opening another terminal and executing @@ -57,15 +58,21 @@ Tutorial steps .. code-block:: shell command interfaces - left_wheel_joint/velocity [available] [claimed] - right_wheel_joint/velocity [available] [claimed] + ackermann_steering_controller/angular/position [unavailable] [unclaimed] + ackermann_steering_controller/linear/velocity [unavailable] [unclaimed] + left_steering_joint/position [available] [claimed] + rear_left_wheel_joint/velocity [available] [claimed] + rear_right_wheel_joint/velocity [available] [claimed] + right_steering_joint/position [available] [claimed] state interfaces - left_wheel_joint/position - left_wheel_joint/velocity - right_wheel_joint/position - right_wheel_joint/velocity + left_steering_joint/position + rear_left_wheel_joint/position + rear_left_wheel_joint/velocity + rear_right_wheel_joint/position + rear_right_wheel_joint/velocity + right_steering_joint/position - The ``[claimed]`` marker on command interfaces means that a controller has access to command *DiffBot*. + The ``[claimed]`` marker on command interfaces means that a controller has access to command *CarlikeBot*. 4. Check if controllers are running @@ -77,47 +84,49 @@ Tutorial steps .. code-block:: shell - diffbot_base_controller[diff_drive_controller/DiffDriveController] active - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + ackermann_steering_controller[ackermann_steering_controller/AckermannSteeringController] 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: + ros2 topic pub --rate 30 /ackermann_steering_controller/reference_unstamped geometry_msgs/msg/Twist "linear: x: 0.7 y: 0.0 z: 0.0 - angular: + angular: x: 0.0 y: 0.0 - z: 1.0" + z: 0.01" 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 - [DiffBotSystemHardware]: Got command 43.33333 for 'left_wheel_joint'! - [DiffBotSystemHardware]: Got command 50.00000 for 'right_wheel_joint'! + [CarlikeBotSystemHardware]: Got command 0.00464 for 'left_steering_joint' + [CarlikeBotSystemHardware]: Got command 0.00465 for 'right_steering_joint' + [CarlikeBotSystemHardware]: Got command 13.97230 for 'rear_left_wheel_joint' + [CarlikeBotSystemHardware]: Got command 14.02830 for 'rear_right_wheel_joint' Files used for this demos -------------------------- -* Launch file: `diffbot.launch.py `__ -* Controllers yaml: `diffbot_controllers.yaml `__ -* URDF file: `diffbot.urdf.xacro `__ +* Launch file: `carlikebot.launch.py `__ +* Controllers yaml: `carlikebot_controllers.yaml `__ +* URDF file: `carlikebot.urdf.xacro `__ - * Description: `diffbot_description.urdf.xacro `__ - * ``ros2_control`` tag: `diffbot.ros2_control.xacro `__ + * Description: `carlikebot_description.urdf.xacro `__ + * ``ros2_control`` tag: `carlikebot.ros2_control.xacro `__ -* RViz configuration: `diffbot.rviz `__ +* RViz configuration: `carlikebot.rviz `__ -* Hardware interface plugin: `diffbot_system.cpp `__ +* Hardware interface plugin: `carlikebot_system.cpp `__ Controllers from this demo -------------------------- * ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ -* ``Diff Drive Controller`` (`ros2_controllers repository `__): `doc `__ +* ``Ackermann Steering Controller`` (`ros2_controllers repository `__): `doc `__ diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index f728ec181..7303e1390 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -301,7 +301,7 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH hw_velocities_[i] = hw_commands_[i]; RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'", hw_commands_[0], + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'", hw_commands_[i], info_.joints[i].name.c_str()); } diff --git a/ros2_control_demos/package.xml b/ros2_control_demos/package.xml index 603939229..0ecf1bfdc 100644 --- a/ros2_control_demos/package.xml +++ b/ros2_control_demos/package.xml @@ -23,6 +23,7 @@ ros2_control_demo_example_8 ros2_control_demo_example_9 ros2_control_demo_example_11 + ros2_control_demo_example_12 ament_cmake From 6acc3c33345ec615495ff8ef3c3125d30d876b9b Mon Sep 17 00:00:00 2001 From: ARK3r Date: Thu, 21 Sep 2023 20:39:41 +0000 Subject: [PATCH 19/37] docs formatting --- example_11/doc/userdoc.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/example_11/doc/userdoc.rst b/example_11/doc/userdoc.rst index 39ad3dbd8..f033fd9b4 100644 --- a/example_11/doc/userdoc.rst +++ b/example_11/doc/userdoc.rst @@ -2,9 +2,9 @@ .. _ros2_control_demos_example_11_userdoc: -********* +************ CarlikeBot -********* +************ *CarlikeBot* is a simple mobile base with ackermann drive. From 4db7277df54686e7c59ba5dc289eff7384cedb44 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Thu, 21 Sep 2023 20:42:56 +0000 Subject: [PATCH 20/37] docs typo; view_robot adhere to example_2 --- doc/index.rst | 2 +- example_11/description/launch/view_robot.launch.py | 3 +++ example_11/doc/userdoc.rst | 2 +- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index 20eafd07a..9c0c8d9a8 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -68,7 +68,7 @@ Example 9: "Gazebo Classic" Demonstrates how to switch between simulation and hardware. Example 11: "CarlikeBot" - *CarlikeBot* with an Ackermann steering controller would be similiar to how a car would be controlled. + *CarlikeBot* with an Ackermann steering controller would be similar to how a car would be controlled. Example 12: "Controller chaining" The example shows a simple chainable controller and its integration to form a controller chain to control the joints of *RRBot*. diff --git a/example_11/description/launch/view_robot.launch.py b/example_11/description/launch/view_robot.launch.py index a22325d60..50b1e9840 100644 --- a/example_11/description/launch/view_robot.launch.py +++ b/example_11/description/launch/view_robot.launch.py @@ -14,6 +14,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node @@ -84,6 +85,7 @@ def generate_launch_description(): joint_state_publisher_node = Node( package="joint_state_publisher_gui", executable="joint_state_publisher_gui", + condition=IfCondition(gui), ) robot_state_publisher_node = Node( package="robot_state_publisher", @@ -97,6 +99,7 @@ def generate_launch_description(): name="rviz2", output="log", arguments=["-d", rviz_config_file], + condition=IfCondition(gui), ) nodes = [ diff --git a/example_11/doc/userdoc.rst b/example_11/doc/userdoc.rst index f033fd9b4..550899021 100644 --- a/example_11/doc/userdoc.rst +++ b/example_11/doc/userdoc.rst @@ -84,7 +84,7 @@ Tutorial steps .. code-block:: shell - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active ackermann_steering_controller[ackermann_steering_controller/AckermannSteeringController] active 5. If everything is fine, now you can send a command to *Diff Drive Controller* using ROS 2 CLI interface: From 1e6b9f2f66da3750075f592d177218140b7b420d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 29 Sep 2023 07:36:27 +0000 Subject: [PATCH 21/37] Cleanup example launch file --- example_11/bringup/launch/carlikebot.launch.py | 11 ++--------- example_11/package.xml | 1 - 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index 10cc6d5d6..40b3285a5 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -59,11 +59,13 @@ def generate_launch_description(): [FindPackageShare("ros2_control_demo_description"), "carlikebot/rviz", "carlikebot.rviz"] ) + # the steering controller libraries by default publish odometry on a separate topic than /tf control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[robot_description, robot_controllers], output="both", + remappings=[("/ackermann_steering_controller/tf_odometry", "/tf")], ) robot_state_pub_ackermann_node = Node( package="robot_state_publisher", @@ -111,21 +113,12 @@ def generate_launch_description(): ) ) - # the steering controller libraries by default publish odometry on a separate topic than /tf - relay_topic_to_tf_node = Node( - package="topic_tools", - executable="relay", - arguments=["/ackermann_steering_controller/tf_odometry", "/tf"], - output="screen", - ) - nodes = [ control_node, robot_state_pub_ackermann_node, joint_state_broadcaster_spawner, delay_rviz_after_joint_state_broadcaster_spawner, delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - relay_topic_to_tf_node, ] return LaunchDescription(declared_arguments + nodes) diff --git a/example_11/package.xml b/example_11/package.xml index aa8ddb22b..f58d98c7e 100644 --- a/example_11/package.xml +++ b/example_11/package.xml @@ -22,7 +22,6 @@ topic_tools ackermann_steering_controller - bicycle_steering_controller controller_manager joint_state_broadcaster joint_state_publisher_gui From 4bc291e7b3952bc13b928ace77a88960be24b253 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 29 Sep 2023 07:36:48 +0000 Subject: [PATCH 22/37] Fix indent of command in doc --- example_11/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/example_11/doc/userdoc.rst b/example_11/doc/userdoc.rst index 550899021..df0ddbb60 100644 --- a/example_11/doc/userdoc.rst +++ b/example_11/doc/userdoc.rst @@ -95,7 +95,7 @@ Tutorial steps x: 0.7 y: 0.0 z: 0.0 - angular: + angular: x: 0.0 y: 0.0 z: 0.01" From d70e650c110410deaae102fd0a3c8ccc7143d907 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 29 Sep 2023 07:40:27 +0000 Subject: [PATCH 23/37] Trim whitespace --- doc/index.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/index.rst b/doc/index.rst index 6cd03cede..facbb538d 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -69,7 +69,7 @@ Example 9: "Gazebo Classic" Example 10: "GPIO interfaces" Industrial robot with GPIO interfaces - + Example 11: "CarlikeBot" *CarlikeBot* with an Ackermann steering controller From e39514001b49ca9cd49d1cc89e253fa8c1cd4bae Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 29 Sep 2023 07:50:41 +0000 Subject: [PATCH 24/37] Remove obsolete xacro file --- .../carlikebot/urdf/carlikebot.urdf.xacro | 27 ------------------- 1 file changed, 27 deletions(-) delete mode 100644 ros2_control_demo_description/carlikebot/urdf/carlikebot.urdf.xacro diff --git a/ros2_control_demo_description/carlikebot/urdf/carlikebot.urdf.xacro b/ros2_control_demo_description/carlikebot/urdf/carlikebot.urdf.xacro deleted file mode 100644 index 42a02b5cf..000000000 --- a/ros2_control_demo_description/carlikebot/urdf/carlikebot.urdf.xacro +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - From 965afaa4b249a08a62e6fefb84b34b1a37c699ad Mon Sep 17 00:00:00 2001 From: ARK3r Date: Tue, 3 Oct 2023 16:51:37 -0400 Subject: [PATCH 25/37] steering pos; traction vel --- .../config/carlikebot_controllers.yaml | 1 + .../carlikebot.ros2_control.xacro | 2 - example_11/hardware/carlikebot_system.cpp | 117 ++++++++++++------ .../carlikebot_system.hpp | 19 ++- 4 files changed, 97 insertions(+), 42 deletions(-) diff --git a/example_11/bringup/config/carlikebot_controllers.yaml b/example_11/bringup/config/carlikebot_controllers.yaml index d0bca8c56..bd2c9d9e0 100644 --- a/example_11/bringup/config/carlikebot_controllers.yaml +++ b/example_11/bringup/config/carlikebot_controllers.yaml @@ -23,6 +23,7 @@ ackermann_steering_controller: odom_frame_id: odom base_frame_id: base_link enable_odom_tf: true + in_chained_mode: false wheelbase: 0.325 front_wheel_track: 0.2 diff --git a/example_11/description/ros2_control/carlikebot.ros2_control.xacro b/example_11/description/ros2_control/carlikebot.ros2_control.xacro index fcbb59014..5ef7801a4 100644 --- a/example_11/description/ros2_control/carlikebot.ros2_control.xacro +++ b/example_11/description/ros2_control/carlikebot.ros2_control.xacro @@ -20,12 +20,10 @@ - - diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index 7303e1390..3c3e6aab4 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -119,7 +119,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( return hardware_interface::CallbackReturn::ERROR; } - if (joint.state_interfaces.size() != 2) + if (joint.state_interfaces.size() != 1) { RCLCPP_FATAL( rclcpp::get_logger("CarlikeBotSystemHardware"), @@ -128,16 +128,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( return hardware_interface::CallbackReturn::ERROR; } - if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) - { - RCLCPP_FATAL( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %s 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; - } - - if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) { RCLCPP_FATAL( rclcpp::get_logger("CarlikeBotSystemHardware"), @@ -154,9 +145,26 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); // // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + // joint name, state, command + hw_interfaces_.resize(4, std::make_tuple( + std::string(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN() + ) + ); + + + + // hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + // hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + // hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + // hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + // hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + // hw_position_states_.resize(2, std::numeric_limits::queit_NaN()); + // hw_velocity_states_.resize(2, std::numeric_limits::queit_NaN()); + return hardware_interface::CallbackReturn::SUCCESS; } @@ -169,13 +177,28 @@ std::vector CarlikeBotSystemHardware::export { bool joint_is_steering = info_.joints[i].name.find("steering") != std::string::npos; - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i])); + // if joint name not in hw_interfaces_, add it + if (std::get<0>(hw_interfaces_[i]).empty()) + { + std::get<0>(hw_interfaces_[i]) = info_.joints[i].name; + } + + // get the index of the joint name in hw_interfaces_ + auto it = std::find_if( + hw_interfaces_.begin(), hw_interfaces_.end(), + [this, i](const std::tuple & element) { + return std::get<0>(element) == info_.joints[i].name; + }); - if (!joint_is_steering) + if (joint_is_steering) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &std::get<1>(*it))); + } + else { state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i])); + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &std::get<1>(*it))); } } @@ -202,15 +225,28 @@ CarlikeBotSystemHardware::export_command_interfaces() { bool joint_is_steering = info_.joints[i].name.find("steering") != std::string::npos; + // if joint name not in hw_interfaces_, add it + if (std::get<0>(hw_interfaces_[i]).empty()) + { + std::get<0>(hw_interfaces_[i]) = info_.joints[i].name; + } + + // get the index of the joint name in hw_interfaces_ + auto it = std::find_if( + hw_interfaces_.begin(), hw_interfaces_.end(), + [this, i](const std::tuple & element) { + return std::get<0>(element) == info_.joints[i].name; + }); + if (joint_is_steering) { command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &std::get<2>(*it))); } else { command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &std::get<2>(*it))); } } @@ -240,11 +276,10 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i); } - for (uint i = 0; i < hw_positions_.size(); i++) + for (uint i = 0; i < hw_interfaces_.size(); i++) { - hw_positions_[i] = 0.0; - hw_velocities_[i] = 0.0; - hw_commands_[i] = 0.0; + std::get<1>(hw_interfaces_[i]) = 0.0; + std::get<2>(hw_interfaces_[i]) = 0.0; } RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully activated!"); @@ -274,17 +309,26 @@ hardware_interface::return_type CarlikeBotSystemHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - for (std::size_t i = 0; i < hw_velocities_.size(); ++i) + for (std::size_t i = 0; i < hw_interfaces_.size(); ++i) { - // Simulate DiffBot wheels's movement as a first-order system + // Simulate CarlikeBot wheels' movement as a first-order system // Update the joint status: this is a revolute joint without any limit. // Simply integrates - hw_positions_[i] += hw_velocities_[i] * period.seconds(); + bool is_steering_joint = std::get<0>(hw_interfaces_[i]).find("steering") != std::string::npos; - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Got position state %.5f and velocity state %.5f for '%s'", hw_positions_[i], - hw_velocities_[i], info_.joints[i].name.c_str()); + if (is_steering_joint) + { + std::get<1>(hw_interfaces_[i]) = std::get<2>(hw_interfaces_[i]); + } + else + { + std::get<1>(hw_interfaces_[i]) += period.seconds() * std::get<2>(hw_interfaces_[i]); + } + + // RCLCPP_INFO( + // rclcpp::get_logger("CarlikeBotSystemHardware"), + // "Got position state %.5f and velocity state %.5f for '%s'", hw_positions_[i], + // hw_velocities_[i], info_.joints[i].name.c_str()); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -296,13 +340,14 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH { RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Writing..."); - for (auto i = 0u; i < hw_commands_.size(); ++i) + for (auto i = 0u; i < hw_interfaces_.size(); ++i) { - hw_velocities_[i] = hw_commands_[i]; + // hw_velocities_[i] = hw_commands_[i]; + std::get<2>(hw_interfaces_[i]) = std::get<2>(hw_interfaces_[i]); - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'", hw_commands_[i], - info_.joints[i].name.c_str()); + // RCLCPP_INFO( + // rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'", hw_commands_[i], + // info_.joints[i].name.c_str()); } return hardware_interface::return_type::OK; diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp index 55739cf6c..76ad73afc 100644 --- a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -73,11 +73,22 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface double hw_stop_sec_; // Store the command for the CarlikeBot robot - std::vector hw_commands_; - std::vector hw_positions_; - std::vector hw_velocities_; + // std::vector hw_position_commands_; + // std::vector hw_velocity_commands_; + // std::vector hw_position_states_; + // std::vector hw_velocity_states_; - std::vector> commands_counterpart_; + // std::vector hw_interfaces_; + // std::vector hw_commands_; + // std::vector hw_states_; + + std::vector> hw_interfaces_; // name of joint, state, command + + // std::vector> commands_counterpart_; + + // std::vector hw_commands_; + // std::vector hw_positions_; + // std::vector hw_velocities_; // std::map hw_commands_; // std::map hw_positions_; From 9d37c177b67a1402d3a183a198b9187cd46b7011 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Tue, 3 Oct 2023 16:55:52 -0400 Subject: [PATCH 26/37] formatting --- example_11/hardware/carlikebot_system.cpp | 27 +++++++------------ .../carlikebot_system.hpp | 3 ++- 2 files changed, 12 insertions(+), 18 deletions(-) diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index 3c3e6aab4..f9e9343ef 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -146,14 +146,10 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( // // END: This part here is for exemplary purposes - Please do not copy to your production code // joint name, state, command - hw_interfaces_.resize(4, std::make_tuple( - std::string(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN() - ) - ); - - + hw_interfaces_.resize( + 4, std::make_tuple( + std::string(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN())); // hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); // hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); @@ -165,7 +161,6 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( // hw_position_states_.resize(2, std::numeric_limits::queit_NaN()); // hw_velocity_states_.resize(2, std::numeric_limits::queit_NaN()); - return hardware_interface::CallbackReturn::SUCCESS; } @@ -186,9 +181,8 @@ std::vector CarlikeBotSystemHardware::export // get the index of the joint name in hw_interfaces_ auto it = std::find_if( hw_interfaces_.begin(), hw_interfaces_.end(), - [this, i](const std::tuple & element) { - return std::get<0>(element) == info_.joints[i].name; - }); + [this, i](const std::tuple & element) + { return std::get<0>(element) == info_.joints[i].name; }); if (joint_is_steering) { @@ -234,9 +228,8 @@ CarlikeBotSystemHardware::export_command_interfaces() // get the index of the joint name in hw_interfaces_ auto it = std::find_if( hw_interfaces_.begin(), hw_interfaces_.end(), - [this, i](const std::tuple & element) { - return std::get<0>(element) == info_.joints[i].name; - }); + [this, i](const std::tuple & element) + { return std::get<0>(element) == info_.joints[i].name; }); if (joint_is_steering) { @@ -346,8 +339,8 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH std::get<2>(hw_interfaces_[i]) = std::get<2>(hw_interfaces_[i]); // RCLCPP_INFO( - // rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'", hw_commands_[i], - // info_.joints[i].name.c_str()); + // rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'", + // hw_commands_[i], info_.joints[i].name.c_str()); } return hardware_interface::return_type::OK; diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp index 76ad73afc..4ee874307 100644 --- a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -82,7 +82,8 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface // std::vector hw_commands_; // std::vector hw_states_; - std::vector> hw_interfaces_; // name of joint, state, command + std::vector> + hw_interfaces_; // name of joint, state, command // std::vector> commands_counterpart_; From a893029a75c6276848d64b1f8a63d6ba980ab0c0 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Thu, 5 Oct 2023 22:51:38 +0000 Subject: [PATCH 27/37] added front drive wheels; carlikebot.launch broken --- .../carlikebot.ros2_control.xacro | 12 ++++- example_11/hardware/carlikebot_system.cpp | 16 ++---- .../carlikebot_system.hpp | 19 ------- .../urdf/carlikebot_description.urdf.xacro | 51 ++++++++++++++----- 4 files changed, 51 insertions(+), 47 deletions(-) diff --git a/example_11/description/ros2_control/carlikebot.ros2_control.xacro b/example_11/description/ros2_control/carlikebot.ros2_control.xacro index 5ef7801a4..4106601a6 100644 --- a/example_11/description/ros2_control/carlikebot.ros2_control.xacro +++ b/example_11/description/ros2_control/carlikebot.ros2_control.xacro @@ -10,14 +10,22 @@ 3.0 1 - + - + + + + + + + + + diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index f9e9343ef..5f16ebd3c 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -37,12 +37,12 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( } // Check if the number of joints is correct based on the mode of operation - if (info_.joints.size() != 4) + if (info_.joints.size() != 6) { RCLCPP_ERROR( rclcpp::get_logger("CarlikeBotSystemHardware"), "CarlikeBotSystemHardware::on_init() - Failed to initialize, " - "because the number of joints %ld is not 4.", + "because the number of joints %ld is not 6.", info_.joints.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -147,20 +147,10 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( // joint name, state, command hw_interfaces_.resize( - 4, std::make_tuple( + 6, std::make_tuple( std::string(), std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN())); - // hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - // hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - // hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - - // hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - // hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - - // hw_position_states_.resize(2, std::numeric_limits::queit_NaN()); - // hw_velocity_states_.resize(2, std::numeric_limits::queit_NaN()); - return hardware_interface::CallbackReturn::SUCCESS; } diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp index 4ee874307..f1556a8cc 100644 --- a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -72,28 +72,9 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface double hw_start_sec_; double hw_stop_sec_; - // Store the command for the CarlikeBot robot - // std::vector hw_position_commands_; - // std::vector hw_velocity_commands_; - // std::vector hw_position_states_; - // std::vector hw_velocity_states_; - - // std::vector hw_interfaces_; - // std::vector hw_commands_; - // std::vector hw_states_; - std::vector> hw_interfaces_; // name of joint, state, command - // std::vector> commands_counterpart_; - - // std::vector hw_commands_; - // std::vector hw_positions_; - // std::vector hw_velocities_; - - // std::map hw_commands_; - // std::map hw_positions_; - // std::map hw_velocities_; }; } // namespace ros2_control_demo_example_11 diff --git a/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro b/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro index 792cb1872..4fba6dda6 100644 --- a/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro +++ b/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro @@ -60,7 +60,6 @@ - @@ -98,7 +97,6 @@ - @@ -136,9 +134,22 @@ + + + + + + + + + + + + + - + @@ -165,17 +176,31 @@ - + + + + + + + + + + + + + + - - + + + - + @@ -202,12 +227,12 @@ - - - - - - + + + + + + From 642989b8dbabb29d238202a0a0907a64ea9c14b9 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Fri, 26 Jan 2024 18:13:42 +0000 Subject: [PATCH 28/37] base urdf for bicycle --- example_11/package.xml | 1 + .../urdf/carlikebot.materials.xacro | 4 + .../urdf/carlikebot_description.urdf.xacro | 84 ++++++++++++------- 3 files changed, 58 insertions(+), 31 deletions(-) diff --git a/example_11/package.xml b/example_11/package.xml index f58d98c7e..634813127 100644 --- a/example_11/package.xml +++ b/example_11/package.xml @@ -27,6 +27,7 @@ joint_state_publisher_gui robot_state_publisher ros2_controllers_test_nodes + ros2_control_demo_description ros2controlcli ros2launch rviz2 diff --git a/ros2_control_demo_description/carlikebot/urdf/carlikebot.materials.xacro b/ros2_control_demo_description/carlikebot/urdf/carlikebot.materials.xacro index 035bf5822..072d83a92 100644 --- a/ros2_control_demo_description/carlikebot/urdf/carlikebot.materials.xacro +++ b/ros2_control_demo_description/carlikebot/urdf/carlikebot.materials.xacro @@ -37,4 +37,8 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc + + + + diff --git a/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro b/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro index 4fba6dda6..6af69eff0 100644 --- a/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro +++ b/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro @@ -57,6 +57,27 @@ + + + + + + + + + + + + + + + + + + + + + @@ -93,6 +114,7 @@ + @@ -130,24 +152,34 @@ + - + - + + + + + + + + + - - - - + + + + - + + @@ -176,29 +208,18 @@ - - - - - - - - - - - - - - + - - + + + - + + @@ -220,20 +241,21 @@ - - - - - - + + + + + + + From 7c2d62735dc7a66b86b97407e84485658e134fec Mon Sep 17 00:00:00 2001 From: ARK3r Date: Fri, 26 Jan 2024 21:44:03 +0000 Subject: [PATCH 29/37] configured base --- .../config/carlikebot_controllers.yaml | 31 ++-- .../bringup/launch/carlikebot.launch.py | 17 +- .../carlikebot.ros2_control.xacro | 21 +-- example_11/hardware/carlikebot_system.cpp | 165 +++++++++--------- .../carlikebot_system.hpp | 30 +++- example_11/package.xml | 2 +- .../urdf/carlikebot_description.urdf.xacro | 42 ++--- 7 files changed, 164 insertions(+), 144 deletions(-) diff --git a/example_11/bringup/config/carlikebot_controllers.yaml b/example_11/bringup/config/carlikebot_controllers.yaml index bd2c9d9e0..cc9bc55ba 100644 --- a/example_11/bringup/config/carlikebot_controllers.yaml +++ b/example_11/bringup/config/carlikebot_controllers.yaml @@ -5,28 +5,25 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - ackermann_steering_controller: - type: ackermann_steering_controller/AckermannSteeringController + bicycle_steering_controller: + type: bicycle_steering_controller/BicycleSteeringController -ackermann_steering_controller: +bicycle_steering_controller: ros__parameters: - - reference_timeout: 2.0 + wheelbase: 0.325 + front_wheel_radius: 0.05 + rear_wheel_radius: 0.05 front_steering: true + reference_timeout: 2.0 + rear_wheels_names: ['virtual_rear_wheel_joint'] + front_wheels_names: ['virtual_front_wheel_joint'] open_loop: false velocity_rolling_window_size: 10 - position_feedback: true - use_stamped_vel: false - front_wheels_names: [right_steering_joint, left_steering_joint] - rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] - odom_frame_id: odom base_frame_id: base_link + odom_frame_id: odom enable_odom_tf: true - in_chained_mode: false - - wheelbase: 0.325 - front_wheel_track: 0.2 - rear_wheel_track: 0.2 - front_wheels_radius: 0.05 - rear_wheels_radius: 0.05 + twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + position_feedback: false + use_stamped_vel: true diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index 40b3285a5..cedfe33e8 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -56,16 +56,22 @@ def generate_launch_description(): ] ) rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_description"), "carlikebot/rviz", "carlikebot.rviz"] + [ + FindPackageShare("ros2_control_demo_description"), + "carlikebot/rviz", + "carlikebot.rviz", + ] ) # the steering controller libraries by default publish odometry on a separate topic than /tf control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, robot_controllers], + parameters=[robot_controllers], output="both", - remappings=[("/ackermann_steering_controller/tf_odometry", "/tf")], + remappings=[ + ("~/robot_description", "/robot_description"), + ], ) robot_state_pub_ackermann_node = Node( package="robot_state_publisher", @@ -73,7 +79,8 @@ def generate_launch_description(): output="both", parameters=[robot_description], remappings=[ - ("/ackermann_steering_controller/reference_unstamped", "/cmd_vel"), + ("/bicycle_steering_controller/reference", "/reference"), + ("~/robot_description", "/robot_description"), ], ) rviz_node = Node( @@ -94,7 +101,7 @@ def generate_launch_description(): robot_ackermann_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["ackermann_steering_controller", "--controller-manager", "/controller_manager"], + arguments=["bicycle_steering_controller", "--controller-manager", "/controller_manager"], ) # Delay rviz start after `joint_state_broadcaster` diff --git a/example_11/description/ros2_control/carlikebot.ros2_control.xacro b/example_11/description/ros2_control/carlikebot.ros2_control.xacro index 4106601a6..f1c547068 100644 --- a/example_11/description/ros2_control/carlikebot.ros2_control.xacro +++ b/example_11/description/ros2_control/carlikebot.ros2_control.xacro @@ -10,29 +10,14 @@ 3.0 1 - + - - - - - - - - - - - - - - - - - + + diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index 5f16ebd3c..4d5f8b747 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -37,21 +37,21 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( } // Check if the number of joints is correct based on the mode of operation - if (info_.joints.size() != 6) + if (info_.joints.size() != 2) { RCLCPP_ERROR( rclcpp::get_logger("CarlikeBotSystemHardware"), "CarlikeBotSystemHardware::on_init() - Failed to initialize, " - "because the number of joints %ld is not 6.", + "because the number of joints %ld is not 2.", info_.joints.size()); return hardware_interface::CallbackReturn::ERROR; } for (const hardware_interface::ComponentInfo & joint : info_.joints) { - bool joint_is_steering = joint.name.find("steering") != std::string::npos; + bool joint_is_steering = joint.name.find("front") != std::string::npos; - // Steering joints have a position command and state interface + // Steering joints have a position command interface and a position state interface if (joint_is_steering) { RCLCPP_INFO( @@ -100,7 +100,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( rclcpp::get_logger("CarlikeBotSystemHardware"), "Joint '%s' is a drive joint.", joint.name.c_str()); - // Drive joints have a velocity command interface and velocity and position state interface + // Drive joints have a velocity command interface and a velocity state interface if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( @@ -119,7 +119,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( return hardware_interface::CallbackReturn::ERROR; } - if (joint.state_interfaces.size() != 1) + if (joint.state_interfaces.size() != 2) { RCLCPP_FATAL( rclcpp::get_logger("CarlikeBotSystemHardware"), @@ -136,6 +136,15 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); return hardware_interface::CallbackReturn::ERROR; } + + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Joint '%s' has %s state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } } } @@ -145,11 +154,14 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); // // END: This part here is for exemplary purposes - Please do not copy to your production code - // joint name, state, command - hw_interfaces_.resize( - 6, std::make_tuple( - std::string(), std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN())); + hw_interfaces_["steering"] = Joint( + "virtual_front_wheel_joint" + ); + + hw_interfaces_["traction"] = Joint( + "virtual_rear_wheel_joint" + ); + return hardware_interface::CallbackReturn::SUCCESS; } @@ -158,34 +170,23 @@ std::vector CarlikeBotSystemHardware::export { std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto & joint : hw_interfaces_) { - bool joint_is_steering = info_.joints[i].name.find("steering") != std::string::npos; - - // if joint name not in hw_interfaces_, add it - if (std::get<0>(hw_interfaces_[i]).empty()) - { - std::get<0>(hw_interfaces_[i]) = info_.joints[i].name; - } - - // get the index of the joint name in hw_interfaces_ - auto it = std::find_if( - hw_interfaces_.begin(), hw_interfaces_.end(), - [this, i](const std::tuple & element) - { return std::get<0>(element) == info_.joints[i].name; }); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + joint.second.joint_name, hardware_interface::HW_IF_POSITION, &joint.second.state.position)); - if (joint_is_steering) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &std::get<1>(*it))); - } - else + if (joint.first == "traction") { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &std::get<1>(*it))); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + joint.second.joint_name, hardware_interface::HW_IF_VELOCITY, + &joint.second.state.velocity)); } } + + RCLCPP_INFO( rclcpp::get_logger("CarlikeBotSystemHardware"), "Exported %zu state interfaces.", state_interfaces.size()); @@ -205,31 +206,21 @@ CarlikeBotSystemHardware::export_command_interfaces() { std::vector command_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) + for (auto & joint : hw_interfaces_) { - bool joint_is_steering = info_.joints[i].name.find("steering") != std::string::npos; - - // if joint name not in hw_interfaces_, add it - if (std::get<0>(hw_interfaces_[i]).empty()) + if (joint.first == "steering") { - std::get<0>(hw_interfaces_[i]) = info_.joints[i].name; + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + joint.second.joint_name, hardware_interface::HW_IF_POSITION, + &joint.second.command.position)); } - - // get the index of the joint name in hw_interfaces_ - auto it = std::find_if( - hw_interfaces_.begin(), hw_interfaces_.end(), - [this, i](const std::tuple & element) - { return std::get<0>(element) == info_.joints[i].name; }); - - if (joint_is_steering) + else if (joint.first == "traction") { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &std::get<2>(*it))); - } - else - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &std::get<2>(*it))); + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + joint.second.joint_name, hardware_interface::HW_IF_VELOCITY, + &joint.second.command.velocity)); } } @@ -259,10 +250,20 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i); } - for (uint i = 0; i < hw_interfaces_.size(); i++) + for (auto & joint : hw_interfaces_) { - std::get<1>(hw_interfaces_[i]) = 0.0; - std::get<2>(hw_interfaces_[i]) = 0.0; + joint.second.state.position = 0.0; + + if (joint.first == "traction") + { + joint.second.state.velocity = 0.0; + joint.second.command.velocity = 0.0; + } + + else if (joint.first == "steering") + { + joint.second.command.position = 0.0; + } } RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully activated!"); @@ -292,26 +293,17 @@ hardware_interface::return_type CarlikeBotSystemHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - for (std::size_t i = 0; i < hw_interfaces_.size(); ++i) + for (auto & joint : hw_interfaces_) { - // Simulate CarlikeBot wheels' movement as a first-order system - // Update the joint status: this is a revolute joint without any limit. - // Simply integrates - bool is_steering_joint = std::get<0>(hw_interfaces_[i]).find("steering") != std::string::npos; - - if (is_steering_joint) + if (joint.first == "steering") { - std::get<1>(hw_interfaces_[i]) = std::get<2>(hw_interfaces_[i]); + joint.second.state.position = joint.second.command.position; } - else + else if (joint.first == "traction") { - std::get<1>(hw_interfaces_[i]) += period.seconds() * std::get<2>(hw_interfaces_[i]); + joint.second.state.velocity = joint.second.command.velocity; + joint.second.state.position += joint.second.state.velocity * period.seconds(); } - - // RCLCPP_INFO( - // rclcpp::get_logger("CarlikeBotSystemHardware"), - // "Got position state %.5f and velocity state %.5f for '%s'", hw_positions_[i], - // hw_velocities_[i], info_.joints[i].name.c_str()); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -323,15 +315,30 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH { RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Writing..."); - for (auto i = 0u; i < hw_interfaces_.size(); ++i) + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + for (auto & joint : hw_interfaces_) { - // hw_velocities_[i] = hw_commands_[i]; - std::get<2>(hw_interfaces_[i]) = std::get<2>(hw_interfaces_[i]); - - // RCLCPP_INFO( - // rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'", - // hw_commands_[i], info_.joints[i].name.c_str()); + if (joint.first == "steering") + { + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Writing to steering joint '%s'.", + joint.second.joint_name.c_str()); + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Position command: %.2f.", + joint.second.command.position); + } + else if (joint.first == "traction") + { + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Writing to traction joint '%s'.", + joint.second.joint_name.c_str()); + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Velocity command: %.2f.", + joint.second.command.velocity); + } } + // END: This part here is for exemplary purposes - Please do not copy to your production code + return hardware_interface::return_type::OK; } diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp index f1556a8cc..6de2c646e 100644 --- a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -36,6 +36,30 @@ namespace ros2_control_demo_example_11 { +struct JointValue +{ + double position{0.0}; + double velocity{0.0}; + double effort{0.0}; +}; + +struct Joint +{ + Joint( + const std::string & name + ) : + joint_name(name) + { + state = JointValue(); + command = JointValue(); + } + + Joint() = default; + + std::string joint_name; + JointValue state; + JointValue command; +}; class CarlikeBotSystemHardware : public hardware_interface::SystemInterface { public: @@ -72,9 +96,9 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface double hw_start_sec_; double hw_stop_sec_; - std::vector> - hw_interfaces_; // name of joint, state, command - + // std::vector> + // hw_interfaces_; // name of joint, state, command + std::map hw_interfaces_; }; } // namespace ros2_control_demo_example_11 diff --git a/example_11/package.xml b/example_11/package.xml index 634813127..c690fb943 100644 --- a/example_11/package.xml +++ b/example_11/package.xml @@ -21,7 +21,7 @@ rclcpp_lifecycle topic_tools - ackermann_steering_controller + bicycle_steering_controller controller_manager joint_state_broadcaster joint_state_publisher_gui diff --git a/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro b/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro index 6af69eff0..3f2024c17 100644 --- a/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro +++ b/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro @@ -39,10 +39,10 @@ - + @@ -73,7 +73,7 @@ - + @@ -100,9 +100,9 @@ + ixx="0.000270" ixy="0.0" ixz="0.0" + iyy="0.000270" iyz="0.0" + izz="0.000426"/> @@ -111,7 +111,7 @@ - + @@ -138,9 +138,9 @@ + ixx="0.000270" ixy="0.0" ixz="0.0" + iyy="0.000270" iyz="0.0" + izz="0.000426"/> @@ -149,7 +149,7 @@ - + @@ -157,7 +157,7 @@ - + @@ -178,7 +178,7 @@ - + @@ -202,9 +202,9 @@ + ixx="0.000270" ixy="0.0" ixz="0.0" + iyy="0.000270" iyz="0.0" + izz="0.000426"/> @@ -241,9 +241,9 @@ + ixx="0.000270" ixy="0.0" ixz="0.0" + iyy="0.000270" iyz="0.0" + izz="0.000426"/> From 0e04086f3da1f27fcb4db501443a0601ee24eff2 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Fri, 26 Jan 2024 21:45:16 +0000 Subject: [PATCH 30/37] format --- example_11/hardware/carlikebot_system.cpp | 37 ++++++------------- .../carlikebot_system.hpp | 31 +++++++--------- 2 files changed, 26 insertions(+), 42 deletions(-) diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index 4d5f8b747..64f7550a6 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -154,14 +154,9 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); // // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_interfaces_["steering"] = Joint( - "virtual_front_wheel_joint" - ); - - hw_interfaces_["traction"] = Joint( - "virtual_rear_wheel_joint" - ); + hw_interfaces_["steering"] = Joint("virtual_front_wheel_joint"); + hw_interfaces_["traction"] = Joint("virtual_rear_wheel_joint"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -172,21 +167,16 @@ std::vector CarlikeBotSystemHardware::export for (auto & joint : hw_interfaces_) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - joint.second.joint_name, hardware_interface::HW_IF_POSITION, &joint.second.state.position)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.second.joint_name, hardware_interface::HW_IF_POSITION, &joint.second.state.position)); if (joint.first == "traction") { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - joint.second.joint_name, hardware_interface::HW_IF_VELOCITY, - &joint.second.state.velocity)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.second.joint_name, hardware_interface::HW_IF_VELOCITY, &joint.second.state.velocity)); } } - - RCLCPP_INFO( rclcpp::get_logger("CarlikeBotSystemHardware"), "Exported %zu state interfaces.", state_interfaces.size()); @@ -210,17 +200,15 @@ CarlikeBotSystemHardware::export_command_interfaces() { if (joint.first == "steering") { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - joint.second.joint_name, hardware_interface::HW_IF_POSITION, - &joint.second.command.position)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.second.joint_name, hardware_interface::HW_IF_POSITION, + &joint.second.command.position)); } else if (joint.first == "traction") { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - joint.second.joint_name, hardware_interface::HW_IF_VELOCITY, - &joint.second.command.velocity)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.second.joint_name, hardware_interface::HW_IF_VELOCITY, + &joint.second.command.velocity)); } } @@ -339,7 +327,6 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH } // END: This part here is for exemplary purposes - Please do not copy to your production code - return hardware_interface::return_type::OK; } diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp index 6de2c646e..1fc539332 100644 --- a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -38,27 +38,24 @@ namespace ros2_control_demo_example_11 { struct JointValue { - double position{0.0}; - double velocity{0.0}; - double effort{0.0}; + double position{0.0}; + double velocity{0.0}; + double effort{0.0}; }; struct Joint { - Joint( - const std::string & name - ) : - joint_name(name) - { - state = JointValue(); - command = JointValue(); - } - - Joint() = default; - - std::string joint_name; - JointValue state; - JointValue command; + Joint(const std::string & name) : joint_name(name) + { + state = JointValue(); + command = JointValue(); + } + + Joint() = default; + + std::string joint_name; + JointValue state; + JointValue command; }; class CarlikeBotSystemHardware : public hardware_interface::SystemInterface { From a0f15ef83b2920a01083a968ade55759f053845e Mon Sep 17 00:00:00 2001 From: ARK3r Date: Fri, 26 Jan 2024 22:11:07 +0000 Subject: [PATCH 31/37] added relay arg and node --- example_11/bringup/launch/carlikebot.launch.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index cedfe33e8..c95775f37 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -32,9 +32,17 @@ def generate_launch_description(): description="Start RViz2 automatically with this launch file.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "relay_odometry_tf", + default_value="false", + description="Relay odometry TF from the steering controller to the TF tree.", + ) + ) # Initialize Arguments gui = LaunchConfiguration("gui") + relay_odometry_tf = LaunchConfiguration("relay_odometry_tf") # Get URDF via xacro robot_description_content = Command( @@ -71,6 +79,7 @@ def generate_launch_description(): output="both", remappings=[ ("~/robot_description", "/robot_description"), + ("/bicycle_steering_controller/odometry_tf", "/tf"), ], ) robot_state_pub_ackermann_node = Node( @@ -104,6 +113,14 @@ def generate_launch_description(): arguments=["bicycle_steering_controller", "--controller-manager", "/controller_manager"], ) + relay_node = Node( + package="topic_tools", + executable="relay", + name="relay_odometry_node", + arguments=["/bicycle_steering_controller/tf_odometry", "/tf"], + condition=IfCondition(relay_odometry_tf), + ) + # Delay rviz start after `joint_state_broadcaster` delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( event_handler=OnProcessExit( @@ -124,6 +141,7 @@ def generate_launch_description(): control_node, robot_state_pub_ackermann_node, joint_state_broadcaster_spawner, + relay_node, delay_rviz_after_joint_state_broadcaster_spawner, delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, ] From ec2759d87713a58c8558b0ea9556e08653bc76fc Mon Sep 17 00:00:00 2001 From: ARK3r Date: Mon, 29 Jan 2024 16:41:38 +0000 Subject: [PATCH 32/37] updated docs --- example_11/doc/userdoc.rst | 69 +++++++++++------------ example_11/hardware/carlikebot_system.cpp | 14 ++--- 2 files changed, 38 insertions(+), 45 deletions(-) diff --git a/example_11/doc/userdoc.rst b/example_11/doc/userdoc.rst index df0ddbb60..d6d3edfe7 100644 --- a/example_11/doc/userdoc.rst +++ b/example_11/doc/userdoc.rst @@ -6,14 +6,18 @@ CarlikeBot ************ -*CarlikeBot* is a simple mobile base with ackermann drive. +*CarlikeBot* is a simple mobile base using bicycle model with 4 wheels. -This example shows how to use the ackermann steering controller, which is a sub-design of the steering controller library. +This example shows how to use the bicycle steering controller, which is a sub-design of the steering controller library. + +Even though the robot has 4 wheels with front steering, the vehicle dynamics of this robot is similar to a bicycle. There is a virtual front wheel joint that is used to control the steering angle of the front wheels and the front wheels on the robot mimic the steering angle of the virtual front wheel joint. Similarly the rear wheels are controlled by a virtual rear wheel joint. + +This example shows how to use the bicycle steering controller to control a carlike robot with 4 wheels but only 2 joints that can be controlled, one for steering and one for driving. * The communication is done using proprietary API to communicate with the robot control box. * Data for all joints is exchanged at once. -The *CarlikeBot* URDF files can be found in ``description/urdf`` folder. +The *CarlikeBot* URDF files can be found in ``ros2_control_demo_description/carlikebot/urdf`` folder. .. include:: ../../doc/run_from_docker.rst @@ -32,20 +36,20 @@ Tutorial steps .. image:: carlikebot.png :width: 400 - :alt: Ackermann Mobile Robot + :alt: Carlike Mobile Robot 2. To start *CarlikeBot* example open a terminal, source your ROS2-workspace and execute its launch file with .. code-block:: shell - ros2 launch ros2_control_demo_example_11 carlikebot.launch.py + ros2 launch ros2_control_demo_example_11 carlikebot.launch.py relay_odometry_tf:=true 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 *CarlikeBot*. + If you can see an orange box with four wheels in *RViz* everything has started properly. By default the controller publishes the odometry of the robot to the ``/tf_odometry`` topic. The ``relay_odometry_tf`` argument is used to relay the odometry TF to the TF tree. If you set this argument to ``false`` (or not set it at all) the TF tree will not be updated with the odometry data. + Now, let's introspect the control system before moving *CarlikeBot*. 3. Check if the hardware interface loaded properly, by opening another terminal and executing @@ -57,20 +61,15 @@ Tutorial steps .. code-block:: shell - command interfaces - ackermann_steering_controller/angular/position [unavailable] [unclaimed] - ackermann_steering_controller/linear/velocity [unavailable] [unclaimed] - left_steering_joint/position [available] [claimed] - rear_left_wheel_joint/velocity [available] [claimed] - rear_right_wheel_joint/velocity [available] [claimed] - right_steering_joint/position [available] [claimed] - state interfaces - left_steering_joint/position - rear_left_wheel_joint/position - rear_left_wheel_joint/velocity - rear_right_wheel_joint/position - rear_right_wheel_joint/velocity - right_steering_joint/position + command interfaces + bicycle_steering_controller/angular/position [unavailable] [unclaimed] + bicycle_steering_controller/linear/velocity [unavailable] [unclaimed] + virtual_front_wheel_joint/position [available] [claimed] + virtual_rear_wheel_joint/velocity [available] [claimed] + state interfaces + virtual_front_wheel_joint/position + virtual_rear_wheel_joint/position + virtual_rear_wheel_joint/velocity The ``[claimed]`` marker on command interfaces means that a controller has access to command *CarlikeBot*. @@ -85,30 +84,30 @@ Tutorial steps .. code-block:: shell joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - ackermann_steering_controller[ackermann_steering_controller/AckermannSteeringController] active + bicycle_steering_controller[bicycle_steering_controller/BicycleSteeringController] 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 /ackermann_steering_controller/reference_unstamped geometry_msgs/msg/Twist "linear: - x: 0.7 - y: 0.0 - z: 0.0 - angular: - x: 0.0 - y: 0.0 - z: 0.01" + ros2 topic pub --rate 30 /bicycle_steering_controller/reference geometry_msgs/msg/TwistStamped " + twist: + linear: + x: 1.0 + y: 0.0 + z: 0.0 + angular: + x: 0.0 + y: 0.0 + z: 0.1" 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 - [CarlikeBotSystemHardware]: Got command 0.00464 for 'left_steering_joint' - [CarlikeBotSystemHardware]: Got command 0.00465 for 'right_steering_joint' - [CarlikeBotSystemHardware]: Got command 13.97230 for 'rear_left_wheel_joint' - [CarlikeBotSystemHardware]: Got command 14.02830 for 'rear_right_wheel_joint' + [CarlikeBotSystemHardware]: Got position command: 0.03 for joint 'virtual_front_wheel_joint'. + [CarlikeBotSystemHardware]: Got velocity command: 20.01 for joint 'virtual_rear_wheel_joint'. Files used for this demos -------------------------- @@ -129,4 +128,4 @@ Controllers from this demo -------------------------- * ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ -* ``Ackermann Steering Controller`` (`ros2_controllers repository `__): `doc `__ +* ``Bicycle Steering Controller`` (`ros2_controllers repository `__): `doc `__ diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index 64f7550a6..bb3e12bae 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -301,28 +301,22 @@ hardware_interface::return_type CarlikeBotSystemHardware::read( hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Writing..."); - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code for (auto & joint : hw_interfaces_) { if (joint.first == "steering") { RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Writing to steering joint '%s'.", + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Got position command: %.2f for joint '%s'.", joint.second.command.position, joint.second.joint_name.c_str()); - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Position command: %.2f.", - joint.second.command.position); } else if (joint.first == "traction") { RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Writing to traction joint '%s'.", + rclcpp::get_logger("CarlikeBotSystemHardware"), + "Got velocity command: %.2f for joint '%s'.", joint.second.command.velocity, joint.second.joint_name.c_str()); - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Velocity command: %.2f.", - joint.second.command.velocity); } } // END: This part here is for exemplary purposes - Please do not copy to your production code From f973b38f6ef74441a2be9d53664b59c5576d6952 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Mon, 29 Jan 2024 17:03:34 +0000 Subject: [PATCH 33/37] cleanup --- doc/index.rst | 2 +- example_11/README.md | 4 +- example_11/doc/userdoc.rst | 2 +- example_11/hardware/carlikebot_system.cpp | 53 ++++++++++----------- example_11/package.xml | 1 + example_11/ros2_control_demo_example_11.xml | 2 +- 6 files changed, 30 insertions(+), 34 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index facbb538d..0837344a5 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -71,7 +71,7 @@ Example 10: "GPIO interfaces" Industrial robot with GPIO interfaces Example 11: "CarlikeBot" - *CarlikeBot* with an Ackermann steering controller + *CarlikeBot* with a bicycle steering controller Example 12: "Controller chaining" The example shows a simple chainable controller and its integration to form a controller chain to control the joints of *RRBot*. diff --git a/example_11/README.md b/example_11/README.md index 841051c0b..23e88cf84 100644 --- a/example_11/README.md +++ b/example_11/README.md @@ -1,6 +1,6 @@ # ros2_control_demo_example_11 - *CarlikeBot*, or ''Carlike Mobile Robot'', is a simple mobile base with [ackermann drive](https://en.wikipedia.org/wiki/Ackermann_steering_geometry). - The robot has two wheels in the front that steer the robot and two wheels in the back that power the robot forward and backwards. + *CarlikeBot*, or ''Carlike Mobile Robot'', is a simple mobile base with bicycle drive. + The robot has two wheels in the front that steer the robot and two wheels in the back that power the robot forward and backwards. However, since each pair of wheels (steering and traction) are controlled by one interface, a bicycle steering model is used. Find the documentation in [doc/userdoc.rst](doc/userdoc.rst) or on [control.ros.org](https://control.ros.org/master/doc/ros2_control_demos/example_11/doc/userdoc.html). diff --git a/example_11/doc/userdoc.rst b/example_11/doc/userdoc.rst index d6d3edfe7..8f8880b83 100644 --- a/example_11/doc/userdoc.rst +++ b/example_11/doc/userdoc.rst @@ -128,4 +128,4 @@ Controllers from this demo -------------------------- * ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ -* ``Bicycle Steering Controller`` (`ros2_controllers repository `__): `doc `__ +* ``Bicycle Steering Controller`` (`ros2_controllers repository `__): `doc `__ diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index bb3e12bae..b91abc036 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -281,18 +281,21 @@ hardware_interface::return_type CarlikeBotSystemHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - for (auto & joint : hw_interfaces_) - { - if (joint.first == "steering") - { - joint.second.state.position = joint.second.command.position; - } - else if (joint.first == "traction") - { - joint.second.state.velocity = joint.second.command.velocity; - joint.second.state.position += joint.second.state.velocity * period.seconds(); - } - } + + hw_interfaces_["steering"].state.position = hw_interfaces_["steering"].command.position; + + hw_interfaces_["traction"].state.velocity = hw_interfaces_["traction"].command.velocity; + hw_interfaces_["traction"].state.position += + hw_interfaces_["traction"].state.velocity * period.seconds(); + + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got position state: %.2f for joint '%s'.", + hw_interfaces_["steering"].command.position, hw_interfaces_["steering"].joint_name.c_str()); + + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got velocity state: %.2f for joint '%s'.", + hw_interfaces_["traction"].command.velocity, hw_interfaces_["traction"].joint_name.c_str()); + // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; @@ -302,23 +305,15 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - for (auto & joint : hw_interfaces_) - { - if (joint.first == "steering") - { - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Got position command: %.2f for joint '%s'.", joint.second.command.position, - joint.second.joint_name.c_str()); - } - else if (joint.first == "traction") - { - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Got velocity command: %.2f for joint '%s'.", joint.second.command.velocity, - joint.second.joint_name.c_str()); - } - } + + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got position command: %.2f for joint '%s'.", + hw_interfaces_["steering"].command.position, hw_interfaces_["steering"].joint_name.c_str()); + + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got velocity command: %.2f for joint '%s'.", + hw_interfaces_["traction"].command.velocity, hw_interfaces_["traction"].joint_name.c_str()); + // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; diff --git a/example_11/package.xml b/example_11/package.xml index c690fb943..70391c873 100644 --- a/example_11/package.xml +++ b/example_11/package.xml @@ -28,6 +28,7 @@ robot_state_publisher ros2_controllers_test_nodes ros2_control_demo_description + topic_tools ros2controlcli ros2launch rviz2 diff --git a/example_11/ros2_control_demo_example_11.xml b/example_11/ros2_control_demo_example_11.xml index 40fac739e..92c7fdb0e 100644 --- a/example_11/ros2_control_demo_example_11.xml +++ b/example_11/ros2_control_demo_example_11.xml @@ -3,7 +3,7 @@ type="ros2_control_demo_example_11::CarlikeBotSystemHardware" base_class_type="hardware_interface::SystemInterface"> - The ros2_control CarlikeBot example using a system hardware interface-type. It uses velocity command and state interface for the drive motor and position command and state interface for the steering. The example is the starting point to implement a hardware interface for ackermann and bicycle drive robots. + The ros2_control CarlikeBot example using a system hardware interface-type. It uses velocity and position command and state interface for the drive motor and position command and state interface for the steering. The example is the starting point to implement a hardware interface for ackermann and bicycle drive robots. From e7a9d69ecb7b9f5335f7f65b5825ade8aed6016a Mon Sep 17 00:00:00 2001 From: ARK3r Date: Mon, 29 Jan 2024 17:06:58 +0000 Subject: [PATCH 34/37] redundant dependency --- example_11/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/example_11/package.xml b/example_11/package.xml index 70391c873..c690fb943 100644 --- a/example_11/package.xml +++ b/example_11/package.xml @@ -28,7 +28,6 @@ robot_state_publisher ros2_controllers_test_nodes ros2_control_demo_description - topic_tools ros2controlcli ros2launch rviz2 From 50c0593af3807627c49d324cb3c5f84597aead15 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 17 Feb 2024 16:47:56 +0000 Subject: [PATCH 35/37] Remove dependency on topic_tools --- .../bringup/launch/carlikebot.launch.py | 40 ++++++++++--------- example_11/doc/userdoc.rst | 31 ++++++++------ example_11/package.xml | 2 - 3 files changed, 39 insertions(+), 34 deletions(-) diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index c95775f37..a15ea1256 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -14,7 +14,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, RegisterEventHandler -from launch.conditions import IfCondition +from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnProcessExit from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration @@ -34,15 +34,15 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - "relay_odometry_tf", + "remap_odometry_tf", default_value="false", - description="Relay odometry TF from the steering controller to the TF tree.", + description="Remap odometry TF from the steering controller to the TF tree.", ) ) # Initialize Arguments gui = LaunchConfiguration("gui") - relay_odometry_tf = LaunchConfiguration("relay_odometry_tf") + remap_odometry_tf = LaunchConfiguration("remap_odometry_tf") # Get URDF via xacro robot_description_content = Command( @@ -72,6 +72,17 @@ def generate_launch_description(): ) # the steering controller libraries by default publish odometry on a separate topic than /tf + control_node_remapped = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers], + output="both", + remappings=[ + ("~/robot_description", "/robot_description"), + ("/bicycle_steering_controller/tf_odometry", "/tf"), + ], + condition=IfCondition(remap_odometry_tf), + ) control_node = Node( package="controller_manager", executable="ros2_control_node", @@ -79,16 +90,15 @@ def generate_launch_description(): output="both", remappings=[ ("~/robot_description", "/robot_description"), - ("/bicycle_steering_controller/odometry_tf", "/tf"), ], + condition=UnlessCondition(remap_odometry_tf), ) - robot_state_pub_ackermann_node = Node( + robot_state_pub_bicycle_node = Node( package="robot_state_publisher", executable="robot_state_publisher", output="both", parameters=[robot_description], remappings=[ - ("/bicycle_steering_controller/reference", "/reference"), ("~/robot_description", "/robot_description"), ], ) @@ -107,20 +117,12 @@ def generate_launch_description(): arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) - robot_ackermann_controller_spawner = Node( + robot_bicycle_controller_spawner = Node( package="controller_manager", executable="spawner", arguments=["bicycle_steering_controller", "--controller-manager", "/controller_manager"], ) - relay_node = Node( - package="topic_tools", - executable="relay", - name="relay_odometry_node", - arguments=["/bicycle_steering_controller/tf_odometry", "/tf"], - condition=IfCondition(relay_odometry_tf), - ) - # Delay rviz start after `joint_state_broadcaster` delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( event_handler=OnProcessExit( @@ -133,15 +135,15 @@ def generate_launch_description(): delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( event_handler=OnProcessExit( target_action=joint_state_broadcaster_spawner, - on_exit=[robot_ackermann_controller_spawner], + on_exit=[robot_bicycle_controller_spawner], ) ) nodes = [ control_node, - robot_state_pub_ackermann_node, + control_node_remapped, + robot_state_pub_bicycle_node, joint_state_broadcaster_spawner, - relay_node, delay_rviz_after_joint_state_broadcaster_spawner, delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, ] diff --git a/example_11/doc/userdoc.rst b/example_11/doc/userdoc.rst index 8f8880b83..c2dbb96d2 100644 --- a/example_11/doc/userdoc.rst +++ b/example_11/doc/userdoc.rst @@ -42,16 +42,21 @@ Tutorial steps .. code-block:: shell - ros2 launch ros2_control_demo_example_11 carlikebot.launch.py relay_odometry_tf:=true + ros2 launch ros2_control_demo_example_11 carlikebot.launch.py remap_odometry_tf:=true 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 with four wheels in *RViz* everything has started properly. By default the controller publishes the odometry of the robot to the ``/tf_odometry`` topic. The ``relay_odometry_tf`` argument is used to relay the odometry TF to the TF tree. If you set this argument to ``false`` (or not set it at all) the TF tree will not be updated with the odometry data. - Now, let's introspect the control system before moving *CarlikeBot*. + If you can see an orange box with four wheels in *RViz* everything has started properly. -3. Check if the hardware interface loaded properly, by opening another terminal and executing + .. note:: + + For robots being fixed to the world frame, like the RRbot examples of this repository, the ``robot_state_publisher`` subscribes to the ``/joint_states`` topic and creates the TF tree. For mobile robots, we need a node publishing the TF tree including the pose of the robot in the world coordinate systems. The most simple one is the odometry calculated by the ``bicycle_steering_controller``. + + By default, the controller publishes the odometry of the robot to the ``~/tf_odometry`` topic. The ``remap_odometry_tf`` argument is used to remap the odometry TF to the ``/tf`` topic. If you set this argument to ``false`` (or not set it at all) the TF tree will not be updated with the odometry data. + +3. Now, let's introspect the control system before moving *CarlikeBot*. Check if the hardware interface loaded properly, by opening another terminal and executing .. code-block:: shell @@ -86,20 +91,20 @@ Tutorial steps joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active bicycle_steering_controller[bicycle_steering_controller/BicycleSteeringController] active -5. If everything is fine, now you can send a command to *Diff Drive Controller* using ROS 2 CLI interface: +5. If everything is fine, now you can send a command to *bicycle_steering_controller* using ROS 2 CLI: .. code-block:: shell ros2 topic pub --rate 30 /bicycle_steering_controller/reference geometry_msgs/msg/TwistStamped " twist: - linear: - x: 1.0 - y: 0.0 - z: 0.0 - angular: - x: 0.0 - y: 0.0 - z: 0.1" + linear: + x: 1.0 + y: 0.0 + z: 0.0 + angular: + x: 0.0 + y: 0.0 + z: 0.1" You should now see an orange box circling in *RViz*. Also, you should see changing states in the terminal where launch file is started. diff --git a/example_11/package.xml b/example_11/package.xml index c690fb943..1514e9ef2 100644 --- a/example_11/package.xml +++ b/example_11/package.xml @@ -19,14 +19,12 @@ pluginlib rclcpp rclcpp_lifecycle - topic_tools bicycle_steering_controller controller_manager joint_state_broadcaster joint_state_publisher_gui robot_state_publisher - ros2_controllers_test_nodes ros2_control_demo_description ros2controlcli ros2launch From 78e9248d6e213d0631b24614798356961d4c4ae0 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 17 Feb 2024 16:48:03 +0000 Subject: [PATCH 36/37] Cleanup --- .../description/ros2_control/carlikebot.ros2_control.xacro | 1 - 1 file changed, 1 deletion(-) diff --git a/example_11/description/ros2_control/carlikebot.ros2_control.xacro b/example_11/description/ros2_control/carlikebot.ros2_control.xacro index f1c547068..d8854f59b 100644 --- a/example_11/description/ros2_control/carlikebot.ros2_control.xacro +++ b/example_11/description/ros2_control/carlikebot.ros2_control.xacro @@ -8,7 +8,6 @@ ros2_control_demo_example_11/CarlikeBotSystemHardware 0 3.0 - 1 From 7d1c240b57df586d57e0a0e9213c485628a084ee Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 17 Feb 2024 16:50:42 +0000 Subject: [PATCH 37/37] Add tests for description --- example_11/CMakeLists.txt | 5 +- example_11/test/test_urdf_xacro.py | 77 +++++++++++++++++++++++ example_11/test/test_view_robot_launch.py | 54 ++++++++++++++++ 3 files changed, 135 insertions(+), 1 deletion(-) create mode 100644 example_11/test/test_urdf_xacro.py create mode 100644 example_11/test/test_view_robot_launch.py diff --git a/example_11/CMakeLists.txt b/example_11/CMakeLists.txt index 2323ecb5e..f21b01f6c 100644 --- a/example_11/CMakeLists.txt +++ b/example_11/CMakeLists.txt @@ -64,7 +64,10 @@ install(TARGETS ros2_control_demo_example_11 ) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + + ament_add_pytest_test(example_11_urdf_xacro test/test_urdf_xacro.py) + ament_add_pytest_test(view_example_11_launch test/test_view_robot_launch.py) endif() ## EXPORTS diff --git a/example_11/test/test_urdf_xacro.py b/example_11/test/test_urdf_xacro.py new file mode 100644 index 000000000..c1a7ae662 --- /dev/null +++ b/example_11/test/test_urdf_xacro.py @@ -0,0 +1,77 @@ +# Copyright (c) 2022 FZI Forschungszentrum Informatik +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Lukas Sackewitz + +import os +import shutil +import subprocess +import tempfile + +from ament_index_python.packages import get_package_share_directory + + +def test_urdf_xacro(): + # General Arguments + description_package = "ros2_control_demo_example_11" + description_file = "carlikebot.urdf.xacro" + + description_file_path = os.path.join( + get_package_share_directory(description_package), "urdf", description_file + ) + + (_, tmp_urdf_output_file) = tempfile.mkstemp(suffix=".urdf") + + # Compose `xacro` and `check_urdf` command + xacro_command = ( + f"{shutil.which('xacro')}" f" {description_file_path}" f" > {tmp_urdf_output_file}" + ) + check_urdf_command = f"{shutil.which('check_urdf')} {tmp_urdf_output_file}" + + # Try to call processes but finally remove the temp file + try: + xacro_process = subprocess.run( + xacro_command, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True + ) + + assert xacro_process.returncode == 0, " --- XACRO command failed ---" + + check_urdf_process = subprocess.run( + check_urdf_command, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True + ) + + assert ( + check_urdf_process.returncode == 0 + ), "\n --- URDF check failed! --- \nYour xacro does not unfold into a proper urdf robot description. Please check your xacro file." + + finally: + os.remove(tmp_urdf_output_file) + + +if __name__ == "__main__": + test_urdf_xacro() diff --git a/example_11/test/test_view_robot_launch.py b/example_11/test/test_view_robot_launch.py new file mode 100644 index 000000000..e4c087d0c --- /dev/null +++ b/example_11/test/test_view_robot_launch.py @@ -0,0 +1,54 @@ +# Copyright (c) 2022 FZI Forschungszentrum Informatik +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Lukas Sackewitz + +import os +import pytest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_11"), + "launch/view_robot.launch.py", + ) + ), + launch_arguments={"gui": "true"}.items(), + ) + + return LaunchDescription([launch_include, ReadyToTest()])