[hybrid planning] Add action abortion and test; improve the existing …

…test (#980) * Add action abortion and test; improve the existing test * Add controller run-dependency * Fix the clearing of robot trajectory when a collision would occur * Fix replanning if local planner is stuck * Lambda function everything * Thread safety for stop_hybrid_planning_ * Thread-safe state_ * Clang tidy * Update the planning scene properly * Update Servo test initial_positions.yaml Co-authored-by: Tyler Weaver <[email protected]>
moveit · Jan 27, 2022 · 5c40566 · 5c40566
1 parent 97776ca
commit 5c40566
Show file tree

Hide file tree

Showing 18 changed files with 502 additions and 171 deletions.
diff --git a/moveit_ros/hybrid_planning/CMakeLists.txt b/moveit_ros/hybrid_planning/CMakeLists.txt
@@ -12,6 +12,7 @@ find_package(moveit_msgs REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(pluginlib REQUIRED)
+find_package(position_controllers REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_components REQUIRED)
@@ -34,13 +35,14 @@ set(LIBRARIES
 )
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
-  rclcpp
-  rclcpp_components
-  rclcpp_action
   moveit_core
   moveit_ros_planning
   moveit_ros_planning_interface
   moveit_msgs
+  position_controllers
+  rclcpp
+  rclcpp_components
+  rclcpp_action
   pluginlib
   std_msgs
   std_srvs
@@ -97,25 +99,4 @@ ament_export_libraries(${LIBRARIES})
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
 
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  find_package(ament_cmake_gtest REQUIRED)
-  find_package(ros_testing REQUIRED)
-  find_package(Boost REQUIRED COMPONENTS filesystem)
-
-  # These don't pass yet, disable them for now
-  set(ament_cmake_copyright_FOUND TRUE)
-  set(ament_cmake_cppcheck_FOUND TRUE)
-  set(ament_cmake_cpplint_FOUND TRUE)
-  set(ament_cmake_flake8_FOUND TRUE)
-  set(ament_cmake_uncrustify_FOUND TRUE)
-
-  # Run all lint tests in package.xml except those listed above
-  ament_lint_auto_find_test_dependencies()
-
-  # Basic launch test
-  add_ros_test(test/launch/test_basic_launching.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
-
-endif()
-
 ament_package()
diff --git a/...anning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h b/...anning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h
@@ -75,13 +75,6 @@ class HybridPlanningManager : public rclcpp::Node
    */
   bool initialize();
 
-  /**
-   * Hybrid planning goal callback for hybrid planning request server
-   * @param goal_handle Hybrid planning goal handle to access feedback and response
-   */
-  void hybridPlanningRequestCallback(
-      std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::HybridPlanner>> goal_handle);
-
   /**
    * Send global planning request to global planner component
    * @return Global planner successfully started yes/no
@@ -113,6 +106,9 @@ class HybridPlanningManager : public rclcpp::Node
   // Flag that indicates whether the manager is initialized
   bool initialized_;
 
+  // Flag that indicates hybrid planning has been canceled
+  std::atomic<bool> stop_hybrid_planning_;
+
   // Shared hybrid planning goal handle
   std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::HybridPlanner>> hybrid_planning_goal_handle_;
 

diff --git a/...hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp b/...hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp
@@ -45,7 +45,7 @@ namespace moveit::hybrid_planning
 using namespace std::chrono_literals;
 
 HybridPlanningManager::HybridPlanningManager(const rclcpp::NodeOptions& options)
-  : Node("hybrid_planning_manager", options), initialized_(false)
+  : Node("hybrid_planning_manager", options), initialized_(false), stop_hybrid_planning_(false)
 {
   // Initialize hybrid planning component after after construction
   // TODO(sjahr) Remove once life cycle component nodes are available
@@ -147,16 +147,46 @@ bool HybridPlanningManager::initialize()
   hybrid_planning_request_server_ = rclcpp_action::create_server<moveit_msgs::action::HybridPlanner>(
       this->get_node_base_interface(), this->get_node_clock_interface(), this->get_node_logging_interface(),
       this->get_node_waitables_interface(), hybrid_planning_action_name,
+      // Goal callback
       [](const rclcpp_action::GoalUUID& /*unused*/,
          std::shared_ptr<const moveit_msgs::action::HybridPlanner::Goal> /*unused*/) {
         RCLCPP_INFO(LOGGER, "Received goal request");
         return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
       },
-      [](const std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::HybridPlanner>>& /*unused*/) {
+      // Cancel callback
+      [this](std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::HybridPlanner>> /*unused*/) {
+        // Prevent any new global or local requests from going out
+        stop_hybrid_planning_ = true;
+
+        // Cancel local action
+        local_planner_action_client_->async_cancel_all_goals();
+
+        // Cancel global action
+        global_planner_action_client_->async_cancel_all_goals();
+
         RCLCPP_INFO(LOGGER, "Received request to cancel goal");
         return rclcpp_action::CancelResponse::ACCEPT;
       },
-      std::bind(&HybridPlanningManager::hybridPlanningRequestCallback, this, std::placeholders::_1));
+      // Request callback
+      [this](std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::HybridPlanner>> goal_handle) {
+        // Reset the "stop" flag if it was set previously
+        stop_hybrid_planning_ = false;
+
+        // Pass goal handle to class member
+        hybrid_planning_goal_handle_ = std::move(goal_handle);
+
+        // react is defined in a hybrid_planning_manager plugin
+        ReactionResult reaction_result =
+            planner_logic_instance_->react(HybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED);
+        if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
+        {
+          auto result = std::make_shared<moveit_msgs::action::HybridPlanner::Result>();
+          result->error_code.val = reaction_result.error_code.val;
+          result->error_message = reaction_result.error_message;
+          hybrid_planning_goal_handle_->abort(result);
+          RCLCPP_ERROR_STREAM(LOGGER, "Hybrid Planning Manager failed to react to  " << reaction_result.event);
+        }
+      });
 
   // Initialize global solution subscriber
   global_solution_sub_ = create_subscription<moveit_msgs::msg::MotionPlanResponse>(
@@ -233,6 +263,12 @@ bool HybridPlanningManager::sendGlobalPlannerAction()
   global_goal_msg.motion_sequence =
       (hybrid_planning_goal_handle_->get_goal())->motion_sequence;  // latest desired motion sequence;
   global_goal_msg.planning_group = (hybrid_planning_goal_handle_->get_goal())->planning_group;  // planning_group_;
+
+  if (stop_hybrid_planning_)
+  {
+    return false;
+  }
+
   // Send global planning goal and wait until it's accepted
   auto goal_handle_future = global_planner_action_client_->async_send_goal(global_goal_msg, global_goal_options);
   return true;  // return always success TODO(sjahr) add more error checking
@@ -301,39 +337,27 @@ bool HybridPlanningManager::sendLocalPlannerAction()
         // Abort hybrid planning if reaction fails
         if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
         {
-          auto result = std::make_shared<moveit_msgs::action::HybridPlanner::Result>();
-          result->error_code.val = reaction_result.error_code.val;
-          result->error_message = reaction_result.error_message;
-
-          hybrid_planning_goal_handle_->abort(result);
-          RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to  '%s'", reaction_result.event.c_str());
+          const moveit_msgs::action::HybridPlanner::Result result([reaction_result]() {
+            moveit_msgs::action::HybridPlanner::Result result;
+            result.error_code.val = reaction_result.error_code.val;
+            result.error_message = reaction_result.error_message;
+            return result;
+          }());
+          hybrid_planning_goal_handle_->abort(std::make_shared<moveit_msgs::action::HybridPlanner::Result>(result));
+          RCLCPP_ERROR_STREAM(LOGGER, "Hybrid Planning Manager failed to react to " << reaction_result.event);
         }
       };
 
+  if (stop_hybrid_planning_)
+  {
+    return false;
+  }
+
   // Send global planning goal
   auto goal_handle_future = local_planner_action_client_->async_send_goal(local_goal_msg, local_goal_options);
   return true;  // return always success TODO(sjahr) add more error checking
 }
 
-void HybridPlanningManager::hybridPlanningRequestCallback(
-    std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::HybridPlanner>> goal_handle)
-{
-  // Pass goal handle to class member
-  hybrid_planning_goal_handle_ = std::move(goal_handle);
-
-  // react is defined in a hybrid_planning_manager plugin
-  ReactionResult reaction_result =
-      planner_logic_instance_->react(HybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED);
-  if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
-  {
-    auto result = std::make_shared<moveit_msgs::action::HybridPlanner::Result>();
-    result->error_code.val = reaction_result.error_code.val;
-    result->error_message = reaction_result.error_message;
-    hybrid_planning_goal_handle_->abort(result);
-    RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to  '%s'", reaction_result.event.c_str());
-  }
-}
-
 void HybridPlanningManager::sendHybridPlanningResponse(bool success)
 {
   // Return hybrid planning action result dependent on the function's argument

diff --git a/...nning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp b/...nning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp
@@ -43,15 +43,7 @@ namespace moveit::hybrid_planning
 {
 ReactionResult ReplanInvalidatedTrajectory::react(const std::string& event)
 {
-  if (event == "collision_ahead")
-  {
-    if (!hybrid_planning_manager_->sendGlobalPlannerAction())  // Start global planning
-    {
-      hybrid_planning_manager_->sendHybridPlanningResponse(false);
-    }
-    return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
-  }
-  if (event == "local_planner_stuck")
+  if ((event == "collision_ahead") || (event == "local_planner_stuck"))
   {
     if (!hybrid_planning_manager_->sendGlobalPlannerAction())  // Start global planning
     {

diff --git a/...traint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h b/...traint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h
@@ -52,12 +52,12 @@ class ForwardTrajectory : public LocalConstraintSolverInterface
   ~ForwardTrajectory() = default;
   bool initialize(const rclcpp::Node::SharedPtr& node,
                   const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
-                  const std::string& group_name) override;
+                  const std::string& /* unused */) override;
   bool reset() override;
 
   moveit_msgs::action::LocalPlanner::Feedback
   solve(const robot_trajectory::RobotTrajectory& local_trajectory,
-        const std::shared_ptr<const moveit_msgs::action::LocalPlanner::Goal> local_goal,
+        const std::shared_ptr<const moveit_msgs::action::LocalPlanner::Goal> /* unused */,
         trajectory_msgs::msg::JointTrajectory& local_solution) override;
 
 private:

diff --git a/.../hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp b/.../hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp
@@ -40,7 +40,7 @@ namespace
 {
 const rclcpp::Logger LOGGER = rclcpp::get_logger("local_planner_component");
 // If stuck for this many iterations or more, abort the local planning action
-constexpr size_t STUCK_ITERATIONS_THRESHOLD = 10;
+constexpr size_t STUCK_ITERATIONS_THRESHOLD = 5;
 constexpr double STUCK_THRESHOLD_RAD = 1e-4;  // L1-norm sum across all joints
 }  // namespace
 
@@ -132,7 +132,7 @@ ForwardTrajectory::solve(const robot_trajectory::RobotTrajectory& local_trajecto
         current_state_command.zeroAccelerations();
       }
       robot_command.empty();
-      robot_command.addSuffixWayPoint(*current_state, 0.0);
+      robot_command.addSuffixWayPoint(*current_state, local_trajectory.getWayPointDurationFromPrevious(0));
     }
 
     // Detect if the local solver is stuck

diff --git a/...al_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h b/...al_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h
@@ -167,8 +167,8 @@ class LocalPlannerComponent
   // Planner configuration
   LocalPlannerConfig config_;
 
-  // Current planner state
-  LocalPlannerState state_;
+  // Current planner state. Must be thread-safe
+  std::atomic<LocalPlannerState> state_;
 
   // Timer to periodically call executeIteration()
   rclcpp::TimerBase::SharedPtr timer_;

diff --git a/...ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp b/...ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp
@@ -159,8 +159,9 @@ bool LocalPlannerComponent::initialize()
         RCLCPP_INFO(LOGGER, "Received local planning goal request");
         return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
       },
-      [](const std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::LocalPlanner>>& /*unused*/) {
+      [this](const std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::LocalPlanner>>& /*unused*/) {
         RCLCPP_INFO(LOGGER, "Received request to cancel local planning goal");
+        state_ = LocalPlannerState::ABORT;
         return rclcpp_action::CancelResponse::ACCEPT;
       },
       [this](std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::LocalPlanner>> goal_handle) {
@@ -280,6 +281,7 @@ void LocalPlannerComponent::executeIteration()
       if (!local_planner_feedback_->feedback.empty())
       {
         local_planning_goal_handle_->publish_feedback(local_planner_feedback_);
+        return;
       }
 
       // Use a configurable message interface like MoveIt servo

diff --git a/moveit_ros/hybrid_planning/package.xml b/moveit_ros/hybrid_planning/package.xml
@@ -16,20 +16,21 @@
   <build_depend>moveit_common</build_depend>
 
   <depend>ament_index_cpp</depend>
-  <depend>rclcpp</depend>
-  <depend>rclcpp_action</depend>
-  <depend>rclcpp_components</depend>
   <depend>moveit_msgs</depend>
   <depend>moveit_core</depend>
   <depend>moveit_ros_planning</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_components</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
   <depend>tf2_ros</depend>
   <depend>trajectory_msgs</depend>
 
   <exec_depend>controller_manager</exec_depend>
+  <exec_depend>position_controllers</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>rviz2</exec_depend>
   <exec_depend>moveit_resources_panda_moveit_config</exec_depend>

diff --git a/moveit_ros/hybrid_planning/test/CMakeLists.txt b/moveit_ros/hybrid_planning/test/CMakeLists.txt
@@ -1,3 +1,28 @@
 add_executable(hybrid_planning_demo_node hybrid_planning_demo_node.cpp)
 ament_target_dependencies(hybrid_planning_demo_node ${THIS_PACKAGE_INCLUDE_DEPENDS})
 target_link_libraries(hybrid_planning_demo_node ${LIBRARIES})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ros_testing REQUIRED)
+  find_package(Boost REQUIRED COMPONENTS filesystem)
+
+  # These don't pass yet, disable them for now
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cppcheck_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  set(ament_cmake_flake8_FOUND TRUE)
+  set(ament_cmake_uncrustify_FOUND TRUE)
+
+  # Run all lint tests in package.xml except those listed above
+  ament_lint_auto_find_test_dependencies()
+
+  # Basic integration tests
+  ament_add_gtest_executable(test_basic_integration
+      test_basic_integration.cpp
+  )
+  ament_target_dependencies(test_basic_integration ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  add_ros_test(launch/test_basic_integration.test.py TIMEOUT 50 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
+endif()
diff --git a/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp b/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp
@@ -77,7 +77,6 @@ class HybridPlanningDemo
     }
     hp_action_client_ =
         rclcpp_action::create_client<moveit_msgs::action::HybridPlanner>(node_, hybrid_planning_action_name);
-    robot_state_publisher_ = node_->create_publisher<moveit_msgs::msg::DisplayRobotState>("display_robot_state", 1);
 
     collision_object_1_.header.frame_id = "panda_link0";
     collision_object_1_.id = "box1";
@@ -193,6 +192,12 @@ class HybridPlanningDemo
 
     // Configure a valid robot state
     robot_state->setToDefaultValues(joint_model_group, "ready");
+    robot_state->update();
+    // Lock the planning scene as briefly as possible
+    {
+      planning_scene_monitor::LockedPlanningSceneRW locked_planning_scene(planning_scene_monitor_);
+      locked_planning_scene->setCurrentState(*robot_state);
+    }
 
     // Create desired motion goal
     moveit_msgs::msg::MotionPlanRequest goal_motion_request;
@@ -260,7 +265,6 @@ class HybridPlanningDemo
 private:
   rclcpp::Node::SharedPtr node_;
   rclcpp_action::Client<moveit_msgs::action::HybridPlanner>::SharedPtr hp_action_client_;
-  rclcpp::Publisher<moveit_msgs::msg::DisplayRobotState>::SharedPtr robot_state_publisher_;
   rclcpp::Subscription<moveit_msgs::msg::MotionPlanResponse>::SharedPtr global_solution_subscriber_;
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
   rclcpp::TimerBase::SharedPtr timer_;
@@ -285,6 +289,7 @@ int main(int argc, char** argv)
 
   HybridPlanningDemo demo(node);
   std::thread run_demo([&demo]() {
+    // This sleep isn't necessary but it gives humans time to process what's going on
     rclcpp::sleep_for(5s);
     demo.run();
   });