Moveit package template

scripts/_RosTeamWs_Defines.bash

@@ -437,6 +437,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"

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

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 '(?<=<name>).*?(?=</name>)' 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 <ENTER>, otherwise <CTRL>+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="<buildtool_depend>ament_cmake<\/buildtool_depend>"
+    sed -i "s/$append_to_string/$append_to_string\\n\\n  <exec_depend>${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}"

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_bringup_package> $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.

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...
+│   ├── <planner>_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 <bringup_pkg_name> $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.

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
+
+