From edb011a1675e0899318459f5e2c8ea30a30c4fe8 Mon Sep 17 00:00:00 2001 From: Reza Kermani Date: Sat, 17 Feb 2024 12:02:17 -0500 Subject: [PATCH] [Example 11] Carlike Robot using Bicycle Steering Controller (#316) Co-authored-by: Christoph Froehlich --- .github/workflows/ci-ros-lint.yml | 1 + README.md | 2 +- doc/index.rst | 4 +- example_11/CMakeLists.txt | 76 ++++ example_11/README.md | 6 + .../config/carlikebot_controllers.yaml | 29 ++ .../bringup/launch/carlikebot.launch.py | 151 ++++++++ .../description/launch/view_robot.launch.py | 111 ++++++ .../carlikebot.ros2_control.xacro | 25 ++ .../description/urdf/carlikebot.urdf.xacro | 19 + example_11/doc/carlikebot.png | Bin 0 -> 20581 bytes example_11/doc/userdoc.rst | 136 ++++++++ example_11/hardware/carlikebot_system.cpp | 326 ++++++++++++++++++ .../carlikebot_system.hpp | 103 ++++++ .../visibility_control.h | 56 +++ example_11/package.xml | 39 +++ example_11/ros2_control_demo_example_11.xml | 9 + example_11/test/test_urdf_xacro.py | 77 +++++ example_11/test/test_view_robot_launch.py | 54 +++ ros2_control_demo_description/CMakeLists.txt | 5 + .../carlikebot/rviz/carlikebot.rviz | 172 +++++++++ .../carlikebot/rviz/carlikebot_view.rviz | 172 +++++++++ .../urdf/carlikebot.materials.xacro | 44 +++ .../urdf/carlikebot_description.urdf.xacro | 263 ++++++++++++++ ros2_control_demos/package.xml | 1 + 25 files changed, 1879 insertions(+), 2 deletions(-) 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/urdf/carlikebot.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 create mode 100644 example_11/test/test_urdf_xacro.py create mode 100644 example_11/test/test_view_robot_launch.py create mode 100644 ros2_control_demo_description/carlikebot/rviz/carlikebot.rviz create mode 100644 ros2_control_demo_description/carlikebot/rviz/carlikebot_view.rviz create mode 100644 ros2_control_demo_description/carlikebot/urdf/carlikebot.materials.xacro create mode 100644 ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index d364a758..a87d1028 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -30,6 +30,7 @@ jobs: ros2_control_demo_example_8 ros2_control_demo_example_9 ros2_control_demo_example_10 + ros2_control_demo_example_11 ros2_control_demo_example_12 ros2_control_demo_example_14 diff --git a/README.md b/README.md index 62370d87..572bf282 100644 --- a/README.md +++ b/README.md @@ -60,7 +60,7 @@ The following examples are part of this demo repository: *RRBot* with GPIO interfaces. -* Example 11: "Car-like robot using steering controller library (tba.)" +* Example 11: ["Car-like robot using steering controller library"](example_11) * Example 12: ["Controller chaining"](example_12) diff --git a/doc/index.rst b/doc/index.rst index f4c4af21..0837344a 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -70,7 +70,8 @@ Example 9: "Gazebo Classic" Example 10: "GPIO interfaces" Industrial robot with GPIO interfaces -Example 11: "Car-like robot using steering controller library (tba.)" +Example 11: "CarlikeBot" + *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*. @@ -270,5 +271,6 @@ Examples Example 8: Using transmissions <../example_8/doc/userdoc.rst> Example 9: Gazebo classic <../example_9/doc/userdoc.rst> Example 10: Industrial robot with GPIO interfaces <../example_10/doc/userdoc.rst> + Example 11: CarlikeBot <../example_11/doc/userdoc.rst> Example 12: Controller chaining <../example_12/doc/userdoc.rst> Example 14: Modular robots with actuators not providing states <../example_14/doc/userdoc.rst> diff --git a/example_11/CMakeLists.txt b/example_11/CMakeLists.txt new file mode 100644 index 00000000..f21b01f6 --- /dev/null +++ b/example_11/CMakeLists.txt @@ -0,0 +1,76 @@ +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 + 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_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 +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 00000000..23e88cf8 --- /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 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/bringup/config/carlikebot_controllers.yaml b/example_11/bringup/config/carlikebot_controllers.yaml new file mode 100644 index 00000000..cc9bc55b --- /dev/null +++ b/example_11/bringup/config/carlikebot_controllers.yaml @@ -0,0 +1,29 @@ +controller_manager: + ros__parameters: + update_rate: 10 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + bicycle_steering_controller: + type: bicycle_steering_controller/BicycleSteeringController + + +bicycle_steering_controller: + ros__parameters: + 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 + base_frame_id: base_link + odom_frame_id: odom + enable_odom_tf: true + 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 new file mode 100644 index 00000000..a15ea125 --- /dev/null +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -0,0 +1,151 @@ +# 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, UnlessCondition +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( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "remap_odometry_tf", + default_value="false", + description="Remap odometry TF from the steering controller to the TF tree.", + ) + ) + + # Initialize Arguments + gui = LaunchConfiguration("gui") + remap_odometry_tf = LaunchConfiguration("remap_odometry_tf") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_11"), "urdf", "carlikebot.urdf.xacro"] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_11"), + "config", + "carlikebot_controllers.yaml", + ] + ) + rviz_config_file = PathJoinSubstitution( + [ + 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_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", + parameters=[robot_controllers], + output="both", + remappings=[ + ("~/robot_description", "/robot_description"), + ], + condition=UnlessCondition(remap_odometry_tf), + ) + robot_state_pub_bicycle_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + remappings=[ + ("~/robot_description", "/robot_description"), + ], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + condition=IfCondition(gui), + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + robot_bicycle_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["bicycle_steering_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_bicycle_controller_spawner], + ) + ) + + nodes = [ + control_node, + control_node_remapped, + 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, + ] + + 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 00000000..50b1e984 --- /dev/null +++ b/example_11/description/launch/view_robot.launch.py @@ -0,0 +1,111 @@ +# 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.conditions import IfCondition +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_description", + 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="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( + "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") + gui = LaunchConfiguration("gui") + prefix = LaunchConfiguration("prefix") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_11"), "urdf", description_file] + ), + " ", + "prefix:=", + prefix, + ] + ) + robot_description = {"robot_description": robot_description_content} + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "carlikebot/rviz", "carlikebot_view.rviz"] + ) + + 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", + 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], + condition=IfCondition(gui), + ) + + 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 00000000..d8854f59 --- /dev/null +++ b/example_11/description/ros2_control/carlikebot.ros2_control.xacro @@ -0,0 +1,25 @@ + + + + + + + + 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 new file mode 100644 index 00000000..cf8a85b2 --- /dev/null +++ b/example_11/description/urdf/carlikebot.urdf.xacro @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + diff --git a/example_11/doc/carlikebot.png b/example_11/doc/carlikebot.png new file mode 100644 index 0000000000000000000000000000000000000000..44a32285cb89745a5907fc666a2caa2ce348b7dc 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`__ +* Controllers yaml: `carlikebot_controllers.yaml `__ +* URDF file: `carlikebot.urdf.xacro `__ + + * Description: `carlikebot_description.urdf.xacro `__ + * ``ros2_control`` tag: `carlikebot.ros2_control.xacro `__ + +* RViz configuration: `carlikebot.rviz `__ + +* Hardware interface plugin: `carlikebot_system.cpp `__ + + +Controllers from this demo +-------------------------- + +* ``Joint State Broadcaster`` (`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 new file mode 100644 index 00000000..b91abc03 --- /dev/null +++ b/example_11/hardware/carlikebot_system.cpp @@ -0,0 +1,326 @@ +// 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_11/carlikebot_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_11 +{ +hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( + const hardware_interface::HardwareInfo & info) +{ + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // Check if the number of joints is correct based on the mode of operation + 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 2.", + info_.joints.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + bool joint_is_steering = joint.name.find("front") != std::string::npos; + + // Steering joints have a position command interface and a position 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( + 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_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) + { + 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; + } + } + 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 a velocity 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_VELOCITY) + { + 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_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; + } + } + } + + // // 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_interfaces_["steering"] = Joint("virtual_front_wheel_joint"); + + hw_interfaces_["traction"] = Joint("virtual_rear_wheel_joint"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector CarlikeBotSystemHardware::export_state_interfaces() +{ + std::vector state_interfaces; + + 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)); + + if (joint.first == "traction") + { + 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()); + + for (auto s : state_interfaces) + { + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Exported state interface '%s'.", + s.get_name().c_str()); + } + + return state_interfaces; +} + +std::vector +CarlikeBotSystemHardware::export_command_interfaces() +{ + std::vector command_interfaces; + + for (auto & joint : hw_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)); + } + 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)); + } + } + + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Exported %zu command interfaces.", + command_interfaces.size()); + + for (auto i = 0u; i < command_interfaces.size(); i++) + { + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Exported command interface '%s'.", + command_interfaces[i].get_name().c_str()); + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + 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 & joint : hw_interfaces_) + { + 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!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +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++) + { + 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; +} + +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 + + 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; +} + +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"), "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; +} + +} // namespace ros2_control_demo_example_11 + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + 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 new file mode 100644 index 00000000..1fc53933 --- /dev/null +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -0,0 +1,103 @@ +// 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_11__CARLIKEBOT_SYSTEM_HPP_ +#define ROS2_CONTROL_DEMO_EXAMPLE_11__CARLIKEBOT_SYSTEM_HPP_ + +#include +#include +#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_11/visibility_control.h" + +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: + RCLCPP_SHARED_PTR_DEFINITIONS(CarlikeBotSystemHardware); + + ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC + std::vector export_state_interfaces() override; + + ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC + std::vector export_command_interfaces() override; + + ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + // Parameters for the CarlikeBot simulation + double hw_start_sec_; + double hw_stop_sec_; + + // std::vector> + // hw_interfaces_; // name of joint, state, command + std::map hw_interfaces_; +}; + +} // namespace ros2_control_demo_example_11 + +#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 new file mode 100644 index 00000000..dff3344d --- /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_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_11_EXPORT __attribute__((dllexport)) +#define ROS2_CONTROL_DEMO_EXAMPLE_11_IMPORT __attribute__((dllimport)) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_11_EXPORT __declspec(dllexport) +#define ROS2_CONTROL_DEMO_EXAMPLE_11_IMPORT __declspec(dllimport) +#endif +#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_11_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_11_IMPORT +#endif +#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_11_EXPORT __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_11_IMPORT +#if __GNUC__ >= 4 +#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_11_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_11_LOCAL +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_11_PUBLIC_TYPE +#endif + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_11__VISIBILITY_CONTROL_H_ diff --git a/example_11/package.xml b/example_11/package.xml new file mode 100644 index 00000000..1514e9ef --- /dev/null +++ b/example_11/package.xml @@ -0,0 +1,39 @@ + + + + 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 + + bicycle_steering_controller + controller_manager + joint_state_broadcaster + joint_state_publisher_gui + robot_state_publisher + ros2_control_demo_description + 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 00000000..92c7fdb0 --- /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 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. + + + diff --git a/example_11/test/test_urdf_xacro.py b/example_11/test/test_urdf_xacro.py new file mode 100644 index 00000000..c1a7ae66 --- /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 00000000..e4c087d0 --- /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()]) diff --git a/ros2_control_demo_description/CMakeLists.txt b/ros2_control_demo_description/CMakeLists.txt index b7aedafb..ebe8c7f1 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/ros2_control_demo_description/carlikebot/rviz/carlikebot.rviz b/ros2_control_demo_description/carlikebot/rviz/carlikebot.rviz new file mode 100644 index 00000000..365d5724 --- /dev/null +++ b/ros2_control_demo_description/carlikebot/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/ros2_control_demo_description/carlikebot/rviz/carlikebot_view.rviz b/ros2_control_demo_description/carlikebot/rviz/carlikebot_view.rviz new file mode 100644 index 00000000..d887a819 --- /dev/null +++ b/ros2_control_demo_description/carlikebot/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/ros2_control_demo_description/carlikebot/urdf/carlikebot.materials.xacro b/ros2_control_demo_description/carlikebot/urdf/carlikebot.materials.xacro new file mode 100644 index 00000000..072d83a9 --- /dev/null +++ b/ros2_control_demo_description/carlikebot/urdf/carlikebot.materials.xacro @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro b/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro new file mode 100644 index 00000000..3f2024c1 --- /dev/null +++ b/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro @@ -0,0 +1,263 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros2_control_demos/package.xml b/ros2_control_demos/package.xml index 65a5d32b..2df0f206 100644 --- a/ros2_control_demos/package.xml +++ b/ros2_control_demos/package.xml @@ -24,6 +24,7 @@ ros2_control_demo_example_8 ros2_control_demo_example_9 ros2_control_demo_example_10 + ros2_control_demo_example_11 ros2_control_demo_example_12 ros2_control_demo_example_14