Removed plan_with_sensing (#1142)

moveit · Aug 9, 2022 · ad9fb46 · ad9fb46
1 parent 048062c
commit ad9fb46
Show file tree

Hide file tree

Showing 7 changed files with 1 addition and 471 deletions.
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp
@@ -42,7 +42,6 @@
 
 #include <moveit/kinematic_constraints/utils.h>
 #include <moveit/plan_execution/plan_execution.h>
-#include <moveit/plan_execution/plan_with_sensing.h>
 #include <moveit/planning_pipeline/planning_pipeline.h>
 #include <moveit/robot_state/conversions.h>
 #include <moveit/trajectory_processing/trajectory_tools.h>
@@ -166,10 +165,6 @@ void MoveGroupSequenceAction::executeSequenceCallbackPlanAndExecute(
   [this, &request = goal->request](plan_execution::ExecutableMotionPlan& plan) {
     return planUsingSequenceManager(request, plan);
   };
-  if (goal->planning_options.look_around && context_->plan_with_sensing_)
-  {
-    RCLCPP_WARN(LOGGER, "Plan with sensing not yet implemented/tested. This option is ignored.");  // LCOV_EXCL_LINE
-  }
 
   plan_execution::ExecutableMotionPlan plan;
   context_->plan_execution_->planAndExecute(plan, planning_scene_diff, opt);

diff --git a/moveit_ros/move_group/include/moveit/move_group/move_group_context.h b/moveit_ros/move_group/include/moveit/move_group/move_group_context.h
@@ -57,8 +57,7 @@ MOVEIT_CLASS_FORWARD(PlanningPipeline);  // Defines PlanningPipelinePtr, ConstPt
 
 namespace plan_execution
 {
-MOVEIT_CLASS_FORWARD(PlanExecution);    // Defines PlanExecutionPtr, ConstPtr, WeakPtr... etc
-MOVEIT_CLASS_FORWARD(PlanWithSensing);  // Defines PlanWithSensingPtr, ConstPtr, WeakPtr... etc
+MOVEIT_CLASS_FORWARD(PlanExecution);  // Defines PlanExecutionPtr, ConstPtr, WeakPtr... etc
 }  // namespace plan_execution
 
 namespace trajectory_execution_manager
@@ -83,7 +82,6 @@ struct MoveGroupContext
   trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_;
   planning_pipeline::PlanningPipelinePtr planning_pipeline_;
   plan_execution::PlanExecutionPtr plan_execution_;
-  plan_execution::PlanWithSensingPtr plan_with_sensing_;
   bool allow_trajectory_execution_;
   bool debug_;
 };

diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp
@@ -39,7 +39,6 @@
 #include <moveit/moveit_cpp/moveit_cpp.h>
 #include <moveit/planning_pipeline/planning_pipeline.h>
 #include <moveit/plan_execution/plan_execution.h>
-#include <moveit/plan_execution/plan_with_sensing.h>
 #include <moveit/trajectory_processing/trajectory_tools.h>
 #include <moveit/kinematic_constraints/utils.h>
 #include <moveit/utils/message_checks.h>
@@ -149,17 +148,6 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const std::shared_pt
   opt.plan_callback_ = [this, &motion_plan_request](plan_execution::ExecutableMotionPlan& plan) {
     return planUsingPlanningPipeline(motion_plan_request, plan);
   };
-  if (goal->get_goal()->planning_options.look_around && context_->plan_with_sensing_)
-  {
-    opt.plan_callback_ = [plan_with_sensing = context_->plan_with_sensing_.get(), planner = opt.plan_callback_,
-                          attempts = goal->get_goal()->planning_options.look_around_attempts,
-                          safe_execution_cost = goal->get_goal()->planning_options.max_safe_execution_cost](
-                             plan_execution::ExecutableMotionPlan& plan) {
-      return plan_with_sensing->computePlan(plan, planner, attempts, safe_execution_cost);
-    };
-
-    context_->plan_with_sensing_->setBeforeLookCallback([this]() { return startMoveLookCallback(); });
-  }
 
   plan_execution::ExecutableMotionPlan plan;
   if (preempt_requested_)

diff --git a/moveit_ros/move_group/src/move_group_context.cpp b/moveit_ros/move_group/src/move_group_context.cpp
@@ -39,7 +39,6 @@
 #include <moveit/moveit_cpp/moveit_cpp.h>
 #include <moveit/planning_pipeline/planning_pipeline.h>
 #include <moveit/plan_execution/plan_execution.h>
-#include <moveit/plan_execution/plan_with_sensing.h>
 
 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_move_group_capabilities_base.move_group_context");
 
@@ -78,16 +77,11 @@ move_group::MoveGroupContext::MoveGroupContext(const moveit_cpp::MoveItCppPtr& m
     trajectory_execution_manager_ = moveit_cpp_->getTrajectoryExecutionManager();
     plan_execution_ = std::make_shared<plan_execution::PlanExecution>(moveit_cpp_->getNode(), planning_scene_monitor_,
                                                                       trajectory_execution_manager_);
-    plan_with_sensing_ =
-        std::make_shared<plan_execution::PlanWithSensing>(moveit_cpp_->getNode(), trajectory_execution_manager_);
-    if (debug)
-      plan_with_sensing_->displayCostSources(true);
   }
 }
 
 move_group::MoveGroupContext::~MoveGroupContext()
 {
-  plan_with_sensing_.reset();
   plan_execution_.reset();
   trajectory_execution_manager_.reset();
   planning_pipeline_.reset();

diff --git a/moveit_ros/planning/plan_execution/CMakeLists.txt b/moveit_ros/planning/plan_execution/CMakeLists.txt
@@ -1,7 +1,6 @@
 set(MOVEIT_LIB_NAME moveit_plan_execution)
 
 add_library(${MOVEIT_LIB_NAME} SHARED
-  src/plan_with_sensing.cpp
   src/plan_execution.cpp)
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 target_link_libraries(${MOVEIT_LIB_NAME}

diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h