Fixes merge conflicts (#3067)

moveit · Nov 7, 2024 · ae2d284 · ae2d284
1 parent 2324407
commit ae2d284
Show file tree

Hide file tree

Showing 5 changed files with 2 additions and 73 deletions.
diff --git a/..._industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h b/..._industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h
@@ -121,27 +121,13 @@ bool pilz_industrial_motion_planner::PlanningContextBase<GeneratorT>::solve(plan
 {
   if (!terminated_)
   {
-    // Use current state as start state if not set
-    if (request_.start_state.joint_state.name.empty())
-    {
-      moveit_msgs::msg::RobotState current_state;
-      moveit::core::robotStateToRobotStateMsg(getPlanningScene()->getCurrentState(), current_state);
-      request_.start_state = current_state;
-    }
-    bool result = generator_.generate(getPlanningScene(), request_, res);
-    return result;
-    // res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
-    // return false; // TODO
+    return generator_.generate(getPlanningScene(), request_, res);
   }
-<<<<<<< HEAD
 
   RCLCPP_ERROR(rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_base"),
                "Using solve on a terminated planning context!");
   res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
   return false;
-=======
-  generator_.generate(getPlanningScene(), request_, res);
->>>>>>> 70e1aae8b (Ports moveit/moveit/pull/3519 to ros2 (#3055))
 }
 
 template <typename GeneratorT>

diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp
@@ -59,15 +59,6 @@ bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::Plannin
     return false;
   }
 
-<<<<<<< HEAD
-  if (!robot_model->getJointModelGroup(group_name)->canSetStateFromIK(link_name))
-  {
-    RCLCPP_ERROR_STREAM(LOGGER, "No valid IK solver exists for " << link_name << " in planning group " << group_name);
-    return false;
-  }
-
-=======
->>>>>>> 70e1aae8b (Ports moveit/moveit/pull/3519 to ros2 (#3055))
   if (frame_id != robot_model->getModelFrame())
   {
     RCLCPP_ERROR_STREAM(LOGGER, "Given frame (" << frame_id << ") is unequal to model frame("

diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp
@@ -348,13 +348,8 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr&
     return false;
   }
 
-<<<<<<< HEAD
-  moveit::core::RobotState start_state(scene->getCurrentState());
-  moveit::core::robotStateMsgToRobotState(req.start_state, start_state, true);
-  setSuccessResponse(start_state, req.group_name, joint_trajectory, planning_begin, res);
-  return true;
-=======
   setSuccessResponse(plan_info.start_scene->getCurrentState(), req.group_name, joint_trajectory, planning_begin, res);
+  return true;
 }
 
 TrajectoryGenerator::MotionPlanInfo::MotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
@@ -377,7 +372,6 @@ TrajectoryGenerator::MotionPlanInfo::MotionPlanInfo(const planning_scene::Planni
       start_joint_position[names[i]] = positions[j];
     }
   }
->>>>>>> 70e1aae8b (Ports moveit/moveit/pull/3519 to ros2 (#3055))
 }
 
 }  // namespace pilz_industrial_motion_planner
diff --git a/...pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp b/...pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp
@@ -271,27 +271,6 @@ TEST_F(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping)
 }
 
 /**
-<<<<<<< HEAD
- * @brief test invalid motion plan request with incomplete start state and
- * cartesian goal
- */
-TEST_F(TrajectoryGeneratorCIRCTest, incompleteStartState)
-{
-  auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
-
-  planning_interface::MotionPlanRequest req{ circ.toRequest() };
-  EXPECT_GT(req.start_state.joint_state.name.size(), 1u);
-  req.start_state.joint_state.name.resize(1);
-  req.start_state.joint_state.position.resize(1);  // prevent failing check for equal sizes
-
-  planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(circ_->generate(planning_scene_, req, res));
-  EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
-}
-
-/**
-=======
->>>>>>> 70e1aae8b (Ports moveit/moveit/pull/3519 to ros2 (#3055))
  * @brief test invalid motion plan request with non zero start velocity
  */
 TEST_F(TrajectoryGeneratorCIRCTest, nonZeroStartVelocity)

diff --git a/.../pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp b/.../pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp
@@ -409,27 +409,6 @@ TEST_F(TrajectoryGeneratorLINTest, IncorrectJointNumber)
 }
 
 /**
-<<<<<<< HEAD
- * @brief test invalid motion plan request with incomplete start state and
- * cartesian goal
- */
-TEST_F(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState)
-{
-  // construct motion plan request
-  moveit_msgs::msg::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() };
-  EXPECT_GT(lin_cart_req.start_state.joint_state.name.size(), 1u);
-  lin_cart_req.start_state.joint_state.name.resize(1);
-  lin_cart_req.start_state.joint_state.position.resize(1);  // prevent failing check for equal sizes
-
-  // generate lin trajectory
-  planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(lin_->generate(planning_scene_, lin_cart_req, res));
-  EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
-}
-
-/**
-=======
->>>>>>> 70e1aae8b (Ports moveit/moveit/pull/3519 to ros2 (#3055))
  * @brief Set a frame id in goal constraint with cartesian goal on both position
  * and orientation constraints
  */