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 cbf3438445..eed0b68eab 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 f30ee07b1d..1f8d871385 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 a1749096f3..18da41f112 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/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 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 b589206bac..5e540e4fbb 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 @@ -263,23 +263,13 @@ TEST_F(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping) EXPECT_EQ(nocm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } - { -<<<<<<< HEAD - std::shared_ptr cjmiss_ex{ new CircJointMissingInStartState("") }; - EXPECT_EQ(cjmiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); - } - { std::shared_ptr cifgi_ex{ new CircInverseForGoalIncalculable("") }; -======= - auto cifgi_ex = std::make_shared(""); ->>>>>>> 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) 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 fe461a72d4..1da2435882 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 @@ -196,17 +196,8 @@ TEST_F(TrajectoryGeneratorLINTest, TestExceptionErrorCodeMapping) EXPECT_EQ(jnm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } - { -<<<<<<< HEAD - std::shared_ptr ljmiss_ex{ new LinJointMissingInStartState("") }; - EXPECT_EQ(ljmiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); - } - { std::shared_ptr lifgi_ex{ new LinInverseForGoalIncalculable("") }; -======= - auto lifgi_ex = std::make_shared(""); ->>>>>>> 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 */