From de569dafcd2320b44b6c7e3dd121d160962d7494 Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Fri, 5 Apr 2024 09:55:22 +0200 Subject: [PATCH 01/23] fix - same inactive and active controller is to be loaded --- templates/ros2_control/robot_ros2_control.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/templates/ros2_control/robot_ros2_control.launch.xml b/templates/ros2_control/robot_ros2_control.launch.xml index ff75915a..f9f7fadd 100644 --- a/templates/ros2_control/robot_ros2_control.launch.xml +++ b/templates/ros2_control/robot_ros2_control.launch.xml @@ -46,7 +46,7 @@ Authors: Riyan Jose, Manuel Muth, Dr. Denis default="joint_trajectory_controller" description="Robot controller to start. Choices are: [forward_position_controller, joint_trajectory_controller]."/> From 76e3413de82950b15588bcd9930403d8dd101853 Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Fri, 5 Apr 2024 15:35:40 +0200 Subject: [PATCH 02/23] set JTC as default active controller for bringup template * so that setup-moveit-package planning and execution works out of the box --- templates/ros2_control/robot_ros2_control.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/templates/ros2_control/robot_ros2_control.launch.xml b/templates/ros2_control/robot_ros2_control.launch.xml index f9f7fadd..ff75915a 100644 --- a/templates/ros2_control/robot_ros2_control.launch.xml +++ b/templates/ros2_control/robot_ros2_control.launch.xml @@ -46,7 +46,7 @@ Authors: Riyan Jose, Manuel Muth, Dr. Denis default="joint_trajectory_controller" description="Robot controller to start. Choices are: [forward_position_controller, joint_trajectory_controller]."/> From da156cc4205beb94887d8fa817c55e011db31e1b Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Fri, 5 Apr 2024 10:23:26 +0200 Subject: [PATCH 03/23] add moveit template folder --- templates/moveit/configs_here_ryan | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 templates/moveit/configs_here_ryan diff --git a/templates/moveit/configs_here_ryan b/templates/moveit/configs_here_ryan new file mode 100644 index 00000000..e69de29b From 783734077fcde1e988d259f5ba2f23667221259b Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Fri, 5 Apr 2024 12:26:57 +0200 Subject: [PATCH 04/23] add temporary workspace for testing. delete after. * 'myrobot' is the robot name * contains description, bringup and moveit packages from which moveit templates will be derived --- .gitignore | 3 + myrobot_ws/src/myrobot_bringup/CMakeLists.txt | 22 + .../config/myrobot_controllers.yaml | 81 ++++ .../config/test_goal_publishers_config.yaml | 57 +++ .../myrobot_bringup/launch/myrobot.launch.py | 235 ++++++++++ .../myrobot_bringup/launch/myrobot.launch.xml | 78 +++ ...test_forward_position_controller.launch.py | 46 ++ ...est_forward_position_controller.launch.xml | 32 ++ ...test_joint_trajectory_controller.launch.py | 46 ++ ...est_joint_trajectory_controller.launch.xml | 32 ++ myrobot_ws/src/myrobot_bringup/package.xml | 33 ++ .../src/myrobot_description/CMakeLists.txt | 25 + myrobot_ws/src/myrobot_description/README.md | 61 +++ .../src/myrobot_description/config/.gitkeep | 0 .../launch/view_myrobot.launch.py | 99 ++++ .../launch/view_myrobot.launch.xml | 37 ++ .../meshes/myrobot/collision/.gitkeep | 0 .../meshes/myrobot/visual/.gitkeep | 0 .../src/myrobot_description/package.xml | 26 + .../src/myrobot_description/rviz/myrobot.rviz | 248 ++++++++++ .../test/myrobot_test_urdf_xacro.py | 83 ++++ .../urdf/common/inertials.xacro | 64 +++ .../urdf/common/materials.xacro | 43 ++ .../urdf/myrobot.urdf.xacro | 34 ++ .../myrobot/myrobot_macro.ros2_control.xacro | 143 ++++++ .../urdf/myrobot/myrobot_macro.xacro | 254 ++++++++++ myrobot_ws/src/myrobot_moveit/CMakeLists.txt | 22 + .../myrobot_moveit/config/joint_limits.yaml | 40 ++ .../src/myrobot_moveit/config/kinematics.yaml | 5 + .../config/moveit_controllers.yaml | 24 + .../myrobot_moveit/config/ompl_planning.yaml | 69 +++ .../myrobot_moveit/launch/moveit.launch.py | 209 +++++++++ myrobot_ws/src/myrobot_moveit/package.xml | 26 + .../src/myrobot_moveit/rviz/moveit.rviz | 443 ++++++++++++++++++ .../myrobot_moveit/srdf/myrobot.srdf.xacro | 10 + .../srdf/myrobot_macro.srdf.xacro | 25 + 36 files changed, 2655 insertions(+) create mode 100644 myrobot_ws/src/myrobot_bringup/CMakeLists.txt create mode 100644 myrobot_ws/src/myrobot_bringup/config/myrobot_controllers.yaml create mode 100644 myrobot_ws/src/myrobot_bringup/config/test_goal_publishers_config.yaml create mode 100644 myrobot_ws/src/myrobot_bringup/launch/myrobot.launch.py create mode 100644 myrobot_ws/src/myrobot_bringup/launch/myrobot.launch.xml create mode 100644 myrobot_ws/src/myrobot_bringup/launch/test_forward_position_controller.launch.py create mode 100644 myrobot_ws/src/myrobot_bringup/launch/test_forward_position_controller.launch.xml create mode 100644 myrobot_ws/src/myrobot_bringup/launch/test_joint_trajectory_controller.launch.py create mode 100644 myrobot_ws/src/myrobot_bringup/launch/test_joint_trajectory_controller.launch.xml create mode 100644 myrobot_ws/src/myrobot_bringup/package.xml create mode 100644 myrobot_ws/src/myrobot_description/CMakeLists.txt create mode 100644 myrobot_ws/src/myrobot_description/README.md create mode 100644 myrobot_ws/src/myrobot_description/config/.gitkeep create mode 100755 myrobot_ws/src/myrobot_description/launch/view_myrobot.launch.py create mode 100644 myrobot_ws/src/myrobot_description/launch/view_myrobot.launch.xml create mode 100644 myrobot_ws/src/myrobot_description/meshes/myrobot/collision/.gitkeep create mode 100644 myrobot_ws/src/myrobot_description/meshes/myrobot/visual/.gitkeep create mode 100644 myrobot_ws/src/myrobot_description/package.xml create mode 100644 myrobot_ws/src/myrobot_description/rviz/myrobot.rviz create mode 100644 myrobot_ws/src/myrobot_description/test/myrobot_test_urdf_xacro.py create mode 100644 myrobot_ws/src/myrobot_description/urdf/common/inertials.xacro create mode 100644 myrobot_ws/src/myrobot_description/urdf/common/materials.xacro create mode 100644 myrobot_ws/src/myrobot_description/urdf/myrobot.urdf.xacro create mode 100644 myrobot_ws/src/myrobot_description/urdf/myrobot/myrobot_macro.ros2_control.xacro create mode 100644 myrobot_ws/src/myrobot_description/urdf/myrobot/myrobot_macro.xacro create mode 100644 myrobot_ws/src/myrobot_moveit/CMakeLists.txt create mode 100644 myrobot_ws/src/myrobot_moveit/config/joint_limits.yaml create mode 100644 myrobot_ws/src/myrobot_moveit/config/kinematics.yaml create mode 100644 myrobot_ws/src/myrobot_moveit/config/moveit_controllers.yaml create mode 100644 myrobot_ws/src/myrobot_moveit/config/ompl_planning.yaml create mode 100644 myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py create mode 100644 myrobot_ws/src/myrobot_moveit/package.xml create mode 100644 myrobot_ws/src/myrobot_moveit/rviz/moveit.rviz create mode 100644 myrobot_ws/src/myrobot_moveit/srdf/myrobot.srdf.xacro create mode 100644 myrobot_ws/src/myrobot_moveit/srdf/myrobot_macro.srdf.xacro diff --git a/.gitignore b/.gitignore index 229ecfad..41baf599 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,4 @@ docs/_build/* +myrobot_ws/build/* +myrobot_ws/install/* +myrobot_ws/log/* \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_bringup/CMakeLists.txt b/myrobot_ws/src/myrobot_bringup/CMakeLists.txt new file mode 100644 index 00000000..849db9ca --- /dev/null +++ b/myrobot_ws/src/myrobot_bringup/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.8) +project(myrobot_bringup) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +install( + DIRECTORY config launch + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) +endif() + +ament_package() diff --git a/myrobot_ws/src/myrobot_bringup/config/myrobot_controllers.yaml b/myrobot_ws/src/myrobot_bringup/config/myrobot_controllers.yaml new file mode 100644 index 00000000..3c984fbd --- /dev/null +++ b/myrobot_ws/src/myrobot_bringup/config/myrobot_controllers.yaml @@ -0,0 +1,81 @@ +# Copyright (c) 20224 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. + +# +# Source of this file are templates in +# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +# +# Author: Dr. Denis +# + +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + forward_position_controller: # delete entry if controller is not applicable + type: forward_command_controller/ForwardCommandController + + forward_velocity_controller: # delete entry if controller is not applicable + type: forward_command_controller/ForwardCommandController + + joint_trajectory_controller: # delete entry if controller is not applicable + type: joint_trajectory_controller/JointTrajectoryController + +forward_position_controller: # delete entry if controller is not applicable + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + interface_name: position + +forward_velocity_controller: # delete entry if controller is not applicable + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + interface_name: velocity + +joint_trajectory_controller: # delete entry if controller is not applicable + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + + command_interfaces: + - position + state_interfaces: + - position + + state_publish_rate: 50.0 # Defaults to 50 + action_monitor_rate: 20.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/myrobot_ws/src/myrobot_bringup/config/test_goal_publishers_config.yaml b/myrobot_ws/src/myrobot_bringup/config/test_goal_publishers_config.yaml new file mode 100644 index 00000000..1de2c039 --- /dev/null +++ b/myrobot_ws/src/myrobot_bringup/config/test_goal_publishers_config.yaml @@ -0,0 +1,57 @@ +# Copyright (c) 20224 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. + +# +# Source of this file are templates in +# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +# + +publisher_forward_position_controller: + ros__parameters: + + controller_name: "forward_position_controller" + wait_sec_between_publish: 5 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785, 0.785, 0.785, 0.785, 0.785] + pos2: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + pos3: [-0.785, -0.785, -0.785, -0.785, -0.785, -0.785] + pos4: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + +publisher_joint_trajectory_controller: + ros__parameters: + + controller_name: "joint_trajectory_controller" + wait_sec_between_publish: 6 + repeat_the_same_goal: 1 # useful to simulate continuous inputs + + goal_time_from_start: 3.0 + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: + positions: [0.785, 0.785, 0.785, 0.785, 0.785, 0.785] + pos2: + positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + pos3: + positions: [-0.785, -0.785, -0.785, -0.785, -0.785, -0.785] + pos4: + positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 diff --git a/myrobot_ws/src/myrobot_bringup/launch/myrobot.launch.py b/myrobot_ws/src/myrobot_bringup/launch/myrobot.launch.py new file mode 100644 index 00000000..03eb6a16 --- /dev/null +++ b/myrobot_ws/src/myrobot_bringup/launch/myrobot.launch.py @@ -0,0 +1,235 @@ +# Copyright (c) 2024, 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. + +# +# Source of this file are templates in +# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +# +# Author: Dr. Denis +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction +from launch.event_handlers import OnProcessExit, OnProcessStart +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( + "runtime_config_package", + default_value="myrobot_bringup", + description='Package with the controller\'s configuration in "config" folder. \ + Usually the argument is not set, it enables use of a custom setup.', + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="myrobot_controllers.yaml", + description="YAML file with the controllers configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="myrobot_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="myrobot.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for \ + multi-robot setup. If changed than also joint names in the controllers' configuration \ + have to be updated.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="true", + description="Start robot with mock hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable mock command interfaces for sensors used for simple simulations. \ + Used only if 'use_mock_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_controller", + default_value="forward_position_controller", + choices=["forward_position_controller", "joint_trajectory_controller"], + description="Robot controller to start.", + ) + ) + + # Initialize Arguments + runtime_config_package = LaunchConfiguration("runtime_config_package") + controllers_file = LaunchConfiguration("controllers_file") + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + prefix = LaunchConfiguration("prefix") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + robot_controller = LaunchConfiguration("robot_controller") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", description_file] + ), + " ", + "prefix:=", + prefix, + " ", + "use_mock_hardware:=", + use_mock_hardware, + " ", + "mock_sensor_commands:=", + mock_sensor_commands, + " ", + ] + ) + + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [FindPackageShare(runtime_config_package), "config", controllers_file] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "rviz", "myrobot.rviz"] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + output="both", + parameters=[robot_description, robot_controllers], + ) + robot_state_pub_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], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + robot_controller_names = [robot_controller] + robot_controller_spawners = [] + for controller in robot_controller_names: + robot_controller_spawners += [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller, "-c", "/controller_manager"], + ) + ] + + inactive_robot_controller_names = ["add_some_controller_name"] + inactive_robot_controller_spawners = [] + for controller in inactive_robot_controller_names: + inactive_robot_controller_spawners += [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller, "-c", "/controller_manager", "--inactive"], + ) + ] + + # Delay loading and activation of `joint_state_broadcaster` after start of ros2_control_node + delay_joint_state_broadcaster_spawner_after_ros2_control_node = RegisterEventHandler( + event_handler=OnProcessStart( + target_action=control_node, + on_start=[ + TimerAction( + period=3.0, + actions=[joint_state_broadcaster_spawner], + ), + ], + ) + ) + + # Delay loading and activation of robot_controller_names after `joint_state_broadcaster` + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner = [] + for i, controller in enumerate(robot_controller_spawners): + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner += [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=robot_controller_spawners[i - 1] + if i > 0 + else joint_state_broadcaster_spawner, + on_exit=[controller], + ) + ) + ] + + # Delay start of inactive_robot_controller_names after other controllers + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner = [] + for i, controller in enumerate(inactive_robot_controller_spawners): + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner += [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=inactive_robot_controller_spawners[i - 1] + if i > 0 + else robot_controller_spawners[-1], + on_exit=[controller], + ) + ) + ] + + return LaunchDescription( + declared_arguments + + [ + control_node, + robot_state_pub_node, + rviz_node, + delay_joint_state_broadcaster_spawner_after_ros2_control_node, + ] + + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner + + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner + ) diff --git a/myrobot_ws/src/myrobot_bringup/launch/myrobot.launch.xml b/myrobot_ws/src/myrobot_bringup/launch/myrobot.launch.xml new file mode 100644 index 00000000..9bd3029e --- /dev/null +++ b/myrobot_ws/src/myrobot_bringup/launch/myrobot.launch.xml @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/myrobot_ws/src/myrobot_bringup/launch/test_forward_position_controller.launch.py b/myrobot_ws/src/myrobot_bringup/launch/test_forward_position_controller.launch.py new file mode 100644 index 00000000..f4beb6dd --- /dev/null +++ b/myrobot_ws/src/myrobot_bringup/launch/test_forward_position_controller.launch.py @@ -0,0 +1,46 @@ +# Copyright (c) 2024, 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. + +# +# Source of this file are templates in +# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +# +# Author: Dr. Denis +# + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + position_goals = PathJoinSubstitution( + [FindPackageShare("myrobot_bringup"), "config", "test_goal_publishers_config.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + name="publisher_forward_position_controller", + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + ] + ) diff --git a/myrobot_ws/src/myrobot_bringup/launch/test_forward_position_controller.launch.xml b/myrobot_ws/src/myrobot_bringup/launch/test_forward_position_controller.launch.xml new file mode 100644 index 00000000..bcabf90a --- /dev/null +++ b/myrobot_ws/src/myrobot_bringup/launch/test_forward_position_controller.launch.xml @@ -0,0 +1,32 @@ + + + + + + + + + diff --git a/myrobot_ws/src/myrobot_bringup/launch/test_joint_trajectory_controller.launch.py b/myrobot_ws/src/myrobot_bringup/launch/test_joint_trajectory_controller.launch.py new file mode 100644 index 00000000..77570322 --- /dev/null +++ b/myrobot_ws/src/myrobot_bringup/launch/test_joint_trajectory_controller.launch.py @@ -0,0 +1,46 @@ +# Copyright (c) 2024, 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. + +# +# Source of this file are templates in +# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +# +# Author: Dr. Denis +# + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + position_goals = PathJoinSubstitution( + [FindPackageShare("myrobot_bringup"), "config", "test_goal_publishers_config.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="ros2_controllers_test_nodes", + executable="publisher_joint_trajectory_controller", + name="publisher_joint_trajectory_controller", + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + ] + ) diff --git a/myrobot_ws/src/myrobot_bringup/launch/test_joint_trajectory_controller.launch.xml b/myrobot_ws/src/myrobot_bringup/launch/test_joint_trajectory_controller.launch.xml new file mode 100644 index 00000000..9ccded93 --- /dev/null +++ b/myrobot_ws/src/myrobot_bringup/launch/test_joint_trajectory_controller.launch.xml @@ -0,0 +1,32 @@ + + + + + + + + + diff --git a/myrobot_ws/src/myrobot_bringup/package.xml b/myrobot_ws/src/myrobot_bringup/package.xml new file mode 100644 index 00000000..da8711f4 --- /dev/null +++ b/myrobot_ws/src/myrobot_bringup/package.xml @@ -0,0 +1,33 @@ + + + + myrobot_bringup + 0.0.0 + testing bringup template + Nibanovic + Copyright (c) 2024, Stogl Robotics + + ament_cmake + + myrobot_description + + controller_manager + + forward_command_controller + + joint_state_broadcaster + + joint_trajectory_controller + + robot_state_publisher + + ros2_controllers_test_nodes + + rviz2 + + xacro + + + ament_cmake + + diff --git a/myrobot_ws/src/myrobot_description/CMakeLists.txt b/myrobot_ws/src/myrobot_description/CMakeLists.txt new file mode 100644 index 00000000..488fbe58 --- /dev/null +++ b/myrobot_ws/src/myrobot_description/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.8) +project(myrobot_description) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +install( + DIRECTORY config launch meshes rviz urdf test + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) +find_package(ament_cmake_pytest REQUIRED) + + ament_add_pytest_test(test_myrobot_urdf_xacro test/myrobot_test_urdf_xacro.py) +endif() + +ament_package() diff --git a/myrobot_ws/src/myrobot_description/README.md b/myrobot_ws/src/myrobot_description/README.md new file mode 100644 index 00000000..409cb751 --- /dev/null +++ b/myrobot_ws/src/myrobot_description/README.md @@ -0,0 +1,61 @@ +myrobot_description\n\n + +Descriptions, meshes, and visualization files for the robots and environments. Corresponding launch and test files are also stored here. + +The structure and files in this package are generated using [RosTeamWorkspace script setup-robot-description](https://rtw.stoglrobotics.de/master/use-cases/ros_packages/setup_robot_description_package.html). You can use the same script to generate initial files for other robots. + +## General details about robot description packages + +This description package follows, as much as possible, the recommendations from [ROS-Industrial Consortium about robot support packages](https://wiki.ros.org/Industrial/Tutorials/WorkingWithRosIndustrialRobotSupportPackages). +The general package structure is the following: + +``` +_description/ # Robot's description files +├── [CMakeLists.txt] # if ament_cmake is used (recommended) +├── package.xml +├── [setup.py] # if amend_python is used +├── [setup.cfg] # if amend_python is used +├── config/ # general YAML files for a robot +│ └── _.yaml +├── launch/ # launch files related to testing robots' description +│ └── test__description.launch.py +├── meshes/ # meshes used in _macro.urdf.xacro +│ ├── collision +│ │ └── # meshes are sorted by robot name or model +│ │ ├── .stl +│ │ └── ... +│ └── visual +│ └── +│ ├── .dae +│ └── ... +├── rviz/ # rviz display configurations +│ └── _default.rviz +└── urdf/ # URDF file for the robot + ├── common.xacro # Common XACRO definitions + ├── .urdf.xacro # Main URDF for a robot - loads macro and other files + └── + ├── _macro.xacro # Macro file of the robot - can add prefix, define origin, etc. + └── _macro.ros2_control.xacro # URDF-part used to configure ros2_control + +``` + +### Testing the validity of robot description + +1. Go to the root of your workspace folder (there where `src`, `build`, `install` and `log` files are). +2. Install the package by calling `colcon build --symlink-install --packages-select myrobot_description` +3. (Re-)Source environment `source install/setup.bash` + + +> **NOTE:** If you use [RosTeamWorkspace (RTW)](https://rtw.stoglrobotics.de) than instead of the previous three steps, use `cb myrobot_description` command. + +Now, launch description test: +``` +ros2 launch myrobot_description view_myrobot.launch.xml +``` +or +``` +ros2 launch myrobot_description view_myrobot.launch.py +``` + +If there are no issues with the description, two windows are opened: `rviz2` and `Joint State Publisher`. +Rviz2 visualizes the robot's state and Joint state Publisher to changes joint values using sliders or generates random but valid configurations. diff --git a/myrobot_ws/src/myrobot_description/config/.gitkeep b/myrobot_ws/src/myrobot_description/config/.gitkeep new file mode 100644 index 00000000..e69de29b diff --git a/myrobot_ws/src/myrobot_description/launch/view_myrobot.launch.py b/myrobot_ws/src/myrobot_description/launch/view_myrobot.launch.py new file mode 100755 index 00000000..c39d185a --- /dev/null +++ b/myrobot_ws/src/myrobot_description/launch/view_myrobot.launch.py @@ -0,0 +1,99 @@ +# Copyright (c) 2024, 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. + +# +# Source of this file is https://github.com/StoglRobotics/ros_team_workspace repository. +# +# Author: Dr. Denis +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="myrobot_description", + description="Description package of the myrobot. Usually the argument is not set, \ + it enables use of a custom description.", + ) + ) + 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") + prefix = LaunchConfiguration("prefix") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", "myrobot.urdf.xacro"] + ), + " ", + "prefix:=", + prefix, + " ", + ] + ) + + robot_description = {"robot_description": robot_description_content} + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "rviz", "myrobot.rviz"] + ) + + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + ) + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + return LaunchDescription( + declared_arguments + + [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node, + ] + ) diff --git a/myrobot_ws/src/myrobot_description/launch/view_myrobot.launch.xml b/myrobot_ws/src/myrobot_description/launch/view_myrobot.launch.xml new file mode 100644 index 00000000..1363064c --- /dev/null +++ b/myrobot_ws/src/myrobot_description/launch/view_myrobot.launch.xml @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + diff --git a/myrobot_ws/src/myrobot_description/meshes/myrobot/collision/.gitkeep b/myrobot_ws/src/myrobot_description/meshes/myrobot/collision/.gitkeep new file mode 100644 index 00000000..e69de29b diff --git a/myrobot_ws/src/myrobot_description/meshes/myrobot/visual/.gitkeep b/myrobot_ws/src/myrobot_description/meshes/myrobot/visual/.gitkeep new file mode 100644 index 00000000..e69de29b diff --git a/myrobot_ws/src/myrobot_description/package.xml b/myrobot_ws/src/myrobot_description/package.xml new file mode 100644 index 00000000..819bcf68 --- /dev/null +++ b/myrobot_ws/src/myrobot_description/package.xml @@ -0,0 +1,26 @@ + + + + myrobot_description + 0.0.0 + Test package + Nibanovic + Copyright (c) 2024, Stogl Robotics + + ament_cmake + + joint_state_publisher_gui + robot_state_publisher + rviz2 + xacro + + ament_cmake_pytest + launch_testing_ament_cmake + launch_testing_ros + liburdfdom-tools + xacro + + + ament_cmake + + diff --git a/myrobot_ws/src/myrobot_description/rviz/myrobot.rviz b/myrobot_ws/src/myrobot_description/rviz/myrobot.rviz new file mode 100644 index 00000000..5cfdaad3 --- /dev/null +++ b/myrobot_ws/src/myrobot_description/rviz/myrobot.rviz @@ -0,0 +1,248 @@ + +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /TF1 + Splitter Ratio: 0.5 + Tree Height: 756 + - 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: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base: + Value: true + base_link: + Value: true + flange: + Value: true + link_1: + Value: true + link_2: + Value: true + link_3: + Value: true + link_4: + Value: true + link_5: + Value: true + link_6: + Value: true + tool0: + Value: true + world: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + base_link: + base: + {} + link_1: + link_2: + link_3: + link_4: + link_5: + link_6: + tool0: + flange: + {} + Update Interval: 0 + Value: 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: 2.962221622467041 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.34074074029922485 + Y: -0.6165840029716492 + Z: 0.3352876305580139 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3553987741470337 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.713573932647705 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1027 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000398fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004400000398000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000011000000398fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004400000398000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005040000039800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 24 diff --git a/myrobot_ws/src/myrobot_description/test/myrobot_test_urdf_xacro.py b/myrobot_ws/src/myrobot_description/test/myrobot_test_urdf_xacro.py new file mode 100644 index 00000000..183ac77f --- /dev/null +++ b/myrobot_ws/src/myrobot_description/test/myrobot_test_urdf_xacro.py @@ -0,0 +1,83 @@ +# Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +# 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 FZI Forschungszentrum Informatik 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. + +# +# Source of this file is https://github.com/StoglRobotics/ros_team_workspace repository. +# Modified from tests in https://github.com/UniversalRobots/Universal_Robots_ROS2_Description +# +# Author: Lukas Sackewitz +# Author (template): Manuel Muth + +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 = "myrobot_description" + description_file = "myrobot.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/myrobot_ws/src/myrobot_description/urdf/common/inertials.xacro b/myrobot_ws/src/myrobot_description/urdf/common/inertials.xacro new file mode 100644 index 00000000..c1a7d059 --- /dev/null +++ b/myrobot_ws/src/myrobot_description/urdf/common/inertials.xacro @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/myrobot_ws/src/myrobot_description/urdf/common/materials.xacro b/myrobot_ws/src/myrobot_description/urdf/common/materials.xacro new file mode 100644 index 00000000..359cdbe9 --- /dev/null +++ b/myrobot_ws/src/myrobot_description/urdf/common/materials.xacro @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/myrobot_ws/src/myrobot_description/urdf/myrobot.urdf.xacro b/myrobot_ws/src/myrobot_description/urdf/myrobot.urdf.xacro new file mode 100644 index 00000000..125fb915 --- /dev/null +++ b/myrobot_ws/src/myrobot_description/urdf/myrobot.urdf.xacro @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/myrobot_ws/src/myrobot_description/urdf/myrobot/myrobot_macro.ros2_control.xacro b/myrobot_ws/src/myrobot_description/urdf/myrobot/myrobot_macro.ros2_control.xacro new file mode 100644 index 00000000..8b9772a5 --- /dev/null +++ b/myrobot_ws/src/myrobot_description/urdf/myrobot/myrobot_macro.ros2_control.xacro @@ -0,0 +1,143 @@ + + + + + + + + + mock_components/GenericSystem + ${mock_sensor_commands} + + + gazebo_ros2_control/GazeboSystem + + + gz_ros2_control/GazeboSimSystem + + + robot_hardware_inteface/RobotHardwareInteface + + + + + -1 + 1 + + + 0.0 + + + + + + + -1 + 1 + + + 0.0 + + + + + + + -1 + 1 + + + 0.0 + + + + + + + -1 + 1 + + + 0.0 + + + + + + + -1 + 1 + + + 0.0 + + + + + + + -1 + 1 + + + 0.0 + + + + + + + + + + + + tool0 + -100 + 100 + -100 + 100 + -200 + 200 + -10 + 10 + -10 + 10 + -15 + 15 + + + + + + + + + + ${simulation_controllers} + + + + + + + + + + + ${simulation_controllers} + ${prefix}controller_manager + + + + + + diff --git a/myrobot_ws/src/myrobot_description/urdf/myrobot/myrobot_macro.xacro b/myrobot_ws/src/myrobot_description/urdf/myrobot/myrobot_macro.xacro new file mode 100644 index 00000000..e078b19f --- /dev/null +++ b/myrobot_ws/src/myrobot_description/urdf/myrobot/myrobot_macro.xacro @@ -0,0 +1,254 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/myrobot_ws/src/myrobot_moveit/CMakeLists.txt b/myrobot_ws/src/myrobot_moveit/CMakeLists.txt new file mode 100644 index 00000000..31300cca --- /dev/null +++ b/myrobot_ws/src/myrobot_moveit/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.8) +project(myrobot_moveit) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +install( + DIRECTORY config launch rviz srdf + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) +endif() + +ament_package() diff --git a/myrobot_ws/src/myrobot_moveit/config/joint_limits.yaml b/myrobot_ws/src/myrobot_moveit/config/joint_limits.yaml new file mode 100644 index 00000000..92ff4b51 --- /dev/null +++ b/myrobot_ws/src/myrobot_moveit/config/joint_limits.yaml @@ -0,0 +1,40 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + joint1: + has_velocity_limits: true + max_velocity: 1.7453292519943295 + has_acceleration_limits: true + max_acceleration: 5.0 + joint2: + has_velocity_limits: true + max_velocity: 1.5707963267948966 + has_acceleration_limits: true + max_acceleration: 5.0 + joint3: + has_velocity_limits: true + max_velocity: 1.5707963267948966 + has_acceleration_limits: true + max_acceleration: 5.0 + joint4: + has_velocity_limits: true + max_velocity: 2.9670597283903604 + has_acceleration_limits: true + max_acceleration: 5.0 + joint5: + has_velocity_limits: true + max_velocity: 2.0943951023931953 + has_acceleration_limits: true + max_acceleration: 5.0 + joint6: + has_velocity_limits: true + max_velocity: 3.3161255787892263 + has_acceleration_limits: true + max_acceleration: 5.0 \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_moveit/config/kinematics.yaml b/myrobot_ws/src/myrobot_moveit/config/kinematics.yaml new file mode 100644 index 00000000..59e9d690 --- /dev/null +++ b/myrobot_ws/src/myrobot_moveit/config/kinematics.yaml @@ -0,0 +1,5 @@ +myrobot_manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_moveit/config/moveit_controllers.yaml b/myrobot_ws/src/myrobot_moveit/config/moveit_controllers.yaml new file mode 100644 index 00000000..d2b3042f --- /dev/null +++ b/myrobot_ws/src/myrobot_moveit/config/moveit_controllers.yaml @@ -0,0 +1,24 @@ +# MoveIt uses this configuration for controller management +moveit_manage_controllers: false +trajectory_execution: + allowed_execution_duration_scaling: 1.2 + allowed_goal_duration_margin: 0.5 + allowed_start_tolerance: 0.01 + +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - joint_trajectory_controller + + joint_trajectory_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_moveit/config/ompl_planning.yaml b/myrobot_ws/src/myrobot_moveit/config/ompl_planning.yaml new file mode 100644 index 00000000..645cf70c --- /dev/null +++ b/myrobot_ws/src/myrobot_moveit/config/ompl_planning.yaml @@ -0,0 +1,69 @@ +planning_plugins: + - ompl_interface/OMPLPlanner +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath +myrobot_manipulator: + planner_configs: + SBLkConfigDefault: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ESTkConfigDefault: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECEkConfigDefault: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECEkConfigDefault: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRTkConfigDefault: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstarkConfigDefault: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRTkConfigDefault: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() + PRMkConfigDefault: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: + type: geometric::PRMstar + ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE + #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) + longest_valid_segment_fraction: 0.01 \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py b/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py new file mode 100644 index 00000000..22760f91 --- /dev/null +++ b/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py @@ -0,0 +1,209 @@ +import os +import yaml + +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import OpaqueFunction + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path) as file: + return yaml.safe_load(file) + except OSError: # parent of IOError, OSError *and* WindowsError where available + return None + +def launch_setup(context, *args, **kwargs): + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + moveit_package = LaunchConfiguration("moveit_package") + semantic_description_file = LaunchConfiguration("semantic_description_file") + prefix = LaunchConfiguration("prefix") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + + # -------------------------------------------------------# + # MoveIt2 MoveGroup setup # + # -------------------------------------------------------# + + # URDF + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", description_file] + ), + " ", + "prefix:=", + prefix, + " ", + "use_mock_hardware:=", + use_mock_hardware, + " ", + "mock_sensor_commands:=", + mock_sensor_commands, + " ", + ] + ) + robot_description = {"robot_description": robot_description_content.perform(context)} + + # SRDF + robot_description_semantic_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(moveit_package), "srdf", semantic_description_file] + ), + + ] + ) + robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content.perform(context)} + + # kinematics + kinematics_yaml = load_yaml("myrobot_moveit", "config/kinematics.yaml") + robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} + + # limits + limits_yaml = load_yaml("myrobot_moveit", "config/joint_limits.yaml") + robot_description_planning = {"robot_description_planning": limits_yaml} + + # planning + ompl_planning_plugin_yaml = load_yaml("myrobot_moveit", "config/ompl_planning.yaml") + ompl_planning_pipeline_config = {"move_group": ompl_planning_plugin_yaml} + + # load Moveit2 controllers + moveit_controllers = load_yaml("myrobot_moveit", "config/moveit_controllers.yaml") + + # configure PlanningSceneMonitor + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + } + + # Load ExecuteTaskSolutionCapability so we can execute found solutions in simulation + move_group_capabilities = { + "capabilities": "move_group/MoveGroupExecuteTrajectoryAction" + } + + # Start the actual move_group node/action server + # -------------------------------------------------------# + # Move Group Node # + # -------------------------------------------------------# + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + robot_description_kinematics, + robot_description_planning, + ompl_planning_pipeline_config, + moveit_controllers, + planning_scene_monitor_parameters, + move_group_capabilities + ], + ) + + # RViz + rviz_directory = os.path.join( + get_package_share_directory( + f"{moveit_package.perform(context)}" + ), "rviz" + ) + rviz_config = os.path.join(rviz_directory, "moveit.rviz") + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config], + parameters=[ + robot_description, + robot_description_semantic, + robot_description_kinematics, + ompl_planning_pipeline_config, + ], + ) + + return [ + move_group_node, + rviz_node, + ] + + +def generate_launch_description(): + # + # ------------- Declare arguments ------------- # + # + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="myrobot_description", + description="Package with the robot URDF/XACRO files. \ + Usually the argument is not set, it enables use of a custom setup.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="myrobot.urdf.xacro", + description="URDF/XACRO description file \ + Usually the argument is not set, it enables use of a custom setup.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "moveit_package", + default_value="myrobot_moveit", + description="MoveIt config package with robot SRDF/XACRO files. \ + Usually the argument is not set, it enables use of a custom setup.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "semantic_description_file", + default_value="myrobot.srdf.xacro", + description="MoveIt SRDF/XACRO description file with the robot.. \ + Usually the argument is not set, it enables use of a custom setup.", + ) + ) + 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.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="true", + description="Start robot with mock hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable mock command interfaces for sensors used for simple simulations. \ + Used only if 'use_mock_hardware' parameter is true.", + ) + ) + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/myrobot_ws/src/myrobot_moveit/package.xml b/myrobot_ws/src/myrobot_moveit/package.xml new file mode 100644 index 00000000..4f33e922 --- /dev/null +++ b/myrobot_ws/src/myrobot_moveit/package.xml @@ -0,0 +1,26 @@ + + + + myrobot_moveit + 0.0.0 + test moveit config template + Nibanovic + Copyright (c) 2024, Stogl Robotics + + ament_cmake + + joint_state_publisher + joint_state_publisher_gui + tf2_ros + xacro + + + moveit_ros_move_group + moveit_kinematics + moveit_planners + moveit_simple_controller_manager + + + ament_cmake + + diff --git a/myrobot_ws/src/myrobot_moveit/rviz/moveit.rviz b/myrobot_ws/src/myrobot_moveit/rviz/moveit.rviz new file mode 100644 index 00000000..3177bc9a --- /dev/null +++ b/myrobot_ws/src/myrobot_moveit/rviz/moveit.rviz @@ -0,0 +1,443 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /RobotModel1/Links1/link61 + - /TF1/Tree1 + - /TF1/Tree1/world1 + - /TF1/Tree1/world1/base_link1 + - /TF1/Tree1/world1/base_link1/link11 + - /MotionPlanning1 + Splitter Ratio: 0.5 + Tree Height: 438 + - 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: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base: + Value: true + base_link: + Value: true + flange: + Value: true + link1: + Value: true + link2: + Value: true + link3: + Value: true + link4: + Value: true + link5: + Value: true + link6: + Value: true + tool0: + Value: true + world: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + base_link: + base: + {} + link1: + link2: + link3: + link4: + link5: + link6: + tool0: + flange: + {} + Update Interval: 0 + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: my_robot_manipulator + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + 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 + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + 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: 2.962221622467041 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.8273206949234009 + Y: -0.11996828764677048 + Z: 0.4445207118988037 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.38039883971214294 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.253565788269043 + Saved: ~ +Window Geometry: + "": + collapsed: false + " - Trajectory Slider": + collapsed: false + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f30000039efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d00000210000000e60100001cfa000000010100000002fb0000000a005600690065007700730100000000ffffffff0000010000fffffffb000000100044006900730070006c0061007900730100000000000001f30000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fbffffffff0100000253000001880000017d00ffffff00000001000001100000039efc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005870000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1366 + Y: 27 diff --git a/myrobot_ws/src/myrobot_moveit/srdf/myrobot.srdf.xacro b/myrobot_ws/src/myrobot_moveit/srdf/myrobot.srdf.xacro new file mode 100644 index 00000000..677264bc --- /dev/null +++ b/myrobot_ws/src/myrobot_moveit/srdf/myrobot.srdf.xacro @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/myrobot_ws/src/myrobot_moveit/srdf/myrobot_macro.srdf.xacro b/myrobot_ws/src/myrobot_moveit/srdf/myrobot_macro.srdf.xacro new file mode 100644 index 00000000..4224afc9 --- /dev/null +++ b/myrobot_ws/src/myrobot_moveit/srdf/myrobot_macro.srdf.xacro @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + From d0bce1b45094da5a7e8fb4c99680223cf9d2c4fe Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Fri, 5 Apr 2024 14:50:26 +0200 Subject: [PATCH 05/23] combine yaml files into one, simplify launch file --- myrobot_ws/src/myrobot_moveit/config/all.yaml | 151 ++++++++++++++++++ .../{config => config_old}/joint_limits.yaml | 0 .../{config => config_old}/kinematics.yaml | 0 .../moveit_controllers.yaml | 0 .../{config => config_old}/ompl_planning.yaml | 0 .../myrobot_moveit/launch/moveit.launch.py | 71 ++------ 6 files changed, 168 insertions(+), 54 deletions(-) create mode 100644 myrobot_ws/src/myrobot_moveit/config/all.yaml rename myrobot_ws/src/myrobot_moveit/{config => config_old}/joint_limits.yaml (100%) rename myrobot_ws/src/myrobot_moveit/{config => config_old}/kinematics.yaml (100%) rename myrobot_ws/src/myrobot_moveit/{config => config_old}/moveit_controllers.yaml (100%) rename myrobot_ws/src/myrobot_moveit/{config => config_old}/ompl_planning.yaml (100%) diff --git a/myrobot_ws/src/myrobot_moveit/config/all.yaml b/myrobot_ws/src/myrobot_moveit/config/all.yaml new file mode 100644 index 00000000..12256ddb --- /dev/null +++ b/myrobot_ws/src/myrobot_moveit/config/all.yaml @@ -0,0 +1,151 @@ +/**: + ros__parameters: + # from launch file + capabilities: move_group/MoveGroupExecuteTrajectoryAction + publish_planning_scene: True + publish_geometry_updates: True + publish_state_updates: True + publish_transforms_updates: True + + # kinematics + robot_description_kinematics: + myrobot_manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 + + # joint limits + robot_description_planning: + default_velocity_scaling_factor: 0.1 + default_acceleration_scaling_factor: 0.1 + + # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] + # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] + joint_limits: + joint1: + has_velocity_limits: true + max_velocity: 1.7453292519943295 + has_acceleration_limits: true + max_acceleration: 5.0 + joint2: + has_velocity_limits: true + max_velocity: 1.5707963267948966 + has_acceleration_limits: true + max_acceleration: 5.0 + joint3: + has_velocity_limits: true + max_velocity: 1.5707963267948966 + has_acceleration_limits: true + max_acceleration: 5.0 + joint4: + has_velocity_limits: true + max_velocity: 2.9670597283903604 + has_acceleration_limits: true + max_acceleration: 5.0 + joint5: + has_velocity_limits: true + max_velocity: 2.0943951023931953 + has_acceleration_limits: true + max_acceleration: 5.0 + joint6: + has_velocity_limits: true + max_velocity: 3.3161255787892263 + has_acceleration_limits: true + max_acceleration: 5.0 + + # Moveit Controllers + moveit_manage_controllers: false + trajectory_execution: + allowed_execution_duration_scaling: 1.2 + allowed_goal_duration_margin: 0.5 + allowed_start_tolerance: 0.01 + + moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + moveit_simple_controller_manager: + controller_names: + - joint_trajectory_controller + + joint_trajectory_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + + # OMPL planning + move_group: + planning_plugins: + - ompl_interface/OMPLPlanner + request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision + response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath + myrobot_manipulator: + planner_configs: + SBLkConfigDefault: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ESTkConfigDefault: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECEkConfigDefault: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECEkConfigDefault: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRTkConfigDefault: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstarkConfigDefault: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRTkConfigDefault: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() + PRMkConfigDefault: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: + type: geometric::PRMstar + ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE + #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) + longest_valid_segment_fraction: 0.01 \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_moveit/config/joint_limits.yaml b/myrobot_ws/src/myrobot_moveit/config_old/joint_limits.yaml similarity index 100% rename from myrobot_ws/src/myrobot_moveit/config/joint_limits.yaml rename to myrobot_ws/src/myrobot_moveit/config_old/joint_limits.yaml diff --git a/myrobot_ws/src/myrobot_moveit/config/kinematics.yaml b/myrobot_ws/src/myrobot_moveit/config_old/kinematics.yaml similarity index 100% rename from myrobot_ws/src/myrobot_moveit/config/kinematics.yaml rename to myrobot_ws/src/myrobot_moveit/config_old/kinematics.yaml diff --git a/myrobot_ws/src/myrobot_moveit/config/moveit_controllers.yaml b/myrobot_ws/src/myrobot_moveit/config_old/moveit_controllers.yaml similarity index 100% rename from myrobot_ws/src/myrobot_moveit/config/moveit_controllers.yaml rename to myrobot_ws/src/myrobot_moveit/config_old/moveit_controllers.yaml diff --git a/myrobot_ws/src/myrobot_moveit/config/ompl_planning.yaml b/myrobot_ws/src/myrobot_moveit/config_old/ompl_planning.yaml similarity index 100% rename from myrobot_ws/src/myrobot_moveit/config/ompl_planning.yaml rename to myrobot_ws/src/myrobot_moveit/config_old/ompl_planning.yaml diff --git a/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py b/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py index 22760f91..743ab70c 100644 --- a/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py +++ b/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py @@ -5,20 +5,9 @@ from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import OpaqueFunction -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path) as file: - return yaml.safe_load(file) - except OSError: # parent of IOError, OSError *and* WindowsError where available - return None - def launch_setup(context, *args, **kwargs): description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") @@ -27,9 +16,10 @@ def launch_setup(context, *args, **kwargs): prefix = LaunchConfiguration("prefix") use_mock_hardware = LaunchConfiguration("use_mock_hardware") mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + use_sim_time = LaunchConfiguration("use_sim_time") # -------------------------------------------------------# - # MoveIt2 MoveGroup setup # + # MoveIt2 MoveGroup setup # # -------------------------------------------------------# # URDF @@ -67,35 +57,10 @@ def launch_setup(context, *args, **kwargs): ) robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content.perform(context)} - # kinematics - kinematics_yaml = load_yaml("myrobot_moveit", "config/kinematics.yaml") - robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} - - # limits - limits_yaml = load_yaml("myrobot_moveit", "config/joint_limits.yaml") - robot_description_planning = {"robot_description_planning": limits_yaml} - - # planning - ompl_planning_plugin_yaml = load_yaml("myrobot_moveit", "config/ompl_planning.yaml") - ompl_planning_pipeline_config = {"move_group": ompl_planning_plugin_yaml} - - # load Moveit2 controllers - moveit_controllers = load_yaml("myrobot_moveit", "config/moveit_controllers.yaml") - - # configure PlanningSceneMonitor - planning_scene_monitor_parameters = { - "publish_planning_scene": True, - "publish_geometry_updates": True, - "publish_state_updates": True, - "publish_transforms_updates": True, - } - - # Load ExecuteTaskSolutionCapability so we can execute found solutions in simulation - move_group_capabilities = { - "capabilities": "move_group/MoveGroupExecuteTrajectoryAction" - } + other_configs = PathJoinSubstitution( + [FindPackageShare(moveit_package), "config", "all.yaml"] + ) - # Start the actual move_group node/action server # -------------------------------------------------------# # Move Group Node # # -------------------------------------------------------# @@ -106,33 +71,24 @@ def launch_setup(context, *args, **kwargs): parameters=[ robot_description, robot_description_semantic, - robot_description_kinematics, - robot_description_planning, - ompl_planning_pipeline_config, - moveit_controllers, - planning_scene_monitor_parameters, - move_group_capabilities + other_configs, + {"use_sim_time": use_sim_time}, ], ) # RViz - rviz_directory = os.path.join( - get_package_share_directory( - f"{moveit_package.perform(context)}" - ), "rviz" + rviz_config = PathJoinSubstitution( + [FindPackageShare(moveit_package), "rviz", "moveit.rviz"] ) - rviz_config = os.path.join(rviz_directory, "moveit.rviz") rviz_node = Node( package="rviz2", executable="rviz2", - name="rviz2", output="log", arguments=["-d", rviz_config], parameters=[ robot_description, robot_description_semantic, - robot_description_kinematics, - ompl_planning_pipeline_config, + other_configs ], ) @@ -203,6 +159,13 @@ def generate_launch_description(): Used only if 'use_mock_hardware' parameter is true.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use simulation clock instead of world clock", + ) + ) return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] From 51717550b1b1124ae87b2f5ecc69711131d2feb2 Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Fri, 5 Apr 2024 14:54:28 +0200 Subject: [PATCH 06/23] add severity level launch argument * rviz severity is hardcoded to WARN * movegroup severity default is INFO --- .../src/myrobot_moveit/launch/moveit.launch.py | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py b/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py index 743ab70c..c1c0f344 100644 --- a/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py +++ b/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py @@ -17,6 +17,7 @@ def launch_setup(context, *args, **kwargs): use_mock_hardware = LaunchConfiguration("use_mock_hardware") mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") use_sim_time = LaunchConfiguration("use_sim_time") + logging_severity = LaunchConfiguration("severity") # -------------------------------------------------------# # MoveIt2 MoveGroup setup # @@ -68,6 +69,7 @@ def launch_setup(context, *args, **kwargs): package="moveit_ros_move_group", executable="move_group", output="screen", + arguments=["--ros-args", "--log-level", logging_severity], parameters=[ robot_description, robot_description_semantic, @@ -84,7 +86,7 @@ def launch_setup(context, *args, **kwargs): package="rviz2", executable="rviz2", output="log", - arguments=["-d", rviz_config], + arguments=["-d", rviz_config, "--ros-args", "--log-level", "WARN"], parameters=[ robot_description, robot_description_semantic, @@ -166,6 +168,14 @@ def generate_launch_description(): description="Use simulation clock instead of world clock", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "severity", + default_value="INFO", + choices=["INFO", "WARN", "ERROR", "DEBUG", "FATAL"], + description="Logging level for the nodes", + ) + ) return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] From a14e8dee142df4e9c3ace4b303adb4b11e1b7229 Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Fri, 5 Apr 2024 15:12:46 +0200 Subject: [PATCH 07/23] separate ompl_planning.yaml config, refactor config --- .../src/myrobot_moveit/config/move_group.yaml | 79 +++++++++++++++++++ .../config/{all.yaml => ompl_planning.yaml} | 79 ------------------- .../config_old/joint_limits.yaml | 40 ---------- .../myrobot_moveit/config_old/kinematics.yaml | 5 -- .../config_old/moveit_controllers.yaml | 24 ------ .../config_old/ompl_planning.yaml | 69 ---------------- .../myrobot_moveit/launch/moveit.launch.py | 21 +++-- 7 files changed, 88 insertions(+), 229 deletions(-) create mode 100644 myrobot_ws/src/myrobot_moveit/config/move_group.yaml rename myrobot_ws/src/myrobot_moveit/config/{all.yaml => ompl_planning.yaml} (64%) delete mode 100644 myrobot_ws/src/myrobot_moveit/config_old/joint_limits.yaml delete mode 100644 myrobot_ws/src/myrobot_moveit/config_old/kinematics.yaml delete mode 100644 myrobot_ws/src/myrobot_moveit/config_old/moveit_controllers.yaml delete mode 100644 myrobot_ws/src/myrobot_moveit/config_old/ompl_planning.yaml diff --git a/myrobot_ws/src/myrobot_moveit/config/move_group.yaml b/myrobot_ws/src/myrobot_moveit/config/move_group.yaml new file mode 100644 index 00000000..0264c560 --- /dev/null +++ b/myrobot_ws/src/myrobot_moveit/config/move_group.yaml @@ -0,0 +1,79 @@ +/**: + ros__parameters: +#-------- Moveit controllers + moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + moveit_simple_controller_manager: + controller_names: + - joint_trajectory_controller # add or remove controllers as necessary + + joint_trajectory_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + +#-------- kinematics + robot_description_kinematics: + myrobot_manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 + +#-------- joint limits + robot_description_planning: + default_velocity_scaling_factor: 0.1 + default_acceleration_scaling_factor: 0.1 + # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] + # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] + joint_limits: + joint1: + has_velocity_limits: true + max_velocity: 1.7453292519943295 + has_acceleration_limits: true + max_acceleration: 5.0 + joint2: + has_velocity_limits: true + max_velocity: 1.5707963267948966 + has_acceleration_limits: true + max_acceleration: 5.0 + joint3: + has_velocity_limits: true + max_velocity: 1.5707963267948966 + has_acceleration_limits: true + max_acceleration: 5.0 + joint4: + has_velocity_limits: true + max_velocity: 2.9670597283903604 + has_acceleration_limits: true + max_acceleration: 5.0 + joint5: + has_velocity_limits: true + max_velocity: 2.0943951023931953 + has_acceleration_limits: true + max_acceleration: 5.0 + joint6: + has_velocity_limits: true + max_velocity: 3.3161255787892263 + has_acceleration_limits: true + max_acceleration: 5.0 + +#-------- other + moveit_manage_controllers: false + trajectory_execution: + allowed_execution_duration_scaling: 1.2 + allowed_goal_duration_margin: 0.5 + allowed_start_tolerance: 0.01 + capabilities: move_group/MoveGroupExecuteTrajectoryAction + publish_planning_scene: True + publish_geometry_updates: True + publish_state_updates: True + publish_transforms_updates: True + + \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_moveit/config/all.yaml b/myrobot_ws/src/myrobot_moveit/config/ompl_planning.yaml similarity index 64% rename from myrobot_ws/src/myrobot_moveit/config/all.yaml rename to myrobot_ws/src/myrobot_moveit/config/ompl_planning.yaml index 12256ddb..d198a254 100644 --- a/myrobot_ws/src/myrobot_moveit/config/all.yaml +++ b/myrobot_ws/src/myrobot_moveit/config/ompl_planning.yaml @@ -1,84 +1,5 @@ /**: ros__parameters: - # from launch file - capabilities: move_group/MoveGroupExecuteTrajectoryAction - publish_planning_scene: True - publish_geometry_updates: True - publish_state_updates: True - publish_transforms_updates: True - - # kinematics - robot_description_kinematics: - myrobot_manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 - - # joint limits - robot_description_planning: - default_velocity_scaling_factor: 0.1 - default_acceleration_scaling_factor: 0.1 - - # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] - # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] - joint_limits: - joint1: - has_velocity_limits: true - max_velocity: 1.7453292519943295 - has_acceleration_limits: true - max_acceleration: 5.0 - joint2: - has_velocity_limits: true - max_velocity: 1.5707963267948966 - has_acceleration_limits: true - max_acceleration: 5.0 - joint3: - has_velocity_limits: true - max_velocity: 1.5707963267948966 - has_acceleration_limits: true - max_acceleration: 5.0 - joint4: - has_velocity_limits: true - max_velocity: 2.9670597283903604 - has_acceleration_limits: true - max_acceleration: 5.0 - joint5: - has_velocity_limits: true - max_velocity: 2.0943951023931953 - has_acceleration_limits: true - max_acceleration: 5.0 - joint6: - has_velocity_limits: true - max_velocity: 3.3161255787892263 - has_acceleration_limits: true - max_acceleration: 5.0 - - # Moveit Controllers - moveit_manage_controllers: false - trajectory_execution: - allowed_execution_duration_scaling: 1.2 - allowed_goal_duration_margin: 0.5 - allowed_start_tolerance: 0.01 - - moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager - moveit_simple_controller_manager: - controller_names: - - joint_trajectory_controller - - joint_trajectory_controller: - type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - # OMPL planning move_group: planning_plugins: - ompl_interface/OMPLPlanner diff --git a/myrobot_ws/src/myrobot_moveit/config_old/joint_limits.yaml b/myrobot_ws/src/myrobot_moveit/config_old/joint_limits.yaml deleted file mode 100644 index 92ff4b51..00000000 --- a/myrobot_ws/src/myrobot_moveit/config_old/joint_limits.yaml +++ /dev/null @@ -1,40 +0,0 @@ -# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed - -# For beginners, we downscale velocity and acceleration limits. -# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 0.1 -default_acceleration_scaling_factor: 0.1 - -# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] -# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] -joint_limits: - joint1: - has_velocity_limits: true - max_velocity: 1.7453292519943295 - has_acceleration_limits: true - max_acceleration: 5.0 - joint2: - has_velocity_limits: true - max_velocity: 1.5707963267948966 - has_acceleration_limits: true - max_acceleration: 5.0 - joint3: - has_velocity_limits: true - max_velocity: 1.5707963267948966 - has_acceleration_limits: true - max_acceleration: 5.0 - joint4: - has_velocity_limits: true - max_velocity: 2.9670597283903604 - has_acceleration_limits: true - max_acceleration: 5.0 - joint5: - has_velocity_limits: true - max_velocity: 2.0943951023931953 - has_acceleration_limits: true - max_acceleration: 5.0 - joint6: - has_velocity_limits: true - max_velocity: 3.3161255787892263 - has_acceleration_limits: true - max_acceleration: 5.0 \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_moveit/config_old/kinematics.yaml b/myrobot_ws/src/myrobot_moveit/config_old/kinematics.yaml deleted file mode 100644 index 59e9d690..00000000 --- a/myrobot_ws/src/myrobot_moveit/config_old/kinematics.yaml +++ /dev/null @@ -1,5 +0,0 @@ -myrobot_manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_moveit/config_old/moveit_controllers.yaml b/myrobot_ws/src/myrobot_moveit/config_old/moveit_controllers.yaml deleted file mode 100644 index d2b3042f..00000000 --- a/myrobot_ws/src/myrobot_moveit/config_old/moveit_controllers.yaml +++ /dev/null @@ -1,24 +0,0 @@ -# MoveIt uses this configuration for controller management -moveit_manage_controllers: false -trajectory_execution: - allowed_execution_duration_scaling: 1.2 - allowed_goal_duration_margin: 0.5 - allowed_start_tolerance: 0.01 - -moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager - -moveit_simple_controller_manager: - controller_names: - - joint_trajectory_controller - - joint_trajectory_controller: - type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_moveit/config_old/ompl_planning.yaml b/myrobot_ws/src/myrobot_moveit/config_old/ompl_planning.yaml deleted file mode 100644 index 645cf70c..00000000 --- a/myrobot_ws/src/myrobot_moveit/config_old/ompl_planning.yaml +++ /dev/null @@ -1,69 +0,0 @@ -planning_plugins: - - ompl_interface/OMPLPlanner -request_adapters: - - default_planning_request_adapters/ResolveConstraintFrames - - default_planning_request_adapters/ValidateWorkspaceBounds - - default_planning_request_adapters/CheckStartStateBounds - - default_planning_request_adapters/CheckStartStateCollision -response_adapters: - - default_planning_response_adapters/AddTimeOptimalParameterization - - default_planning_response_adapters/ValidateSolution - - default_planning_response_adapters/DisplayMotionPath -myrobot_manipulator: - planner_configs: - SBLkConfigDefault: - type: geometric::SBL - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ESTkConfigDefault: - type: geometric::EST - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - LBKPIECEkConfigDefault: - type: geometric::LBKPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - BKPIECEkConfigDefault: - type: geometric::BKPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 - failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - KPIECEkConfigDefault: - type: geometric::KPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] - failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - RRTkConfigDefault: - type: geometric::RRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - RRTConnectkConfigDefault: - type: geometric::RRTConnect - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - RRTstarkConfigDefault: - type: geometric::RRTstar - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 - TRRTkConfigDefault: - type: geometric::TRRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - max_states_failed: 10 # when to start increasing temp. default: 10 - temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 - min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 - init_temperature: 10e-6 # initial temperature. default: 10e-6 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() - frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 - k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() - PRMkConfigDefault: - type: geometric::PRM - max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 - PRMstarkConfigDefault: - type: geometric::PRMstar - ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE - #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) - longest_valid_segment_fraction: 0.01 \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py b/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py index c1c0f344..ba31e8ad 100644 --- a/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py +++ b/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py @@ -1,6 +1,3 @@ -import os -import yaml - from launch.actions import DeclareLaunchArgument from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node @@ -43,8 +40,6 @@ def launch_setup(context, *args, **kwargs): " ", ] ) - robot_description = {"robot_description": robot_description_content.perform(context)} - # SRDF robot_description_semantic_content = Command( [ @@ -53,14 +48,13 @@ def launch_setup(context, *args, **kwargs): PathJoinSubstitution( [FindPackageShare(moveit_package), "srdf", semantic_description_file] ), - ] ) - robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content.perform(context)} - other_configs = PathJoinSubstitution( - [FindPackageShare(moveit_package), "config", "all.yaml"] - ) + robot_description = {"robot_description": robot_description_content.perform(context)} + robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content.perform(context)} + planning_config = PathJoinSubstitution([FindPackageShare(moveit_package), "config", "ompl_planning.yaml"]) + move_group_config = PathJoinSubstitution([FindPackageShare(moveit_package), "config", "move_group.yaml"]) # -------------------------------------------------------# # Move Group Node # @@ -73,7 +67,8 @@ def launch_setup(context, *args, **kwargs): parameters=[ robot_description, robot_description_semantic, - other_configs, + planning_config, + move_group_config, {"use_sim_time": use_sim_time}, ], ) @@ -90,7 +85,9 @@ def launch_setup(context, *args, **kwargs): parameters=[ robot_description, robot_description_semantic, - other_configs + planning_config, + move_group_config, + {"use_sim_time": use_sim_time}, ], ) From 0e2968284b98f05d13a952fdfcce23f735c0713a Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Fri, 5 Apr 2024 16:17:28 +0200 Subject: [PATCH 08/23] [myrobot_ws] remove unecessary dependencies --- myrobot_ws/src/myrobot_moveit/package.xml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/myrobot_ws/src/myrobot_moveit/package.xml b/myrobot_ws/src/myrobot_moveit/package.xml index 4f33e922..14a0b469 100644 --- a/myrobot_ws/src/myrobot_moveit/package.xml +++ b/myrobot_ws/src/myrobot_moveit/package.xml @@ -9,9 +9,7 @@ ament_cmake - joint_state_publisher - joint_state_publisher_gui - tf2_ros + myrobot_description xacro From 78c244086e97292947af3effdb37f5892deadaba Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Fri, 5 Apr 2024 16:17:57 +0200 Subject: [PATCH 09/23] add setup-robot-moveit.bash script --- scripts/_RosTeamWs_Defines.bash | 1 + scripts/setup-robot-moveit.bash | 153 ++++++++++++++++++++++++++++++++ 2 files changed, 154 insertions(+) create mode 100755 scripts/setup-robot-moveit.bash diff --git a/scripts/_RosTeamWs_Defines.bash b/scripts/_RosTeamWs_Defines.bash index 9139b4f4..8f725ec2 100755 --- a/scripts/_RosTeamWs_Defines.bash +++ b/scripts/_RosTeamWs_Defines.bash @@ -427,6 +427,7 @@ function set_framework_default_paths { PACKAGE_TEMPLATES="$FRAMEWORK_BASE_PATH/templates/package" ROBOT_DESCRIPTION_TEMPLATES="$FRAMEWORK_BASE_PATH/templates/robot_description" ROS2_CONTROL_TEMPLATES="$FRAMEWORK_BASE_PATH/templates/ros2_control" + MOVEIT_TEMPLATES="$FRAMEWORK_BASE_PATH/templates/moveit" ROS2_CONTROL_HW_ITF_TEMPLATES="$ROS2_CONTROL_TEMPLATES/hardware" ROS2_CONTROL_CONTROLLER_TEMPLATES="$ROS2_CONTROL_TEMPLATES/controller" LICENSE_TEMPLATES="$FRAMEWORK_BASE_PATH/templates/licenses" diff --git a/scripts/setup-robot-moveit.bash b/scripts/setup-robot-moveit.bash new file mode 100755 index 00000000..05308b6a --- /dev/null +++ b/scripts/setup-robot-moveit.bash @@ -0,0 +1,153 @@ +#!/bin/bash +# +# Copyright 2021 Denis Stogl (Stogl Robotics Consulting) +# +# 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. + +usage="setup-robot-bringup ROBOT_NAME DESCRIPTION_PKG_NAME" + +# Load Framework defines +script_own_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +source $script_own_dir/../setup.bash +check_and_set_ros_distro_and_version "${ROS_DISTRO}" + +ROBOT_NAME=$1 +if [ -z "$ROBOT_NAME" ]; then + print_and_exit "ERROR: You should provide robot name! Nothing to do 😯" "$usage" +fi + +DESCR_PKG_NAME=$2 +if [ -z "$DESCR_PKG_NAME" ]; then + print_and_exit "ERROR: You should provide description package name! Nothing to do 😯" "$usage" +fi + +echo "Which launchfiles should be added? Choose from the following options:" +echo "1) xml" +echo "2) python" +echo "3) both" + +read -p "Enter your choice:" choice + +LAUNCH_FILE_TYPES=() + +case $choice in +1) + LAUNCH_FILE_TYPES+=(".xml") + ;; +2) + LAUNCH_FILE_TYPES+=(".py") + ;; +3) + LAUNCH_FILE_TYPES+=(".xml" ".py") + ;; +*) + print_and_exit "Invalid choice. Exiting." + ;; +esac + +if [ ! -f "package.xml" ]; then + print_and_exit "ERROR: 'package.xml' not found. You should execute this script at the top level of your package folder. Nothing to do 😯" "$usage" +fi +PKG_NAME="$(grep -Po '(?<=).*?(?=)' package.xml | sed -e 's/[[:space:]]//g')" + +echo "" +echo -e "${TERMINAL_COLOR_USER_NOTICE}ATTENTION: Setting up moveit package for robot '$ROBOT_NAME' in package '$PKG_NAME' in folder '$(pwd)' with robot description package '$DESCR_PKG_NAME'.${TERMINAL_COLOR_NC}" +echo -e "${TERMINAL_COLOR_USER_CONFIRMATION}If correct press , otherwise +C and start the script again from the package folder and/or with correct robot name.${TERMINAL_COLOR_NC}" +read + +# Remove include and src folders - in this package should be no source +RM_FOLDERS=("include" "src") + +for FOLDER in "${RM_FOLDERS[@]}"; do + if [[ -d $FOLDER && ! "$(ls -A $FOLDER)" ]]; then + rm -r $FOLDER + fi +done + +# Create folders +mkdir -p config +mkdir -p launch +mkdir -p rviz +mkdir -p srdf + +# Copy rviz files +mkdir -p rviz +ROBOT_RVIZ="rviz/moveit.rviz" +cp -n "$MOVEIT_TEMPLATES/moveit.rviz" $ROBOT_RVIZ + +# Copy config files +MOVE_GROUP_CONFIG_YAML="config/move_group.yaml" +OMPL_PLANNING_CONFIG_YAML="config/ompl_planning.yaml" +cp -n $MOVEIT_TEMPLATES/move_group.yaml $MOVE_GROUP_CONFIG_YAML +cp -n $MOVEIT_TEMPLATES/ompl_planning.yaml $OMPL_PLANNING_CONFIG_YAML + +# Copy SRDF/xacro files +ROBOT_SRDF="srdf/${ROBOT_NAME}.srdf.xacro" +ROBOT_SRDF_MACRO="srdf/${ROBOT_NAME}_macro.srdf.xacro" +cp -n "$MOVEIT_TEMPLATES/robot.srdf.xacro" $ROBOT_SRDF +cp -n "$MOVEIT_TEMPLATES/robot_macro.xacro" $ROBOT_SRDF_MACRO + + +# Copy launch files +for file_type in "${LAUNCH_FILE_TYPES[@]}"; do + # Construct the file paths + MOVEIT_LAUNCH="launch/moveit.launch${file_type}" + + # Copy the templates to the destination with the specified file type + cp -n "$MOVEIT_TEMPLATES/moveit.launch${file_type}" "${MOVEIT_LAUNCH}" +done + +# sed all needed files +FILES_TO_SED=($MOVEIT_LAUNCH $ROBOT_SRDF $ROBOT_SRDF_MACRO) + +for SED_FILE in "${FILES_TO_SED[@]}"; do + sed -i "s/\\\$PKG_NAME\\\$/${PKG_NAME}/g" $SED_FILE + sed -i "s/\\\$MOVEIT_CONFIG_PKG_NAME\\\$/${PKG_NAME}/g" $SED_FILE + sed -i "s/\\\$ROBOT_NAME\\\$/${ROBOT_NAME}/g" $SED_FILE + sed -i "s/\\\$DESCR_PKG_NAME\\\$/${DESCR_PKG_NAME}/g" $SED_FILE +done + +# package.xml: Add dependencies if they not exist +DEP_PKGS=( + "xacro" + "$DESCR_PKG_NAME" + "moveit_ros_move_group" + "moveit_kinematics" + "moveit_planners" + "moveit_simple_controller_manager" + ) + +for DEP_PKG in "${DEP_PKGS[@]}"; do + if $(grep -q $DEP_PKG package.xml); then + echo "'$DEP_PKG' is already listed in package.xml" + else + append_to_string="ament_cmake<\/buildtool_depend>" + sed -i "s/$append_to_string/$append_to_string\\n\\n ${DEP_PKG}<\/exec_depend>/g" package.xml + fi +done + +# CMakeLists.txt: Add install paths of the files +prepend_to_string="if(BUILD_TESTING)" +sed -i "s/$prepend_to_string/install\(\\n DIRECTORY config launch rviz srdf\\n DESTINATION share\/\$\{PROJECT_NAME\}\\n\)\\n\\n$prepend_to_string/g" CMakeLists.txt + +# TODO: add README with general instructions + +# TODO: Add license checks + +# skip compilation, let the user choose which moveit packages to install, as MoveIt introduces +# breaking changes often. +echo "" +echo -e "${TERMINAL_COLOR_USER_NOTICE}NOTICE: To compile, install the required MoveIt package dependencies manually.${TERMINAL_COLOR_NC}" + +echo "" +echo -e "${TERMINAL_COLOR_USER_NOTICE}FINISHED: You can test the configuration by executing 'ros2 launch $PKG_NAME moveit.launch${LAUNCH_FILE_TYPES[*]}'${TERMINAL_COLOR_NC}" From f79d6c352bd5baa827f59b7b2af867479610f1b9 Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Fri, 5 Apr 2024 16:18:30 +0200 Subject: [PATCH 10/23] add template files to moveit template folder --- templates/moveit/move_group.yaml | 79 +++++ templates/moveit/moveit.launch.py | 179 ++++++++++ templates/moveit/moveit.rviz | 443 ++++++++++++++++++++++++ templates/moveit/ompl_planning.yaml | 72 ++++ templates/moveit/robot.srdf.xacro | 10 + templates/moveit/robot_macro.srdf.xacro | 25 ++ 6 files changed, 808 insertions(+) create mode 100644 templates/moveit/move_group.yaml create mode 100644 templates/moveit/moveit.launch.py create mode 100644 templates/moveit/moveit.rviz create mode 100644 templates/moveit/ompl_planning.yaml create mode 100644 templates/moveit/robot.srdf.xacro create mode 100644 templates/moveit/robot_macro.srdf.xacro diff --git a/templates/moveit/move_group.yaml b/templates/moveit/move_group.yaml new file mode 100644 index 00000000..0264c560 --- /dev/null +++ b/templates/moveit/move_group.yaml @@ -0,0 +1,79 @@ +/**: + ros__parameters: +#-------- Moveit controllers + moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + moveit_simple_controller_manager: + controller_names: + - joint_trajectory_controller # add or remove controllers as necessary + + joint_trajectory_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + +#-------- kinematics + robot_description_kinematics: + myrobot_manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 + +#-------- joint limits + robot_description_planning: + default_velocity_scaling_factor: 0.1 + default_acceleration_scaling_factor: 0.1 + # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] + # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] + joint_limits: + joint1: + has_velocity_limits: true + max_velocity: 1.7453292519943295 + has_acceleration_limits: true + max_acceleration: 5.0 + joint2: + has_velocity_limits: true + max_velocity: 1.5707963267948966 + has_acceleration_limits: true + max_acceleration: 5.0 + joint3: + has_velocity_limits: true + max_velocity: 1.5707963267948966 + has_acceleration_limits: true + max_acceleration: 5.0 + joint4: + has_velocity_limits: true + max_velocity: 2.9670597283903604 + has_acceleration_limits: true + max_acceleration: 5.0 + joint5: + has_velocity_limits: true + max_velocity: 2.0943951023931953 + has_acceleration_limits: true + max_acceleration: 5.0 + joint6: + has_velocity_limits: true + max_velocity: 3.3161255787892263 + has_acceleration_limits: true + max_acceleration: 5.0 + +#-------- other + moveit_manage_controllers: false + trajectory_execution: + allowed_execution_duration_scaling: 1.2 + allowed_goal_duration_margin: 0.5 + allowed_start_tolerance: 0.01 + capabilities: move_group/MoveGroupExecuteTrajectoryAction + publish_planning_scene: True + publish_geometry_updates: True + publish_state_updates: True + publish_transforms_updates: True + + \ No newline at end of file diff --git a/templates/moveit/moveit.launch.py b/templates/moveit/moveit.launch.py new file mode 100644 index 00000000..ba31e8ad --- /dev/null +++ b/templates/moveit/moveit.launch.py @@ -0,0 +1,179 @@ +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch import LaunchDescription +from launch.actions import OpaqueFunction + +def launch_setup(context, *args, **kwargs): + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + moveit_package = LaunchConfiguration("moveit_package") + semantic_description_file = LaunchConfiguration("semantic_description_file") + prefix = LaunchConfiguration("prefix") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + use_sim_time = LaunchConfiguration("use_sim_time") + logging_severity = LaunchConfiguration("severity") + + # -------------------------------------------------------# + # MoveIt2 MoveGroup setup # + # -------------------------------------------------------# + + # URDF + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", description_file] + ), + " ", + "prefix:=", + prefix, + " ", + "use_mock_hardware:=", + use_mock_hardware, + " ", + "mock_sensor_commands:=", + mock_sensor_commands, + " ", + ] + ) + # SRDF + robot_description_semantic_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(moveit_package), "srdf", semantic_description_file] + ), + ] + ) + + robot_description = {"robot_description": robot_description_content.perform(context)} + robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content.perform(context)} + planning_config = PathJoinSubstitution([FindPackageShare(moveit_package), "config", "ompl_planning.yaml"]) + move_group_config = PathJoinSubstitution([FindPackageShare(moveit_package), "config", "move_group.yaml"]) + + # -------------------------------------------------------# + # Move Group Node # + # -------------------------------------------------------# + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + arguments=["--ros-args", "--log-level", logging_severity], + parameters=[ + robot_description, + robot_description_semantic, + planning_config, + move_group_config, + {"use_sim_time": use_sim_time}, + ], + ) + + # RViz + rviz_config = PathJoinSubstitution( + [FindPackageShare(moveit_package), "rviz", "moveit.rviz"] + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + output="log", + arguments=["-d", rviz_config, "--ros-args", "--log-level", "WARN"], + parameters=[ + robot_description, + robot_description_semantic, + planning_config, + move_group_config, + {"use_sim_time": use_sim_time}, + ], + ) + + return [ + move_group_node, + rviz_node, + ] + + +def generate_launch_description(): + # + # ------------- Declare arguments ------------- # + # + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="myrobot_description", + description="Package with the robot URDF/XACRO files. \ + Usually the argument is not set, it enables use of a custom setup.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="myrobot.urdf.xacro", + description="URDF/XACRO description file \ + Usually the argument is not set, it enables use of a custom setup.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "moveit_package", + default_value="myrobot_moveit", + description="MoveIt config package with robot SRDF/XACRO files. \ + Usually the argument is not set, it enables use of a custom setup.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "semantic_description_file", + default_value="myrobot.srdf.xacro", + description="MoveIt SRDF/XACRO description file with the robot.. \ + Usually the argument is not set, it enables use of a custom setup.", + ) + ) + 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.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="true", + description="Start robot with mock hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable mock command interfaces for sensors used for simple simulations. \ + Used only if 'use_mock_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use simulation clock instead of world clock", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "severity", + default_value="INFO", + choices=["INFO", "WARN", "ERROR", "DEBUG", "FATAL"], + description="Logging level for the nodes", + ) + ) + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/templates/moveit/moveit.rviz b/templates/moveit/moveit.rviz new file mode 100644 index 00000000..3177bc9a --- /dev/null +++ b/templates/moveit/moveit.rviz @@ -0,0 +1,443 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /RobotModel1/Links1/link61 + - /TF1/Tree1 + - /TF1/Tree1/world1 + - /TF1/Tree1/world1/base_link1 + - /TF1/Tree1/world1/base_link1/link11 + - /MotionPlanning1 + Splitter Ratio: 0.5 + Tree Height: 438 + - 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: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base: + Value: true + base_link: + Value: true + flange: + Value: true + link1: + Value: true + link2: + Value: true + link3: + Value: true + link4: + Value: true + link5: + Value: true + link6: + Value: true + tool0: + Value: true + world: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + base_link: + base: + {} + link1: + link2: + link3: + link4: + link5: + link6: + tool0: + flange: + {} + Update Interval: 0 + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: my_robot_manipulator + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + 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 + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + 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: 2.962221622467041 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.8273206949234009 + Y: -0.11996828764677048 + Z: 0.4445207118988037 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.38039883971214294 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.253565788269043 + Saved: ~ +Window Geometry: + "": + collapsed: false + " - Trajectory Slider": + collapsed: false + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f30000039efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d00000210000000e60100001cfa000000010100000002fb0000000a005600690065007700730100000000ffffffff0000010000fffffffb000000100044006900730070006c0061007900730100000000000001f30000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fbffffffff0100000253000001880000017d00ffffff00000001000001100000039efc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005870000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1366 + Y: 27 diff --git a/templates/moveit/ompl_planning.yaml b/templates/moveit/ompl_planning.yaml new file mode 100644 index 00000000..d198a254 --- /dev/null +++ b/templates/moveit/ompl_planning.yaml @@ -0,0 +1,72 @@ +/**: + ros__parameters: + move_group: + planning_plugins: + - ompl_interface/OMPLPlanner + request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision + response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath + myrobot_manipulator: + planner_configs: + SBLkConfigDefault: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ESTkConfigDefault: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECEkConfigDefault: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECEkConfigDefault: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRTkConfigDefault: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstarkConfigDefault: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRTkConfigDefault: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() + PRMkConfigDefault: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: + type: geometric::PRMstar + ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE + #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) + longest_valid_segment_fraction: 0.01 \ No newline at end of file diff --git a/templates/moveit/robot.srdf.xacro b/templates/moveit/robot.srdf.xacro new file mode 100644 index 00000000..677264bc --- /dev/null +++ b/templates/moveit/robot.srdf.xacro @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/templates/moveit/robot_macro.srdf.xacro b/templates/moveit/robot_macro.srdf.xacro new file mode 100644 index 00000000..4224afc9 --- /dev/null +++ b/templates/moveit/robot_macro.srdf.xacro @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + From fe8ad5c73c7fa20ae19a99f5f2528dcc403c5573 Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Mon, 8 Apr 2024 09:18:05 +0200 Subject: [PATCH 11/23] add variable names to template files for SED-ing --- scripts/setup-robot-moveit.bash | 1 - templates/moveit/move_group.yaml | 2 +- templates/moveit/moveit.launch.py | 8 ++++---- templates/moveit/moveit.rviz | 2 +- templates/moveit/ompl_planning.yaml | 2 +- templates/moveit/robot.srdf.xacro | 6 +++--- templates/moveit/robot_macro.srdf.xacro | 4 ++-- 7 files changed, 12 insertions(+), 13 deletions(-) diff --git a/scripts/setup-robot-moveit.bash b/scripts/setup-robot-moveit.bash index 05308b6a..3d3c47f8 100755 --- a/scripts/setup-robot-moveit.bash +++ b/scripts/setup-robot-moveit.bash @@ -112,7 +112,6 @@ FILES_TO_SED=($MOVEIT_LAUNCH $ROBOT_SRDF $ROBOT_SRDF_MACRO) for SED_FILE in "${FILES_TO_SED[@]}"; do sed -i "s/\\\$PKG_NAME\\\$/${PKG_NAME}/g" $SED_FILE - sed -i "s/\\\$MOVEIT_CONFIG_PKG_NAME\\\$/${PKG_NAME}/g" $SED_FILE sed -i "s/\\\$ROBOT_NAME\\\$/${ROBOT_NAME}/g" $SED_FILE sed -i "s/\\\$DESCR_PKG_NAME\\\$/${DESCR_PKG_NAME}/g" $SED_FILE done diff --git a/templates/moveit/move_group.yaml b/templates/moveit/move_group.yaml index 0264c560..908d6532 100644 --- a/templates/moveit/move_group.yaml +++ b/templates/moveit/move_group.yaml @@ -20,7 +20,7 @@ #-------- kinematics robot_description_kinematics: - myrobot_manipulator: + $ROBOT_NAME$_manipulator: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 diff --git a/templates/moveit/moveit.launch.py b/templates/moveit/moveit.launch.py index ba31e8ad..5babd891 100644 --- a/templates/moveit/moveit.launch.py +++ b/templates/moveit/moveit.launch.py @@ -105,7 +105,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "description_package", - default_value="myrobot_description", + default_value="$DESCR_PKG_NAME$", description="Package with the robot URDF/XACRO files. \ Usually the argument is not set, it enables use of a custom setup.", ) @@ -113,7 +113,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "description_file", - default_value="myrobot.urdf.xacro", + default_value="$ROBOT_NAME$.urdf.xacro", description="URDF/XACRO description file \ Usually the argument is not set, it enables use of a custom setup.", ) @@ -121,7 +121,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "moveit_package", - default_value="myrobot_moveit", + default_value="$PKG_NAME$", description="MoveIt config package with robot SRDF/XACRO files. \ Usually the argument is not set, it enables use of a custom setup.", ) @@ -129,7 +129,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "semantic_description_file", - default_value="myrobot.srdf.xacro", + default_value="$ROBOT_NAME$.srdf.xacro", description="MoveIt SRDF/XACRO description file with the robot.. \ Usually the argument is not set, it enables use of a custom setup.", ) diff --git a/templates/moveit/moveit.rviz b/templates/moveit/moveit.rviz index 3177bc9a..cadc9425 100644 --- a/templates/moveit/moveit.rviz +++ b/templates/moveit/moveit.rviz @@ -276,7 +276,7 @@ Visualization Manager: Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: my_robot_manipulator + Planning Group: $ROBOT_NAME$_manipulator Query Goal State: true Query Start State: false Show Workspace: false diff --git a/templates/moveit/ompl_planning.yaml b/templates/moveit/ompl_planning.yaml index d198a254..c90d7c9b 100644 --- a/templates/moveit/ompl_planning.yaml +++ b/templates/moveit/ompl_planning.yaml @@ -12,7 +12,7 @@ - default_planning_response_adapters/AddTimeOptimalParameterization - default_planning_response_adapters/ValidateSolution - default_planning_response_adapters/DisplayMotionPath - myrobot_manipulator: + $ROBOT_NAME$_manipulator: planner_configs: SBLkConfigDefault: type: geometric::SBL diff --git a/templates/moveit/robot.srdf.xacro b/templates/moveit/robot.srdf.xacro index 677264bc..27e021fe 100644 --- a/templates/moveit/robot.srdf.xacro +++ b/templates/moveit/robot.srdf.xacro @@ -1,10 +1,10 @@ - + - + - + diff --git a/templates/moveit/robot_macro.srdf.xacro b/templates/moveit/robot_macro.srdf.xacro index 4224afc9..0b01420b 100644 --- a/templates/moveit/robot_macro.srdf.xacro +++ b/templates/moveit/robot_macro.srdf.xacro @@ -1,8 +1,8 @@ - + - + From bbac85036e2e349b4f1d9f50d22910eba4ca359f Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Mon, 8 Apr 2024 09:47:50 +0200 Subject: [PATCH 12/23] add setup-robot-moveit alias, minor refactor --- scripts/_Team_Defines.bash | 2 ++ scripts/setup-robot-moveit.bash | 15 +++++++-------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/scripts/_Team_Defines.bash b/scripts/_Team_Defines.bash index f07dc05f..3a2ffba8 100644 --- a/scripts/_Team_Defines.bash +++ b/scripts/_Team_Defines.bash @@ -74,6 +74,8 @@ alias setup-robot-bringup=$RosTeamWS_FRAMEWORK_SCRIPTS_PATH/setup-robot-bringup. alias setup-robot-description=$RosTeamWS_FRAMEWORK_SCRIPTS_PATH/setup-robot-description.bash +alias setup-robot-moveit=$RosTeamWS_FRAMEWORK_SCRIPTS_PATH/setup-robot-moveit.bash + # ros2_control alias ros2_control_setup-hardware-interface-package=$RosTeamWS_FRAMEWORK_SCRIPTS_PATH/ros2_control/setup-hardware-interface-package.bash alias ros2_control_setup-controller-package=$RosTeamWS_FRAMEWORK_SCRIPTS_PATH/ros2_control/setup-controller-package.bash diff --git a/scripts/setup-robot-moveit.bash b/scripts/setup-robot-moveit.bash index 3d3c47f8..9d53f81f 100755 --- a/scripts/setup-robot-moveit.bash +++ b/scripts/setup-robot-moveit.bash @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -usage="setup-robot-bringup ROBOT_NAME DESCRIPTION_PKG_NAME" +usage="setup-robot-moveit ROBOT_NAME DESCRIPTION_PKG_NAME" # Load Framework defines script_own_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" @@ -66,7 +66,7 @@ echo -e "${TERMINAL_COLOR_USER_CONFIRMATION}If correct press , otherwise read # Remove include and src folders - in this package should be no source -RM_FOLDERS=("include" "src") +RM_FOLDERS=("src") for FOLDER in "${RM_FOLDERS[@]}"; do if [[ -d $FOLDER && ! "$(ls -A $FOLDER)" ]]; then @@ -95,7 +95,7 @@ cp -n $MOVEIT_TEMPLATES/ompl_planning.yaml $OMPL_PLANNING_CONFIG_YAML ROBOT_SRDF="srdf/${ROBOT_NAME}.srdf.xacro" ROBOT_SRDF_MACRO="srdf/${ROBOT_NAME}_macro.srdf.xacro" cp -n "$MOVEIT_TEMPLATES/robot.srdf.xacro" $ROBOT_SRDF -cp -n "$MOVEIT_TEMPLATES/robot_macro.xacro" $ROBOT_SRDF_MACRO +cp -n "$MOVEIT_TEMPLATES/robot_macro.srdf.xacro" $ROBOT_SRDF_MACRO # Copy launch files @@ -108,7 +108,7 @@ for file_type in "${LAUNCH_FILE_TYPES[@]}"; do done # sed all needed files -FILES_TO_SED=($MOVEIT_LAUNCH $ROBOT_SRDF $ROBOT_SRDF_MACRO) +FILES_TO_SED=($MOVEIT_LAUNCH $ROBOT_SRDF $ROBOT_SRDF_MACRO $MOVE_GROUP_CONFIG_YAML $OMPL_PLANNING_CONFIG_YAML) for SED_FILE in "${FILES_TO_SED[@]}"; do sed -i "s/\\\$PKG_NAME\\\$/${PKG_NAME}/g" $SED_FILE @@ -143,10 +143,9 @@ sed -i "s/$prepend_to_string/install\(\\n DIRECTORY config launch rviz srdf\\n # TODO: Add license checks -# skip compilation, let the user choose which moveit packages to install, as MoveIt introduces +# skip compilation, let the user install MoveIt manually, as it can introduce # breaking changes often. -echo "" -echo -e "${TERMINAL_COLOR_USER_NOTICE}NOTICE: To compile, install the required MoveIt package dependencies manually.${TERMINAL_COLOR_NC}" echo "" -echo -e "${TERMINAL_COLOR_USER_NOTICE}FINISHED: You can test the configuration by executing 'ros2 launch $PKG_NAME moveit.launch${LAUNCH_FILE_TYPES[*]}'${TERMINAL_COLOR_NC}" +echo -e "${TERMINAL_COLOR_USER_NOTICE}FINISHED: You can test the configuration by first launching the ros2_control bringup, followed by +'ros2 launch $PKG_NAME moveit.launch${LAUNCH_FILE_TYPES[*]}'${TERMINAL_COLOR_NC}" From 64b82dd4ead03667de3383ef0b3de03686e986ca Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Mon, 8 Apr 2024 09:56:52 +0200 Subject: [PATCH 13/23] remove unecessary files --- .gitignore | 5 +- myrobot_ws/src/myrobot_bringup/CMakeLists.txt | 22 - .../config/myrobot_controllers.yaml | 81 ---- .../config/test_goal_publishers_config.yaml | 57 --- .../myrobot_bringup/launch/myrobot.launch.py | 235 ---------- .../myrobot_bringup/launch/myrobot.launch.xml | 78 --- ...test_forward_position_controller.launch.py | 46 -- ...est_forward_position_controller.launch.xml | 32 -- ...test_joint_trajectory_controller.launch.py | 46 -- ...est_joint_trajectory_controller.launch.xml | 32 -- myrobot_ws/src/myrobot_bringup/package.xml | 33 -- .../src/myrobot_description/CMakeLists.txt | 25 - myrobot_ws/src/myrobot_description/README.md | 61 --- .../src/myrobot_description/config/.gitkeep | 0 .../launch/view_myrobot.launch.py | 99 ---- .../launch/view_myrobot.launch.xml | 37 -- .../meshes/myrobot/collision/.gitkeep | 0 .../meshes/myrobot/visual/.gitkeep | 0 .../src/myrobot_description/package.xml | 26 - .../src/myrobot_description/rviz/myrobot.rviz | 248 ---------- .../test/myrobot_test_urdf_xacro.py | 83 ---- .../urdf/common/inertials.xacro | 64 --- .../urdf/common/materials.xacro | 43 -- .../urdf/myrobot.urdf.xacro | 34 -- .../myrobot/myrobot_macro.ros2_control.xacro | 143 ------ .../urdf/myrobot/myrobot_macro.xacro | 254 ---------- myrobot_ws/src/myrobot_moveit/CMakeLists.txt | 22 - .../src/myrobot_moveit/config/move_group.yaml | 79 ---- .../myrobot_moveit/config/ompl_planning.yaml | 72 --- .../myrobot_moveit/launch/moveit.launch.py | 179 ------- myrobot_ws/src/myrobot_moveit/package.xml | 24 - .../src/myrobot_moveit/rviz/moveit.rviz | 443 ------------------ .../myrobot_moveit/srdf/myrobot.srdf.xacro | 10 - .../srdf/myrobot_macro.srdf.xacro | 25 - templates/moveit/configs_here_ryan | 0 .../robot_srdf_examples.xacro | 47 -- 36 files changed, 1 insertion(+), 2684 deletions(-) delete mode 100644 myrobot_ws/src/myrobot_bringup/CMakeLists.txt delete mode 100644 myrobot_ws/src/myrobot_bringup/config/myrobot_controllers.yaml delete mode 100644 myrobot_ws/src/myrobot_bringup/config/test_goal_publishers_config.yaml delete mode 100644 myrobot_ws/src/myrobot_bringup/launch/myrobot.launch.py delete mode 100644 myrobot_ws/src/myrobot_bringup/launch/myrobot.launch.xml delete mode 100644 myrobot_ws/src/myrobot_bringup/launch/test_forward_position_controller.launch.py delete mode 100644 myrobot_ws/src/myrobot_bringup/launch/test_forward_position_controller.launch.xml delete mode 100644 myrobot_ws/src/myrobot_bringup/launch/test_joint_trajectory_controller.launch.py delete mode 100644 myrobot_ws/src/myrobot_bringup/launch/test_joint_trajectory_controller.launch.xml delete mode 100644 myrobot_ws/src/myrobot_bringup/package.xml delete mode 100644 myrobot_ws/src/myrobot_description/CMakeLists.txt delete mode 100644 myrobot_ws/src/myrobot_description/README.md delete mode 100644 myrobot_ws/src/myrobot_description/config/.gitkeep delete mode 100755 myrobot_ws/src/myrobot_description/launch/view_myrobot.launch.py delete mode 100644 myrobot_ws/src/myrobot_description/launch/view_myrobot.launch.xml delete mode 100644 myrobot_ws/src/myrobot_description/meshes/myrobot/collision/.gitkeep delete mode 100644 myrobot_ws/src/myrobot_description/meshes/myrobot/visual/.gitkeep delete mode 100644 myrobot_ws/src/myrobot_description/package.xml delete mode 100644 myrobot_ws/src/myrobot_description/rviz/myrobot.rviz delete mode 100644 myrobot_ws/src/myrobot_description/test/myrobot_test_urdf_xacro.py delete mode 100644 myrobot_ws/src/myrobot_description/urdf/common/inertials.xacro delete mode 100644 myrobot_ws/src/myrobot_description/urdf/common/materials.xacro delete mode 100644 myrobot_ws/src/myrobot_description/urdf/myrobot.urdf.xacro delete mode 100644 myrobot_ws/src/myrobot_description/urdf/myrobot/myrobot_macro.ros2_control.xacro delete mode 100644 myrobot_ws/src/myrobot_description/urdf/myrobot/myrobot_macro.xacro delete mode 100644 myrobot_ws/src/myrobot_moveit/CMakeLists.txt delete mode 100644 myrobot_ws/src/myrobot_moveit/config/move_group.yaml delete mode 100644 myrobot_ws/src/myrobot_moveit/config/ompl_planning.yaml delete mode 100644 myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py delete mode 100644 myrobot_ws/src/myrobot_moveit/package.xml delete mode 100644 myrobot_ws/src/myrobot_moveit/rviz/moveit.rviz delete mode 100644 myrobot_ws/src/myrobot_moveit/srdf/myrobot.srdf.xacro delete mode 100644 myrobot_ws/src/myrobot_moveit/srdf/myrobot_macro.srdf.xacro delete mode 100644 templates/moveit/configs_here_ryan delete mode 100644 templates/robot_description/robot_srdf_examples.xacro diff --git a/.gitignore b/.gitignore index 41baf599..9c764c21 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1 @@ -docs/_build/* -myrobot_ws/build/* -myrobot_ws/install/* -myrobot_ws/log/* \ No newline at end of file +docs/_build/* \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_bringup/CMakeLists.txt b/myrobot_ws/src/myrobot_bringup/CMakeLists.txt deleted file mode 100644 index 849db9ca..00000000 --- a/myrobot_ws/src/myrobot_bringup/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(myrobot_bringup) - -if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -install( - DIRECTORY config launch - DESTINATION share/${PROJECT_NAME} -) - -if(BUILD_TESTING) -endif() - -ament_package() diff --git a/myrobot_ws/src/myrobot_bringup/config/myrobot_controllers.yaml b/myrobot_ws/src/myrobot_bringup/config/myrobot_controllers.yaml deleted file mode 100644 index 3c984fbd..00000000 --- a/myrobot_ws/src/myrobot_bringup/config/myrobot_controllers.yaml +++ /dev/null @@ -1,81 +0,0 @@ -# Copyright (c) 20224 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. - -# -# Source of this file are templates in -# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -# -# Author: Dr. Denis -# - -controller_manager: - ros__parameters: - update_rate: 100 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - - forward_position_controller: # delete entry if controller is not applicable - type: forward_command_controller/ForwardCommandController - - forward_velocity_controller: # delete entry if controller is not applicable - type: forward_command_controller/ForwardCommandController - - joint_trajectory_controller: # delete entry if controller is not applicable - type: joint_trajectory_controller/JointTrajectoryController - -forward_position_controller: # delete entry if controller is not applicable - ros__parameters: - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - interface_name: position - -forward_velocity_controller: # delete entry if controller is not applicable - ros__parameters: - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - interface_name: velocity - -joint_trajectory_controller: # delete entry if controller is not applicable - ros__parameters: - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - command_interfaces: - - position - state_interfaces: - - position - - state_publish_rate: 50.0 # Defaults to 50 - action_monitor_rate: 20.0 # Defaults to 20 - - allow_partial_joints_goal: false # Defaults to false - constraints: - stopped_velocity_tolerance: 0.01 # Defaults to 0.01 - goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/myrobot_ws/src/myrobot_bringup/config/test_goal_publishers_config.yaml b/myrobot_ws/src/myrobot_bringup/config/test_goal_publishers_config.yaml deleted file mode 100644 index 1de2c039..00000000 --- a/myrobot_ws/src/myrobot_bringup/config/test_goal_publishers_config.yaml +++ /dev/null @@ -1,57 +0,0 @@ -# Copyright (c) 20224 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. - -# -# Source of this file are templates in -# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -# - -publisher_forward_position_controller: - ros__parameters: - - controller_name: "forward_position_controller" - wait_sec_between_publish: 5 - - goal_names: ["pos1", "pos2", "pos3", "pos4"] - pos1: [0.785, 0.785, 0.785, 0.785, 0.785, 0.785] - pos2: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - pos3: [-0.785, -0.785, -0.785, -0.785, -0.785, -0.785] - pos4: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - -publisher_joint_trajectory_controller: - ros__parameters: - - controller_name: "joint_trajectory_controller" - wait_sec_between_publish: 6 - repeat_the_same_goal: 1 # useful to simulate continuous inputs - - goal_time_from_start: 3.0 - goal_names: ["pos1", "pos2", "pos3", "pos4"] - pos1: - positions: [0.785, 0.785, 0.785, 0.785, 0.785, 0.785] - pos2: - positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - pos3: - positions: [-0.785, -0.785, -0.785, -0.785, -0.785, -0.785] - pos4: - positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 diff --git a/myrobot_ws/src/myrobot_bringup/launch/myrobot.launch.py b/myrobot_ws/src/myrobot_bringup/launch/myrobot.launch.py deleted file mode 100644 index 03eb6a16..00000000 --- a/myrobot_ws/src/myrobot_bringup/launch/myrobot.launch.py +++ /dev/null @@ -1,235 +0,0 @@ -# Copyright (c) 2024, 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. - -# -# Source of this file are templates in -# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -# -# Author: Dr. Denis -# - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction -from launch.event_handlers import OnProcessExit, OnProcessStart -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( - "runtime_config_package", - default_value="myrobot_bringup", - description='Package with the controller\'s configuration in "config" folder. \ - Usually the argument is not set, it enables use of a custom setup.', - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "controllers_file", - default_value="myrobot_controllers.yaml", - description="YAML file with the controllers configuration.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="myrobot_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="myrobot.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "use_mock_hardware", - default_value="true", - description="Start robot with mock hardware mirroring command to its states.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "mock_sensor_commands", - default_value="false", - description="Enable mock command interfaces for sensors used for simple simulations. \ - Used only if 'use_mock_hardware' parameter is true.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "robot_controller", - default_value="forward_position_controller", - choices=["forward_position_controller", "joint_trajectory_controller"], - description="Robot controller to start.", - ) - ) - - # Initialize Arguments - runtime_config_package = LaunchConfiguration("runtime_config_package") - controllers_file = LaunchConfiguration("controllers_file") - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - prefix = LaunchConfiguration("prefix") - use_mock_hardware = LaunchConfiguration("use_mock_hardware") - mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") - robot_controller = LaunchConfiguration("robot_controller") - - # Get URDF via xacro - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(description_package), "urdf", description_file] - ), - " ", - "prefix:=", - prefix, - " ", - "use_mock_hardware:=", - use_mock_hardware, - " ", - "mock_sensor_commands:=", - mock_sensor_commands, - " ", - ] - ) - - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [FindPackageShare(runtime_config_package), "config", controllers_file] - ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", "myrobot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - output="both", - parameters=[robot_description, robot_controllers], - ) - robot_state_pub_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], - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], - ) - - robot_controller_names = [robot_controller] - robot_controller_spawners = [] - for controller in robot_controller_names: - robot_controller_spawners += [ - Node( - package="controller_manager", - executable="spawner", - arguments=[controller, "-c", "/controller_manager"], - ) - ] - - inactive_robot_controller_names = ["add_some_controller_name"] - inactive_robot_controller_spawners = [] - for controller in inactive_robot_controller_names: - inactive_robot_controller_spawners += [ - Node( - package="controller_manager", - executable="spawner", - arguments=[controller, "-c", "/controller_manager", "--inactive"], - ) - ] - - # Delay loading and activation of `joint_state_broadcaster` after start of ros2_control_node - delay_joint_state_broadcaster_spawner_after_ros2_control_node = RegisterEventHandler( - event_handler=OnProcessStart( - target_action=control_node, - on_start=[ - TimerAction( - period=3.0, - actions=[joint_state_broadcaster_spawner], - ), - ], - ) - ) - - # Delay loading and activation of robot_controller_names after `joint_state_broadcaster` - delay_robot_controller_spawners_after_joint_state_broadcaster_spawner = [] - for i, controller in enumerate(robot_controller_spawners): - delay_robot_controller_spawners_after_joint_state_broadcaster_spawner += [ - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=robot_controller_spawners[i - 1] - if i > 0 - else joint_state_broadcaster_spawner, - on_exit=[controller], - ) - ) - ] - - # Delay start of inactive_robot_controller_names after other controllers - delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner = [] - for i, controller in enumerate(inactive_robot_controller_spawners): - delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner += [ - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=inactive_robot_controller_spawners[i - 1] - if i > 0 - else robot_controller_spawners[-1], - on_exit=[controller], - ) - ) - ] - - return LaunchDescription( - declared_arguments - + [ - control_node, - robot_state_pub_node, - rviz_node, - delay_joint_state_broadcaster_spawner_after_ros2_control_node, - ] - + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner - + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner - ) diff --git a/myrobot_ws/src/myrobot_bringup/launch/myrobot.launch.xml b/myrobot_ws/src/myrobot_bringup/launch/myrobot.launch.xml deleted file mode 100644 index 9bd3029e..00000000 --- a/myrobot_ws/src/myrobot_bringup/launch/myrobot.launch.xml +++ /dev/null @@ -1,78 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/myrobot_ws/src/myrobot_bringup/launch/test_forward_position_controller.launch.py b/myrobot_ws/src/myrobot_bringup/launch/test_forward_position_controller.launch.py deleted file mode 100644 index f4beb6dd..00000000 --- a/myrobot_ws/src/myrobot_bringup/launch/test_forward_position_controller.launch.py +++ /dev/null @@ -1,46 +0,0 @@ -# Copyright (c) 2024, 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. - -# -# Source of this file are templates in -# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -# -# Author: Dr. Denis -# - -from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - position_goals = PathJoinSubstitution( - [FindPackageShare("myrobot_bringup"), "config", "test_goal_publishers_config.yaml"] - ) - - return LaunchDescription( - [ - Node( - package="ros2_controllers_test_nodes", - executable="publisher_forward_position_controller", - name="publisher_forward_position_controller", - parameters=[position_goals], - output={ - "stdout": "screen", - "stderr": "screen", - }, - ) - ] - ) diff --git a/myrobot_ws/src/myrobot_bringup/launch/test_forward_position_controller.launch.xml b/myrobot_ws/src/myrobot_bringup/launch/test_forward_position_controller.launch.xml deleted file mode 100644 index bcabf90a..00000000 --- a/myrobot_ws/src/myrobot_bringup/launch/test_forward_position_controller.launch.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - diff --git a/myrobot_ws/src/myrobot_bringup/launch/test_joint_trajectory_controller.launch.py b/myrobot_ws/src/myrobot_bringup/launch/test_joint_trajectory_controller.launch.py deleted file mode 100644 index 77570322..00000000 --- a/myrobot_ws/src/myrobot_bringup/launch/test_joint_trajectory_controller.launch.py +++ /dev/null @@ -1,46 +0,0 @@ -# Copyright (c) 2024, 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. - -# -# Source of this file are templates in -# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -# -# Author: Dr. Denis -# - -from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - position_goals = PathJoinSubstitution( - [FindPackageShare("myrobot_bringup"), "config", "test_goal_publishers_config.yaml"] - ) - - return LaunchDescription( - [ - Node( - package="ros2_controllers_test_nodes", - executable="publisher_joint_trajectory_controller", - name="publisher_joint_trajectory_controller", - parameters=[position_goals], - output={ - "stdout": "screen", - "stderr": "screen", - }, - ) - ] - ) diff --git a/myrobot_ws/src/myrobot_bringup/launch/test_joint_trajectory_controller.launch.xml b/myrobot_ws/src/myrobot_bringup/launch/test_joint_trajectory_controller.launch.xml deleted file mode 100644 index 9ccded93..00000000 --- a/myrobot_ws/src/myrobot_bringup/launch/test_joint_trajectory_controller.launch.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - diff --git a/myrobot_ws/src/myrobot_bringup/package.xml b/myrobot_ws/src/myrobot_bringup/package.xml deleted file mode 100644 index da8711f4..00000000 --- a/myrobot_ws/src/myrobot_bringup/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - myrobot_bringup - 0.0.0 - testing bringup template - Nibanovic - Copyright (c) 2024, Stogl Robotics - - ament_cmake - - myrobot_description - - controller_manager - - forward_command_controller - - joint_state_broadcaster - - joint_trajectory_controller - - robot_state_publisher - - ros2_controllers_test_nodes - - rviz2 - - xacro - - - ament_cmake - - diff --git a/myrobot_ws/src/myrobot_description/CMakeLists.txt b/myrobot_ws/src/myrobot_description/CMakeLists.txt deleted file mode 100644 index 488fbe58..00000000 --- a/myrobot_ws/src/myrobot_description/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(myrobot_description) - -if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -install( - DIRECTORY config launch meshes rviz urdf test - DESTINATION share/${PROJECT_NAME} -) - -if(BUILD_TESTING) -find_package(ament_cmake_pytest REQUIRED) - - ament_add_pytest_test(test_myrobot_urdf_xacro test/myrobot_test_urdf_xacro.py) -endif() - -ament_package() diff --git a/myrobot_ws/src/myrobot_description/README.md b/myrobot_ws/src/myrobot_description/README.md deleted file mode 100644 index 409cb751..00000000 --- a/myrobot_ws/src/myrobot_description/README.md +++ /dev/null @@ -1,61 +0,0 @@ -myrobot_description\n\n - -Descriptions, meshes, and visualization files for the robots and environments. Corresponding launch and test files are also stored here. - -The structure and files in this package are generated using [RosTeamWorkspace script setup-robot-description](https://rtw.stoglrobotics.de/master/use-cases/ros_packages/setup_robot_description_package.html). You can use the same script to generate initial files for other robots. - -## General details about robot description packages - -This description package follows, as much as possible, the recommendations from [ROS-Industrial Consortium about robot support packages](https://wiki.ros.org/Industrial/Tutorials/WorkingWithRosIndustrialRobotSupportPackages). -The general package structure is the following: - -``` -_description/ # Robot's description files -├── [CMakeLists.txt] # if ament_cmake is used (recommended) -├── package.xml -├── [setup.py] # if amend_python is used -├── [setup.cfg] # if amend_python is used -├── config/ # general YAML files for a robot -│ └── _.yaml -├── launch/ # launch files related to testing robots' description -│ └── test__description.launch.py -├── meshes/ # meshes used in _macro.urdf.xacro -│ ├── collision -│ │ └── # meshes are sorted by robot name or model -│ │ ├── .stl -│ │ └── ... -│ └── visual -│ └── -│ ├── .dae -│ └── ... -├── rviz/ # rviz display configurations -│ └── _default.rviz -└── urdf/ # URDF file for the robot - ├── common.xacro # Common XACRO definitions - ├── .urdf.xacro # Main URDF for a robot - loads macro and other files - └── - ├── _macro.xacro # Macro file of the robot - can add prefix, define origin, etc. - └── _macro.ros2_control.xacro # URDF-part used to configure ros2_control - -``` - -### Testing the validity of robot description - -1. Go to the root of your workspace folder (there where `src`, `build`, `install` and `log` files are). -2. Install the package by calling `colcon build --symlink-install --packages-select myrobot_description` -3. (Re-)Source environment `source install/setup.bash` - - -> **NOTE:** If you use [RosTeamWorkspace (RTW)](https://rtw.stoglrobotics.de) than instead of the previous three steps, use `cb myrobot_description` command. - -Now, launch description test: -``` -ros2 launch myrobot_description view_myrobot.launch.xml -``` -or -``` -ros2 launch myrobot_description view_myrobot.launch.py -``` - -If there are no issues with the description, two windows are opened: `rviz2` and `Joint State Publisher`. -Rviz2 visualizes the robot's state and Joint state Publisher to changes joint values using sliders or generates random but valid configurations. diff --git a/myrobot_ws/src/myrobot_description/config/.gitkeep b/myrobot_ws/src/myrobot_description/config/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/myrobot_ws/src/myrobot_description/launch/view_myrobot.launch.py b/myrobot_ws/src/myrobot_description/launch/view_myrobot.launch.py deleted file mode 100755 index c39d185a..00000000 --- a/myrobot_ws/src/myrobot_description/launch/view_myrobot.launch.py +++ /dev/null @@ -1,99 +0,0 @@ -# Copyright (c) 2024, 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. - -# -# Source of this file is https://github.com/StoglRobotics/ros_team_workspace repository. -# -# Author: Dr. Denis -# - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="myrobot_description", - description="Description package of the myrobot. Usually the argument is not set, \ - it enables use of a custom description.", - ) - ) - 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") - prefix = LaunchConfiguration("prefix") - - # Get URDF via xacro - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(description_package), "urdf", "myrobot.urdf.xacro"] - ), - " ", - "prefix:=", - prefix, - " ", - ] - ) - - robot_description = {"robot_description": robot_description_content} - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", "myrobot.rviz"] - ) - - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - ) - - return LaunchDescription( - declared_arguments - + [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - ) diff --git a/myrobot_ws/src/myrobot_description/launch/view_myrobot.launch.xml b/myrobot_ws/src/myrobot_description/launch/view_myrobot.launch.xml deleted file mode 100644 index 1363064c..00000000 --- a/myrobot_ws/src/myrobot_description/launch/view_myrobot.launch.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/myrobot_ws/src/myrobot_description/meshes/myrobot/collision/.gitkeep b/myrobot_ws/src/myrobot_description/meshes/myrobot/collision/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/myrobot_ws/src/myrobot_description/meshes/myrobot/visual/.gitkeep b/myrobot_ws/src/myrobot_description/meshes/myrobot/visual/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/myrobot_ws/src/myrobot_description/package.xml b/myrobot_ws/src/myrobot_description/package.xml deleted file mode 100644 index 819bcf68..00000000 --- a/myrobot_ws/src/myrobot_description/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - myrobot_description - 0.0.0 - Test package - Nibanovic - Copyright (c) 2024, Stogl Robotics - - ament_cmake - - joint_state_publisher_gui - robot_state_publisher - rviz2 - xacro - - ament_cmake_pytest - launch_testing_ament_cmake - launch_testing_ros - liburdfdom-tools - xacro - - - ament_cmake - - diff --git a/myrobot_ws/src/myrobot_description/rviz/myrobot.rviz b/myrobot_ws/src/myrobot_description/rviz/myrobot.rviz deleted file mode 100644 index 5cfdaad3..00000000 --- a/myrobot_ws/src/myrobot_description/rviz/myrobot.rviz +++ /dev/null @@ -1,248 +0,0 @@ - -Panels: - - Class: rviz_common/Displays - Help Height: 87 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /RobotModel1 - - /TF1 - Splitter Ratio: 0.5 - Tree Height: 756 - - 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: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - flange: - Alpha: 1 - Show Axes: false - Show Trail: false - link_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: RobotModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - base: - Value: true - base_link: - Value: true - flange: - Value: true - link_1: - Value: true - link_2: - Value: true - link_3: - Value: true - link_4: - Value: true - link_5: - Value: true - link_6: - Value: true - tool0: - Value: true - world: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - world: - base_link: - base: - {} - link_1: - link_2: - link_3: - link_4: - link_5: - link_6: - tool0: - flange: - {} - Update Interval: 0 - Value: 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: 2.962221622467041 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.34074074029922485 - Y: -0.6165840029716492 - Z: 0.3352876305580139 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.3553987741470337 - Target Frame: - Value: Orbit (rviz) - Yaw: 4.713573932647705 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1027 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000398fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004400000398000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000011000000398fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004400000398000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005040000039800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 1920 - Y: 24 diff --git a/myrobot_ws/src/myrobot_description/test/myrobot_test_urdf_xacro.py b/myrobot_ws/src/myrobot_description/test/myrobot_test_urdf_xacro.py deleted file mode 100644 index 183ac77f..00000000 --- a/myrobot_ws/src/myrobot_description/test/myrobot_test_urdf_xacro.py +++ /dev/null @@ -1,83 +0,0 @@ -# Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -# 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 FZI Forschungszentrum Informatik 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. - -# -# Source of this file is https://github.com/StoglRobotics/ros_team_workspace repository. -# Modified from tests in https://github.com/UniversalRobots/Universal_Robots_ROS2_Description -# -# Author: Lukas Sackewitz -# Author (template): Manuel Muth - -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 = "myrobot_description" - description_file = "myrobot.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/myrobot_ws/src/myrobot_description/urdf/common/inertials.xacro b/myrobot_ws/src/myrobot_description/urdf/common/inertials.xacro deleted file mode 100644 index c1a7d059..00000000 --- a/myrobot_ws/src/myrobot_description/urdf/common/inertials.xacro +++ /dev/null @@ -1,64 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/myrobot_ws/src/myrobot_description/urdf/common/materials.xacro b/myrobot_ws/src/myrobot_description/urdf/common/materials.xacro deleted file mode 100644 index 359cdbe9..00000000 --- a/myrobot_ws/src/myrobot_description/urdf/common/materials.xacro +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/myrobot_ws/src/myrobot_description/urdf/myrobot.urdf.xacro b/myrobot_ws/src/myrobot_description/urdf/myrobot.urdf.xacro deleted file mode 100644 index 125fb915..00000000 --- a/myrobot_ws/src/myrobot_description/urdf/myrobot.urdf.xacro +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/myrobot_ws/src/myrobot_description/urdf/myrobot/myrobot_macro.ros2_control.xacro b/myrobot_ws/src/myrobot_description/urdf/myrobot/myrobot_macro.ros2_control.xacro deleted file mode 100644 index 8b9772a5..00000000 --- a/myrobot_ws/src/myrobot_description/urdf/myrobot/myrobot_macro.ros2_control.xacro +++ /dev/null @@ -1,143 +0,0 @@ - - - - - - - - - mock_components/GenericSystem - ${mock_sensor_commands} - - - gazebo_ros2_control/GazeboSystem - - - gz_ros2_control/GazeboSimSystem - - - robot_hardware_inteface/RobotHardwareInteface - - - - - -1 - 1 - - - 0.0 - - - - - - - -1 - 1 - - - 0.0 - - - - - - - -1 - 1 - - - 0.0 - - - - - - - -1 - 1 - - - 0.0 - - - - - - - -1 - 1 - - - 0.0 - - - - - - - -1 - 1 - - - 0.0 - - - - - - - - - - - - tool0 - -100 - 100 - -100 - 100 - -200 - 200 - -10 - 10 - -10 - 10 - -15 - 15 - - - - - - - - - - ${simulation_controllers} - - - - - - - - - - - ${simulation_controllers} - ${prefix}controller_manager - - - - - - diff --git a/myrobot_ws/src/myrobot_description/urdf/myrobot/myrobot_macro.xacro b/myrobot_ws/src/myrobot_description/urdf/myrobot/myrobot_macro.xacro deleted file mode 100644 index e078b19f..00000000 --- a/myrobot_ws/src/myrobot_description/urdf/myrobot/myrobot_macro.xacro +++ /dev/null @@ -1,254 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/myrobot_ws/src/myrobot_moveit/CMakeLists.txt b/myrobot_ws/src/myrobot_moveit/CMakeLists.txt deleted file mode 100644 index 31300cca..00000000 --- a/myrobot_ws/src/myrobot_moveit/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(myrobot_moveit) - -if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -install( - DIRECTORY config launch rviz srdf - DESTINATION share/${PROJECT_NAME} -) - -if(BUILD_TESTING) -endif() - -ament_package() diff --git a/myrobot_ws/src/myrobot_moveit/config/move_group.yaml b/myrobot_ws/src/myrobot_moveit/config/move_group.yaml deleted file mode 100644 index 0264c560..00000000 --- a/myrobot_ws/src/myrobot_moveit/config/move_group.yaml +++ /dev/null @@ -1,79 +0,0 @@ -/**: - ros__parameters: -#-------- Moveit controllers - moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager - moveit_simple_controller_manager: - controller_names: - - joint_trajectory_controller # add or remove controllers as necessary - - joint_trajectory_controller: - type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - -#-------- kinematics - robot_description_kinematics: - myrobot_manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 - -#-------- joint limits - robot_description_planning: - default_velocity_scaling_factor: 0.1 - default_acceleration_scaling_factor: 0.1 - # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] - # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] - joint_limits: - joint1: - has_velocity_limits: true - max_velocity: 1.7453292519943295 - has_acceleration_limits: true - max_acceleration: 5.0 - joint2: - has_velocity_limits: true - max_velocity: 1.5707963267948966 - has_acceleration_limits: true - max_acceleration: 5.0 - joint3: - has_velocity_limits: true - max_velocity: 1.5707963267948966 - has_acceleration_limits: true - max_acceleration: 5.0 - joint4: - has_velocity_limits: true - max_velocity: 2.9670597283903604 - has_acceleration_limits: true - max_acceleration: 5.0 - joint5: - has_velocity_limits: true - max_velocity: 2.0943951023931953 - has_acceleration_limits: true - max_acceleration: 5.0 - joint6: - has_velocity_limits: true - max_velocity: 3.3161255787892263 - has_acceleration_limits: true - max_acceleration: 5.0 - -#-------- other - moveit_manage_controllers: false - trajectory_execution: - allowed_execution_duration_scaling: 1.2 - allowed_goal_duration_margin: 0.5 - allowed_start_tolerance: 0.01 - capabilities: move_group/MoveGroupExecuteTrajectoryAction - publish_planning_scene: True - publish_geometry_updates: True - publish_state_updates: True - publish_transforms_updates: True - - \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_moveit/config/ompl_planning.yaml b/myrobot_ws/src/myrobot_moveit/config/ompl_planning.yaml deleted file mode 100644 index d198a254..00000000 --- a/myrobot_ws/src/myrobot_moveit/config/ompl_planning.yaml +++ /dev/null @@ -1,72 +0,0 @@ -/**: - ros__parameters: - move_group: - planning_plugins: - - ompl_interface/OMPLPlanner - request_adapters: - - default_planning_request_adapters/ResolveConstraintFrames - - default_planning_request_adapters/ValidateWorkspaceBounds - - default_planning_request_adapters/CheckStartStateBounds - - default_planning_request_adapters/CheckStartStateCollision - response_adapters: - - default_planning_response_adapters/AddTimeOptimalParameterization - - default_planning_response_adapters/ValidateSolution - - default_planning_response_adapters/DisplayMotionPath - myrobot_manipulator: - planner_configs: - SBLkConfigDefault: - type: geometric::SBL - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ESTkConfigDefault: - type: geometric::EST - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - LBKPIECEkConfigDefault: - type: geometric::LBKPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - BKPIECEkConfigDefault: - type: geometric::BKPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 - failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - KPIECEkConfigDefault: - type: geometric::KPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] - failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - RRTkConfigDefault: - type: geometric::RRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - RRTConnectkConfigDefault: - type: geometric::RRTConnect - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - RRTstarkConfigDefault: - type: geometric::RRTstar - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 - TRRTkConfigDefault: - type: geometric::TRRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - max_states_failed: 10 # when to start increasing temp. default: 10 - temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 - min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 - init_temperature: 10e-6 # initial temperature. default: 10e-6 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() - frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 - k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() - PRMkConfigDefault: - type: geometric::PRM - max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 - PRMstarkConfigDefault: - type: geometric::PRMstar - ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE - #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) - longest_valid_segment_fraction: 0.01 \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py b/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py deleted file mode 100644 index ba31e8ad..00000000 --- a/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py +++ /dev/null @@ -1,179 +0,0 @@ -from launch.actions import DeclareLaunchArgument -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -from launch import LaunchDescription -from launch.actions import OpaqueFunction - -def launch_setup(context, *args, **kwargs): - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - moveit_package = LaunchConfiguration("moveit_package") - semantic_description_file = LaunchConfiguration("semantic_description_file") - prefix = LaunchConfiguration("prefix") - use_mock_hardware = LaunchConfiguration("use_mock_hardware") - mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") - use_sim_time = LaunchConfiguration("use_sim_time") - logging_severity = LaunchConfiguration("severity") - - # -------------------------------------------------------# - # MoveIt2 MoveGroup setup # - # -------------------------------------------------------# - - # URDF - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(description_package), "urdf", description_file] - ), - " ", - "prefix:=", - prefix, - " ", - "use_mock_hardware:=", - use_mock_hardware, - " ", - "mock_sensor_commands:=", - mock_sensor_commands, - " ", - ] - ) - # SRDF - robot_description_semantic_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(moveit_package), "srdf", semantic_description_file] - ), - ] - ) - - robot_description = {"robot_description": robot_description_content.perform(context)} - robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content.perform(context)} - planning_config = PathJoinSubstitution([FindPackageShare(moveit_package), "config", "ompl_planning.yaml"]) - move_group_config = PathJoinSubstitution([FindPackageShare(moveit_package), "config", "move_group.yaml"]) - - # -------------------------------------------------------# - # Move Group Node # - # -------------------------------------------------------# - move_group_node = Node( - package="moveit_ros_move_group", - executable="move_group", - output="screen", - arguments=["--ros-args", "--log-level", logging_severity], - parameters=[ - robot_description, - robot_description_semantic, - planning_config, - move_group_config, - {"use_sim_time": use_sim_time}, - ], - ) - - # RViz - rviz_config = PathJoinSubstitution( - [FindPackageShare(moveit_package), "rviz", "moveit.rviz"] - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - output="log", - arguments=["-d", rviz_config, "--ros-args", "--log-level", "WARN"], - parameters=[ - robot_description, - robot_description_semantic, - planning_config, - move_group_config, - {"use_sim_time": use_sim_time}, - ], - ) - - return [ - move_group_node, - rviz_node, - ] - - -def generate_launch_description(): - # - # ------------- Declare arguments ------------- # - # - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="myrobot_description", - description="Package with the robot URDF/XACRO files. \ - Usually the argument is not set, it enables use of a custom setup.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="myrobot.urdf.xacro", - description="URDF/XACRO description file \ - Usually the argument is not set, it enables use of a custom setup.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "moveit_package", - default_value="myrobot_moveit", - description="MoveIt config package with robot SRDF/XACRO files. \ - Usually the argument is not set, it enables use of a custom setup.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "semantic_description_file", - default_value="myrobot.srdf.xacro", - description="MoveIt SRDF/XACRO description file with the robot.. \ - Usually the argument is not set, it enables use of a custom setup.", - ) - ) - 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.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "use_mock_hardware", - default_value="true", - description="Start robot with mock hardware mirroring command to its states.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "mock_sensor_commands", - default_value="false", - description="Enable mock command interfaces for sensors used for simple simulations. \ - Used only if 'use_mock_hardware' parameter is true.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "use_sim_time", - default_value="false", - description="Use simulation clock instead of world clock", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "severity", - default_value="INFO", - choices=["INFO", "WARN", "ERROR", "DEBUG", "FATAL"], - description="Logging level for the nodes", - ) - ) - - return LaunchDescription( - declared_arguments + [OpaqueFunction(function=launch_setup)] - ) diff --git a/myrobot_ws/src/myrobot_moveit/package.xml b/myrobot_ws/src/myrobot_moveit/package.xml deleted file mode 100644 index 14a0b469..00000000 --- a/myrobot_ws/src/myrobot_moveit/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - myrobot_moveit - 0.0.0 - test moveit config template - Nibanovic - Copyright (c) 2024, Stogl Robotics - - ament_cmake - - myrobot_description - xacro - - - moveit_ros_move_group - moveit_kinematics - moveit_planners - moveit_simple_controller_manager - - - ament_cmake - - diff --git a/myrobot_ws/src/myrobot_moveit/rviz/moveit.rviz b/myrobot_ws/src/myrobot_moveit/rviz/moveit.rviz deleted file mode 100644 index 3177bc9a..00000000 --- a/myrobot_ws/src/myrobot_moveit/rviz/moveit.rviz +++ /dev/null @@ -1,443 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /RobotModel1/Links1/link61 - - /TF1/Tree1 - - /TF1/Tree1/world1 - - /TF1/Tree1/world1/base_link1 - - /TF1/Tree1/world1/base_link1/link11 - - /MotionPlanning1 - Splitter Ratio: 0.5 - Tree Height: 438 - - 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: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - flange: - Alpha: 1 - Show Axes: false - Show Trail: false - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Mass Properties: - Inertia: false - Mass: false - Name: RobotModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - base: - Value: true - base_link: - Value: true - flange: - Value: true - link1: - Value: true - link2: - Value: true - link3: - Value: true - link4: - Value: true - link5: - Value: true - link6: - Value: true - tool0: - Value: true - world: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - world: - base_link: - base: - {} - link1: - link2: - link3: - link4: - link5: - link6: - tool0: - flange: - {} - Update Interval: 0 - Value: true - - Acceleration_Scaling_Factor: 0.1 - Class: moveit_rviz_plugin/MotionPlanning - Enabled: true - Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false - MoveIt_Allow_External_Program: false - MoveIt_Allow_Replanning: false - MoveIt_Allow_Sensor_Positioning: false - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Cartesian_Path: false - MoveIt_Use_Constraint_Aware_IK: false - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 - Name: MotionPlanning - Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - flange: - Alpha: 1 - Show Axes: false - Show Trail: false - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Loop Animation: false - Robot Alpha: 0.5 - Robot Color: 150; 50; 150 - Show Robot Collision: false - Show Robot Visual: true - Show Trail: false - State Display Time: 3x - Trail Step Size: 1 - Trajectory Topic: /display_planned_path - Use Sim Time: false - Planning Metrics: - Payload: 1 - Show Joint Torques: false - Show Manipulability: false - Show Manipulability Index: false - Show Weight Limit: false - TextHeight: 0.07999999821186066 - Planning Request: - Colliding Link Color: 255; 0; 0 - Goal State Alpha: 1 - Goal State Color: 250; 128; 0 - Interactive Marker Size: 0 - Joint Violation Color: 255; 0; 255 - Planning Group: my_robot_manipulator - Query Goal State: true - Query Start State: false - Show Workspace: false - Start State Alpha: 1 - Start State Color: 0; 255; 0 - Planning Scene Topic: /monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 0.8999999761581421 - Scene Color: 50; 230; 50 - Scene Display Time: 0.009999999776482582 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - flange: - Alpha: 1 - Show Axes: false - Show Trail: false - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true - Value: true - Velocity_Scaling_Factor: 0.1 - 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 - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - 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: 2.962221622467041 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.8273206949234009 - Y: -0.11996828764677048 - Z: 0.4445207118988037 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.38039883971214294 - Target Frame: - Value: Orbit (rviz) - Yaw: 5.253565788269043 - Saved: ~ -Window Geometry: - "": - collapsed: false - " - Trajectory Slider": - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f30000039efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d00000210000000e60100001cfa000000010100000002fb0000000a005600690065007700730100000000ffffffff0000010000fffffffb000000100044006900730070006c0061007900730100000000000001f30000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fbffffffff0100000253000001880000017d00ffffff00000001000001100000039efc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005870000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 1366 - Y: 27 diff --git a/myrobot_ws/src/myrobot_moveit/srdf/myrobot.srdf.xacro b/myrobot_ws/src/myrobot_moveit/srdf/myrobot.srdf.xacro deleted file mode 100644 index 677264bc..00000000 --- a/myrobot_ws/src/myrobot_moveit/srdf/myrobot.srdf.xacro +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/myrobot_ws/src/myrobot_moveit/srdf/myrobot_macro.srdf.xacro b/myrobot_ws/src/myrobot_moveit/srdf/myrobot_macro.srdf.xacro deleted file mode 100644 index 4224afc9..00000000 --- a/myrobot_ws/src/myrobot_moveit/srdf/myrobot_macro.srdf.xacro +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/templates/moveit/configs_here_ryan b/templates/moveit/configs_here_ryan deleted file mode 100644 index e69de29b..00000000 diff --git a/templates/robot_description/robot_srdf_examples.xacro b/templates/robot_description/robot_srdf_examples.xacro deleted file mode 100644 index 8a7ef973..00000000 --- a/templates/robot_description/robot_srdf_examples.xacro +++ /dev/null @@ -1,47 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - TBA - - From 87086b5cf3122039cc047dcdac40bb20c7cc7c18 Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Mon, 8 Apr 2024 10:54:35 +0200 Subject: [PATCH 14/23] add .xml launch file --- scripts/setup-robot-moveit.bash | 14 +++--- templates/moveit/moveit.launch.py | 4 +- templates/moveit/moveit.launch.xml | 64 +++++++++++++++++++++++++ templates/moveit/robot_macro.srdf.xacro | 14 +++--- 4 files changed, 80 insertions(+), 16 deletions(-) create mode 100644 templates/moveit/moveit.launch.xml diff --git a/scripts/setup-robot-moveit.bash b/scripts/setup-robot-moveit.bash index 9d53f81f..4937b7f7 100755 --- a/scripts/setup-robot-moveit.bash +++ b/scripts/setup-robot-moveit.bash @@ -105,15 +105,15 @@ for file_type in "${LAUNCH_FILE_TYPES[@]}"; do # Copy the templates to the destination with the specified file type cp -n "$MOVEIT_TEMPLATES/moveit.launch${file_type}" "${MOVEIT_LAUNCH}" -done -# sed all needed files -FILES_TO_SED=($MOVEIT_LAUNCH $ROBOT_SRDF $ROBOT_SRDF_MACRO $MOVE_GROUP_CONFIG_YAML $OMPL_PLANNING_CONFIG_YAML) + # sed all needed files + FILES_TO_SED=($MOVEIT_LAUNCH $ROBOT_SRDF $ROBOT_SRDF_MACRO $MOVE_GROUP_CONFIG_YAML $OMPL_PLANNING_CONFIG_YAML) -for SED_FILE in "${FILES_TO_SED[@]}"; do - sed -i "s/\\\$PKG_NAME\\\$/${PKG_NAME}/g" $SED_FILE - sed -i "s/\\\$ROBOT_NAME\\\$/${ROBOT_NAME}/g" $SED_FILE - sed -i "s/\\\$DESCR_PKG_NAME\\\$/${DESCR_PKG_NAME}/g" $SED_FILE + for SED_FILE in "${FILES_TO_SED[@]}"; do + sed -i "s/\\\$PKG_NAME\\\$/${PKG_NAME}/g" $SED_FILE + sed -i "s/\\\$ROBOT_NAME\\\$/${ROBOT_NAME}/g" $SED_FILE + sed -i "s/\\\$DESCR_PKG_NAME\\\$/${DESCR_PKG_NAME}/g" $SED_FILE + done done # package.xml: Add dependencies if they not exist diff --git a/templates/moveit/moveit.launch.py b/templates/moveit/moveit.launch.py index 5babd891..fde3701c 100644 --- a/templates/moveit/moveit.launch.py +++ b/templates/moveit/moveit.launch.py @@ -130,7 +130,7 @@ def generate_launch_description(): DeclareLaunchArgument( "semantic_description_file", default_value="$ROBOT_NAME$.srdf.xacro", - description="MoveIt SRDF/XACRO description file with the robot.. \ + description="MoveIt SRDF/XACRO description file with the robot. \ Usually the argument is not set, it enables use of a custom setup.", ) ) @@ -170,7 +170,7 @@ def generate_launch_description(): "severity", default_value="INFO", choices=["INFO", "WARN", "ERROR", "DEBUG", "FATAL"], - description="Logging level for the nodes", + description="Logging level for the nodes.", ) ) diff --git a/templates/moveit/moveit.launch.xml b/templates/moveit/moveit.launch.xml new file mode 100644 index 00000000..6bc259c4 --- /dev/null +++ b/templates/moveit/moveit.launch.xml @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/templates/moveit/robot_macro.srdf.xacro b/templates/moveit/robot_macro.srdf.xacro index 0b01420b..d8036b79 100644 --- a/templates/moveit/robot_macro.srdf.xacro +++ b/templates/moveit/robot_macro.srdf.xacro @@ -1,16 +1,16 @@ - + - - - - - - + + + + + From 2b4900b18cee0046f56a890506f8804a55cca577 Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Mon, 8 Apr 2024 11:14:57 +0200 Subject: [PATCH 15/23] add README.md instructions --- scripts/setup-robot-moveit.bash | 7 +++- templates/moveit/append_to_MAIN_README.md | 15 ++++++++ templates/moveit/append_to_README.md | 44 +++++++++++++++++++++++ 3 files changed, 65 insertions(+), 1 deletion(-) create mode 100644 templates/moveit/append_to_MAIN_README.md create mode 100644 templates/moveit/append_to_README.md diff --git a/scripts/setup-robot-moveit.bash b/scripts/setup-robot-moveit.bash index 4937b7f7..38ce3938 100755 --- a/scripts/setup-robot-moveit.bash +++ b/scripts/setup-robot-moveit.bash @@ -139,7 +139,12 @@ done prepend_to_string="if(BUILD_TESTING)" sed -i "s/$prepend_to_string/install\(\\n DIRECTORY config launch rviz srdf\\n DESTINATION share\/\$\{PROJECT_NAME\}\\n\)\\n\\n$prepend_to_string/g" CMakeLists.txt -# TODO: add README with general instructions +# extend README with general instructions +if [ -f README.md ]; then + cat $MOVEIT_TEMPLATES/append_to_README.md >>README.md + sed -i "s/\\\$PKG_NAME\\\$/${PKG_NAME}/g" README.md + sed -i "s/\\\$ROBOT_NAME\\\$/${ROBOT_NAME}/g" README.md +fi # TODO: Add license checks diff --git a/templates/moveit/append_to_MAIN_README.md b/templates/moveit/append_to_MAIN_README.md new file mode 100644 index 00000000..031fa4af --- /dev/null +++ b/templates/moveit/append_to_MAIN_README.md @@ -0,0 +1,15 @@ + + +## Moving the robot using MoveIt2 +> **NOTE:** If you are not familiar with MoveIt2, check the docs for some [useful examples](https://moveit.picknik.ai/main/doc/tutorials/getting_started/getting_started.html) + +To move the robot using Moveit2, we first bring up the robot with the mock hardware enabled: +``` +ros2 launch $ROBOT_NAME$.launch.xml use_mock_hardware:=true +``` + +After that, in another terminal we launch MoveIt2: +``` +ros2 launch $ROBOT_NAME$_moveit moveit.launch.xml +``` +Now we can use the `MotionPlanning` widget in `rviz2` to assign goals, plan and execute motions. diff --git a/templates/moveit/append_to_README.md b/templates/moveit/append_to_README.md new file mode 100644 index 00000000..a21e9801 --- /dev/null +++ b/templates/moveit/append_to_README.md @@ -0,0 +1,44 @@ + + +## General details about robot MoveIt2 packages + +A moveit package holds config and launch files for robotic manipulation using MoveIt2. +It builds on description and bringup packages generated by `setup-robot-description` and `setup-robot-bringup`. + +The general package structure is the following: + +``` +$PKG_NAME$/ # Launch and config files for robot manipulation using MoveIt +├── [CMakeLists.txt] # if ament_cmake is used (recommended) +├── package.xml +├── [setup.py] # if ament_python is used +├── [setup.cfg] # if ament_python is used +├── config/ +│ ├── move_group.yaml # Various configuration needed for move_group_node: controllers, kinematics, action execution... +│ ├── _planning.yaml # Specific planner configuration. Default is OMPL +└── launch/ + ├── moveit.launch.py # MoveIt launch file. +└── srdf/ + ├── $ROBOT_NAME$_macro.srdf.xacro # Semantic robot description macro + ├── $ROBOT_NAME$.srdf.xacro # Semantic robot description required for MoveIt. +└── rviz/ + ├── moveit.launch.py # RViZ config with MoveIt MotionPlanning widget. + +``` +## Compiling the package + +To sucessfuly compile and run this package, it is necessary to install `MoveIt`. For a simple binary install, [follow the official guide](https://moveit.ros.org/install-moveit2/binary/) + +## Moving the *mock_robot* using MoveIt2 + +1. Start robot's hardware and load controllers (default configuration starts mock hardware) + ``` + ros2 launch $ROBOT_NAME$.launch.xml + ``` + +2. Open another terminal and launch MoveIt + ``` + ros2 launch $ROBOT_NAME$_moveit moveit.launch.xml + ``` + +3. You should now be able to use `MotionPlanning` widget in `rviz2` to assign, plan and execute robot motion. From e6491fef742ffb7fe0105ada0233d1f496ce5704 Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Mon, 8 Apr 2024 11:17:33 +0200 Subject: [PATCH 16/23] add licenses --- templates/moveit/move_group.yaml | 16 ++++++++++++++++ templates/moveit/moveit.launch.py | 16 ++++++++++++++++ templates/moveit/ompl_planning.yaml | 16 ++++++++++++++++ templates/moveit/robot.srdf.xacro | 18 ++++++++++++++++++ templates/moveit/robot_macro.srdf.xacro | 18 ++++++++++++++++++ 5 files changed, 84 insertions(+) diff --git a/templates/moveit/move_group.yaml b/templates/moveit/move_group.yaml index 908d6532..045b3dd2 100644 --- a/templates/moveit/move_group.yaml +++ b/templates/moveit/move_group.yaml @@ -1,3 +1,19 @@ +# Copyright (c) 2024, 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. +# +# Source of this file are templates in https://github.com/StoglRobotics/ros_team_workspace repository. + /**: ros__parameters: #-------- Moveit controllers diff --git a/templates/moveit/moveit.launch.py b/templates/moveit/moveit.launch.py index fde3701c..a05690b4 100644 --- a/templates/moveit/moveit.launch.py +++ b/templates/moveit/moveit.launch.py @@ -1,3 +1,19 @@ +# Copyright (c) 2024, 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. +# +# Source of this file are templates in https://github.com/StoglRobotics/ros_team_workspace repository. + from launch.actions import DeclareLaunchArgument from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node diff --git a/templates/moveit/ompl_planning.yaml b/templates/moveit/ompl_planning.yaml index c90d7c9b..307533a9 100644 --- a/templates/moveit/ompl_planning.yaml +++ b/templates/moveit/ompl_planning.yaml @@ -1,3 +1,19 @@ +# Copyright (c) 2024, 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. +# +# Source of this file are templates in https://github.com/StoglRobotics/ros_team_workspace repository. + /**: ros__parameters: move_group: diff --git a/templates/moveit/robot.srdf.xacro b/templates/moveit/robot.srdf.xacro index 27e021fe..1aadb143 100644 --- a/templates/moveit/robot.srdf.xacro +++ b/templates/moveit/robot.srdf.xacro @@ -1,3 +1,21 @@ + + diff --git a/templates/moveit/robot_macro.srdf.xacro b/templates/moveit/robot_macro.srdf.xacro index d8036b79..22cefd28 100644 --- a/templates/moveit/robot_macro.srdf.xacro +++ b/templates/moveit/robot_macro.srdf.xacro @@ -1,3 +1,21 @@ + + From 11b32409d0f8b847aa7bc55e0c488a3b9cb80cc4 Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Thu, 11 Apr 2024 15:05:25 +0200 Subject: [PATCH 17/23] adjust existing description files * lift robot so it does not clip to ground plane in sim * new joint position for joint2, same reason as above --- templates/robot_description/robot.rviz | 67 ++++++++++--------- templates/robot_description/robot.urdf.xacro | 2 +- templates/robot_description/robot_macro.xacro | 2 +- 3 files changed, 38 insertions(+), 33 deletions(-) diff --git a/templates/robot_description/robot.rviz b/templates/robot_description/robot.rviz index 5cfdaad3..b9d86a1e 100644 --- a/templates/robot_description/robot.rviz +++ b/templates/robot_description/robot.rviz @@ -1,4 +1,3 @@ - Panels: - Class: rviz_common/Displays Help Height: 87 @@ -10,7 +9,7 @@ Panels: - /RobotModel1 - /TF1 Splitter Ratio: 0.5 - Tree Height: 756 + Tree Height: 778 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -76,32 +75,32 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - link_1: + link1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + link2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + link3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + link4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + link5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + link6: Alpha: 1 Show Axes: false Show Trail: false @@ -114,6 +113,9 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + Mass Properties: + Inertia: false + Mass: false Name: RobotModel TF Prefix: "" Update Interval: 0 @@ -130,17 +132,17 @@ Visualization Manager: Value: true flange: Value: true - link_1: + link1: Value: true - link_2: + link2: Value: true - link_3: + link3: Value: true - link_4: + link4: Value: true - link_5: + link5: Value: true - link_6: + link6: Value: true tool0: Value: true @@ -156,12 +158,12 @@ Visualization Manager: base_link: base: {} - link_1: - link_2: - link_3: - link_4: - link_5: - link_6: + link1: + link2: + link3: + link4: + link5: + link6: tool0: flange: {} @@ -170,7 +172,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: base_link + Fixed Frame: world Frame Rate: 30 Name: root Tools: @@ -182,6 +184,9 @@ Visualization Manager: - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile @@ -210,33 +215,33 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 2.962221622467041 + Distance: 3.6623029708862305 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.34074074029922485 - Y: -0.6165840029716492 - Z: 0.3352876305580139 + X: 0.8972788453102112 + Y: -0.37396764755249023 + Z: 0.9526680707931519 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.3553987741470337 + Pitch: 0.3503987491130829 Target Frame: Value: Orbit (rviz) - Yaw: 4.713573932647705 + Yaw: 5.3735857009887695 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1027 + Height: 1016 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000398fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004400000398000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000011000000398fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004400000398000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005040000039800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001100000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004fa0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: @@ -244,5 +249,5 @@ Window Geometry: Views: collapsed: false Width: 1920 - X: 1920 - Y: 24 + X: 1366 + Y: 27 diff --git a/templates/robot_description/robot.urdf.xacro b/templates/robot_description/robot.urdf.xacro index 389c4965..88b9ba20 100644 --- a/templates/robot_description/robot.urdf.xacro +++ b/templates/robot_description/robot.urdf.xacro @@ -19,7 +19,7 @@ - + - + From d7289204401a839e4c6e7dbcb2dfddb0f07893f4 Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Thu, 11 Apr 2024 15:06:12 +0200 Subject: [PATCH 18/23] update gazebo launch file and add env hooks * env hook file will be in the description package so it exposes meshes to gazebo during build --- .../robot_ros2_control_sim.launch.py | 17 +---------------- templates/ros2_control/robot_sim.dsv.in | 4 ++++ 2 files changed, 5 insertions(+), 16 deletions(-) create mode 100644 templates/ros2_control/robot_sim.dsv.in diff --git a/templates/ros2_control/robot_ros2_control_sim.launch.py b/templates/ros2_control/robot_ros2_control_sim.launch.py index 44cb3f0f..1741952a 100644 --- a/templates/ros2_control/robot_ros2_control_sim.launch.py +++ b/templates/ros2_control/robot_ros2_control_sim.launch.py @@ -75,14 +75,6 @@ def generate_launch_description(): have to be updated.", ) ) - declared_arguments.append( - DeclareLaunchArgument( - "robot_controller", - default_value="forward_position_controller", - choices=["forward_position_controller", "joint_trajectory_controller"], - description="Robot controller to start.", - ) - ) # Initialize Arguments runtime_config_package = LaunchConfiguration("runtime_config_package") @@ -90,7 +82,6 @@ def generate_launch_description(): description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") prefix = LaunchConfiguration("prefix") - robot_controller = LaunchConfiguration("robot_controller") robot_controllers = PathJoinSubstitution( [FindPackageShare(runtime_config_package), "config", controllers_file] @@ -112,12 +103,6 @@ def generate_launch_description(): "prefix:=", prefix, " ", - "use_mock_hardware:=false", - " ", - "mock_sensor_commands:=false", - " ", - "sim_gazebo_classic:=false", - " ", "sim_gazebo:=true", " ", "simulation_controllers:=", @@ -164,7 +149,7 @@ def generate_launch_description(): arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) - robot_controllers = [robot_controller] + robot_controllers = ["joint_trajectory_controller"] robot_controller_spawners = [] for controller in robot_controllers: robot_controller_spawners += [ diff --git a/templates/ros2_control/robot_sim.dsv.in b/templates/ros2_control/robot_sim.dsv.in new file mode 100644 index 00000000..f3371115 --- /dev/null +++ b/templates/ros2_control/robot_sim.dsv.in @@ -0,0 +1,4 @@ +# Explanation of env-hooks can be found here: https://github.com/ros-simulation/gazebo_ros_pkgs/wiki/ROS-2-Migration:-Gazebo-ROS-Paths#env-hooks + +prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share/ +prepend-non-duplicate;IGN_GAZEBO_MODEL_PATH;share/ From 966c61cb08160301694ada4ead03456d6fe5a7b9 Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Thu, 11 Apr 2024 15:34:38 +0200 Subject: [PATCH 19/23] updated bringup script to ask if gazebo is to be added --- scripts/setup-gazebo-bringup.bash | 57 +++++++++++++++++++++++++++++++ scripts/setup-robot-bringup.bash | 11 ++++-- 2 files changed, 66 insertions(+), 2 deletions(-) create mode 100755 scripts/setup-gazebo-bringup.bash diff --git a/scripts/setup-gazebo-bringup.bash b/scripts/setup-gazebo-bringup.bash new file mode 100755 index 00000000..be54de10 --- /dev/null +++ b/scripts/setup-gazebo-bringup.bash @@ -0,0 +1,57 @@ +#!/bin/bash +# +# Copyright 2021 Denis Stogl (Stogl Robotics Consulting) +# +# 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 script is run in setup-robot-bringup in case the user wants a gazebo starting point. +## + +# Create env hook folder +mkdir -p hooks + +# Copy env hook file +GAZEBO_ENV_HOOK="hooks/${ROBOT_NAME}_sim.dsv.in" +cp -n $ROS2_CONTROL_TEMPLATES/robot_sim.dsv.in $GAZEBO_ENV_HOOK + +# Copy launch files +GAZEBO_LAUNCH="launch/${ROBOT_NAME}_sim.launch.py" +cp -n "$ROS2_CONTROL_TEMPLATES/robot_ros2_control_sim.launch.py" "${GAZEBO_LAUNCH}" +# sed all needed files +FILES_TO_SED=($GAZEBO_LAUNCH) +for SED_FILE in "${FILES_TO_SED[@]}"; do + sed -i "s/\\\$PKG_NAME\\\$/${PKG_NAME}/g" $SED_FILE + sed -i "s/\\\$RUNTIME_CONFIG_PKG_NAME\\\$/${PKG_NAME}/g" $SED_FILE + sed -i "s/\\\$ROBOT_NAME\\\$/${ROBOT_NAME}/g" $SED_FILE + sed -i "s/\\\$DESCR_PKG_NAME\\\$/${DESCR_PKG_NAME}/g" $SED_FILE +done + +# package.xml: Add dependencies if they not exist +DEP_PKGS=("gz_ros2_control") + +for DEP_PKG in "${DEP_PKGS[@]}"; do + if $(grep -q $DEP_PKG package.xml); then + echo "'$DEP_PKG' is already listed in package.xml" + else + append_to_string="ament_cmake<\/buildtool_depend>" + sed -i "s/$append_to_string/$append_to_string\\n\\n ${DEP_PKG}<\/exec_depend>/g" package.xml + fi +done + +# CMakeLists.txt: Add install paths of the files +prepend_to_string="ament_package()" +sed -i "/$prepend_to_string/i\ +# Explanation of env-hooks can be found here: https://github.com/ros-simulation/gazebo_ros_pkgs/wiki/ROS-2-Migration:-Gazebo-ROS-Paths#env-hooks\\n\ +# tldr: enables gazebo to find meshes in this package.\\n\ +ament_environment_hooks(\"\${CMAKE_CURRENT_SOURCE_DIR}/hooks/${ROBOT_NAME}_sim.dsv.in\")\\n" CMakeLists.txt \ No newline at end of file diff --git a/scripts/setup-robot-bringup.bash b/scripts/setup-robot-bringup.bash index 91b70432..4f8a0bcf 100755 --- a/scripts/setup-robot-bringup.bash +++ b/scripts/setup-robot-bringup.bash @@ -16,8 +16,8 @@ usage="setup-robot-bringup ROBOT_NAME DESCRIPTION_PKG_NAME" -# Load Framework defines -script_own_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" + # Load Framework defines + script_own_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" source $script_own_dir/../setup.bash check_and_set_ros_distro_and_version "${ROS_DISTRO}" @@ -131,6 +131,13 @@ if [ -f README.md ]; then sed -i "s/\\\$DESCR_PKG_NAME\\\$/${DESCR_PKG_NAME}/g" $SED_FILE fi +# ask about gazebo setup +echo -e "\nDo you wish to add gazebo launch file? y/n" +read -p "> " add_gazebo_bringup +if [ $add_gazebo_bringup == "y" ]; then + source $script_own_dir/setup-gazebo-bringup.bash +fi + # TODO: Add license checks # Compile and add new package the to the path From 6e49fcc9cfe32f5cc3818980e38e38596afc3046 Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Thu, 11 Apr 2024 16:09:15 +0200 Subject: [PATCH 20/23] minor template adjustments --- templates/robot_description/robot.urdf.xacro | 2 +- templates/ros2_control/robot_ros2_control.launch.py | 2 +- templates/ros2_control/robot_ros2_control_sim.launch.py | 7 +++---- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/templates/robot_description/robot.urdf.xacro b/templates/robot_description/robot.urdf.xacro index 88b9ba20..af3cc343 100644 --- a/templates/robot_description/robot.urdf.xacro +++ b/templates/robot_description/robot.urdf.xacro @@ -19,7 +19,7 @@ - + Date: Thu, 11 Apr 2024 16:27:26 +0200 Subject: [PATCH 21/23] moved gazebo env hooks to description pkg template * now description package automatically exposes mesh files to gazebo * additionally, added deletion of include/ and src/ directiories when setting up description and bringup --- scripts/setup-gazebo-bringup.bash | 16 +------------- scripts/setup-robot-bringup.bash | 2 +- scripts/setup-robot-description.bash | 21 +++++++++++++++++++ .../robot_description/append_to_README.md | 2 ++ .../robot_sim.dsv.in | 3 +++ 5 files changed, 28 insertions(+), 16 deletions(-) rename templates/{ros2_control => robot_description}/robot_sim.dsv.in (71%) diff --git a/scripts/setup-gazebo-bringup.bash b/scripts/setup-gazebo-bringup.bash index be54de10..13d9e634 100755 --- a/scripts/setup-gazebo-bringup.bash +++ b/scripts/setup-gazebo-bringup.bash @@ -18,13 +18,6 @@ ## This script is run in setup-robot-bringup in case the user wants a gazebo starting point. ## -# Create env hook folder -mkdir -p hooks - -# Copy env hook file -GAZEBO_ENV_HOOK="hooks/${ROBOT_NAME}_sim.dsv.in" -cp -n $ROS2_CONTROL_TEMPLATES/robot_sim.dsv.in $GAZEBO_ENV_HOOK - # Copy launch files GAZEBO_LAUNCH="launch/${ROBOT_NAME}_sim.launch.py" cp -n "$ROS2_CONTROL_TEMPLATES/robot_ros2_control_sim.launch.py" "${GAZEBO_LAUNCH}" @@ -47,11 +40,4 @@ for DEP_PKG in "${DEP_PKGS[@]}"; do append_to_string="ament_cmake<\/buildtool_depend>" sed -i "s/$append_to_string/$append_to_string\\n\\n ${DEP_PKG}<\/exec_depend>/g" package.xml fi -done - -# CMakeLists.txt: Add install paths of the files -prepend_to_string="ament_package()" -sed -i "/$prepend_to_string/i\ -# Explanation of env-hooks can be found here: https://github.com/ros-simulation/gazebo_ros_pkgs/wiki/ROS-2-Migration:-Gazebo-ROS-Paths#env-hooks\\n\ -# tldr: enables gazebo to find meshes in this package.\\n\ -ament_environment_hooks(\"\${CMAKE_CURRENT_SOURCE_DIR}/hooks/${ROBOT_NAME}_sim.dsv.in\")\\n" CMakeLists.txt \ No newline at end of file +done \ No newline at end of file diff --git a/scripts/setup-robot-bringup.bash b/scripts/setup-robot-bringup.bash index 4f8a0bcf..3d1c399a 100755 --- a/scripts/setup-robot-bringup.bash +++ b/scripts/setup-robot-bringup.bash @@ -66,7 +66,7 @@ echo -e "${TERMINAL_COLOR_USER_CONFIRMATION}If correct press , otherwise read # Remove include and src folders - in this package should be no source -RM_FOLDERS=("include" "src") +RM_FOLDERS=("include/$PKG_NAME" "include" "src") for FOLDER in "${RM_FOLDERS[@]}"; do if [[ -d $FOLDER && ! "$(ls -A $FOLDER)" ]]; then diff --git a/scripts/setup-robot-description.bash b/scripts/setup-robot-description.bash index 9ac2530f..0c14d515 100755 --- a/scripts/setup-robot-description.bash +++ b/scripts/setup-robot-description.bash @@ -63,6 +63,15 @@ echo -e "${TERMINAL_COLOR_USER_NOTICE}ATTENTION: Setting up description configur echo -e "${TERMINAL_COLOR_USER_CONFIRMATION}If correct press , otherwise +C and start the script again from the package folder and/or with correct robot name.${TERMINAL_COLOR_NC}" read +# Remove include and src folders - in this package should be no source +RM_FOLDERS=("include/$PKG_NAME" "include" "src") + +for FOLDER in "${RM_FOLDERS[@]}"; do + if [[ -d $FOLDER && ! "$(ls -A $FOLDER)" ]]; then + rm -r $FOLDER + fi +done + # Create folders for meshes F_NAME="meshes/${ROBOT_NAME}/collision" mkdir -p $F_NAME @@ -186,6 +195,18 @@ pattern='if(BUILD_TESTING)' # Use sed to find the pattern and append the lines after it in CMakeLists.txt sed -i "/$pattern/a$lines_to_append" CMakeLists.txt + +## Expose mesh files to gazebo +mkdir -p hooks +GAZEBO_ENV_HOOK="hooks/${ROBOT_NAME}_sim.dsv.in" +cp -n $ROBOT_DESCRIPTION_TEMPLATES/robot_sim.dsv.in $GAZEBO_ENV_HOOK +# CMakeLists.txt: Add env hook +prepend_to_string="ament_package()" +sed -i "/$prepend_to_string/i\ +# Explanation of env-hooks can be found here: https://github.com/ros-simulation/gazebo_ros_pkgs/wiki/ROS-2-Migration:-Gazebo-ROS-Paths#env-hooks\\n\ +# tldr: enables gazebo to find meshes in this package.\\n\ +ament_environment_hooks(\"\${CMAKE_CURRENT_SOURCE_DIR}/hooks/${ROBOT_NAME}_sim.dsv.in\")\\n" CMakeLists.txt + # extend README with general instructions if [ ! -f README.md ]; then echo "${PKG_NAME}\n\n" > README.md diff --git a/templates/robot_description/append_to_README.md b/templates/robot_description/append_to_README.md index 8e8c06c2..2da4b192 100644 --- a/templates/robot_description/append_to_README.md +++ b/templates/robot_description/append_to_README.md @@ -16,6 +16,8 @@ The general package structure is the following: ├── [setup.cfg] # if amend_python is used ├── config/ # general YAML files for a robot │ └── _.yaml +├── hooks/ # environment hook which exposes mesh files to gazebo +│ └── _sim.dsv.in ├── launch/ # launch files related to testing robots' description │ └── test__description.launch.py ├── meshes/ # meshes used in _macro.urdf.xacro diff --git a/templates/ros2_control/robot_sim.dsv.in b/templates/robot_description/robot_sim.dsv.in similarity index 71% rename from templates/ros2_control/robot_sim.dsv.in rename to templates/robot_description/robot_sim.dsv.in index f3371115..bf5dbaa4 100644 --- a/templates/ros2_control/robot_sim.dsv.in +++ b/templates/robot_description/robot_sim.dsv.in @@ -2,3 +2,6 @@ prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share/ prepend-non-duplicate;IGN_GAZEBO_MODEL_PATH;share/ + +prepend-non-duplicate;GAZEBO_RESOURCE_PATH;share/ +prepend-non-duplicate;GAZEBO_MODEL_PATH;share/ From 4f41ce47f3eed49d05c4ab66e0950e766a8e2d0e Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Mon, 22 Apr 2024 10:55:26 +0200 Subject: [PATCH 22/23] issue [202]: add pathing flag for gazebo * during simulation, absolute file:// is used * on actual robot, relative package:// is used --- templates/robot_description/robot.urdf.xacro | 7 ++- templates/robot_description/robot_macro.xacro | 45 ++++++++++++------- 2 files changed, 35 insertions(+), 17 deletions(-) diff --git a/templates/robot_description/robot.urdf.xacro b/templates/robot_description/robot.urdf.xacro index af3cc343..d93fd39b 100644 --- a/templates/robot_description/robot.urdf.xacro +++ b/templates/robot_description/robot.urdf.xacro @@ -18,8 +18,11 @@ - - + + - + + + + + + + + + + @@ -15,7 +30,7 @@ - + @@ -23,7 +38,7 @@ - + @@ -36,7 +51,7 @@ - + @@ -44,7 +59,7 @@ - + @@ -58,7 +73,7 @@ - + @@ -66,7 +81,7 @@ - + @@ -79,7 +94,7 @@ - + @@ -87,7 +102,7 @@ - + @@ -100,7 +115,7 @@ - + @@ -108,7 +123,7 @@ - + @@ -121,7 +136,7 @@ - + @@ -129,7 +144,7 @@ - + @@ -142,7 +157,7 @@ - + @@ -150,7 +165,7 @@ - + From 3256e131306ff0778c789ce8e7007f52ff1b5fe3 Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Mon, 22 Apr 2024 11:09:36 +0200 Subject: [PATCH 23/23] remove mention of gazebo classic in templates * it is already not being accounted for in gazebo bringup files --- templates/robot_description/robot.urdf.xacro | 2 -- .../robot_macro.ros2_control.xacro | 17 +---------------- templates/robot_description/robot_sim.dsv.in | 3 --- 3 files changed, 1 insertion(+), 21 deletions(-) diff --git a/templates/robot_description/robot.urdf.xacro b/templates/robot_description/robot.urdf.xacro index d93fd39b..c46d3bed 100644 --- a/templates/robot_description/robot.urdf.xacro +++ b/templates/robot_description/robot.urdf.xacro @@ -6,7 +6,6 @@ - @@ -30,7 +29,6 @@ prefix="$(arg prefix)" use_mock_hardware="$(arg use_mock_hardware)" mock_sensor_commands="$(arg mock_sensor_commands)" - sim_gazebo_classic="$(arg sim_gazebo_classic)" sim_gazebo="$(arg sim_gazebo)" simulation_controllers="$(arg simulation_controllers)" /> diff --git a/templates/robot_description/robot_macro.ros2_control.xacro b/templates/robot_description/robot_macro.ros2_control.xacro index e7c7d075..da585278 100644 --- a/templates/robot_description/robot_macro.ros2_control.xacro +++ b/templates/robot_description/robot_macro.ros2_control.xacro @@ -6,7 +6,6 @@ prefix use_mock_hardware:=false mock_sensor_commands:=false - sim_gazebo_classic:=false sim_gazebo:=false simulation_controllers" > @@ -17,13 +16,10 @@ mock_components/GenericSystem ${mock_sensor_commands} - - gazebo_ros2_control/GazeboSystem - gz_ros2_control/GazeboSimSystem - + robot_hardware_inteface/RobotHardwareInteface @@ -116,17 +112,6 @@ - - - - - - - ${simulation_controllers} - - - - diff --git a/templates/robot_description/robot_sim.dsv.in b/templates/robot_description/robot_sim.dsv.in index bf5dbaa4..f3371115 100644 --- a/templates/robot_description/robot_sim.dsv.in +++ b/templates/robot_description/robot_sim.dsv.in @@ -2,6 +2,3 @@ prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share/ prepend-non-duplicate;IGN_GAZEBO_MODEL_PATH;share/ - -prepend-non-duplicate;GAZEBO_RESOURCE_PATH;share/ -prepend-non-duplicate;GAZEBO_MODEL_PATH;share/