Restructure hybrid_planning package (moveit#426)

* Add forward_trajectory local solver plugin (moveit#359) * Use ros2_control based joint simulation and remove unnecessary comment * Update copyrights * Restructure hybrid planning package * Fix formatting and add missing time stamp in local solver plugin * Remove unnecessary logging and param loading * Enable different interfaces between local planner and controller * Use JointGroupPositionController as demo hardware controller
sjahr · Oct 19, 2021 · 9acd9fb · 9acd9fb
1 parent c9f1fda
commit 9acd9fb
Show file tree

Hide file tree

Showing 39 changed files with 500 additions and 188 deletions.
diff --git a/moveit_ros/hybrid_planning/CMakeLists.txt b/moveit_ros/hybrid_planning/CMakeLists.txt
@@ -14,6 +14,7 @@ find_package(moveit_msgs REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_core REQUIRED)
 find_package(std_srvs REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(trajectory_msgs REQUIRED)
 find_package(control_toolbox REQUIRED)
@@ -30,6 +31,7 @@ set(LIBRARIES
   single_plan_execution_plugin
   decelerate_before_collision_plugin
   replan_invalidated_trajectory_plugin
+  forward_trajectory_plugin
 )
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
@@ -39,6 +41,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   moveit_core
   moveit_ros_planning
   moveit_msgs
+  std_msgs
   std_srvs
   tf2_ros
   trajectory_msgs
@@ -47,20 +50,17 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
 
 set(THIS_PACKAGE_INCLUDE_DIRS
   global_planner/include
-  local_planner/include
-  hybrid_planning_manager/include
-  planner_logic_plugins/include
-  trajectory_operator_plugins/include
-  local_constraint_solver_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(planner_logic_plugins)
-add_subdirectory(trajectory_operator_plugins)
-add_subdirectory(local_constraint_solver_plugins)
 add_subdirectory(test)
 
 rclcpp_components_register_nodes(moveit_hybrid_planning_manager "moveit_hybrid_planning::HybridPlanningManager")
@@ -70,8 +70,7 @@ rclcpp_components_register_nodes(moveit_local_planner_component "moveit_hybrid_p
 install(TARGETS ${LIBRARIES} hybrid_planning_test_node
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
-  DESTINATION lib/${PROJECT_NAME})
+  RUNTIME DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY ${THIS_PACKAGE_INCLUDE_DIRS} DESTINATION include)
 
@@ -82,6 +81,7 @@ pluginlib_export_plugin_description_file(moveit_hybrid_planning single_plan_exec
 pluginlib_export_plugin_description_file(moveit_hybrid_planning next_waypoint_sampler_plugin.xml)
 pluginlib_export_plugin_description_file(moveit_hybrid_planning decelerate_before_collision_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})

diff --git a/moveit_ros/hybrid_planning/forward_trajectory_plugin.xml b/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 trajectory operator plugin that appends new global trajectories to the local planner's reference trajectory and extracts the next trajectory point based on the current robot state and an index.
+    </description>
+  </class>
+</library>
diff --git a/...s/hybrid_planning/global_planner/include/moveit/global_planner/global_planner_component.h b/...s/hybrid_planning/global_planner/include/moveit/global_planner/global_planner_component.h
@@ -1,7 +1,7 @@
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2020, PickNik LLC
+ *  Copyright (c) 2020, PickNik Inc.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     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
+ *   * 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.
  *

diff --git a/moveit_ros/hybrid_planning/global_planner/src/global_planner_component.cpp b/moveit_ros/hybrid_planning/global_planner/src/global_planner_component.cpp
@@ -1,7 +1,7 @@
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2020, PickNik LLC
+ *  Copyright (c) 2020, PickNik Inc.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     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
+ *   * 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.
  *

diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/CMakeLists.txt b/moveit_ros/hybrid_planning/hybrid_planning_manager/CMakeLists.txt
@@ -1,11 +1,2 @@
-# Trajectory operator interface
-add_library(planner_logic_interface SHARED
-  src/planner_logic_interface.cpp
-)
-set_target_properties(planner_logic_interface PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(planner_logic_interface ${THIS_PACKAGE_INCLUDE_DEPENDS})
-
-# Hybrid planning manager component
-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})
+add_subdirectory(hybrid_planning_manager_component)
+add_subdirectory(planner_logic_plugins)
diff --git a/.../hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/CMakeLists.txt b/.../hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/CMakeLists.txt
@@ -0,0 +1,11 @@
+# Trajectory operator interface
+add_library(planner_logic_interface SHARED
+  src/planner_logic_interface.cpp
+)
+set_target_properties(planner_logic_interface PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(planner_logic_interface ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# Hybrid planning manager component
+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})
diff --git a/...planning_manager/hybrid_planning_events.h → ...planning_manager/hybrid_planning_events.h b/...planning_manager/hybrid_planning_events.h → ...planning_manager/hybrid_planning_events.h
@@ -1,7 +1,7 @@
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2020, PickNik LLC
+ *  Copyright (c) 2020, PickNik Inc.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     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
+ *   * 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.
  *
@@ -54,4 +54,4 @@ enum class BasicHybridPlanningEvent
   // Indicates that the local planning action finished (Does not indicate planning success! The action response contains this information)
   LOCAL_PLANNING_ACTION_FINISHED,
 };
-}  // namespace moveit_hybrid_planning
+}  // namespace moveit_hybrid_planning
diff --git a/...lanning_manager/hybrid_planning_manager.h → ...lanning_manager/hybrid_planning_manager.h b/...lanning_manager/hybrid_planning_manager.h → ...lanning_manager/hybrid_planning_manager.h
@@ -1,7 +1,7 @@
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2020, PickNik LLC
+ *  Copyright (c) 2020, PickNik Inc.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     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
+ *   * 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.
  *
@@ -129,4 +129,4 @@ class HybridPlanningManager : public rclcpp::Node
   // Global solution subscriber
   rclcpp::Subscription<moveit_msgs::msg::MotionPlanResponse>::SharedPtr global_solution_sub_;
 };
-}  // namespace moveit_hybrid_planning
+}  // namespace moveit_hybrid_planning
diff --git a/...lanning_manager/planner_logic_interface.h → ...lanning_manager/planner_logic_interface.h b/...lanning_manager/planner_logic_interface.h → ...lanning_manager/planner_logic_interface.h
@@ -1,7 +1,7 @@
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2020, PickNik LLC
+ *  Copyright (c) 2020, PickNik Inc.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     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
+ *   * 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.
  *

diff --git a/...g_manager/src/hybrid_planning_manager.cpp → ...component/src/hybrid_planning_manager.cpp b/...g_manager/src/hybrid_planning_manager.cpp → ...component/src/hybrid_planning_manager.cpp
@@ -1,7 +1,7 @@
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2020, PickNik LLC
+ *  Copyright (c) 2020, PickNik Inc.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     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
+ *   * 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.
  *

diff --git a/...g_manager/src/planner_logic_interface.cpp → ...component/src/planner_logic_interface.cpp b/...g_manager/src/planner_logic_interface.cpp → ...component/src/planner_logic_interface.cpp
@@ -1,7 +1,7 @@
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2020, PickNik LLC
+ *  Copyright (c) 2020, PickNik Inc.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     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
+ *   * 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.
  *
@@ -40,4 +40,4 @@
 namespace moveit_hybrid_planning
 {
 // Empty because this library only defines an interface
-}  // namespace moveit_hybrid_planning
+}  // namespace moveit_hybrid_planning
diff --git a/...ning/planner_logic_plugins/CMakeLists.txt → ...ager/planner_logic_plugins/CMakeLists.txt b/...ning/planner_logic_plugins/CMakeLists.txt → ...ager/planner_logic_plugins/CMakeLists.txt
diff --git a/...c_plugins/replan_invalidated_trajectory.h → ...c_plugins/replan_invalidated_trajectory.h b/...c_plugins/replan_invalidated_trajectory.h → ...c_plugins/replan_invalidated_trajectory.h
@@ -1,7 +1,7 @@
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2020, PickNik LLC
+ *  Copyright (c) 2020, PickNik Inc.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     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
+ *   * 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.
  *

diff --git a/...ner_logic_plugins/single_plan_execution.h → ...ner_logic_plugins/single_plan_execution.h b/...ner_logic_plugins/single_plan_execution.h → ...ner_logic_plugins/single_plan_execution.h
@@ -1,7 +1,7 @@
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2020, PickNik LLC
+ *  Copyright (c) 2020, PickNik Inc.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     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
+ *   * 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.
  *

diff --git a/...ins/src/replan_invalidated_trajectory.cpp → ...ins/src/replan_invalidated_trajectory.cpp b/...ins/src/replan_invalidated_trajectory.cpp → ...ins/src/replan_invalidated_trajectory.cpp
@@ -1,7 +1,7 @@
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2020, PickNik LLC
+ *  Copyright (c) 2020, PickNik Inc.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     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
+ *   * 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.
  *

diff --git a/...gic_plugins/src/single_plan_execution.cpp → ...gic_plugins/src/single_plan_execution.cpp b/...gic_plugins/src/single_plan_execution.cpp → ...gic_plugins/src/single_plan_execution.cpp
@@ -1,7 +1,7 @@
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2020, PickNik LLC
+ *  Copyright (c) 2020, PickNik Inc.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     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
+ *   * 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.
  *

diff --git a/moveit_ros/hybrid_planning/local_planner/CMakeLists.txt b/moveit_ros/hybrid_planning/local_planner/CMakeLists.txt
@@ -1,20 +1,3 @@
-# Local_planner_component
-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})
-
-# Trajectory operator interface
-add_library(trajectory_operator_interface SHARED
-  src/trajectory_operator_interface.cpp
-)
-set_target_properties(trajectory_operator_interface PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(trajectory_operator_interface ${THIS_PACKAGE_INCLUDE_DEPENDS})
-
-# Constraint solver interface
-add_library(local_constraint_solver_interface SHARED
-  src/local_constraint_solver_interface.cpp
-)
-set_target_properties(local_constraint_solver_interface PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(local_constraint_solver_interface ${THIS_PACKAGE_INCLUDE_DEPENDS})
+add_subdirectory(local_planner_component)
+add_subdirectory(trajectory_operator_plugins)
+add_subdirectory(local_constraint_solver_plugins)
diff --git a/..._constraint_solver_plugins/CMakeLists.txt → ..._constraint_solver_plugins/CMakeLists.txt b/..._constraint_solver_plugins/CMakeLists.txt → ..._constraint_solver_plugins/CMakeLists.txt
@@ -4,3 +4,10 @@ add_library(decelerate_before_collision_plugin SHARED
 set_target_properties(decelerate_before_collision_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 ament_target_dependencies(decelerate_before_collision_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS})
 target_link_libraries(decelerate_before_collision_plugin local_constraint_solver_interface)
+
+add_library(forward_trajectory_plugin SHARED
+  src/forward_trajectory.cpp
+)
+set_target_properties(forward_trajectory_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(forward_trajectory_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_link_libraries(forward_trajectory_plugin local_constraint_solver_interface)
diff --git a/...ver_plugins/decelerate_before_collision.h → ...ver_plugins/decelerate_before_collision.h b/...ver_plugins/decelerate_before_collision.h → ...ver_plugins/decelerate_before_collision.h
@@ -1,7 +1,7 @@
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2020, PickNik LLC
+ *  Copyright (c) 2020, PickNik Inc.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     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
+ *   * 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.
  *