Freespace plan callback in working state. Prev issue: Passing nested …

…composite instruction to freespace task when it expect composite instruction one layer deep
BryanMqz · May 29, 2024 · 7e7ff3b · 7e7ff3b
1 parent 3e99d9b
commit 7e7ff3b
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 15 deletions.
diff --git a/snp_application/src/bt/snp_bt_ros_nodes.cpp b/snp_application/src/bt/snp_bt_ros_nodes.cpp
@@ -416,8 +416,10 @@ bool FreespaceMotionPlanPubNode::setMessage(trajectory_msgs::msg::JointTrajector
 {
   try
   {
-    msg = combine(msg, getBTInput<trajectory_msgs::msg::JointTrajectory>(this, PROCESS_INPUT_PORT_KEY));
-    msg = combine(msg, getBTInput<trajectory_msgs::msg::JointTrajectory>(this, TRAJECTORY_INPUT_PORT_KEY));
+    // msg = combine(msg, getBTInput<trajectory_msgs::msg::JointTrajectory>(this, PROCESS_INPUT_PORT_KEY));
+    // msg = combine(msg, getBTInput<trajectory_msgs::msg::JointTrajectory>(this, TRAJECTORY_INPUT_PORT_KEY));
+    msg = getBTInput<trajectory_msgs::msg::JointTrajectory>(this, TRAJECTORY_INPUT_PORT_KEY);
+
   }
   catch (const std::exception& ex)
   {

diff --git a/snp_motion_planning/src/planning_server.cpp b/snp_motion_planning/src/planning_server.cpp
@@ -634,24 +634,24 @@ class PlanningServer
 
       tesseract_planning::CompositeInstruction freespace_program(PROFILE, tesseract_planning::CompositeInstructionOrder::ORDERED,
                                                                  manip_info);
-      tesseract_planning::CompositeInstruction repeat_process(PROFILE);
-      repeat_process.setDescription("repeat_process");
+      // tesseract_planning::CompositeInstruction repeat_process(PROFILE);
+      // repeat_process.setDescription("repeat_process");
 
       tesseract_planning::JointWaypoint wp1 = rosJointStateToJointWaypoint(req->js1);
       tesseract_planning::JointWaypoint wp2 = rosJointStateToJointWaypoint(req->js2);
 
       // Define a freespace move to the first waypoint
-      repeat_process.appendMoveInstruction(
+      freespace_program.appendMoveInstruction(
           tesseract_planning::MoveInstruction(tesseract_planning::JointWaypointPoly{wp1},
                                               tesseract_planning::MoveInstructionType::FREESPACE, PROFILE, manip_info));
 
       // Define a freespace move to the second waypoint
-      repeat_process.appendMoveInstruction(
+      freespace_program.appendMoveInstruction(
           tesseract_planning::MoveInstruction(tesseract_planning::JointWaypointPoly{wp2},
                                               tesseract_planning::MoveInstructionType::FREESPACE, PROFILE, manip_info));
 
       // Add the composite to the program
-      freespace_program.push_back(repeat_process);
+      // freespace_program.push_back(repeat_process);
 
       // Create a manipulator info and program from the service request
       // const std::string& base_frame = req->tool_paths.at(0).segments.at(0).header.frame_id;
@@ -683,14 +683,14 @@ class PlanningServer
       // Remove scan link?
       removeScanLink();
 
-      if (program_results.size() < 1)
-      {
-        std::stringstream ss;
-        ss << "The composite instruction must have at least 1 child. "
-              "This result only has "
-           << program_results.size();
-        throw std::runtime_error(ss.str());
-      }
+      // if (program_results.size() != 1)
+      // {
+      //   std::stringstream ss;
+      //   ss << "The composite instruction must have at least 1 child. "
+      //         "This result only has "
+      //      << program_results.size();
+      //   throw std::runtime_error(ss.str());
+      // }
 
       // Return results