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 00000000..44a32285 Binary files /dev/null and b/example_11/doc/carlikebot.png differ diff --git a/example_11/doc/userdoc.rst b/example_11/doc/userdoc.rst new file mode 100644 index 00000000..c2dbb96d --- /dev/null +++ b/example_11/doc/userdoc.rst @@ -0,0 +1,136 @@ +:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_11/doc/userdoc.rst + +.. _ros2_control_demos_example_11_userdoc: + +************ +CarlikeBot +************ + +*CarlikeBot* is a simple mobile base using bicycle model with 4 wheels. + +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 ``ros2_control_demo_description/carlikebot/urdf`` folder. + +.. include:: ../../doc/run_from_docker.rst + +Tutorial steps +-------------------------- + +1. To check that *CarlikeBot* description is working properly use following launch commands + + .. code-block:: shell + + 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``. + This happens because ``joint_state_publisher_gui`` node needs some time to start. + + .. image:: carlikebot.png + :width: 400 + :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 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. + + .. 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 + + ros2 control list_hardware_interfaces + + You should get + + .. code-block:: shell + + 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*. + +4. Check if controllers are running + + .. code-block:: shell + + ros2 control list_controllers + + You should get + + .. code-block:: shell + + 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 *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" + + 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 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 +-------------------------- + +* Launch file: `carlikebot.launch.py `__ +* 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