Gazebo template files

.gitignore

@@ -1 +1 @@
-docs/_build/*
+docs/_build/*

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"

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-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="<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

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 <ENTER>, 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

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 <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=("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

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.