diff --git a/ros2_control_demo_robot/controllers/rrbot_gazebo_forward_controller_position.yaml b/ros2_control_demo_robot/controllers/rrbot_gazebo_forward_controller_position.yaml new file mode 100644 index 000000000..5df0e5d8e --- /dev/null +++ b/ros2_control_demo_robot/controllers/rrbot_gazebo_forward_controller_position.yaml @@ -0,0 +1,16 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_controller: + type: joint_state_controller/JointStateController + +joint_trajectory_controller: + ros__parameters: + joints: + - joint1 + - joint2 + interface_name: position diff --git a/ros2_control_demo_robot/description/rrbot/rrbot.gazebo.xacro b/ros2_control_demo_robot/description/rrbot/rrbot.gazebo.xacro index bdbc2a7f8..347d6f61f 100644 --- a/ros2_control_demo_robot/description/rrbot/rrbot.gazebo.xacro +++ b/ros2_control_demo_robot/description/rrbot/rrbot.gazebo.xacro @@ -9,9 +9,8 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - - /rrbot - gazebo_ros_control/DefaultRobotHWSim + + $(find ros2_control_demo_robot)/controllers/rrbot_gazebo_forward_controller_position.yaml diff --git a/ros2_control_demo_robot/description/rrbot/rrbot_system_position_only.ros2_control.xacro b/ros2_control_demo_robot/description/rrbot/rrbot_system_position_only.ros2_control.xacro index a97ecdeeb..633b05e6e 100644 --- a/ros2_control_demo_robot/description/rrbot/rrbot_system_position_only.ros2_control.xacro +++ b/ros2_control_demo_robot/description/rrbot/rrbot_system_position_only.ros2_control.xacro @@ -1,15 +1,24 @@ - + - - ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware - 2.0 - 3.0 - 2.0 - + + + + gazebo_ros2_control/GazeboSystem + + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + -1 @@ -29,4 +38,3 @@ - diff --git a/ros2_control_demo_robot/description/rrbot_system_position_only.urdf.xacro b/ros2_control_demo_robot/description/rrbot_system_position_only.urdf.xacro index c25494ab7..9f6c0f5a5 100644 --- a/ros2_control_demo_robot/description/rrbot_system_position_only.urdf.xacro +++ b/ros2_control_demo_robot/description/rrbot_system_position_only.urdf.xacro @@ -5,7 +5,8 @@ Copied and modified from ROS1 example: https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/rrbot.xacro --> - + + @@ -34,6 +35,6 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + diff --git a/ros2_control_demo_robot/launch/rrbot_system_position_only_gazebo.launch.py b/ros2_control_demo_robot/launch/rrbot_system_position_only_gazebo.launch.py new file mode 100644 index 000000000..0114209d5 --- /dev/null +++ b/ros2_control_demo_robot/launch/rrbot_system_position_only_gazebo.launch.py @@ -0,0 +1,60 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# 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. + +import os + +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_ros.actions import Node + +import xacro + + +def generate_launch_description(): + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']), + ) + + robot_description_path = os.path.join( + get_package_share_directory('ros2_control_demo_robot'), + 'description', + 'rrbot_system_position_only.urdf.xacro') + robot_description_config = xacro.process_file( + robot_description_path, + mappings={'use_sim': 'true'}) + robot_description = {'robot_description': robot_description_config.toxml()} + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[robot_description] + ) + + spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', + arguments=['-topic', 'robot_description', + '-entity', 'rrbot_system_position'], + output='screen') + + return LaunchDescription([ + gazebo, + node_robot_state_publisher, + spawn_entity, + ]) diff --git a/ros2_control_demo_robot/package.xml b/ros2_control_demo_robot/package.xml index b0c761395..4322595a3 100644 --- a/ros2_control_demo_robot/package.xml +++ b/ros2_control_demo_robot/package.xml @@ -14,6 +14,9 @@ hardware_interface pluginlib rclcpp + gazebo_ros2_control + + xacro ros2_control_demo_hardware