diff --git a/.gitignore b/.gitignore index 229ecfad..9c764c21 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1 @@ -docs/_build/* +docs/_build/* \ No newline at end of file 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/_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-gazebo-bringup.bash b/scripts/setup-gazebo-bringup.bash new file mode 100755 index 00000000..13d9e634 --- /dev/null +++ b/scripts/setup-gazebo-bringup.bash @@ -0,0 +1,43 @@ +#!/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. +## + +# 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 \ No newline at end of file diff --git a/scripts/setup-robot-bringup.bash b/scripts/setup-robot-bringup.bash index 91b70432..3d1c399a 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}" @@ -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 @@ -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 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/scripts/setup-robot-moveit.bash b/scripts/setup-robot-moveit.bash new file mode 100755 index 00000000..38ce3938 --- /dev/null +++ b/scripts/setup-robot-moveit.bash @@ -0,0 +1,156 @@ +#!/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-moveit 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=("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.srdf.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}" + + # 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 + done +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 + +# 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 + +# skip compilation, let the user install MoveIt manually, as it can introduce +# breaking changes often. + +echo "" +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}" 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. diff --git a/templates/moveit/move_group.yaml b/templates/moveit/move_group.yaml new file mode 100644 index 00000000..045b3dd2 --- /dev/null +++ b/templates/moveit/move_group.yaml @@ -0,0 +1,95 @@ +# 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 + 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: + $ROBOT_NAME$_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..a05690b4 --- /dev/null +++ b/templates/moveit/moveit.launch.py @@ -0,0 +1,195 @@ +# 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 +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="$DESCR_PKG_NAME$", + 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="$ROBOT_NAME$.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="$PKG_NAME$", + 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="$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.", + ) + ) + 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.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/moveit.rviz b/templates/moveit/moveit.rviz new file mode 100644 index 00000000..cadc9425 --- /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: $ROBOT_NAME$_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..307533a9 --- /dev/null +++ b/templates/moveit/ompl_planning.yaml @@ -0,0 +1,88 @@ +# 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: + 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 + $ROBOT_NAME$_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..1aadb143 --- /dev/null +++ b/templates/moveit/robot.srdf.xacro @@ -0,0 +1,28 @@ + + + + + + + + + + + + diff --git a/templates/moveit/robot_macro.srdf.xacro b/templates/moveit/robot_macro.srdf.xacro new file mode 100644 index 00000000..22cefd28 --- /dev/null +++ b/templates/moveit/robot_macro.srdf.xacro @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + 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/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..c46d3bed 100644 --- a/templates/robot_description/robot.urdf.xacro +++ b/templates/robot_description/robot.urdf.xacro @@ -6,7 +6,6 @@ - @@ -18,8 +17,11 @@ - - + + 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_macro.xacro b/templates/robot_description/robot_macro.xacro index a99729eb..2dbe87f9 100644 --- a/templates/robot_description/robot_macro.xacro +++ b/templates/robot_description/robot_macro.xacro @@ -4,7 +4,22 @@ - + + + + + + + + + + @@ -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 @@ - + @@ -186,7 +201,7 @@ - + diff --git a/templates/robot_description/robot_sim.dsv.in b/templates/robot_description/robot_sim.dsv.in new file mode 100644 index 00000000..f3371115 --- /dev/null +++ b/templates/robot_description/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/ 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 - - diff --git a/templates/ros2_control/robot_ros2_control.launch.py b/templates/ros2_control/robot_ros2_control.launch.py index 45d4193a..bf60a0f1 100644 --- a/templates/ros2_control/robot_ros2_control.launch.py +++ b/templates/ros2_control/robot_ros2_control.launch.py @@ -170,7 +170,7 @@ def generate_launch_description(): ) ] - inactive_robot_controller_names = ["add_some_controller_name"] + inactive_robot_controller_names = [] # add controller names here inactive_robot_controller_spawners = [] for controller in inactive_robot_controller_names: inactive_robot_controller_spawners += [ diff --git a/templates/ros2_control/robot_ros2_control_sim.launch.py b/templates/ros2_control/robot_ros2_control_sim.launch.py index 44cb3f0f..cae7bcf6 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:=", @@ -144,16 +129,15 @@ def generate_launch_description(): # Gazebo nodes gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( - [FindPackageShare("ros_ign_gazebo"), "/launch", "/ign_gazebo.launch.py"] + [FindPackageShare("ros_gz_sim"), "/launch", "/gz_sim.launch.py"] ), - launch_arguments={"ign_args": " -r -v 3 empty.sdf"}.items(), + launch_arguments={"gz_args": " -r -v 3 empty.sdf"}.items(), ) # Spawn robot gazebo_spawn_robot = Node( - package="ros_ign_gazebo", + package="ros_gz_sim", executable="create", - name="spawn_rrbot", arguments=["-name", "$ROBOT_NAME$", "-topic", "robot_description"], output="screen", ) @@ -164,7 +148,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 += [