Add hybrid planning feature

.github/workflows/ci.yaml

@@ -8,7 +8,7 @@ on:
   pull_request:
   push:
     branches:
-      - main
+      - feature/hybrid_planning
 
 jobs:
   default:

.github/workflows/format.yaml

@@ -8,7 +8,7 @@ on:
   pull_request:
   push:
     branches:
-      - main
+      - feature/hybrid_planning
 
 jobs:
   pre-commit:

moveit2.repos

@@ -1,4 +1,9 @@
 repositories:
+# TODO(sjahr): Remove once hybrid planning action definitions are finalized and merged
+  moveit_msgs:
+    type: git
+    url: https://github.com/sjahr/moveit_msgs
+    version: hybrid_planning
   geometric_shapes:
     type: git
     url: https://github.com/ros-planning/geometric_shapes

moveit_ros/hybrid_planning/CMakeLists.txt

@@ -0,0 +1,92 @@
+cmake_minimum_required(VERSION 3.10.2)
+project(moveit_hybrid_planning)
+
+# Common cmake code applied to all moveit packages
+find_package(moveit_common REQUIRED)
+moveit_package()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(moveit_msgs REQUIRED)
+find_package(moveit_ros_planning REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(trajectory_msgs REQUIRED)
+
+set(LIBRARIES
+  # Components
+  moveit_global_planner_component
+  moveit_hybrid_planning_manager
+  moveit_local_planner_component
+  # Plugins
+  forward_trajectory_plugin
+  motion_planning_pipeline_plugin
+  replan_invalidated_trajectory_plugin
+  simple_sampler_plugin
+  single_plan_execution_plugin
+)
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  rclcpp
+  rclcpp_components
+  rclcpp_action
+  moveit_core
+  moveit_ros_planning
+  moveit_ros_planning_interface
+  moveit_msgs
+  pluginlib
+  std_msgs
+  std_srvs
+  tf2_ros
+  trajectory_msgs
+)
+
+set(THIS_PACKAGE_INCLUDE_DIRS
+  global_planner/global_planner_component/include/
+  global_planner/global_planner_plugins/include/
+  hybrid_planning_manager/hybrid_planning_manager_component/include/
+  hybrid_planning_manager/planner_logic_plugins/include/
+  local_planner/local_planner_component/include/
+  local_planner/trajectory_operator_plugins/include/
+  local_planner/local_constraint_solver_plugins/include/
+)
+include_directories(${THIS_PACKAGE_INCLUDE_DIRS})
+
+add_subdirectory(hybrid_planning_manager)
+add_subdirectory(global_planner)
+add_subdirectory(local_planner)
+add_subdirectory(test)
+
+rclcpp_components_register_nodes(moveit_hybrid_planning_manager "moveit_hybrid_planning::HybridPlanningManager")
+rclcpp_components_register_nodes(moveit_global_planner_component "moveit_hybrid_planning::GlobalPlannerComponent")
+rclcpp_components_register_nodes(moveit_local_planner_component "moveit_hybrid_planning::LocalPlannerComponent")
+
+install(TARGETS ${LIBRARIES} hybrid_planning_test_node
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+  INCLUDES DESTINATION include)
+
+install(DIRECTORY ${THIS_PACKAGE_INCLUDE_DIRS} DESTINATION include)
+
+install(DIRECTORY test/launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME})
+
+pluginlib_export_plugin_description_file(moveit_hybrid_planning single_plan_execution_plugin.xml)
+pluginlib_export_plugin_description_file(moveit_hybrid_planning moveit_planning_pipeline_plugin.xml)
+pluginlib_export_plugin_description_file(moveit_hybrid_planning simple_sampler_plugin.xml)
+pluginlib_export_plugin_description_file(moveit_hybrid_planning replan_invalidated_trajectory_plugin.xml)
+pluginlib_export_plugin_description_file(moveit_hybrid_planning forward_trajectory_plugin.xml)
+
+ament_export_include_directories(include)
+ament_export_libraries(${LIBRARIES})
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+ament_package()

moveit_ros/hybrid_planning/README.md

@@ -0,0 +1,25 @@
+# Hybrid Planning
+A Hybrid Planning architecture. You can find more information in the project's issues[#300](https://github.com/ros-planning/moveit2/issues/300), [#433](https://github.com/ros-planning/moveit2/issues/433) and on the [MoveIt 2 roadmap](https://moveit.ros.org/documentation/contributing/roadmap/). Furthermore, there is an extensive tutorial available [here](https://github.com/ros-planning/moveit2_tutorials/pull/97).
+
+## Getting started
+1. Build moveit2 from source and make sure you use [this fork](https://github.com/sjahr/moveit_msgs) of moveit_msgs for the hybrid planning specific action definitions
+2. To start the demo run:
+```
+ros2 launch moveit_hybrid_planning hybrid_planning.launch.py
+```
+You can exchange the planner logic plugin in the hybrid_planning_manager.yaml. Currently available demo plugins are:
+- planner logic plugins:
+  - replan_invalidated_trajectory: Runs the global planner once and starts executing the global solution
+   with the local planner. In case the local planner detects a collision the global planner is rerun to update the
+   invalidated global trajectory.
+  - single_plan_execution: Run the global planner once and starts executing the global solution
+    with the local planner
+- global_planner plugins:
+  - moveit_planning_pipeline: Global planner plugin that utilizes MoveIt's planning pipeline accessed via the MoveItCpp API.
+- trajectory operator plugins:
+  - simple_sampler: Samples the next global trajectory waypoint as local goal constraint
+   based on the current robot state. When the waypoint is reached the index that marks the current local goal constraint
+   is updated to the next global trajectory waypoint. Global trajectory updates simply replace the reference trajectory.
+- local solver plugins:
+  - forward_trajectory: Forwards the next waypoint of the sampled local trajectory.
+   Additionally, it is possible to enable collision checking, which lets the robot stop in front of a collision object.

moveit_ros/hybrid_planning/forward_trajectory_plugin.xml

@@ -0,0 +1,7 @@
+<library path="forward_trajectory_plugin">
+  <class name="moveit_hybrid_planning/ForwardTrajectory" type="moveit_hybrid_planning::ForwardTrajectory" base_class_type="moveit_hybrid_planning::LocalConstraintSolverInterface">
+    <description>
+    Simple local solver plugin that forwards the next waypoint of the sampled local trajectory.
+    </description>
+  </class>
+</library>

moveit_ros/hybrid_planning/global_planner/CMakeLists.txt

@@ -0,0 +1,2 @@
+add_subdirectory(global_planner_component)
+add_subdirectory(global_planner_plugins)

moveit_ros/hybrid_planning/global_planner/global_planner_component/CMakeLists.txt

@@ -0,0 +1,6 @@
+# Global planner component
+add_library(moveit_global_planner_component SHARED
+  src/global_planner_component.cpp
+)
+set_target_properties(moveit_global_planner_component PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(moveit_global_planner_component ${THIS_PACKAGE_INCLUDE_DEPENDS})

moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_component.h

@@ -0,0 +1,89 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, PickNik Inc.
+ *  All rights reserved.
+ *
+ *  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 PickNik Inc. 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 OWNER 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.
+ *********************************************************************/
+
+/* Author: Sebastian Jahr
+ */
+
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+
+#include <pluginlib/class_loader.hpp>
+
+#include <moveit/global_planner/global_planner_interface.h>
+
+#include <moveit_msgs/action/global_planner.hpp>
+#include <moveit_msgs/msg/motion_plan_request.hpp>
+#include <moveit_msgs/msg/motion_plan_response.hpp>
+
+namespace moveit_hybrid_planning
+{
+// Component node containing the global planner
+class GlobalPlannerComponent : public rclcpp::Node
+{
+public:
+  GlobalPlannerComponent(const rclcpp::NodeOptions& options);
+
+  // TODO(sjahr) implement get_last_solution service
+
+private:
+  rclcpp::TimerBase::SharedPtr timer_;
+  bool initialized_;
+
+  std::string global_planner_name_;
+
+  // Global planner plugin loader
+  std::unique_ptr<pluginlib::ClassLoader<moveit_hybrid_planning::GlobalPlannerInterface>> global_planner_plugin_loader_;
+
+  // Global planner instance
+  std::shared_ptr<moveit_hybrid_planning::GlobalPlannerInterface> global_planner_instance_;
+
+  moveit_msgs::msg::MotionPlanResponse last_global_solution_;
+
+  // Global planning request action server
+  rclcpp_action::Server<moveit_msgs::action::GlobalPlanner>::SharedPtr global_planning_request_server_;
+
+  // Global trajectory publisher
+  rclcpp::Publisher<moveit_msgs::msg::MotionPlanResponse>::SharedPtr global_trajectory_pub_;
+
+  // Goal callback for global planning request action server
+  void globalPlanningRequestCallback(
+      std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::GlobalPlanner>> goal_handle);
+
+  // Initialize planning scene monitor and load pipelines
+  bool init();
+};
+
+}  // namespace moveit_hybrid_planning

moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.h

@@ -0,0 +1,78 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, PickNik Inc.
+ *  All rights reserved.
+ *
+ *  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 PickNik Inc. 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 OWNER 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.
+ *********************************************************************/
+
+/* Author: Sebastian Jahr
+   Description: Defines an interface for a global motion planner plugin implementation used in the global planner
+   component node.
+ */
+
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
+
+#include <moveit_msgs/action/global_planner.hpp>
+#include <moveit_msgs/msg/motion_plan_response.hpp>
+
+namespace moveit_hybrid_planning
+{
+/**
+ * Class GlobalPlannerInterface - Base class for a global planner implementation
+ */
+class GlobalPlannerInterface
+{
+public:
+  /**
+   * Initialize global planner
+   * @return True if initialization was successful
+   */
+  virtual bool initialize(const rclcpp::Node::SharedPtr& node) = 0;
+
+  /**
+   * Solve global planning problem
+   * @param global_goal_handle Action goal handle to access the planning goal and publish feedback during planning
+   * @return Motion Plan that contains the solution for a given motion planning problem
+   */
+  virtual moveit_msgs::msg::MotionPlanResponse plan(
+      const std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::GlobalPlanner>> global_goal_handle) = 0;
+
+  /**
+   * Reset global planner plugin
+   * @return True if reset was successful
+   */
+  virtual bool reset() = 0;
+};
+}  // namespace moveit_hybrid_planning