Fixes merge conflicts

moveit_planners/pilz_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>

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("

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp

@@ -347,13 +347,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,
@@ -376,7 +371,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

moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp

@@ -263,23 +263,13 @@ TEST_F(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping)
     EXPECT_EQ(nocm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
   }
 
-  {
-<<<<<<< HEAD
-    std::shared_ptr<CircJointMissingInStartState> cjmiss_ex{ new CircJointMissingInStartState("") };
-    EXPECT_EQ(cjmiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
-  }
-
   {
     std::shared_ptr<CircInverseForGoalIncalculable> cifgi_ex{ new CircInverseForGoalIncalculable("") };
-=======
-    auto cifgi_ex = std::make_shared<CircInverseForGoalIncalculable>("");
->>>>>>> 70e1aae8b (Ports moveit/moveit/pull/3519 to ros2 (#3055))
     EXPECT_EQ(cifgi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION);
   }
 }
 
 /**
-<<<<<<< HEAD
  * @brief Construct a TrajectoryGeneratorCirc with no limits given
  */
 TEST_F(TrajectoryGeneratorCIRCTest, noLimits)
@@ -290,26 +280,6 @@ TEST_F(TrajectoryGeneratorCIRCTest, noLimits)
 }
 
 /**
- * @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)

moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp

@@ -196,17 +196,8 @@ TEST_F(TrajectoryGeneratorLINTest, TestExceptionErrorCodeMapping)
     EXPECT_EQ(jnm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
   }
 
-  {
-<<<<<<< HEAD
-    std::shared_ptr<LinJointMissingInStartState> ljmiss_ex{ new LinJointMissingInStartState("") };
-    EXPECT_EQ(ljmiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
-  }
-
   {
     std::shared_ptr<LinInverseForGoalIncalculable> lifgi_ex{ new LinInverseForGoalIncalculable("") };
-=======
-    auto lifgi_ex = std::make_shared<LinInverseForGoalIncalculable>("");
->>>>>>> 70e1aae8b (Ports moveit/moveit/pull/3519 to ros2 (#3055))
     EXPECT_EQ(lifgi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION);
   }
 }
@@ -433,27 +424,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
  */