diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h index 9208428a32..69de1a0eae 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h +++ b/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::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 diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 57a7b08ca0..ca38bceb9d 100644 --- 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 index 7e050ec180..a003b465be 100644 --- 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/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp index a07055825e..a4826ada86 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp +++ b/moveit_planners/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/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp index e23f074c12..8785c6f94a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp +++ b/moveit_planners/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 */