Add hybrid_planning skeleton code

moveit_ros/hybrid_planning/CMakeLists.txt

@@ -0,0 +1,69 @@
+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(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(moveit_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+
+set(LIBRARIES
+  moveit_hybrid_planning_manager
+  moveit_local_planner_component
+  moveit_global_planner_component
+)
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  rclcpp
+  rclcpp_components
+  rclcpp_action 
+  moveit_msgs
+  std_srvs
+)
+
+include_directories(include)
+
+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})
+
+rclcpp_components_register_nodes(moveit_global_planner_component "hybrid_planning::GlobalPlannerComponent")
+
+add_library(moveit_hybrid_planning_manager SHARED src/hybrid_planning_manager.cpp)
+set_target_properties(moveit_hybrid_planning_manager PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(moveit_hybrid_planning_manager ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+rclcpp_components_register_nodes(moveit_hybrid_planning_manager "hybrid_planning::HybridPlanningManager")
+
+add_library(moveit_local_planner_component SHARED src/local_planner_component.cpp)
+set_target_properties(moveit_local_planner_component PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(moveit_local_planner_component ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+rclcpp_components_register_nodes(moveit_local_planner_component "hybrid_planning::LocalPlannerComponent")
+
+add_executable(dummy_action_client test/dummy_action_client.cpp)
+ament_target_dependencies(dummy_action_client ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_link_libraries(dummy_action_client ${LIBRARIES})
+
+install(TARGETS ${LIBRARIES} dummy_action_client
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY include/ DESTINATION include)
+
+install(DIRECTORY include DESTINATION include)
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+
+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,3 @@
+# Hybrid Planning
+
+This is a first prototype for the new MoveIt 2 hybrid planning architecture. You find more information in the [project's issue](https://github.com/ros-planning/moveit2/issues/300) and on the [MoveIt 2 roadmap](https://moveit.ros.org/documentation/contributing/roadmap/).

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

@@ -0,0 +1,63 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, PickNik LLC
+ *  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 LLC 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 <moveit_msgs/action/plan_global_trajectory.hpp>
+
+namespace hybrid_planning_action = moveit_msgs::action;
+
+namespace hybrid_planning
+{
+// Component node containing the global planner
+class GlobalPlannerComponent : public rclcpp::Node
+{
+public:
+  GlobalPlannerComponent(const rclcpp::NodeOptions& options);
+
+private:
+  // Global planning request action server
+  rclcpp_action::Server<hybrid_planning_action::PlanGlobalTrajectory>::SharedPtr global_planning_request_server_;
+
+  // Goal callback for global planning request action server
+  void globalPlanningGoalCallback(
+      std::shared_ptr<rclcpp_action::ServerGoalHandle<hybrid_planning_action::PlanGlobalTrajectory>> goal_handle);
+};
+}  // namespace hybrid_planning

moveit_ros/hybrid_planning/include/moveit/hybrid_planning/hybrid_planning_manager.h

@@ -0,0 +1,93 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, PickNik LLC
+ *  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 LLC 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 <moveit_msgs/action/operate_local_planner.hpp>
+#include <moveit_msgs/action/plan_global_trajectory.hpp>
+#include <moveit_msgs/action/run_hybrid_planning.hpp>
+
+#include <moveit_msgs/msg/move_it_error_codes.hpp>
+
+namespace hybrid_planning_action = moveit_msgs::action;
+
+namespace hybrid_planning
+{
+// Hybrid planning manager component node
+class HybridPlanningManager : public rclcpp::Node
+{
+public:
+  HybridPlanningManager(const rclcpp::NodeOptions& options);
+
+private:
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  std::shared_ptr<rclcpp_action::ServerGoalHandle<hybrid_planning_action::RunHybridPlanning>>
+      hybrid_planning_goal_handle_;
+  std::shared_ptr<moveit_msgs::action::RunHybridPlanning_Feedback> hybrid_planning_progess_;
+
+  bool global_planning_started_;
+  bool local_planning_started_;
+
+  bool global_planning_executed_;
+  bool local_planning_executed_;
+
+  bool abort_;
+
+  // Planning request action clients
+  rclcpp_action::Client<hybrid_planning_action::OperateLocalPlanner>::SharedPtr local_planner_action_client_;
+  rclcpp_action::Client<hybrid_planning_action::PlanGlobalTrajectory>::SharedPtr global_planner_action_client_;
+
+  // Hybrid planning request action server
+  rclcpp_action::Server<hybrid_planning_action::RunHybridPlanning>::SharedPtr hybrid_planning_request_server_;
+
+  // Hybrid planning goal callback for hybrid planning request server
+  void runHybridPlanning(
+      std::shared_ptr<rclcpp_action::ServerGoalHandle<hybrid_planning_action::RunHybridPlanning>> goal_handle);
+
+  void hybridPlanningLoop();
+
+  // Request global planning
+  int planGlobalTrajectory();
+
+  // Start local planning and execution
+  int runLocalPlanner();
+};
+}  // namespace hybrid_planning

moveit_ros/hybrid_planning/include/moveit/hybrid_planning/local_planner_component.h

@@ -0,0 +1,63 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, PickNik LLC
+ *  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 LLC 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 <moveit_msgs/action/operate_local_planner.hpp>
+
+namespace hybrid_planning_action = moveit_msgs::action;
+
+namespace hybrid_planning
+{
+// Component node containing the local planner
+class LocalPlannerComponent : public rclcpp::Node
+{
+public:
+  LocalPlannerComponent(const rclcpp::NodeOptions& options);
+
+private:
+  // Local planning request action server
+  rclcpp_action::Server<hybrid_planning_action::OperateLocalPlanner>::SharedPtr local_planning_request_server_;
+
+  // Goal callback for local planning request action server
+  void localPlanningGoalCallback(
+      std::shared_ptr<rclcpp_action::ServerGoalHandle<hybrid_planning_action::OperateLocalPlanner>> goal_handle);
+};
+}  // namespace hybrid_planning

moveit_ros/hybrid_planning/launch/hybrid_planning.launch.py

@@ -0,0 +1,30 @@
+import launch
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+    """Generate launch description with multiple components."""
+    container = ComposableNodeContainer(
+            name='hybrid_planning_container',
+            namespace='/',
+            package='rclcpp_components',
+            executable='component_container',
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='moveit_hybrid_planning',
+                    plugin='hybrid_planning::GlobalPlannerComponent',
+                    name='global_planner'),
+                ComposableNode(
+                    package='moveit_hybrid_planning',
+                    plugin='hybrid_planning::LocalPlannerComponent',
+                    name='local_planner'),
+                ComposableNode(
+                    package='moveit_hybrid_planning',
+                    plugin='hybrid_planning::HybridPlanningManager',
+                    name='hybrid_planning_manager')
+            ],
+            output='screen',
+    )
+
+    return launch.LaunchDescription([container])

moveit_ros/hybrid_planning/package.xml

@@ -0,0 +1,32 @@
+<package format="2">
+  <name>moveit_hybrid_planning</name>
+  <version>0.0.1</version>
+  <description>Hybrid planning components of MoveIt 2</description>
+  <author email="sebastian.jahr@picknik.ai">Sebastian Jahr</author>
+
+  <maintainer email="todo@to.do">TODO</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://moveit.ros.org</url>
+  <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
+  <url type="repository">https://github.com/ros-planning/moveit</url>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>moveit_common</build_depend>
+
+  <depend>ament_index_cpp</depend>
+  <depend>moveit_msgs</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_components</depend>
+  <depend>rclcpp</depend>
+  <depend>std_srvs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>