Fixed rework crashing bug

Author: marrts

crs_application/config/main/motion_planning/MP_config.yaml

@@ -1,7 +1,7 @@
 general:
   use_trajopt_freespace: true
   use_trajopt_surface: true
-  default_to_descartes: true
+  default_to_descartes: false
   default_to_ompl: true
   simplify_start_end_freespace: true
   manipulator: "manipulator"

crs_application/include/crs_application/task_managers/process_execution_manager.h

@@ -94,9 +94,11 @@ class ProcessExecutionManager
   void resetIndexes();
   bool changeActiveController(const bool turn_on_cart);
   bool toggleSander(const bool turn_on_sander);
+  common::ActionResult moveRobotIfNotAtStart(const trajectory_msgs::msg::JointTrajectory& traj);
   common::ActionResult execTrajectory(const trajectory_msgs::msg::JointTrajectory& traj);
   common::ActionResult execSurfaceTrajectory(const cartesian_trajectory_msgs::msg::CartesianTrajectory& traj,
-                                             const crs_motion_planning::cartesianTrajectoryConfig& traj_config);
+                                             const crs_motion_planning::cartesianTrajectoryConfig& traj_config,
+                                             const trajectory_msgs::msg::JointTrajectory& joint_traj);
   common::ActionResult checkPreReq();
 
   void jointCallback(const sensor_msgs::msg::JointState::SharedPtr joint_msg);

crs_application/src/crs_executive.cpp

@@ -540,7 +540,7 @@ bool CRSExecutive::setupScanAcquisitionStates()
     async : false,
     exit_cb : nullptr,
     on_done_action : "",
-    on_failed_action: ""
+    on_failed_action : ""
   };
 
   st_callbacks_map[scan::CAPTURE] = StateCallbackInfo{

crs_application/src/task_managers/motion_planning_manager.cpp

@@ -438,6 +438,11 @@ common::ActionResult MotionPlanningManager::planMediaChanges()
     const trajectory_msgs::msg::JointTrajectory& next_traj = result_.process_plans[i + 1].process_motions.front();
     if (!next_traj.points.empty())
     {
+      if (!result_.process_plans[i + 1].start.points.empty())
+      {
+        result_.process_plans[i + 1].start.points.clear();
+        result_.process_plans[i + 1].start.joint_names.clear();
+      }
       req->goal_position.position = next_traj.points.front().positions;
       req->goal_position.name = next_traj.joint_names;
     }

crs_application/src/task_managers/process_execution_manager.cpp

@@ -147,12 +147,10 @@ common::ActionResult ProcessExecutionManager::execProcess()
   for (std::size_t i = 0; i < process_plan.process_motions.size(); i++)
   {
     RCLCPP_INFO(node_->get_logger(), "%s Executing process path %i", MANAGER_NAME.c_str(), i);
-    std::chrono::duration<double> sleep_dur(WAIT_ROBOT_STOP);
-    RCLCPP_INFO(node_->get_logger(), "Waiting %f seconds for robot to fully stop", WAIT_ROBOT_STOP);
-    rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::seconds>(sleep_dur));
     if (config_->force_controlled_trajectories)
     {
-      if (!execSurfaceTrajectory(process_plan.force_controlled_process_motions[i], cartesian_traj_config))
+      if (!execSurfaceTrajectory(
+              process_plan.force_controlled_process_motions[i], cartesian_traj_config, process_plan.process_motions[i]))
       {
         common::ActionResult res;
         res.err_msg = boost::str(boost::format("%s failed to execute process motion %i") % MANAGER_NAME.c_str() % i);
@@ -435,30 +433,27 @@ bool ProcessExecutionManager::toggleSander(const bool turn_on_sander)
   return true;
 }
 
-common::ActionResult ProcessExecutionManager::execTrajectory(const trajectory_msgs::msg::JointTrajectory& traj)
+common::ActionResult ProcessExecutionManager::moveRobotIfNotAtStart(const trajectory_msgs::msg::JointTrajectory& traj)
 {
-  using namespace control_msgs::action;
-  static const double GOAL_ACCEPT_TIMEOUT_PERIOD = 1.0;
+  // Check if joint state is in correct position before executing, if not then call a freespace motion from current
+  // joint state to beginning of trajectory
 
   common::ActionResult res = false;
 
-  if (config_->force_controlled_trajectories && !ProcessExecutionManager::changeActiveController(false))
-  {
-    res.succeeded = false;
-    return res;
-  }
-
-  // Check if joint state is in correct position before executing, if not then call a freespace motion from current
-  // joint state to beginning of trajectory
   sensor_msgs::msg::JointState::SharedPtr curr_joint_state =
       crs_application::common::getCurrentState(node_, JOINT_STATES_TOPIC, 1.0);
   if (!crs_motion_planning::checkStartState(traj, *curr_joint_state, config_->joint_tolerance[0]))
   {
+    if (!ProcessExecutionManager::changeActiveController(false))
+    {
+      res.succeeded = false;
+      return res;
+    }
     RCLCPP_WARN(node_->get_logger(), "Not at the correct joint state to start freespace motion");
     if (!call_freespace_client_->service_is_ready())
     {
       RCLCPP_ERROR(node_->get_logger(), "%s Freespace Motion is not ready`", MANAGER_NAME.c_str());
-      return false;
+      return res;
     }
     crs_msgs::srv::CallFreespaceMotion::Request::SharedPtr freespace_req =
         std::make_shared<crs_msgs::srv::CallFreespaceMotion::Request>();
@@ -473,17 +468,36 @@ common::ActionResult ProcessExecutionManager::execTrajectory(const trajectory_ms
     if (status != std::future_status::ready)
     {
       RCLCPP_ERROR(node_->get_logger(), "%s Call Freespace Motion service call timedout", MANAGER_NAME.c_str());
-      return false;
+      return res;
     }
     auto result = result_future.get();
 
     if (!result->success)
     {
       RCLCPP_ERROR(node_->get_logger(), "%s %s", MANAGER_NAME.c_str(), result->message.c_str());
-      return false;
+      return res;
     }
   }
 
+  return true;
+}
+
+common::ActionResult ProcessExecutionManager::execTrajectory(const trajectory_msgs::msg::JointTrajectory& traj)
+{
+  using namespace control_msgs::action;
+  static const double GOAL_ACCEPT_TIMEOUT_PERIOD = 1.0;
+
+  common::ActionResult res = false;
+
+  if (config_->force_controlled_trajectories && !ProcessExecutionManager::changeActiveController(false))
+  {
+    res.succeeded = false;
+    return res;
+  }
+
+  if (!ProcessExecutionManager::moveRobotIfNotAtStart(traj))
+    return res;
+
   res.succeeded = crs_motion_planning::execTrajectory(trajectory_exec_client_, node_->get_logger(), traj);
   if (!res)
   {
@@ -495,14 +509,22 @@ common::ActionResult ProcessExecutionManager::execTrajectory(const trajectory_ms
 
 common::ActionResult
 ProcessExecutionManager::execSurfaceTrajectory(const cartesian_trajectory_msgs::msg::CartesianTrajectory& traj,
-                                               const crs_motion_planning::cartesianTrajectoryConfig& traj_config)
+                                               const crs_motion_planning::cartesianTrajectoryConfig& traj_config,
+                                               const trajectory_msgs::msg::JointTrajectory& joint_traj)
 {
   using namespace control_msgs::action;
   static const double GOAL_ACCEPT_TIMEOUT_PERIOD = 1.0;
 
   // rclcpp::Duration traj_dur(traj.points.back().time_from_start);
   common::ActionResult res = false;
 
+  if (!ProcessExecutionManager::moveRobotIfNotAtStart(joint_traj))
+    return res;
+
+  std::chrono::duration<double> sleep_dur(WAIT_ROBOT_STOP);
+  RCLCPP_INFO(node_->get_logger(), "Waiting %f seconds for robot to fully stop", WAIT_ROBOT_STOP);
+  rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::seconds>(sleep_dur));
+
   if (config_->force_controlled_trajectories && !ProcessExecutionManager::changeActiveController(true))
   {
     res.succeeded = false;

crs_application/src/task_managers/scan_acquisition_manager.cpp

@@ -187,7 +187,6 @@ common::ActionResult ScanAcquisitionManager::moveRobot()
   freespace_motion_request->goal_pose = scan_poses_.at(scan_index_);
   freespace_motion_request->execute = true;
 
-
   auto result_future = call_freespace_motion_client_->async_send_request(freespace_motion_request);
 
   std::future_status status = result_future.wait_for(std::chrono::duration<double>(WAIT_MOTION_COMPLETION));

crs_motion_planning/src/motion_planning_server.cpp

@@ -210,7 +210,7 @@ class MotionPlanningServer : public rclcpp::Node
         }
         std::vector<double> last_pose = returned_plans[last_success_i].process_motions.back().points.back().positions;
         std::vector<std::string> last_joint_names = returned_plans[last_success_i].process_motions.back().joint_names;
-        motion_planner_config->use_start = false;
+        motion_planner_config->use_start = true;
         motion_planner_config->start_pose =
             std::make_shared<tesseract_motion_planners::JointWaypoint>(last_pose, last_joint_names);
         returned_plans[last_success_i].end.points.clear();

crs_motion_planning/src/path_planning_utils.cpp

@@ -836,6 +836,7 @@ bool crsMotionPlanner::generateOMPLSeed(const tesseract_motion_planners::JointWa
   ompl_planner_config->n_output_states = config_->ompl_config.n_output_states;
   ompl_planner_config->longest_valid_segment_fraction = config_->ompl_config.longest_valid_segment_fraction;
   ompl_planner_config->longest_valid_segment_length = config_->ompl_config.longest_valid_segment_length;
+  //  ompl_planner_config->
   RCLCPP_INFO(logger_, "OUTPUT STATES: %i", config_->ompl_config.n_output_states);
   std::cout << "GENERATING OMPL SEED FROM \n"
             << start_pose->getPositions().matrix() << "\nTO\n"