diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h index 72db4fa777..7f8bbadcb6 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h @@ -48,7 +48,7 @@ namespace planning_interface struct MotionPlanResponse { /** \brief Constructor */ - MotionPlanResponse() : trajectory_(nullptr), planning_time_(0.0), error_code_(moveit::core::MoveItErrorCode::FAILURE) + MotionPlanResponse() : trajectory(nullptr), planning_time(0.0), error_code(moveit::core::MoveItErrorCode::FAILURE) { } @@ -56,19 +56,19 @@ struct MotionPlanResponse void getMessage(moveit_msgs::msg::MotionPlanResponse& msg) const; // Trajectory generated by the queried planner - robot_trajectory::RobotTrajectoryPtr trajectory_; - // Time to plan the respond to the planning query - double planning_time_; + robot_trajectory::RobotTrajectoryPtr trajectory; + // Time to plan the response to the planning query + double planning_time; // Result status of the query - moveit::core::MoveItErrorCode error_code_; + moveit::core::MoveItErrorCode error_code; /// The full starting state used for planning - moveit_msgs::msg::RobotState start_state_; - std::string planner_id_; + moveit_msgs::msg::RobotState start_state; + std::string planner_id; // \brief Enable checking of query success or failure, for example if(response) ... explicit operator bool() const { - return bool(error_code_); + return bool(error_code); } }; @@ -76,11 +76,11 @@ struct MotionPlanDetailedResponse { void getMessage(moveit_msgs::msg::MotionPlanDetailedResponse& msg) const; - std::vector trajectory_; - std::vector description_; - std::vector processing_time_; - moveit_msgs::msg::MoveItErrorCodes error_code_; - std::string planner_id_; + std::vector trajectory; + std::vector description; + std::vector processing_time; + moveit_msgs::msg::MoveItErrorCodes error_code; + std::string planner_id; }; } // namespace planning_interface diff --git a/moveit_core/planning_interface/src/planning_response.cpp b/moveit_core/planning_interface/src/planning_response.cpp index abaf2e3fa7..8b93391e80 100644 --- a/moveit_core/planning_interface/src/planning_response.cpp +++ b/moveit_core/planning_interface/src/planning_response.cpp @@ -39,19 +39,19 @@ void planning_interface::MotionPlanResponse::getMessage(moveit_msgs::msg::MotionPlanResponse& msg) const { - msg.error_code = error_code_; - msg.planning_time = planning_time_; - if (trajectory_ && !trajectory_->empty()) + msg.error_code = error_code; + msg.planning_time = planning_time; + if (trajectory && !trajectory->empty()) { - moveit::core::robotStateToRobotStateMsg(trajectory_->getFirstWayPoint(), msg.trajectory_start); - trajectory_->getRobotTrajectoryMsg(msg.trajectory); - msg.group_name = trajectory_->getGroupName(); + moveit::core::robotStateToRobotStateMsg(trajectory->getFirstWayPoint(), msg.trajectory_start); + trajectory->getRobotTrajectoryMsg(msg.trajectory); + msg.group_name = trajectory->getGroupName(); } } void planning_interface::MotionPlanDetailedResponse::getMessage(moveit_msgs::msg::MotionPlanDetailedResponse& msg) const { - msg.error_code = error_code_; + msg.error_code = error_code; msg.trajectory.clear(); msg.description.clear(); @@ -59,21 +59,21 @@ void planning_interface::MotionPlanDetailedResponse::getMessage(moveit_msgs::msg bool first = true; - for (std::size_t i = 0; i < trajectory_.size(); ++i) + for (std::size_t i = 0; i < trajectory.size(); ++i) { - if (trajectory_[i]->empty()) + if (trajectory[i]->empty()) continue; if (first) { first = false; - moveit::core::robotStateToRobotStateMsg(trajectory_[i]->getFirstWayPoint(), msg.trajectory_start); - msg.group_name = trajectory_[i]->getGroupName(); + moveit::core::robotStateToRobotStateMsg(trajectory[i]->getFirstWayPoint(), msg.trajectory_start); + msg.group_name = trajectory[i]->getGroupName(); } msg.trajectory.resize(msg.trajectory.size() + 1); - trajectory_[i]->getRobotTrajectoryMsg(msg.trajectory.back()); - if (description_.size() > i) - msg.description.push_back(description_[i]); - if (processing_time_.size() > i) - msg.processing_time.push_back(processing_time_[i]); + trajectory[i]->getRobotTrajectoryMsg(msg.trajectory.back()); + if (description.size() > i) + msg.description.push_back(description[i]); + if (processing_time.size() > i) + msg.processing_time.push_back(processing_time[i]); } } diff --git a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp index 5739f8fd05..92349a06a1 100644 --- a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp +++ b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp @@ -51,7 +51,7 @@ bool callPlannerInterfaceSolve(const planning_interface::PlannerManager& planner const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) { - planning_interface::PlanningContextPtr context = planner.getPlanningContext(planning_scene, req, res.error_code_); + planning_interface::PlanningContextPtr context = planner.getPlanningContext(planning_scene, req, res.error_code); if (context) { return context->solve(res); @@ -70,7 +70,7 @@ bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAda try { bool result = adapter.adaptAndPlan(planner, planning_scene, req, res, added_path_index); - RCLCPP_DEBUG_STREAM(LOGGER, adapter.getDescription() << ": " << moveit::core::error_code_to_string(res.error_code_)); + RCLCPP_DEBUG_STREAM(LOGGER, adapter.getDescription() << ": " << moveit::core::error_code_to_string(res.error_code)); return result; } catch (std::exception& ex) diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp index 1575066053..d2f9efb6fa 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp @@ -57,12 +57,12 @@ bool CHOMPPlanningContext::solve(planning_interface::MotionPlanResponse& res) planning_interface::MotionPlanDetailedResponse res_detailed; bool planning_success = solve(res_detailed); - res.error_code_ = res_detailed.error_code_; + res.error_code = res_detailed.error_code; if (planning_success) { - res.trajectory_ = res_detailed.trajectory_[0]; - res.planning_time_ = res_detailed.processing_time_[0]; + res.trajectory = res_detailed.trajectory[0]; + res.planning_time = res_detailed.processing_time[0]; } return planning_success; diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp index 485e67b12e..8e79efa70b 100644 --- a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp @@ -58,7 +58,7 @@ TEST_F(CHOMPMoveitTest, jointSpaceGoodGoal) move_group_.setJointValueTarget(std::vector({ 1.0, 1.0 })); moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_); - EXPECT_GT(my_plan_.trajectory_.joint_trajectory.points.size(), 0u); + EXPECT_GT(my_plan_.trajectory.joint_trajectory.points.size(), 0u); EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::SUCCESS); } diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp index 4ba5c3eeec..dbfdac0036 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp @@ -56,7 +56,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s if (!planning_scene) { RCLCPP_ERROR(LOGGER, "No planning scene initialized."); - res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; return false; } @@ -67,7 +67,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s if (!start_state.satisfiesBounds()) { RCLCPP_ERROR(LOGGER, "Start state violates joint limits"); - res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE; return false; } @@ -77,7 +77,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s if (req.goal_constraints.size() != 1) { RCLCPP_ERROR(LOGGER, "Expecting exactly one goal constraint, got: %zd", req.goal_constraints.size()); - res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; return false; } @@ -85,7 +85,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s !req.goal_constraints[0].orientation_constraints.empty()) { RCLCPP_ERROR(LOGGER, "Only joint-space goals are supported"); - res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; return false; } @@ -96,7 +96,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s if (!goal_state.satisfiesBounds()) { RCLCPP_ERROR(LOGGER, "Goal state violates joint limits"); - res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE; return false; } robotStateToArray(goal_state, req.group_name, trajectory.getTrajectoryPoint(goal_index)); @@ -137,12 +137,12 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s } else if (params.trajectory_initialization_method_.compare("fillTrajectory") == 0) { - if (res.trajectory_.empty()) + if (res.trajectory.empty()) { RCLCPP_ERROR(LOGGER, "No input trajectory specified"); return false; } - else if (!(trajectory.fillInFromTrajectory(*res.trajectory_[0]))) + else if (!(trajectory.fillInFromTrajectory(*res.trajectory[0]))) { RCLCPP_ERROR(LOGGER, "Input trajectory has less than 2 points, " "trajectory must contain at least start and goal state"); @@ -195,7 +195,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s if (!optimizer->isInitialized()) { RCLCPP_ERROR(LOGGER, "Could not initialize optimizer"); - res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; return false; } @@ -254,24 +254,24 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s result->addSuffixWayPoint(state, 0.0); } - res.trajectory_.resize(1); - res.trajectory_[0] = result; - res.description_.resize(1); - res.description_[0] = "plan"; + res.trajectory.resize(1); + res.trajectory[0] = result; + res.description.resize(1); + res.description[0] = "plan"; RCLCPP_DEBUG(LOGGER, "Bottom took %ld sec to create", (std::chrono::system_clock::now() - create_time).count()); RCLCPP_DEBUG(LOGGER, "Serviced planning request in %ld wall-seconds", (std::chrono::system_clock::now() - start_time).count()); - res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - res.processing_time_.resize(1); - res.processing_time_[0] = std::chrono::duration(std::chrono::system_clock::now() - start_time).count(); + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + res.processing_time.resize(1); + res.processing_time[0] = std::chrono::duration(std::chrono::system_clock::now() - start_time).count(); // report planning failure if path has collisions if (not optimizer->isCollisionFree()) { RCLCPP_ERROR(LOGGER, "Motion plan is invalid."); - res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } @@ -283,13 +283,13 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s if (!jc.configure(constraint) || !jc.decide(last_state).satisfied) { RCLCPP_ERROR(LOGGER, "Goal constraints are violated: %s", constraint.joint_name.c_str()); - res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED; return false; } } - res.processing_time_.resize(1); - res.processing_time_[0] = std::chrono::duration(std::chrono::system_clock::now() - start_time).count(); + res.processing_time.resize(1); + res.processing_time[0] = std::chrono::duration(std::chrono::system_clock::now() - start_time).count(); return true; } diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp index 0c197d83a4..a21402dafb 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp +++ b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp @@ -194,16 +194,16 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter chomp::ChompPlanner chomp_planner; planning_interface::MotionPlanDetailedResponse res_detailed; - res_detailed.trajectory_.push_back(res.trajectory_); + res_detailed.trajectory.push_back(res.trajectory); bool planning_success = chomp_planner.solve(planning_scene, req, params_, res_detailed); if (planning_success) { - res.trajectory_ = res_detailed.trajectory_[0]; - res.planning_time_ += res_detailed.processing_time_[0]; + res.trajectory = res_detailed.trajectory[0]; + res.planning_time += res_detailed.processing_time[0]; } - res.error_code_ = res_detailed.error_code_; + res.error_code = res_detailed.error_code; return planning_success; } diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 72c8437a79..01945a8135 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -762,8 +762,8 @@ void ompl_interface::ModelBasedPlanningContext::postSolve() bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& res) { - res.error_code_ = solve(request_.allowed_planning_time, request_.num_planning_attempts); - if (res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + res.error_code = solve(request_.allowed_planning_time, request_.num_planning_attempts); + if (res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { double ptime = getLastPlanTime(); if (simplify_solutions_) @@ -781,9 +781,9 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion RCLCPP_DEBUG(LOGGER, "%s: Returning successful solution with %lu states", getName().c_str(), getOMPLSimpleSetup()->getSolutionPath().getStateCount()); - res.trajectory_ = std::make_shared(getRobotModel(), getGroupName()); - getSolutionPath(*res.trajectory_); - res.planning_time_ = ptime; + res.trajectory = std::make_shared(getRobotModel(), getGroupName()); + getSolutionPath(*res.trajectory); + res.planning_time = ptime; return true; } else @@ -799,47 +799,47 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion solve(request_.allowed_planning_time, request_.num_planning_attempts); if (moveit_result.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { - res.trajectory_.reserve(3); + res.trajectory.reserve(3); // add info about planned solution double ptime = getLastPlanTime(); - res.processing_time_.push_back(ptime); - res.description_.emplace_back("plan"); - res.trajectory_.resize(res.trajectory_.size() + 1); - res.trajectory_.back() = std::make_shared(getRobotModel(), getGroupName()); - getSolutionPath(*res.trajectory_.back()); + res.processing_time.push_back(ptime); + res.description.emplace_back("plan"); + res.trajectory.resize(res.trajectory.size() + 1); + res.trajectory.back() = std::make_shared(getRobotModel(), getGroupName()); + getSolutionPath(*res.trajectory.back()); // simplify solution if time remains if (simplify_solutions_) { simplifySolution(request_.allowed_planning_time - ptime); - res.processing_time_.push_back(getLastSimplifyTime()); - res.description_.emplace_back("simplify"); - res.trajectory_.resize(res.trajectory_.size() + 1); - res.trajectory_.back() = std::make_shared(getRobotModel(), getGroupName()); - getSolutionPath(*res.trajectory_.back()); + res.processing_time.push_back(getLastSimplifyTime()); + res.description.emplace_back("simplify"); + res.trajectory.resize(res.trajectory.size() + 1); + res.trajectory.back() = std::make_shared(getRobotModel(), getGroupName()); + getSolutionPath(*res.trajectory.back()); } if (interpolate_) { ompl::time::point start_interpolate = ompl::time::now(); interpolateSolution(); - res.processing_time_.push_back(ompl::time::seconds(ompl::time::now() - start_interpolate)); - res.description_.emplace_back("interpolate"); - res.trajectory_.resize(res.trajectory_.size() + 1); - res.trajectory_.back() = std::make_shared(getRobotModel(), getGroupName()); - getSolutionPath(*res.trajectory_.back()); + res.processing_time.push_back(ompl::time::seconds(ompl::time::now() - start_interpolate)); + res.description.emplace_back("interpolate"); + res.trajectory.resize(res.trajectory.size() + 1); + res.trajectory.back() = std::make_shared(getRobotModel(), getGroupName()); + getSolutionPath(*res.trajectory.back()); } RCLCPP_DEBUG(LOGGER, "%s: Returning successful solution with %lu states", getName().c_str(), getOMPLSimpleSetup()->getSolutionPath().getStateCount()); - res.error_code_.val = moveit_result.val; + res.error_code.val = moveit_result.val; return true; } else { RCLCPP_INFO(LOGGER, "Unable to solve the planning problem"); - res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; return false; } } diff --git a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp index 470bf9876a..55dcbdf0a3 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp @@ -162,7 +162,7 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public // Check if all the states in the solution satisfy the path constraints. // A detailed response returns 3 solutions: the ompl solution, the simplified solution and the interpolated // solution. We test all of them here. - for (const robot_trajectory::RobotTrajectoryPtr& trajectory : response.trajectory_) + for (const robot_trajectory::RobotTrajectoryPtr& trajectory : response.trajectory) { for (std::size_t pt_index = 0; pt_index < trajectory->getWayPointCount(); ++pt_index) { @@ -197,7 +197,7 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public // Check if all the states in the solution satisfy the path constraints. // A detailed response returns 3 solutions: the ompl solution, the simplified solution and the interpolated // solution. We test all of them here. - for (const robot_trajectory::RobotTrajectoryPtr& trajectory : response2.trajectory_) + for (const robot_trajectory::RobotTrajectoryPtr& trajectory : response2.trajectory) { for (std::size_t pt_index = 0; pt_index < trajectory->getWayPointCount(); ++pt_index) { 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 e81abbeae7..bef598c805 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 @@ -130,13 +130,13 @@ bool pilz_industrial_motion_planner::PlanningContextBase::solve(plan } bool result = generator_.generate(getPlanningScene(), request_, res); return result; - // res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; + // res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; // return false; // TODO } 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; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; return false; } @@ -148,19 +148,19 @@ bool pilz_industrial_motion_planner::PlanningContextBase::solve( planning_interface::MotionPlanResponse undetailed_response; bool result = solve(undetailed_response); - res.description_.push_back("plan"); - res.trajectory_.push_back(undetailed_response.trajectory_); - res.processing_time_.push_back(undetailed_response.planning_time_); + res.description.push_back("plan"); + res.trajectory.push_back(undetailed_response.trajectory); + res.processing_time.push_back(undetailed_response.planning_time); - res.description_.push_back("simplify"); - res.trajectory_.push_back(undetailed_response.trajectory_); - res.processing_time_.push_back(0); + res.description.push_back("simplify"); + res.trajectory.push_back(undetailed_response.trajectory); + res.processing_time.push_back(0); - res.description_.push_back("interpolate"); - res.trajectory_.push_back(undetailed_response.trajectory_); - res.processing_time_.push_back(0); + res.description.push_back("interpolate"); + res.trajectory.push_back(undetailed_response.trajectory); + res.processing_time.push_back(0); - res.error_code_ = undetailed_response.error_code_; + res.error_code = undetailed_response.error_code; return result; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp index 6650dbdd3e..5f84806869 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp @@ -99,7 +99,7 @@ RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConst plan_comp_builder_.reset(); for (MotionResponseCont::size_type i = 0; i < resp_cont.size(); ++i) { - plan_comp_builder_.append(planning_scene, resp_cont.at(i).trajectory_, + plan_comp_builder_.append(planning_scene, resp_cont.at(i).trajectory, // The blend radii has to be "attached" to // the second part of a blend trajectory, // therefore: "i-1". @@ -144,7 +144,7 @@ void CommandListManager::checkForOverlappingRadii(const MotionResponseCont& resp for (MotionResponseCont::size_type i = 0; i < resp_cont.size() - 2; ++i) { - if (checkRadiiForOverlap(*(resp_cont.at(i).trajectory_), radii.at(i), *(resp_cont.at(i + 1).trajectory_), + if (checkRadiiForOverlap(*(resp_cont.at(i).trajectory), radii.at(i), *(resp_cont.at(i + 1).trajectory), radii.at(i + 1))) { std::ostringstream os; @@ -160,9 +160,9 @@ CommandListManager::getPreviousEndState(const MotionResponseCont& motion_plan_re for (MotionResponseCont::const_reverse_iterator it = motion_plan_responses.crbegin(); it != motion_plan_responses.crend(); ++it) { - if (it->trajectory_->getGroupName() == group_name) + if (it->trajectory->getGroupName() == group_name) { - return std::reference_wrapper(it->trajectory_->getLastWayPoint()); + return std::reference_wrapper(it->trajectory->getLastWayPoint()); } } return {}; @@ -240,11 +240,11 @@ CommandListManager::solveSequenceItems(const planning_scene::PlanningSceneConstP planning_interface::MotionPlanResponse res; planning_pipeline->generatePlan(planning_scene, req, res); - if (res.error_code_.val != res.error_code_.SUCCESS) + if (res.error_code.val != res.error_code.SUCCESS) { std::ostringstream os; os << "Could not solve request\n"; // TODO(henning): re-enable "---\n" << req << "\n---\n"; - throw PlanningPipelineException(os.str(), res.error_code_.val); + throw PlanningPipelineException(os.str(), res.error_code.val); } motion_plan_responses.emplace_back(res); RCLCPP_DEBUG_STREAM(LOGGER, "Solved [" << ++curr_req_index << '/' << num_req << ']'); diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp index 3c58f8978a..e1465fbfb5 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp @@ -158,12 +158,12 @@ void MoveGroupSequenceAction::executeSequenceCallbackPlanAndExecute( goal->planning_options.planning_scene_diff : clearSceneRobotState(goal->planning_options.planning_scene_diff); - opt.replan_ = goal->planning_options.replan; - opt.replan_attempts_ = goal->planning_options.replan_attempts; - opt.replan_delay_ = goal->planning_options.replan_delay; + opt.replan = goal->planning_options.replan; + opt.replan_attemps = goal->planning_options.replan_attempts; + opt.replan_delay = goal->planning_options.replan_delay; opt.before_execution_callback_ = [this] { startMoveExecutionCallback(); }; - opt.plan_callback_ = [this, &request = goal->request](plan_execution::ExecutableMotionPlan& plan) { + opt.plan_callback = [this, &request = goal->request](plan_execution::ExecutableMotionPlan& plan) { return planUsingSequenceManager(request, plan); }; @@ -171,7 +171,7 @@ void MoveGroupSequenceAction::executeSequenceCallbackPlanAndExecute( context_->plan_execution_->planAndExecute(plan, planning_scene_diff, opt); StartStatesMsg start_states_msg; - convertToMsg(plan.plan_components_, start_states_msg, action_res->response.planned_trajectories); + convertToMsg(plan.plan_components, start_states_msg, action_res->response.planned_trajectories); try { action_res->response.sequence_start = start_states_msg.at(0); @@ -180,7 +180,7 @@ void MoveGroupSequenceAction::executeSequenceCallbackPlanAndExecute( { RCLCPP_WARN(LOGGER, "Can not determine start state from empty sequence."); } - action_res->response.error_code = plan.error_code_; + action_res->response.error_code = plan.error_code; } void MoveGroupSequenceAction::convertToMsg(const ExecutableTrajs& trajs, StartStatesMsg& start_states_msg, @@ -190,8 +190,8 @@ void MoveGroupSequenceAction::convertToMsg(const ExecutableTrajs& trajs, StartSt planned_trajs_msgs.resize(trajs.size()); for (size_t i = 0; i < trajs.size(); ++i) { - moveit::core::robotStateToRobotStateMsg(trajs.at(i).trajectory_->getFirstWayPoint(), start_states_msg.at(i)); - trajs.at(i).trajectory_->getRobotTrajectoryMsg(planned_trajs_msgs.at(i)); + moveit::core::robotStateToRobotStateMsg(trajs.at(i).trajectory->getFirstWayPoint(), start_states_msg.at(i)); + trajs.at(i).trajectory->getRobotTrajectoryMsg(planned_trajs_msgs.at(i)); } } @@ -269,7 +269,7 @@ bool MoveGroupSequenceAction::planUsingSequenceManager(const moveit_msgs::msg::M { setMoveState(move_group::PLANNING); - planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_); + planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor); RobotTrajCont traj_vec; try { @@ -283,34 +283,34 @@ bool MoveGroupSequenceAction::planUsingSequenceManager(const moveit_msgs::msg::M return false; } - traj_vec = command_list_manager_->solve(plan.planning_scene_, planning_pipeline, req); + traj_vec = command_list_manager_->solve(plan.planning_scene, planning_pipeline, req); } catch (const MoveItErrorCodeException& ex) { RCLCPP_ERROR_STREAM(LOGGER, "Planning pipeline threw an exception (error code: " << ex.getErrorCode() << "): " << ex.what()); - plan.error_code_.val = ex.getErrorCode(); + plan.error_code.val = ex.getErrorCode(); return false; } // LCOV_EXCL_START // Keep MoveIt up even if lower parts throw catch (const std::exception& ex) { RCLCPP_ERROR_STREAM(LOGGER, "Planning pipeline threw an exception: " << ex.what()); - plan.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + plan.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; return false; } // LCOV_EXCL_STOP if (!traj_vec.empty()) { - plan.plan_components_.resize(traj_vec.size()); + plan.plan_components.resize(traj_vec.size()); for (size_t i = 0; i < traj_vec.size(); ++i) { - plan.plan_components_.at(i).trajectory_ = traj_vec.at(i); - plan.plan_components_.at(i).description_ = "plan"; + plan.plan_components.at(i).trajectory = traj_vec.at(i); + plan.plan_components.at(i).description = "plan"; } } - plan.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + plan.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; return true; } 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 18d3ba91c6..fe41f33a7e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -265,19 +265,19 @@ void TrajectoryGenerator::setSuccessResponse(const moveit::core::RobotState& sta auto rt = std::make_shared(robot_model_, group_name); rt->setRobotTrajectoryMsg(start_state, joint_trajectory); - res.trajectory_ = rt; - res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - res.planning_time_ = (clock_->now() - planning_start).seconds(); + res.trajectory = rt; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + res.planning_time = (clock_->now() - planning_start).seconds(); } void TrajectoryGenerator::setFailureResponse(const rclcpp::Time& planning_start, planning_interface::MotionPlanResponse& res) const { - if (res.trajectory_) + if (res.trajectory) { - res.trajectory_->clear(); + res.trajectory->clear(); } - res.planning_time_ = (clock_->now() - planning_start).seconds(); + res.planning_time = (clock_->now() - planning_start).seconds(); } std::unique_ptr @@ -314,7 +314,7 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& catch (const MoveItErrorCodeException& ex) { RCLCPP_ERROR_STREAM(LOGGER, ex.what()); - res.error_code_.val = ex.getErrorCode(); + res.error_code.val = ex.getErrorCode(); setFailureResponse(planning_begin, res); return false; } @@ -326,7 +326,7 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& catch (const MoveItErrorCodeException& ex) { RCLCPP_ERROR_STREAM(LOGGER, ex.what()); - res.error_code_.val = ex.getErrorCode(); + res.error_code.val = ex.getErrorCode(); setFailureResponse(planning_begin, res); return false; } @@ -339,7 +339,7 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& catch (const MoveItErrorCodeException& ex) { RCLCPP_ERROR_STREAM(LOGGER, ex.what()); - res.error_code_.val = ex.getErrorCode(); + res.error_code.val = ex.getErrorCode(); setFailureResponse(planning_begin, res); return false; } @@ -352,7 +352,7 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& catch (const MoveItErrorCodeException& ex) { RCLCPP_ERROR_STREAM(LOGGER, ex.what()); - res.error_code_.val = ex.getErrorCode(); + res.error_code.val = ex.getErrorCode(); setFailureResponse(planning_begin, res); return false; } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp index 75261f4f06..d07dd1ce6a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp @@ -204,7 +204,7 @@ TYPED_TEST(PlanningContextTest, NoRequest) bool result = this->planning_context_->solve(res); EXPECT_FALSE(result) << testutils::demangle(typeid(TypeParam).name()); - EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN, res.error_code_.val) + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN, res.error_code.val) << testutils::demangle(typeid(TypeParam).name()); } @@ -222,14 +222,14 @@ TYPED_TEST(PlanningContextTest, SolveValidRequest) bool result = this->planning_context_->solve(res); EXPECT_TRUE(result) << testutils::demangle(typeid(TypeParam).name()); - EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, res.error_code_.val) + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, res.error_code.val) << testutils::demangle(typeid(TypeParam).name()); planning_interface::MotionPlanDetailedResponse res_detailed; bool result_detailed = this->planning_context_->solve(res_detailed); EXPECT_TRUE(result_detailed) << testutils::demangle(typeid(TypeParam).name()); - EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, res.error_code_.val) + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, res.error_code.val) << testutils::demangle(typeid(TypeParam).name()); } @@ -245,7 +245,7 @@ TYPED_TEST(PlanningContextTest, SolveValidRequestDetailedResponse) bool result = this->planning_context_->solve(res); EXPECT_TRUE(result) << testutils::demangle(typeid(TypeParam).name()); - EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, res.error_code_.val) + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, res.error_code.val) << testutils::demangle(typeid(TypeParam).name()); } @@ -265,7 +265,7 @@ TYPED_TEST(PlanningContextTest, SolveOnTerminated) bool result = this->planning_context_->solve(res); EXPECT_FALSE(result) << testutils::demangle(typeid(TypeParam).name()); - EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED, res.error_code_.val) + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED, res.error_code.val) << testutils::demangle(typeid(TypeParam).name()); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp index 5317cb10c2..1c7386b250 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp @@ -138,7 +138,7 @@ class TrajectoryBlenderTransitionWindowTest : public testing::Test // for first) if (index > 0) { - moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory_->getLastWayPoint(), req.start_state); + moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory->getLastWayPoint(), req.start_state); } // generate trajectory planning_interface::MotionPlanResponse resp; @@ -192,8 +192,8 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testInvalidGroupName) blend_req.group_name = "invalid_group_name"; blend_req.link_name = target_link_; - blend_req.first_trajectory = res.at(0).trajectory_; - blend_req.second_trajectory = res.at(1).trajectory_; + blend_req.first_trajectory = res.at(0).trajectory; + blend_req.second_trajectory = res.at(1).trajectory; blend_req.blend_radius = seq.getBlendRadius(0); EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); @@ -221,8 +221,8 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testInvalidTargetLink) blend_req.group_name = planning_group_; blend_req.link_name = "invalid_target_link"; - blend_req.first_trajectory = res.at(0).trajectory_; - blend_req.second_trajectory = res.at(1).trajectory_; + blend_req.first_trajectory = res.at(0).trajectory; + blend_req.second_trajectory = res.at(1).trajectory; blend_req.blend_radius = seq.getBlendRadius(0); EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); @@ -251,8 +251,8 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testNegativeRadius) blend_req.group_name = planning_group_; blend_req.link_name = target_link_; - blend_req.first_trajectory = res.at(0).trajectory_; - blend_req.second_trajectory = res.at(1).trajectory_; + blend_req.first_trajectory = res.at(0).trajectory; + blend_req.second_trajectory = res.at(1).trajectory; blend_req.blend_radius = -0.1; EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); @@ -280,8 +280,8 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testZeroRadius) blend_req.group_name = planning_group_; blend_req.link_name = target_link_; - blend_req.first_trajectory = res.at(0).trajectory_; - blend_req.second_trajectory = res.at(1).trajectory_; + blend_req.first_trajectory = res.at(0).trajectory; + blend_req.second_trajectory = res.at(1).trajectory; blend_req.blend_radius = 0.; EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); @@ -314,7 +314,7 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes) // for first) if (index > 0) { - moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory_->getLastWayPoint(), req.start_state); + moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory->getLastWayPoint(), req.start_state); sampling_time_ *= 2; } // generate trajectory @@ -331,8 +331,8 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes) blend_req.group_name = planning_group_; blend_req.link_name = target_link_; - blend_req.first_trajectory = responses[0].trajectory_; - blend_req.second_trajectory = responses[1].trajectory_; + blend_req.first_trajectory = responses[0].trajectory; + blend_req.second_trajectory = responses[1].trajectory; blend_req.blend_radius = seq.getBlendRadius(0); EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); } @@ -358,16 +358,16 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testNonUniformSamplingTime) std::vector res{ generateLinTrajs(seq, 2) }; // Modify first time interval - EXPECT_GT(res[0].trajectory_->getWayPointCount(), 2u); - res[0].trajectory_->setWayPointDurationFromPrevious(1, 2 * sampling_time_); + EXPECT_GT(res[0].trajectory->getWayPointCount(), 2u); + res[0].trajectory->setWayPointDurationFromPrevious(1, 2 * sampling_time_); pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; blend_req.group_name = planning_group_; blend_req.link_name = target_link_; - blend_req.first_trajectory = res.at(0).trajectory_; - blend_req.second_trajectory = res.at(1).trajectory_; + blend_req.first_trajectory = res.at(0).trajectory; + blend_req.second_trajectory = res.at(1).trajectory; blend_req.blend_radius = seq.getBlendRadius(0); EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); } @@ -396,10 +396,10 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testNotIntersectingTrajectories) blend_req.group_name = planning_group_; blend_req.link_name = target_link_; - blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.first_trajectory = res.at(0).trajectory; // replace the second trajectory to make the two trajectories timely not // intersect - blend_req.second_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(0).trajectory; blend_req.blend_radius = seq.getBlendRadius(0); EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); } @@ -429,8 +429,8 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testNonStationaryPoint) blend_req.link_name = target_link_; blend_req.blend_radius = seq.getBlendRadius(0); - blend_req.first_trajectory = res.at(0).trajectory_; - blend_req.second_trajectory = res.at(1).trajectory_; + blend_req.first_trajectory = res.at(0).trajectory; + blend_req.second_trajectory = res.at(1).trajectory; // Modify last waypoint of first trajectory and first point of second // trajectory @@ -460,8 +460,8 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testTraj1InsideBlendRadius) std::vector res{ generateLinTrajs(seq, 2) }; double lin1_distance; - lin1_distance = (res[0].trajectory_->getFirstWayPoint().getFrameTransform(target_link_).translation() - - res[0].trajectory_->getLastWayPoint().getFrameTransform(target_link_).translation()) + lin1_distance = (res[0].trajectory->getFirstWayPoint().getFrameTransform(target_link_).translation() - + res[0].trajectory->getLastWayPoint().getFrameTransform(target_link_).translation()) .norm(); pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; @@ -471,8 +471,8 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testTraj1InsideBlendRadius) blend_req.link_name = target_link_; blend_req.blend_radius = 1.1 * lin1_distance; - blend_req.first_trajectory = res.at(0).trajectory_; - blend_req.second_trajectory = res.at(1).trajectory_; + blend_req.first_trajectory = res.at(0).trajectory; + blend_req.second_trajectory = res.at(1).trajectory; EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); } @@ -503,8 +503,8 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testTraj2InsideBlendRadius) blend_req.link_name = target_link_; blend_req.blend_radius = seq.getBlendRadius(0); - blend_req.first_trajectory = res.at(0).trajectory_; - blend_req.second_trajectory = res.at(1).trajectory_; + blend_req.first_trajectory = res.at(0).trajectory; + blend_req.second_trajectory = res.at(1).trajectory; EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); } @@ -540,8 +540,8 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testLinLinBlending) blend_req.link_name = target_link_; blend_req.blend_radius = seq.getBlendRadius(0); - blend_req.first_trajectory = res.at(0).trajectory_; - blend_req.second_trajectory = res.at(1).trajectory_; + blend_req.first_trajectory = res.at(0).trajectory; + blend_req.second_trajectory = res.at(1).trajectory; EXPECT_TRUE(blender_->blend(planning_scene_, blend_req, blend_res)); @@ -583,8 +583,8 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testOverlappingBlendTrajectories) blend_req.group_name = planning_group_; blend_req.link_name = target_link_; - blend_req.first_trajectory = res.at(0).trajectory_; - blend_req.second_trajectory = res.at(1).trajectory_; + blend_req.first_trajectory = res.at(0).trajectory; + blend_req.second_trajectory = res.at(1).trajectory; blend_req.blend_radius = seq.getBlendRadius(0); EXPECT_TRUE(blender_->blend(planning_scene_, blend_req, blend_res)); @@ -630,7 +630,7 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) for (size_t traj_index = 0; traj_index < 2; ++traj_index) { - auto lin_traj{ res.at(traj_index).trajectory_ }; + auto lin_traj{ res.at(traj_index).trajectory }; CartesianTrajectory cart_traj; trajectory_msgs::msg::JointTrajectory joint_traj; 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 8c923621e6..896c36ce2a 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 @@ -128,7 +128,7 @@ class TrajectoryGeneratorCIRCTest : public testing::Test { moveit_msgs::msg::MotionPlanResponse res_msg; res.getMessage(res_msg); - EXPECT_TRUE(testutils::isGoalReached(res.trajectory_->getFirstWayPointPtr()->getRobotModel(), + EXPECT_TRUE(testutils::isGoalReached(res.trajectory->getFirstWayPointPtr()->getRobotModel(), res_msg.trajectory.joint_trajectory, req, other_tolerance_)); EXPECT_TRUE( @@ -141,23 +141,23 @@ class TrajectoryGeneratorCIRCTest : public testing::Test Eigen::Vector3d circ_center; getCircCenter(req, res, circ_center); - for (std::size_t i = 0; i < res.trajectory_->getWayPointCount(); ++i) + for (std::size_t i = 0; i < res.trajectory->getWayPointCount(); ++i) { - Eigen::Affine3d waypoint_pose = res.trajectory_->getWayPointPtr(i)->getFrameTransform(target_link_); + Eigen::Affine3d waypoint_pose = res.trajectory->getWayPointPtr(i)->getFrameTransform(target_link_); EXPECT_NEAR( - (res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation() - circ_center).norm(), + (res.trajectory->getFirstWayPointPtr()->getFrameTransform(target_link_).translation() - circ_center).norm(), (circ_center - waypoint_pose.translation()).norm(), cartesian_position_tolerance_); } // check translational and rotational paths - ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory_, target_link_, acceleration_tolerance_)); - ASSERT_TRUE(testutils::checkCartesianRotationalPath(res.trajectory_, target_link_, angular_acc_tolerance_, + ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory, target_link_, acceleration_tolerance_)); + ASSERT_TRUE(testutils::checkCartesianRotationalPath(res.trajectory, target_link_, angular_acc_tolerance_, rot_axis_norm_tolerance_)); - for (size_t idx = 0; idx < res.trajectory_->getLastWayPointPtr()->getVariableCount(); ++idx) + for (size_t idx = 0; idx < res.trajectory->getLastWayPointPtr()->getVariableCount(); ++idx) { - EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableVelocity(idx), other_tolerance_); - EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableAcceleration(idx), other_tolerance_); + EXPECT_NEAR(0.0, res.trajectory->getLastWayPointPtr()->getVariableVelocity(idx), other_tolerance_); + EXPECT_NEAR(0.0, res.trajectory->getLastWayPointPtr()->getVariableAcceleration(idx), other_tolerance_); } } @@ -174,8 +174,8 @@ class TrajectoryGeneratorCIRCTest : public testing::Test Eigen::Vector3d interim; tf2::fromMsg(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position, interim); - Eigen::Vector3d start = res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation(); - Eigen::Vector3d goal = res.trajectory_->getLastWayPointPtr()->getFrameTransform(target_link_).translation(); + Eigen::Vector3d start = res.trajectory->getFirstWayPointPtr()->getFrameTransform(target_link_).translation(); + Eigen::Vector3d goal = res.trajectory->getLastWayPointPtr()->getFrameTransform(target_link_).translation(); const Eigen::Vector3d t = interim - start; const Eigen::Vector3d u = goal - start; @@ -284,7 +284,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, incompleteStartState) 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); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -298,7 +298,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, nonZeroStartVelocity) req.start_state.joint_state.velocity.push_back(1.0); 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); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); req.start_state.joint_state.velocity.clear(); } @@ -308,7 +308,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, ValidCommand) planning_interface::MotionPlanResponse res; EXPECT_TRUE(circ_->generate(planning_scene_, circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); } /** @@ -321,7 +321,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, velScaleToHigh) circ.setVelocityScale(1.0); planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED); } /** @@ -334,7 +334,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, accScaleToHigh) circ.setAccelerationScale(1.0); planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED); } /** @@ -356,7 +356,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, samePointsWithCenter) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -379,7 +379,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, samePointsWithInterim) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -395,7 +395,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, emptyAux) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -411,7 +411,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, invalidAuxName) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -428,7 +428,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, invalidAuxLinkName) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME); } /** @@ -442,7 +442,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, invalidCenter) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -462,7 +462,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, colinearCenter) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -483,7 +483,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, colinearInterim) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -501,7 +501,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, colinearCenterDueToInterim) planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(planning_scene_, circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); } /** @@ -534,7 +534,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, colinearCenterAndInterim) planning_interface::MotionPlanResponse res; EXPECT_TRUE(circ_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -568,7 +568,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, interimLarger180Degree) planning_interface::MotionPlanResponse res; EXPECT_TRUE(circ_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -582,7 +582,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, centerPointJointGoal) planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -609,7 +609,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, InvalidAdditionalPrimitivePose) planning_interface::MotionPlanResponse res; ASSERT_FALSE(circ_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -631,7 +631,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, InvalidExtraJointConstraint) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -645,7 +645,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoal) planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -662,7 +662,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdPositionConstraint planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -678,7 +678,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdOrientationConstra planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -697,7 +697,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdBothConstraints) planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -712,7 +712,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, InterimPointJointGoal) planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -733,7 +733,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, InterimPointJointGoalStartVelNearZero) planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -747,7 +747,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, InterimPointPoseGoal) planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp index 7097312f6a..0e0faa8b25 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp @@ -193,22 +193,22 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideScalingFactor) { this->req_.max_velocity_scaling_factor = 2.0; EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); this->req_.max_velocity_scaling_factor = 1.0; this->req_.max_acceleration_scaling_factor = 0; EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); this->req_.max_velocity_scaling_factor = 0.00001; this->req_.max_acceleration_scaling_factor = 1; EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); this->req_.max_velocity_scaling_factor = 1; this->req_.max_acceleration_scaling_factor = -1; EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -218,7 +218,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidGroupName) { this->req_.group_name = "foot"; EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code_.val); + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code.val); } /** @@ -228,7 +228,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTestNoGripper, GripperGroup) { this->req_.group_name = "gripper"; EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code_.val); + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code.val); } /** @@ -238,7 +238,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTestNoGripper, GripperGroup) // { // this->req_.group_name = "gripper"; // EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); -// EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS, this->res_.error_code_.val); +// EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS, this->res_.error_code.val); // } /** @@ -252,7 +252,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTestNoGripper, GripperGroup) // TYPED_TEST(TrajectoryGeneratorCommonTest, NoIKSolver) //{ // EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); -// EXPECT_EQ(this->res_.error_code_.val, +// EXPECT_EQ(this->res_.error_code.val, // moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME); //} @@ -263,7 +263,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyJointNamesInStartState) { this->req_.start_state.joint_state.name.clear(); EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -273,7 +273,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InconsistentStartState) { this->req_.start_state.joint_state.name.push_back("joint_7"); EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -283,7 +283,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, StartPostionOutOfLimit) { this->req_.start_state.joint_state.position[0] = 100; EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -297,7 +297,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, StartPositionVelocityNoneZero) { this->req_.start_state.joint_state.velocity[0] = 100; EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -307,7 +307,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyGoalConstraints) { this->req_.goal_constraints.clear(); EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -324,7 +324,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) this->req_.goal_constraints.push_back(goal_constraint); this->req_.goal_constraints.push_back(goal_constraint); EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); // one joint constraint and one orientation constraint goal_constraint.joint_constraints.push_back(joint_constraint); @@ -332,7 +332,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); // one joint constraint and one Cartesian constraint goal_constraint.position_constraints.push_back(position_constraint); @@ -340,7 +340,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); // two Cartesian constraints goal_constraint.joint_constraints.clear(); @@ -351,7 +351,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -363,7 +363,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideJointNameInGoal) joint_constraint.joint_name = "test_joint_2"; this->req_.goal_constraints.front().joint_constraints[0] = joint_constraint; EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -375,7 +375,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MissingJointConstraint) joint_constraint.joint_name = "test_joint_2"; this->req_.goal_constraints.front().joint_constraints.pop_back(); //<-- Missing joint constraint EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -385,7 +385,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideJointPositionInGoal) { this->req_.goal_constraints.front().joint_constraints[0].position = 100; EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -402,7 +402,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidLinkNameInCartesianGoal) this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); // different link names in position and orientation goals goal_constraint.position_constraints.front().link_name = "test_link_1"; @@ -410,14 +410,14 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidLinkNameInCartesianGoal) this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); // no solver for the link goal_constraint.orientation_constraints.front().link_name = "test_link_1"; this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); } /** @@ -437,7 +437,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyPrimitivePoses) this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } int main(int argc, char** argv) 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 12344c03f3..c4e3044b70 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 @@ -217,7 +217,7 @@ TEST_F(TrajectoryGeneratorLINTest, nonZeroStartVelocity) // try to generate the result planning_interface::MotionPlanResponse res; EXPECT_FALSE(lin_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -230,7 +230,7 @@ TEST_F(TrajectoryGeneratorLINTest, jointSpaceGoal) // generate the LIN trajectory planning_interface::MotionPlanResponse res; ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res)); - EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory EXPECT_TRUE(checkLinResponse(lin_joint_req, res)); @@ -251,7 +251,7 @@ TEST_F(TrajectoryGeneratorLINTest, jointSpaceGoalNearZeroStartVelocity) // generate the LIN trajectory planning_interface::MotionPlanResponse res; ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res)); - EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory EXPECT_TRUE(checkLinResponse(lin_joint_req, res)); @@ -268,7 +268,7 @@ TEST_F(TrajectoryGeneratorLINTest, cartesianSpaceGoal) // generate lin trajectory planning_interface::MotionPlanResponse res; ASSERT_TRUE(lin_->generate(planning_scene_, lin_cart_req, res)); - EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory EXPECT_TRUE(checkLinResponse(lin_cart_req, res)); @@ -292,15 +292,15 @@ TEST_F(TrajectoryGeneratorLINTest, cartesianTrapezoidProfile) /// +++++++++++++++++++++++ planning_interface::MotionPlanResponse res; ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res, 0.01)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); - ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory_, target_link_hcd_)); + ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory, target_link_hcd_)); // check last point for vel=acc=0 - for (size_t idx = 0; idx < res.trajectory_->getLastWayPointPtr()->getVariableCount(); ++idx) + for (size_t idx = 0; idx < res.trajectory->getLastWayPointPtr()->getVariableCount(); ++idx) { - EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableVelocity(idx), other_tolerance_); - EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableAcceleration(idx), other_tolerance_); + EXPECT_NEAR(0.0, res.trajectory->getLastWayPointPtr()->getVariableVelocity(idx), other_tolerance_); + EXPECT_NEAR(0.0, res.trajectory->getLastWayPointPtr()->getVariableAcceleration(idx), other_tolerance_); } } @@ -377,7 +377,7 @@ TEST_F(TrajectoryGeneratorLINTest, LinStartEqualsGoal) // generate the LIN trajectory planning_interface::MotionPlanResponse res; ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res)); - EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory EXPECT_TRUE(checkLinResponse(lin_joint_req, res)); @@ -404,7 +404,7 @@ TEST_F(TrajectoryGeneratorLINTest, IncorrectJointNumber) // generate the LIN trajectory planning_interface::MotionPlanResponse res; ASSERT_FALSE(lin_->generate(planning_scene_, lin_joint_req, res)); - EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -422,7 +422,7 @@ TEST_F(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState) // 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); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -440,7 +440,7 @@ TEST_F(TrajectoryGeneratorLINTest, cartGoalFrameIdBothConstraints) // generate lin trajectory planning_interface::MotionPlanResponse res; ASSERT_TRUE(lin_->generate(planning_scene_, lin_cart_req, res)); - EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory EXPECT_TRUE(checkLinResponse(lin_cart_req, res)); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp index bb5c0ddd34..df24d36a44 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp @@ -179,14 +179,14 @@ TEST_F(TrajectoryGeneratorPTPTest, noLimits) } /** - * @brief Send an empty request, define res.trajectory_ + * @brief Send an empty request, define res.trajectory * * - Test Sequence: * 1. Create request, define a trajectory in the result * 2. assign at least one joint limit will all required limits * * - Expected Results: - * 1. the res.trajectory_ should be cleared (contain no waypoints) + * 1. the res.trajectory should be cleared (contain no waypoints) */ TEST_F(TrajectoryGeneratorPTPTest, emptyRequest) { @@ -197,13 +197,13 @@ TEST_F(TrajectoryGeneratorPTPTest, emptyRequest) new robot_trajectory::RobotTrajectory(this->robot_model_, planning_group_)); moveit::core::RobotState state(this->robot_model_); trajectory->addPrefixWayPoint(state, 0); - res.trajectory_ = trajectory; + res.trajectory = trajectory; - EXPECT_FALSE(res.trajectory_->empty()); + EXPECT_FALSE(res.trajectory->empty()); EXPECT_FALSE(ptp_->generate(planning_scene_, req, res)); - EXPECT_TRUE(res.trajectory_->empty()); + EXPECT_TRUE(res.trajectory->empty()); } /** @@ -369,7 +369,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testCartesianGoal) //*** test robot model without gripper *** //**************************************** ASSERT_TRUE(ptp_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); moveit_msgs::msg::MotionPlanResponse res_msg; res.getMessage(res_msg); @@ -417,12 +417,12 @@ TEST_F(TrajectoryGeneratorPTPTest, testCartesianGoalMissingLinkNameConstraints) planning_interface::MotionPlanRequest req_no_position_constaint_link_name = req; req_no_position_constaint_link_name.goal_constraints.front().position_constraints.front().link_name = ""; ASSERT_FALSE(ptp_->generate(planning_scene_, req_no_position_constaint_link_name, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); planning_interface::MotionPlanRequest req_no_orientation_constaint_link_name = req; req_no_orientation_constaint_link_name.goal_constraints.front().orientation_constraints.front().link_name = ""; ASSERT_FALSE(ptp_->generate(planning_scene_, req_no_orientation_constaint_link_name, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -449,8 +449,8 @@ TEST_F(TrajectoryGeneratorPTPTest, testInvalidCartesianGoal) req.goal_constraints.push_back(pose_goal); ASSERT_FALSE(ptp_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); - EXPECT_EQ(res.trajectory_, nullptr); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); + EXPECT_EQ(res.trajectory, nullptr); } /** @@ -475,7 +475,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAlreadyReached) // TODO lin and circ has different settings ASSERT_TRUE(ptp_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); moveit_msgs::msg::MotionPlanResponse res_msg; res.getMessage(res_msg); @@ -547,19 +547,19 @@ TEST_F(TrajectoryGeneratorPTPTest, testScalingFactor) req.max_acceleration_scaling_factor = 1.0 / 3.0; ASSERT_TRUE(ptp_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); moveit_msgs::msg::MotionPlanResponse res_msg; res.getMessage(res_msg); EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer())); // trajectory duration - EXPECT_NEAR(4.5, res.trajectory_->getWayPointDurationFromStart(res.trajectory_->getWayPointCount()), + EXPECT_NEAR(4.5, res.trajectory->getWayPointDurationFromStart(res.trajectory->getWayPointCount()), joint_acceleration_tolerance_); // way point at 1s int index; - index = testutils::getWayPointIndex(res.trajectory_, 1.0); + index = testutils::getWayPointIndex(res.trajectory, 1.0); // joint_1 EXPECT_NEAR(0.125, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); @@ -580,7 +580,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testScalingFactor) EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[4], joint_acceleration_tolerance_); // way point at 2s - index = testutils::getWayPointIndex(res.trajectory_, 2.0); + index = testutils::getWayPointIndex(res.trajectory, 2.0); // joint_1 EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); @@ -597,7 +597,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testScalingFactor) EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_); // way point at 3s - index = testutils::getWayPointIndex(res.trajectory_, 3.0); + index = testutils::getWayPointIndex(res.trajectory, 3.0); // joint_1 EXPECT_NEAR(1, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); @@ -617,7 +617,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testScalingFactor) EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], joint_acceleration_tolerance_); // way point at 4s - index = testutils::getWayPointIndex(res.trajectory_, 4.0); + index = testutils::getWayPointIndex(res.trajectory, 4.0); // joint_1 EXPECT_NEAR(2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); @@ -634,7 +634,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testScalingFactor) EXPECT_NEAR(-1.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); // way point at 4.5s - index = testutils::getWayPointIndex(res.trajectory_, 4.5); + index = testutils::getWayPointIndex(res.trajectory, 4.5); // joint_1 EXPECT_NEAR(1.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); @@ -674,19 +674,19 @@ TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity) req.goal_constraints.push_back(gc); ASSERT_TRUE(ptp_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); moveit_msgs::msg::MotionPlanResponse res_msg; res.getMessage(res_msg); EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer())); // trajectory duration - EXPECT_NEAR(4.5, res.trajectory_->getWayPointDurationFromStart(res.trajectory_->getWayPointCount()), + EXPECT_NEAR(4.5, res.trajectory->getWayPointDurationFromStart(res.trajectory->getWayPointCount()), joint_acceleration_tolerance_); // way point at 1s int index; - index = testutils::getWayPointIndex(res.trajectory_, 1.0); + index = testutils::getWayPointIndex(res.trajectory, 1.0); // joint_1 EXPECT_NEAR(0.125, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); @@ -707,7 +707,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity) EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[4], joint_acceleration_tolerance_); // way point at 2s - index = testutils::getWayPointIndex(res.trajectory_, 2.0); + index = testutils::getWayPointIndex(res.trajectory, 2.0); // joint_1 EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); @@ -724,7 +724,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity) EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_); // way point at 3s - index = testutils::getWayPointIndex(res.trajectory_, 3.0); + index = testutils::getWayPointIndex(res.trajectory, 3.0); // joint_1 EXPECT_NEAR(1, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); @@ -744,7 +744,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity) EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], joint_acceleration_tolerance_); // way point at 4s - index = testutils::getWayPointIndex(res.trajectory_, 4.0); + index = testutils::getWayPointIndex(res.trajectory, 4.0); // joint_1 EXPECT_NEAR(2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); @@ -761,7 +761,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity) EXPECT_NEAR(-1.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); // way point at 4.5s - index = testutils::getWayPointIndex(res.trajectory_, 4.5); + index = testutils::getWayPointIndex(res.trajectory, 4.5); // joint_1 EXPECT_NEAR(1.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); @@ -816,14 +816,14 @@ TEST_F(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel) req.goal_constraints.push_back(gc); ASSERT_TRUE(ptp_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); moveit_msgs::msg::MotionPlanResponse res_msg; res.getMessage(res_msg); EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer())); // trajectory duration - EXPECT_NEAR(4.5, res.trajectory_->getWayPointDurationFromStart(res.trajectory_->getWayPointCount()), + EXPECT_NEAR(4.5, res.trajectory->getWayPointDurationFromStart(res.trajectory->getWayPointCount()), joint_position_tolerance_); // way point at 0s @@ -845,7 +845,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel) // way point at 1s int index; - index = testutils::getWayPointIndex(res.trajectory_, 1.0); + index = testutils::getWayPointIndex(res.trajectory, 1.0); // joint_1 EXPECT_NEAR(0.125, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); @@ -871,7 +871,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel) EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); // way point at 2s - index = testutils::getWayPointIndex(res.trajectory_, 2.0); + index = testutils::getWayPointIndex(res.trajectory, 2.0); // joint_1 EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); @@ -890,7 +890,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel) EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); // way point at 3s - index = testutils::getWayPointIndex(res.trajectory_, 3.0); + index = testutils::getWayPointIndex(res.trajectory, 3.0); // joint_1 EXPECT_NEAR(1, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); @@ -914,7 +914,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel) EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); // way point at 4s - index = testutils::getWayPointIndex(res.trajectory_, 4.0); + index = testutils::getWayPointIndex(res.trajectory, 4.0); // joint_1 EXPECT_NEAR(2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); @@ -940,7 +940,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel) EXPECT_NEAR(-1.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); // way point at 4.5s - index = testutils::getWayPointIndex(res.trajectory_, 4.5); + index = testutils::getWayPointIndex(res.trajectory, 4.5); // joint_1 EXPECT_NEAR(1.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); diff --git a/moveit_planners/trajopt/src/trajopt_planning_context.cpp b/moveit_planners/trajopt/src/trajopt_planning_context.cpp index f77ee77e55..4a7405642e 100644 --- a/moveit_planners/trajopt/src/trajopt_planning_context.cpp +++ b/moveit_planners/trajopt/src/trajopt_planning_context.cpp @@ -27,22 +27,22 @@ bool TrajOptPlanningContext::solve(planning_interface::MotionPlanDetailedRespons if (trajopt_solved) { - res.trajectory_.resize(1); - res.trajectory_[0] = std::make_shared(robot_model_, getGroupName()); + res.trajectory.resize(1); + res.trajectory[0] = std::make_shared(robot_model_, getGroupName()); moveit::core::RobotState start_state(robot_model_); moveit::core::robotStateMsgToRobotState(res_msg.trajectory_start, start_state); - res.trajectory_[0]->setRobotTrajectoryMsg(start_state, res_msg.trajectory[0]); + res.trajectory[0]->setRobotTrajectoryMsg(start_state, res_msg.trajectory[0]); - res.description_.push_back("plan"); + res.description.push_back("plan"); // TODO: Add the initial trajectory to res (MotionPlanDetailedResponse) - res.processing_time_ = res_msg.processing_time; - res.error_code_ = res_msg.error_code; + res.processing_time = res_msg.processing_time; + res.error_code = res_msg.error_code; return true; } else { - res.error_code_ = res_msg.error_code; + res.error_code = res_msg.error_code; return false; } } @@ -52,12 +52,12 @@ bool TrajOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) planning_interface::MotionPlanDetailedResponse res_detailed; bool planning_success = solve(res_detailed); - res.error_code_ = res_detailed.error_code_; + res.error_code = res_detailed.error_code; if (planning_success) { - res.trajectory_ = res_detailed.trajectory_[0]; - res.planning_time_ = res_detailed.processing_time_[0]; + res.trajectory = res_detailed.trajectory[0]; + res.planning_time = res_detailed.processing_time[0]; } return planning_success; diff --git a/moveit_planners/trajopt/test/trajectory_test.cpp b/moveit_planners/trajopt/test/trajectory_test.cpp index 213cfbdb4a..53a44b052b 100644 --- a/moveit_planners/trajopt/test/trajectory_test.cpp +++ b/moveit_planners/trajopt/test/trajectory_test.cpp @@ -176,10 +176,10 @@ TEST_F(TrajectoryTest, goalTolerance) // Create planning context // ======================================================================================== planning_interface::PlanningContextPtr context = - planner_instance->getPlanningContext(planning_scene, req, res.error_code_); + planner_instance->getPlanningContext(planning_scene, req, res.error_code); context->solve(res); - EXPECT_EQ(res.error_code_.val, res.error_code_.SUCCESS); + EXPECT_EQ(res.error_code.val, res.error_code.SUCCESS); moveit_msgs::MotionPlanResponse response; res.getMessage(response); diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 72879ac7f2..a546f3a82f 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -841,12 +841,12 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::msg::MotionPlanRequest request // The planning pipeline does not support MotionPlanDetailedResponse planning_interface::MotionPlanResponse response; solved[j] = planning_pipeline->generatePlan(planning_scene_, request, response); - responses[j].error_code_ = response.error_code_; - if (response.trajectory_) + responses[j].error_code = response.error_code; + if (response.trajectory) { - responses[j].description_.push_back("plan"); - responses[j].trajectory_.push_back(response.trajectory_); - responses[j].processing_time_.push_back(response.planning_time_); + responses[j].description.push_back("plan"); + responses[j].trajectory.push_back(response.trajectory); + responses[j].processing_time.push_back(response.planning_time); } } std::chrono::duration dt = std::chrono::system_clock::now() - start; @@ -892,12 +892,12 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, bool correct = true; // entire trajectory collision free and in bounds double process_time = total_time; - for (std::size_t j = 0; j < mp_res.trajectory_.size(); ++j) + for (std::size_t j = 0; j < mp_res.trajectory.size(); ++j) { correct = true; traj_len = 0.0; clearance = 0.0; - const robot_trajectory::RobotTrajectory& p = *mp_res.trajectory_[j]; + const robot_trajectory::RobotTrajectory& p = *mp_res.trajectory[j]; // compute path length traj_len = robot_trajectory::path_length(p); @@ -924,21 +924,21 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, return s.has_value() ? s.value() : 0.0; }(); - metrics["path_" + mp_res.description_[j] + "_correct BOOLEAN"] = correct ? "true" : "false"; - metrics["path_" + mp_res.description_[j] + "_length REAL"] = moveit::core::toString(traj_len); - metrics["path_" + mp_res.description_[j] + "_clearance REAL"] = moveit::core::toString(clearance); - metrics["path_" + mp_res.description_[j] + "_smoothness REAL"] = moveit::core::toString(smoothness); - metrics["path_" + mp_res.description_[j] + "_time REAL"] = moveit::core::toString(mp_res.processing_time_[j]); + metrics["path_" + mp_res.description[j] + "_correct BOOLEAN"] = correct ? "true" : "false"; + metrics["path_" + mp_res.description[j] + "_length REAL"] = moveit::core::toString(traj_len); + metrics["path_" + mp_res.description[j] + "_clearance REAL"] = moveit::core::toString(clearance); + metrics["path_" + mp_res.description[j] + "_smoothness REAL"] = moveit::core::toString(smoothness); + metrics["path_" + mp_res.description[j] + "_time REAL"] = moveit::core::toString(mp_res.processing_time[j]); - if (j == mp_res.trajectory_.size() - 1) + if (j == mp_res.trajectory.size() - 1) { metrics["final_path_correct BOOLEAN"] = correct ? "true" : "false"; metrics["final_path_length REAL"] = moveit::core::toString(traj_len); metrics["final_path_clearance REAL"] = moveit::core::toString(clearance); metrics["final_path_smoothness REAL"] = moveit::core::toString(smoothness); - metrics["final_path_time REAL"] = moveit::core::toString(mp_res.processing_time_[j]); + metrics["final_path_time REAL"] = moveit::core::toString(mp_res.processing_time[j]); } - process_time -= mp_res.processing_time_[j]; + process_time -= mp_res.processing_time[j]; } if (process_time <= 0.0) process_time = 0.0; @@ -970,8 +970,8 @@ void BenchmarkExecutor::computeAveragePathSimilarities( continue; // Get final trajectories - const robot_trajectory::RobotTrajectory& traj_first = *responses[first_traj_i].trajectory_.back(); - const robot_trajectory::RobotTrajectory& traj_second = *responses[second_traj_i].trajectory_.back(); + const robot_trajectory::RobotTrajectory& traj_first = *responses[first_traj_i].trajectory.back(); + const robot_trajectory::RobotTrajectory& traj_second = *responses[second_traj_i].trajectory.back(); // Compute trajectory distance double trajectory_distance; diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp index fa9995cb7e..47d0d97e4a 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp @@ -132,17 +132,17 @@ moveit_msgs::msg::MotionPlanResponse MoveItPlanningPipeline::plan( // Plan motion auto plan_solution = planning_components->plan(plan_params); - if (!bool(plan_solution.error_code_)) + if (!bool(plan_solution.error_code)) { - response.error_code = plan_solution.error_code_; + response.error_code = plan_solution.error_code; return response; } // Transform solution into MotionPlanResponse and publish it - response.trajectory_start = plan_solution.start_state_; + response.trajectory_start = plan_solution.start_state; response.group_name = motion_plan_req.group_name; - plan_solution.trajectory_->getRobotTrajectoryMsg(response.trajectory); - response.error_code = plan_solution.error_code_; + plan_solution.trajectory->getRobotTrajectoryMsg(response.trajectory); + response.error_code = plan_solution.error_code; return response; } diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index 1fca6e8b57..03ecce5ced 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp @@ -150,12 +150,12 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const std::shared_pt goal->get_goal()->planning_options.planning_scene_diff : clearSceneRobotState(goal->get_goal()->planning_options.planning_scene_diff); - opt.replan_ = goal->get_goal()->planning_options.replan; - opt.replan_attempts_ = goal->get_goal()->planning_options.replan_attempts; - opt.replan_delay_ = goal->get_goal()->planning_options.replan_delay; + opt.replan = goal->get_goal()->planning_options.replan; + opt.replan_attemps = goal->get_goal()->planning_options.replan_attempts; + opt.replan_delay = goal->get_goal()->planning_options.replan_delay; opt.before_execution_callback_ = [this] { startMoveExecutionCallback(); }; - opt.plan_callback_ = [this, &motion_plan_request](plan_execution::ExecutableMotionPlan& plan) { + opt.plan_callback = [this, &motion_plan_request](plan_execution::ExecutableMotionPlan& plan) { return planUsingPlanningPipeline(motion_plan_request, plan); }; @@ -169,10 +169,10 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const std::shared_pt context_->plan_execution_->planAndExecute(plan, planning_scene_diff, opt); - convertToMsg(plan.plan_components_, action_res->trajectory_start, action_res->planned_trajectory); - if (plan.executed_trajectory_) - plan.executed_trajectory_->getRobotTrajectoryMsg(action_res->executed_trajectory); - action_res->error_code = plan.error_code_; + convertToMsg(plan.plan_components, action_res->trajectory_start, action_res->planned_trajectory); + if (plan.executed_trajectory) + plan.executed_trajectory->getRobotTrajectoryMsg(action_res->executed_trajectory); + action_res->error_code = plan.error_code; } void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const std::shared_ptr& goal, @@ -211,12 +211,12 @@ void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const std::shared_ptrtrajectory_start, action_res->planned_trajectory); - action_res->error_code = res.error_code_; - action_res->planning_time = res.planning_time_; + convertToMsg(res.trajectory, action_res->trajectory_start, action_res->planned_trajectory); + action_res->error_code = res.error_code; + action_res->planning_time = res.planning_time; } bool MoveGroupMoveAction::planUsingPlanningPipeline(const planning_interface::MotionPlanRequest& req, @@ -231,27 +231,27 @@ bool MoveGroupMoveAction::planUsingPlanningPipeline(const planning_interface::Mo const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req.pipeline_id); if (!planning_pipeline) { - res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; return solved; } - planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_); + planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor); try { - solved = planning_pipeline->generatePlan(plan.planning_scene_, req, res); + solved = planning_pipeline->generatePlan(plan.planning_scene, req, res); } catch (std::exception& ex) { RCLCPP_ERROR(LOGGER, "Planning pipeline threw an exception: %s", ex.what()); - res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; } - if (res.trajectory_) + if (res.trajectory) { - plan.plan_components_.resize(1); - plan.plan_components_[0].trajectory_ = res.trajectory_; - plan.plan_components_[0].description_ = "plan"; + plan.plan_components.resize(1); + plan.plan_components[0].trajectory = res.trajectory; + plan.plan_components[0].description = "plan"; } - plan.error_code_ = res.error_code_; + plan.error_code = res.error_code; return solved; } diff --git a/moveit_ros/move_group/src/move_group_capability.cpp b/moveit_ros/move_group/src/move_group_capability.cpp index 63bf346a9e..886076297e 100644 --- a/moveit_ros/move_group/src/move_group_capability.cpp +++ b/moveit_ros/move_group/src/move_group_capability.cpp @@ -60,14 +60,14 @@ void move_group::MoveGroupCapability::convertToMsg(const std::vectorempty()) + if (first && !trajectory[i].trajectory->empty()) { - moveit::core::robotStateToRobotStateMsg(trajectory[i].trajectory_->getFirstWayPoint(), first_state_msg); + moveit::core::robotStateToRobotStateMsg(trajectory[i].trajectory->getFirstWayPoint(), first_state_msg); first = false; } - trajectory[i].trajectory_->getRobotTrajectoryMsg(trajectory_msg[i]); + trajectory[i].trajectory->getRobotTrajectoryMsg(trajectory_msg[i]); } } } @@ -91,7 +91,7 @@ void move_group::MoveGroupCapability::convertToMsg(const std::vector 1) RCLCPP_ERROR_STREAM(LOGGER, "Internal logic error: trajectory component ignored. !!! THIS IS A SERIOUS ERROR !!!"); if (!trajectory.empty()) - convertToMsg(trajectory[0].trajectory_, first_state_msg, trajectory_msg); + convertToMsg(trajectory[0].trajectory, first_state_msg, trajectory_msg); } planning_interface::MotionPlanRequest diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp index 203e58c31b..a15f1f4661 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp @@ -78,7 +78,7 @@ class PlanSolutions /** \brief Function that returns the shortest solution out of a vector of solutions based on robot_trajectory::path_length(...) * \param [in] solutions Vector of solutions to chose the shortest one from - * \return Shortest solution, trajectory_ of the returned MotionPlanResponse is a nullptr if no solution is found! + * \return Shortest solution, trajectory of the returned MotionPlanResponse is a nullptr if no solution is found! */ static inline planning_interface::MotionPlanResponse getShortestSolution(const std::vector& solutions) @@ -90,8 +90,8 @@ getShortestSolution(const std::vector& s // If both solutions were successful, check which path is shorter if (solution_a && solution_b) { - return robot_trajectory::path_length(*solution_a.trajectory_) < - robot_trajectory::path_length(*solution_b.trajectory_); + return robot_trajectory::path_length(*solution_a.trajectory) < + robot_trajectory::path_length(*solution_b.trajectory); } // If only solution a is successful, return a else if (solution_a) diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 37cb99dea4..7a7d0f8045 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -120,7 +120,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest if (!joint_model_group_) { RCLCPP_ERROR(LOGGER, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); - plan_solution.error_code_ = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME; + plan_solution.error_code = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME; return plan_solution; } @@ -156,7 +156,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest if (current_goal_constraints_.empty()) { RCLCPP_ERROR(LOGGER, "No goal constraints set for planning request"); - plan_solution.error_code_ = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; + plan_solution.error_code = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; return plan_solution; } req.goal_constraints = current_goal_constraints_; @@ -172,22 +172,22 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest if (it == pipelines.end()) { RCLCPP_ERROR(LOGGER, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str()); - plan_solution.error_code_ = moveit::core::MoveItErrorCode::FAILURE; + plan_solution.error_code = moveit::core::MoveItErrorCode::FAILURE; return plan_solution; } const planning_pipeline::PlanningPipelinePtr pipeline = it->second; pipeline->generatePlan(planning_scene, req, res); - plan_solution.error_code_ = res.error_code_; - if (res.error_code_.val != res.error_code_.SUCCESS) + plan_solution.error_code = res.error_code; + if (res.error_code.val != res.error_code.SUCCESS) { RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); return plan_solution; } - plan_solution.trajectory_ = res.trajectory_; - plan_solution.planning_time_ = res.planning_time_; - plan_solution.start_state_ = req.start_state; - plan_solution.error_code_ = res.error_code_.val; + plan_solution.trajectory = res.trajectory; + plan_solution.planning_time = res.planning_time; + plan_solution.start_state = req.start_state; + plan_solution.error_code = res.error_code.val; // TODO(henningkayser): Visualize trajectory // std::vector eef_links; @@ -240,9 +240,9 @@ PlanningComponent::plan(const MultiPipelinePlanRequestParameters& parameters, RCLCPP_ERROR_STREAM(LOGGER, "Planning pipeline '" << plan_request_parameter.planning_pipeline.c_str() << "' threw exception '" << e.what() << '\''); plan_solution = planning_interface::MotionPlanResponse(); - plan_solution.error_code_ = moveit::core::MoveItErrorCode::FAILURE; + plan_solution.error_code = moveit::core::MoveItErrorCode::FAILURE; } - plan_solution.planner_id_ = plan_request_parameter.planner_id; + plan_solution.planner_id = plan_request_parameter.planner_id; planning_solutions.pushBack(plan_solution); if (stopping_criterion_callback != nullptr) diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h index 349b22e458..f1642c6981 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h @@ -55,22 +55,22 @@ class PlanExecution public: struct Options { - Options() : replan_(false), replan_attempts_(0), replan_delay_(0.0) + Options() : replan(false), replan_attemps(0), replan_delay(0.0) { } /// Flag indicating whether replanning is allowed - bool replan_; + bool replan; /// If replanning is allowed, this variable specifies how many replanning attempts there can be, at most, before /// failure - unsigned int replan_attempts_; + unsigned int replan_attemps; /// The amount of time to wait in between replanning attempts (in seconds) - double replan_delay_; + double replan_delay; /// Callback for computing motion plans. This callback must always be specified. - ExecutableMotionPlanComputationFn plan_callback_; + ExecutableMotionPlanComputationFn plan_callback; /// Callback for repairing motion plans. This is optional. A new plan is re-computed if repairing routines are not /// specified. diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h index e2a1c49484..2248857584 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h @@ -48,40 +48,40 @@ struct ExecutableMotionPlan; /** \brief Representation of a trajectory that can be executed */ struct ExecutableTrajectory { - ExecutableTrajectory() : trajectory_monitoring_(true) + ExecutableTrajectory() : trajectory_monitoring(true) { } ExecutableTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, const std::string& description, std::vector controller_names = {}) - : trajectory_(trajectory) - , description_(description) - , trajectory_monitoring_(true) - , controller_names_(std::move(controller_names)) + : trajectory(trajectory) + , description(description) + , trajectory_monitoring(true) + , controller_name(std::move(controller_names)) { } - robot_trajectory::RobotTrajectoryPtr trajectory_; - std::string description_; - bool trajectory_monitoring_; - collision_detection::AllowedCollisionMatrixConstPtr allowed_collision_matrix_; - std::function effect_on_success_; - std::vector controller_names_; + robot_trajectory::RobotTrajectoryPtr trajectory; + std::string description; + bool trajectory_monitoring; + collision_detection::AllowedCollisionMatrixConstPtr allowed_collision_matrix; + std::function effect_on_success; + std::vector controller_name; }; /// A generic representation on what a computed motion plan looks like struct ExecutableMotionPlan { - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - planning_scene::PlanningSceneConstPtr planning_scene_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor; + planning_scene::PlanningSceneConstPtr planning_scene; - std::vector plan_components_; + std::vector plan_components; /// The trace of the trajectory recorded during execution - robot_trajectory::RobotTrajectoryPtr executed_trajectory_; + robot_trajectory::RobotTrajectoryPtr executed_trajectory; /// An error code reflecting what went wrong (if anything) - moveit_msgs::msg::MoveItErrorCodes error_code_; + moveit_msgs::msg::MoveItErrorCodes error_code; }; /// The signature of a function that can compute a motion plan diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 34b533bd88..378bd1ec82 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -114,8 +114,8 @@ void plan_execution::PlanExecution::stop() void plan_execution::PlanExecution::planAndExecute(ExecutableMotionPlan& plan, const Options& opt) { - plan.planning_scene_monitor_ = planning_scene_monitor_; - plan.planning_scene_ = planning_scene_monitor_->getPlanningScene(); + plan.planning_scene_monitor = planning_scene_monitor_; + plan.planning_scene = planning_scene_monitor_->getPlanningScene(); planAndExecuteHelper(plan, opt); } @@ -129,13 +129,13 @@ void plan_execution::PlanExecution::planAndExecute(ExecutableMotionPlan& plan, } else { - plan.planning_scene_monitor_ = planning_scene_monitor_; + plan.planning_scene_monitor = planning_scene_monitor_; { planning_scene_monitor::LockedPlanningSceneRO lscene(planning_scene_monitor_); // lock the scene so that it does // not modify the world // representation while diff() is // called - plan.planning_scene_ = lscene->diff(scene_diff); + plan.planning_scene = lscene->diff(scene_diff); } planAndExecuteHelper(plan, opt); } @@ -150,7 +150,7 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p // run the actual motion plan & execution unsigned int max_replan_attempts = - opt.replan_ ? (opt.replan_attempts_ > 0 ? opt.replan_attempts_ : default_max_replan_attempts_) : 1; + opt.replan ? (opt.replan_attemps > 0 ? opt.replan_attemps : default_max_replan_attempts_) : 1; unsigned int replan_attempts = 0; bool previously_solved = false; @@ -171,7 +171,7 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p // try to repair the plan we previously had; bool solved = (!previously_solved || !opt.repair_plan_callback_) ? - opt.plan_callback_(plan) : + opt.plan_callback(plan) : opt.repair_plan_callback_(plan, trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex()); preempt_requested = preempt_.checkAndClear(); @@ -180,14 +180,14 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p // if planning fails in a manner that is not recoverable, we exit the loop, // otherwise, we attempt to continue, if replanning attempts are left - if (plan.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED || - plan.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN || - plan.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA) + if (plan.error_code.val == moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED || + plan.error_code.val == moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN || + plan.error_code.val == moveit_msgs::msg::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA) { - if (plan.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA && - opt.replan_delay_ > 0.0) + if (plan.error_code.val == moveit_msgs::msg::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA && + opt.replan_delay > 0.0) { - auto replan_delay_seconds = std::chrono::duration(opt.replan_delay_); + auto replan_delay_seconds = std::chrono::duration(opt.replan_delay); rclcpp::sleep_for(std::chrono::duration_cast(replan_delay_seconds)); } continue; @@ -203,7 +203,7 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p break; } - if (plan.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + if (plan.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { if (opt.before_execution_callback_) opt.before_execution_callback_(); @@ -213,24 +213,24 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p break; // execute the trajectory, and monitor its execution - plan.error_code_ = executeAndMonitor(plan, false); + plan.error_code = executeAndMonitor(plan, false); } - if (plan.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::PREEMPTED) + if (plan.error_code.val == moveit_msgs::msg::MoveItErrorCodes::PREEMPTED) preempt_requested = true; // if execution succeeded or failed in a manner that we do not consider recoverable, we exit the loop (with failure) - if (plan.error_code_.val != moveit_msgs::msg::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE) + if (plan.error_code.val != moveit_msgs::msg::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE) { break; } else { // otherwise, we wait (if needed) - if (opt.replan_delay_ > 0.0) + if (opt.replan_delay > 0.0) { - RCLCPP_INFO(LOGGER, "Waiting for a %lf seconds before attempting a new plan ...", opt.replan_delay_); - auto replan_delay_seconds = std::chrono::duration(opt.replan_delay_); + RCLCPP_INFO(LOGGER, "Waiting for a %lf seconds before attempting a new plan ...", opt.replan_delay); + auto replan_delay_seconds = std::chrono::duration(opt.replan_delay); rclcpp::sleep_for(std::chrono::duration_cast(replan_delay_seconds)); RCLCPP_INFO(node_->get_logger(), "Done waiting"); } @@ -245,20 +245,20 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p if (preempt_requested) { RCLCPP_DEBUG(LOGGER, "PlanExecution was preempted"); - plan.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::PREEMPTED; + plan.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PREEMPTED; } if (opt.done_callback_) opt.done_callback_(); - if (plan.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + if (plan.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { RCLCPP_DEBUG(LOGGER, "PlanExecution finished successfully."); } else { - RCLCPP_DEBUG(LOGGER, "PlanExecution terminating with error code %d - '%s'", plan.error_code_.val, - moveit::core::error_code_to_string(plan.error_code_).c_str()); + RCLCPP_DEBUG(LOGGER, "PlanExecution terminating with error code %d - '%s'", plan.error_code.val, + moveit::core::error_code_to_string(plan.error_code).c_str()); } } @@ -266,16 +266,16 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP const std::pair& path_segment) { if (path_segment.first >= 0 && - plan.plan_components_[path_segment.first].trajectory_monitoring_) // If path_segment.second <= 0, the function - // will fallback to check the entire trajectory + plan.plan_components[path_segment.first].trajectory_monitoring) // If path_segment.second <= 0, the function + // will fallback to check the entire trajectory { - planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_); // lock the scene so that it - // does not modify the world - // representation while - // isStateValid() is called - const robot_trajectory::RobotTrajectory& t = *plan.plan_components_[path_segment.first].trajectory_; + planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor); // lock the scene so that it + // does not modify the world + // representation while + // isStateValid() is called + const robot_trajectory::RobotTrajectory& t = *plan.plan_components[path_segment.first].trajectory; const collision_detection::AllowedCollisionMatrix* acm = - plan.plan_components_[path_segment.first].allowed_collision_matrix_.get(); + plan.plan_components[path_segment.first].allowed_collision_matrix.get(); std::size_t wpc = t.getWayPointCount(); collision_detection::CollisionRequest req; req.group_name = t.getGroupName(); @@ -284,30 +284,30 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP collision_detection::CollisionResult res; if (acm) { - plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm); + plan.planning_scene->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm); } else { - plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i)); + plan.planning_scene->checkCollisionUnpadded(req, res, t.getWayPoint(i)); } - if (res.collision || !plan.planning_scene_->isStateFeasible(t.getWayPoint(i), false)) + if (res.collision || !plan.planning_scene->isStateFeasible(t.getWayPoint(i), false)) { // Dave's debacle RCLCPP_INFO(LOGGER, "Trajectory component '%s' is invalid", - plan.plan_components_[path_segment.first].description_.c_str()); + plan.plan_components[path_segment.first].description.c_str()); // call the same functions again, in verbose mode, to show what issues have been detected - plan.planning_scene_->isStateFeasible(t.getWayPoint(i), true); + plan.planning_scene->isStateFeasible(t.getWayPoint(i), true); req.verbose = true; res.clear(); if (acm) { - plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm); + plan.planning_scene->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm); } else { - plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i)); + plan.planning_scene->checkCollisionUnpadded(req, res, t.getWayPoint(i)); } return false; } @@ -322,10 +322,10 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni if (reset_preempted) preempt_.checkAndClear(); - if (!plan.planning_scene_monitor_) - plan.planning_scene_monitor_ = planning_scene_monitor_; - if (!plan.planning_scene_) - plan.planning_scene_ = planning_scene_monitor_->getPlanningScene(); + if (!plan.planning_scene_monitor) + plan.planning_scene_monitor = planning_scene_monitor_; + if (!plan.planning_scene) + plan.planning_scene = planning_scene_monitor_->getPlanningScene(); moveit_msgs::msg::MoveItErrorCodes result; @@ -339,7 +339,7 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni return result; } - if (plan.plan_components_.empty()) + if (plan.plan_components.empty()) { result.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; return result; @@ -349,7 +349,7 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni // push the trajectories we have slated for execution to the trajectory execution manager int prev = -1; - for (size_t component_idx = 0; component_idx < plan.plan_components_.size(); ++component_idx) + for (size_t component_idx = 0; component_idx < plan.plan_components.size(); ++component_idx) { // \todo should this be in trajectory_execution ? Maybe. Then that will have to use kinematic_trajectory too; // splitting trajectories for controllers becomes interesting: tied to groups instead of joints. this could cause @@ -361,13 +361,13 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni { // Search backward for a previous component having the same group. // If the group is the same, unwind this component based on the last waypoint of the previous one. - if (plan.plan_components_.at(prev_component).trajectory_ && - plan.plan_components_.at(prev_component).trajectory_->getGroup() == - plan.plan_components_.at(prev_component).trajectory_->getGroup() && - !plan.plan_components_.at(prev_component).trajectory_->empty()) + if (plan.plan_components.at(prev_component).trajectory && + plan.plan_components.at(prev_component).trajectory->getGroup() == + plan.plan_components.at(prev_component).trajectory->getGroup() && + !plan.plan_components.at(prev_component).trajectory->empty()) { - plan.plan_components_.at(component_idx) - .trajectory_->unwind(plan.plan_components_.at(prev_component).trajectory_->getLastWayPoint()); + plan.plan_components.at(component_idx) + .trajectory->unwind(plan.plan_components.at(prev_component).trajectory->getLastWayPoint()); unwound = true; // Break so each component is only unwound once break; @@ -379,25 +379,24 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni // unwind the path to execute based on the current state of the system if (prev < 0) { - plan.plan_components_[component_idx].trajectory_->unwind( - plan.planning_scene_monitor_ && plan.planning_scene_monitor_->getStateMonitor() ? - *plan.planning_scene_monitor_->getStateMonitor()->getCurrentState() : - plan.planning_scene_->getCurrentState()); + plan.plan_components[component_idx].trajectory->unwind( + plan.planning_scene_monitor && plan.planning_scene_monitor->getStateMonitor() ? + *plan.planning_scene_monitor->getStateMonitor()->getCurrentState() : + plan.planning_scene->getCurrentState()); } else { - plan.plan_components_[component_idx].trajectory_->unwind( - plan.plan_components_[prev].trajectory_->getLastWayPoint()); + plan.plan_components[component_idx].trajectory->unwind(plan.plan_components[prev].trajectory->getLastWayPoint()); } } - if (plan.plan_components_[component_idx].trajectory_ && !plan.plan_components_[component_idx].trajectory_->empty()) + if (plan.plan_components[component_idx].trajectory && !plan.plan_components[component_idx].trajectory->empty()) prev = component_idx; // convert to message, pass along moveit_msgs::msg::RobotTrajectory msg; - plan.plan_components_[component_idx].trajectory_->getRobotTrajectoryMsg(msg); - if (!trajectory_execution_manager_->push(msg, plan.plan_components_[component_idx].controller_names_)) + plan.plan_components[component_idx].trajectory->getRobotTrajectoryMsg(msg); + if (!trajectory_execution_manager_->push(msg, plan.plan_components[component_idx].controller_name)) { trajectory_execution_manager_->clear(); RCLCPP_ERROR(LOGGER, "Apparently trajectory initialization failed"); @@ -440,7 +439,7 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni if (!isRemainingPathValid(plan, current_index)) { RCLCPP_INFO(LOGGER, "Trajectory component '%s' is invalid after scene update", - plan.plan_components_[current_index.first].description_.c_str()); + plan.plan_components[current_index.first].description.c_str()); path_became_invalid_ = true; break; } @@ -474,9 +473,9 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni if (trajectory_monitor_) { trajectory_monitor_->stopTrajectoryMonitor(); - plan.executed_trajectory_ = + plan.executed_trajectory = std::make_shared(planning_scene_monitor_->getRobotModel(), ""); - trajectory_monitor_->swapTrajectory(*plan.executed_trajectory_); + trajectory_monitor_->swapTrajectory(*plan.executed_trajectory); } // decide return value @@ -528,17 +527,17 @@ void plan_execution::PlanExecution::doneWithTrajectoryExecution( void plan_execution::PlanExecution::successfulTrajectorySegmentExecution(const ExecutableMotionPlan& plan, std::size_t index) { - if (plan.plan_components_.empty()) + if (plan.plan_components.empty()) { RCLCPP_WARN(LOGGER, "Length of provided motion plan is zero."); return; } // if any side-effects are associated to the trajectory part that just completed, execute them - RCLCPP_DEBUG(LOGGER, "Completed '%s'", plan.plan_components_[index].description_.c_str()); - if (plan.plan_components_[index].effect_on_success_) + RCLCPP_DEBUG(LOGGER, "Completed '%s'", plan.plan_components[index].description.c_str()); + if (plan.plan_components[index].effect_on_success) { - if (!plan.plan_components_[index].effect_on_success_(&plan)) + if (!plan.plan_components[index].effect_on_success(&plan)) { // execution of side-effect failed RCLCPP_ERROR(LOGGER, "Execution of path-completion side-effect failed. Preempting."); @@ -549,14 +548,14 @@ void plan_execution::PlanExecution::successfulTrajectorySegmentExecution(const E // if there is a next trajectory, check it for validity, before we start execution ++index; - if (index < plan.plan_components_.size() && plan.plan_components_[index].trajectory_ && - !plan.plan_components_[index].trajectory_->empty()) + if (index < plan.plan_components.size() && plan.plan_components[index].trajectory && + !plan.plan_components[index].trajectory->empty()) { std::pair next_index(static_cast(index), 0); if (!isRemainingPathValid(plan, next_index)) { RCLCPP_INFO(LOGGER, "Upcoming trajectory component '%s' is invalid", - plan.plan_components_[next_index.first].description_.c_str()); + plan.plan_components[next_index.first].description.c_str()); path_became_invalid_ = true; } } diff --git a/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp b/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp index fd37f873d0..b77d9ab388 100644 --- a/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp +++ b/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp @@ -66,7 +66,7 @@ int main(int argc, char** argv) executor.add_node(node); robot_model_loader::RobotModelLoader::Options opt; - opt.robot_description_ = "robot_description"; + opt.robot_description = "robot_description"; auto rml = std::make_shared(node, opt); planning_scene_monitor::PlanningSceneMonitor psm(node, rml); psm.startWorldGeometryMonitor(); diff --git a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp index d28acca712..b5b57c338c 100644 --- a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp +++ b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp @@ -86,8 +86,8 @@ int main(int argc, char** argv) } robot_model_loader::RobotModelLoader::Options opt; - opt.robot_description_ = "robot_description"; - opt.load_kinematics_solvers_ = false; + opt.robot_description = "robot_description"; + opt.load_kinematics_solvers = false; auto rml = std::make_shared(node, opt); planning_scene::PlanningScene ps(rml->getModel()); diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 45948e2d62..964cc6e7ef 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -287,7 +287,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla else { planning_interface::PlanningContextPtr context = - planner_instance_->getPlanningContext(planning_scene, req, res.error_code_); + planner_instance_->getPlanningContext(planning_scene, req, res.error_code); solved = context ? context->solve(res) : false; } } @@ -301,9 +301,9 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla } bool valid = true; - if (solved && res.trajectory_) + if (solved && res.trajectory) { - std::size_t state_count = res.trajectory_->getWayPointCount(); + std::size_t state_count = res.trajectory->getWayPointCount(); RCLCPP_DEBUG(LOGGER, "Motion planner reported a solution path with %ld states", state_count); if (check_solution_paths_) { @@ -313,7 +313,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla arr.markers.push_back(m); std::vector index; - if (!planning_scene->isPathValid(*res.trajectory_, req.path_constraints, req.group_name, false, &index)) + if (!planning_scene->isPathValid(*res.trajectory, req.path_constraints, req.group_name, false, &index)) { // check to see if there is any problem with the states that are found to be invalid // they are considered ok if they were added by a planning request adapter @@ -341,7 +341,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla else { valid = false; - res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; // display error messages std::stringstream ss; @@ -357,7 +357,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla for (std::size_t it : index) { // check validity with verbose on - const moveit::core::RobotState& robot_state = res.trajectory_->getWayPoint(it); + const moveit::core::RobotState& robot_state = res.trajectory->getWayPoint(it); planning_scene->isStateValid(robot_state, req.path_constraints, req.group_name, true); // compute the contacts if any @@ -398,8 +398,8 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla moveit_msgs::msg::DisplayTrajectory disp; disp.model_id = robot_model_->getName(); disp.trajectory.resize(1); - res.trajectory_->getRobotTrajectoryMsg(disp.trajectory[0]); - moveit::core::robotStateToRobotStateMsg(res.trajectory_->getFirstWayPoint(), disp.trajectory_start); + res.trajectory->getRobotTrajectoryMsg(disp.trajectory[0]); + moveit::core::robotStateToRobotStateMsg(res.trajectory->getFirstWayPoint(), disp.trajectory_start); display_path_publisher_->publish(disp); } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp index b8f8f5fc69..1941fd5e29 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp @@ -65,9 +65,9 @@ class AddRuckigTrajectorySmoothing : public planning_request_adapter::PlanningRe std::vector& /*added_path_index*/) const override { bool result = planner(planning_scene, req, res); - if (result && res.trajectory_) + if (result && res.trajectory) { - if (!smoother_.applySmoothing(*res.trajectory_, req.max_velocity_scaling_factor, + if (!smoother_.applySmoothing(*res.trajectory, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) { result = false; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp index 8e07d2625a..6ee7c21974 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp @@ -69,12 +69,11 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning std::vector& /*added_path_index*/) const override { bool result = planner(planning_scene, req, res); - if (result && res.trajectory_) + if (result && res.trajectory) { RCLCPP_DEBUG(LOGGER, " Running '%s'", getDescription().c_str()); TimeOptimalTrajectoryGeneration totg(path_tolerance_, resample_dt_, min_angle_change_); - if (!totg.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor, - req.max_acceleration_scaling_factor)) + if (!totg.computeTimeStamps(*res.trajectory, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) { RCLCPP_WARN(LOGGER, " Time parametrization for the solution path failed."); result = false; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp index 8c2199c815..017d24fed5 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp @@ -177,13 +177,13 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap solved = planner(planning_scene, req, res); // re-add the prefix state, if it was constructed - if (prefix_state && res.trajectory_ && !res.trajectory_->empty()) + if (prefix_state && res.trajectory && !res.trajectory->empty()) { // heuristically decide a duration offset for the trajectory (induced by the additional point added as a prefix to // the computed trajectory) - res.trajectory_->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, - res.trajectory_->getAverageSegmentDuration())); - res.trajectory_->addPrefixWayPoint(prefix_state, 0.0); + res.trajectory->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, + res.trajectory->getAverageSegmentDuration())); + res.trajectory->addPrefixWayPoint(prefix_state, 0.0); // we add a prefix point, so we need to bump any previously added index positions for (std::size_t& added_index : added_path_index) added_index++; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp index f410017c83..d98187ce7d 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp @@ -137,13 +137,13 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA planning_interface::MotionPlanRequest req2 = req; moveit::core::robotStateToRobotStateMsg(start_state, req2.start_state); bool solved = planner(planning_scene, req2, res); - if (solved && !res.trajectory_->empty()) + if (solved && !res.trajectory->empty()) { // heuristically decide a duration offset for the trajectory (induced by the additional point added as a // prefix to the computed trajectory) - res.trajectory_->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, - res.trajectory_->getAverageSegmentDuration())); - res.trajectory_->addPrefixWayPoint(prefix_state, 0.0); + res.trajectory->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, + res.trajectory->getAverageSegmentDuration())); + res.trajectory->addPrefixWayPoint(prefix_state, 0.0); // we add a prefix point, so we need to bump any previously added index positions for (std::size_t& added_index : added_path_index) added_index++; @@ -157,7 +157,7 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA "Unable to find a valid state nearby the start state (using jiggle fraction of %lf and %u sampling " "attempts). Passing the original planning request to the planner.", jiggle_fraction_, sampling_attempts_); - res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_IN_COLLISION; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_IN_COLLISION; return false; // skip remaining adapters and/or planner } } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp index 17b09f1de5..11d638bf14 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp @@ -99,23 +99,23 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe RCLCPP_INFO(LOGGER, "Planned to path constraints. Resuming original planning request."); // extract the last state of the computed motion plan and set it as the new start state - moveit::core::robotStateToRobotStateMsg(res2.trajectory_->getLastWayPoint(), req3.start_state); + moveit::core::robotStateToRobotStateMsg(res2.trajectory->getLastWayPoint(), req3.start_state); bool solved2 = planner(planning_scene, req3, res); - res.planning_time_ += res2.planning_time_; + res.planning_time += res2.planning_time; if (solved2) { // since we add a prefix, we need to correct any existing index positions for (std::size_t& added_index : added_path_index) - added_index += res2.trajectory_->getWayPointCount(); + added_index += res2.trajectory->getWayPointCount(); // we mark the fact we insert a prefix path (we specify the index position we just added) - for (std::size_t i = 0; i < res2.trajectory_->getWayPointCount(); ++i) + for (std::size_t i = 0; i < res2.trajectory->getWayPointCount(); ++i) added_path_index.push_back(i); // we need to append the solution paths. - res2.trajectory_->append(*res.trajectory_, 0.0); - res2.trajectory_->swap(*res.trajectory_); + res2.trajectory->append(*res.trajectory, 0.0); + res2.trajectory->swap(*res.trajectory); return true; } else @@ -126,8 +126,8 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe else { RCLCPP_WARN(LOGGER, "Unable to plan to path constraints. Running usual motion plan."); - res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_VIOLATES_PATH_CONSTRAINTS; - res.planning_time_ = res2.planning_time_; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_VIOLATES_PATH_CONSTRAINTS; + res.planning_time = res2.planning_time; return false; // skip remaining adapters and/or planner } } diff --git a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h index d800f11cbe..ba2e447354 100644 --- a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h +++ b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h @@ -53,27 +53,27 @@ class RobotModelLoader struct Options { Options(const std::string& robot_description = "robot_description") - : robot_description_(robot_description), load_kinematics_solvers_(true) + : robot_description(robot_description), load_kinematics_solvers(true) { } Options(const std::string& urdf_string, const std::string& srdf_string) - : urdf_string_(urdf_string), srdf_string_(srdf_string), load_kinematics_solvers_(true) + : urdf_string_(urdf_string), srdf_string(srdf_string), load_kinematics_solvers(true) { } /** @brief The string name corresponding to the ROS param where the URDF is loaded; Using the same parameter name plus the "_planning" suffix, additional configuration can be specified (e.g., additional joint limits). Loading from the param server is attempted only if loading from string fails. */ - std::string robot_description_; + std::string robot_description; /** @brief The string content of the URDF and SRDF documents. Loading from string is attempted only if loading from * XML fails */ - std::string urdf_string_, srdf_string_; + std::string urdf_string_, srdf_string; /** @brief Flag indicating whether the kinematics solvers should be loaded as well, using specified ROS parameters */ - bool load_kinematics_solvers_; + bool load_kinematics_solvers; }; /** @brief Default constructor */ diff --git a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp index 1a912c8841..2c2e04bc04 100644 --- a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp +++ b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp @@ -52,7 +52,7 @@ RobotModelLoader::RobotModelLoader(const rclcpp::Node::SharedPtr& node, const st : node_(node) { Options opt(robot_description); - opt.load_kinematics_solvers_ = load_kinematics_solvers; + opt.load_kinematics_solvers = load_kinematics_solvers; configure(opt); } @@ -105,13 +105,13 @@ void RobotModelLoader::configure(const Options& opt) { rclcpp::Clock clock; rclcpp::Time start = clock.now(); - if (!opt.urdf_string_.empty() && !opt.srdf_string_.empty()) + if (!opt.urdf_string_.empty() && !opt.srdf_string.empty()) { - rdf_loader_ = std::make_shared(opt.urdf_string_, opt.srdf_string_); + rdf_loader_ = std::make_shared(opt.urdf_string_, opt.srdf_string); } else { - rdf_loader_ = std::make_shared(node_, opt.robot_description_); + rdf_loader_ = std::make_shared(node_, opt.robot_description); } if (rdf_loader_->getURDF()) { @@ -246,7 +246,7 @@ void RobotModelLoader::configure(const Options& opt) } } - if (model_ && opt.load_kinematics_solvers_) + if (model_ && opt.load_kinematics_solvers) loadKinematicsSolvers(); RCLCPP_DEBUG(node_->get_logger(), "Loaded kinematic model in %f seconds", (clock.now() - start).seconds()); diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index a41dad2dfe..c5615af464 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -87,23 +87,23 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface struct Options { Options(std::string group_name, std::string desc = ROBOT_DESCRIPTION, std::string move_group_namespace = "") - : group_name_(std::move(group_name)) - , robot_description_(std::move(desc)) - , move_group_namespace_(std::move(move_group_namespace)) + : group_name(std::move(group_name)) + , robot_description(std::move(desc)) + , move_group_namespace(std::move(move_group_namespace)) { } /// The group to construct the class instance for - std::string group_name_; + std::string group_name; /// The robot description parameter name (if different from default) - std::string robot_description_; + std::string robot_description; /// Optionally, an instance of the RobotModel to use can be also specified - moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model; /// The namespace for the move group node - std::string move_group_namespace_; + std::string move_group_namespace; }; MOVEIT_STRUCT_FORWARD(Plan); @@ -112,13 +112,13 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface struct Plan { /// The full starting state used for planning - moveit_msgs::msg::RobotState start_state_; + moveit_msgs::msg::RobotState start_state; /// The trajectory of the robot (may not contain joints that are the same as for the start_state_) - moveit_msgs::msg::RobotTrajectory trajectory_; + moveit_msgs::msg::RobotTrajectory trajectory; /// The amount of time it took to generate the plan - double planning_time_; + double planning_time; }; /** diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index d1fd1d6e61..3c7ca5dfe7 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -119,7 +119,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl callback_executor_.add_callback_group(callback_group_, node->get_node_base_interface()); callback_thread_ = std::thread([this]() { callback_executor_.spin(); }); - robot_model_ = opt.robot_model_ ? opt.robot_model_ : getSharedRobotModel(node_, opt.robot_description_); + robot_model = opt.robot_model ? opt.robot_model : getSharedRobotModel(node_, opt.robot_description); if (!getRobotModel()) { std::string error = "Unable to construct robot model. Please make sure all needed information is on the " @@ -128,14 +128,14 @@ class MoveGroupInterface::MoveGroupInterfaceImpl throw std::runtime_error(error); } - if (!getRobotModel()->hasJointModelGroup(opt.group_name_)) + if (!getRobotModel()->hasJointModelGroup(opt.group_name)) { - std::string error = "Group '" + opt.group_name_ + "' was not found."; + std::string error = "Group '" + opt.group_name + "' was not found."; RCLCPP_FATAL_STREAM(LOGGER, error); throw std::runtime_error(error); } - joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_); + joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name); joint_state_target_ = std::make_shared(getRobotModel()); joint_state_target_->setToDefaultValues(); @@ -161,37 +161,37 @@ class MoveGroupInterface::MoveGroupInterfaceImpl pose_reference_frame_ = getRobotModel()->getModelFrame(); // Append the slash between two topic components trajectory_event_publisher_ = node_->create_publisher( - rclcpp::names::append(opt_.move_group_namespace_, + rclcpp::names::append(opt_.move_group_namespace, trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC), 1); attached_object_publisher_ = node_->create_publisher( - rclcpp::names::append(opt_.move_group_namespace_, + rclcpp::names::append(opt_.move_group_namespace, planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC), 1); - current_state_monitor_ = getSharedStateMonitor(node_, robot_model_, tf_buffer_); + current_state_monitor_ = getSharedStateMonitor(node_, robot_model, tf_buffer_); move_action_client_ = rclcpp_action::create_client( - node_, rclcpp::names::append(opt_.move_group_namespace_, move_group::MOVE_ACTION), callback_group_); + node_, rclcpp::names::append(opt_.move_group_namespace, move_group::MOVE_ACTION), callback_group_); move_action_client_->wait_for_action_server(wait_for_servers.to_chrono>()); execute_action_client_ = rclcpp_action::create_client( - node_, rclcpp::names::append(opt_.move_group_namespace_, move_group::EXECUTE_ACTION_NAME), callback_group_); + node_, rclcpp::names::append(opt_.move_group_namespace, move_group::EXECUTE_ACTION_NAME), callback_group_); execute_action_client_->wait_for_action_server(wait_for_servers.to_chrono>()); query_service_ = node_->create_client( - rclcpp::names::append(opt_.move_group_namespace_, move_group::QUERY_PLANNERS_SERVICE_NAME), qos_default(), + rclcpp::names::append(opt_.move_group_namespace, move_group::QUERY_PLANNERS_SERVICE_NAME), qos_default(), callback_group_); get_params_service_ = node_->create_client( - rclcpp::names::append(opt_.move_group_namespace_, move_group::GET_PLANNER_PARAMS_SERVICE_NAME), qos_default(), + rclcpp::names::append(opt_.move_group_namespace, move_group::GET_PLANNER_PARAMS_SERVICE_NAME), qos_default(), callback_group_); set_params_service_ = node_->create_client( - rclcpp::names::append(opt_.move_group_namespace_, move_group::SET_PLANNER_PARAMS_SERVICE_NAME), qos_default(), + rclcpp::names::append(opt_.move_group_namespace, move_group::SET_PLANNER_PARAMS_SERVICE_NAME), qos_default(), callback_group_); cartesian_path_service_ = node_->create_client( - rclcpp::names::append(opt_.move_group_namespace_, move_group::CARTESIAN_PATH_SERVICE_NAME), qos_default(), + rclcpp::names::append(opt_.move_group_namespace, move_group::CARTESIAN_PATH_SERVICE_NAME), qos_default(), callback_group_); - RCLCPP_INFO_STREAM(LOGGER, "Ready to take commands for planning group " << opt.group_name_ << '.'); + RCLCPP_INFO_STREAM(LOGGER, "Ready to take commands for planning group " << opt.group_name << '.'); } ~MoveGroupInterfaceImpl() @@ -217,7 +217,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl const moveit::core::RobotModelConstPtr& getRobotModel() const { - return robot_model_; + return robot_model; } const moveit::core::JointModelGroup* getJointModelGroup() const @@ -501,7 +501,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!end_effector_link_.empty()) { const std::vector& possible_eefs = - getRobotModel()->getJointModelGroup(opt_.group_name_)->getAttachedEndEffectorNames(); + getRobotModel()->getJointModelGroup(opt_.group_name)->getAttachedEndEffectorNames(); for (const std::string& possible_eef : possible_eefs) { if (getRobotModel()->getEndEffector(possible_eef)->hasLinkModel(end_effector_link_)) @@ -609,7 +609,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!current_state_monitor_->isActive()) current_state_monitor_->startStateMonitor(); - current_state_monitor_->waitForCompleteState(opt_.group_name_, wait); + current_state_monitor_->waitForCompleteState(opt_.group_name, wait); return true; } @@ -735,7 +735,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // auto request = std::make_shared(); // moveit_msgs::srv::GraspPlanning::Response::SharedPtr response; // - // request->group_name = opt_.group_name_; + // request->group_name = opt_.group_name; // request->target = object; // request->support_surfaces.push_back(support_surface_); // @@ -826,10 +826,10 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return res->error_code; } - plan.trajectory_ = res->planned_trajectory; - plan.start_state_ = res->trajectory_start; - plan.planning_time_ = res->planning_time; - RCLCPP_INFO(LOGGER, "time taken to generate plan: %g seconds", plan.planning_time_); + plan.trajectory = res->planned_trajectory; + plan.start_state = res->trajectory_start; + plan.planning_time = res->planning_time; + RCLCPP_INFO(LOGGER, "time taken to generate plan: %g seconds", plan.planning_time); return res->error_code; } @@ -988,7 +988,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl req->start_state.is_diff = true; } - req->group_name = opt_.group_name_; + req->group_name = opt_.group_name; req->header.frame_id = getPoseReferenceFrame(); req->header.stamp = getClock()->now(); req->waypoints = waypoints; @@ -1131,7 +1131,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request) const { - request.group_name = opt_.group_name_; + request.group_name = opt_.group_name; request.num_planning_attempts = num_planning_attempts_; request.max_velocity_scaling_factor = max_velocity_scaling_factor_; request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_; @@ -1202,7 +1202,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // { // moveit_msgs::action::Pickup::Goal goal; // goal.target_name = object; - // goal.group_name = opt_.group_name_; + // goal.group_name = opt_.group_name; // goal.end_effector = getEndEffector(); // goal.support_surface_name = support_surface_; // goal.possible_grasps = std::move(grasps); @@ -1229,7 +1229,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // bool plan_only = false) const // { // moveit_msgs::action::Place::Goal goal; - // goal.group_name = opt_.group_name_; + // goal.group_name = opt_.group_name; // goal.attached_object_name = object; // goal.support_surface_name = support_surface_; // goal.place_locations = std::move(locations); @@ -1261,7 +1261,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (constraints_storage_) { moveit_warehouse::ConstraintsWithMetadata msg_m; - if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_)) + if (constraints_storage_->getConstraints(msg_m, constraint, robot_model->getName(), opt_.group_name)) { path_constraints_ = std::make_unique(static_cast(*msg_m)); @@ -1299,7 +1299,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl std::vector c; if (constraints_storage_) - constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_); + constraints_storage_->getKnownConstraints(c, robot_model->getName(), opt_.group_name); return c; } @@ -1380,7 +1380,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl rclcpp::executors::SingleThreadedExecutor callback_executor_; std::thread callback_thread_; std::shared_ptr tf_buffer_; - moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model; planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_; std::shared_ptr> move_action_client_; @@ -1478,7 +1478,7 @@ MoveGroupInterface& MoveGroupInterface::operator=(MoveGroupInterface&& other) no const std::string& MoveGroupInterface::getName() const { - return impl_->getOptions().group_name_; + return impl_->getOptions().group_name; } const std::vector& MoveGroupInterface::getNamedTargets() const @@ -1577,7 +1577,7 @@ moveit::core::MoveItErrorCode MoveGroupInterface::move() moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const Plan& plan) { - return impl_->execute(plan.trajectory_, false); + return impl_->execute(plan.trajectory, false); } moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory) @@ -1587,7 +1587,7 @@ moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs moveit::core::MoveItErrorCode MoveGroupInterface::execute(const Plan& plan) { - return impl_->execute(plan.trajectory_, true); + return impl_->execute(plan.trajectory, true); } moveit::core::MoveItErrorCode MoveGroupInterface::execute(const moveit_msgs::msg::RobotTrajectory& trajectory) diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index 2db651a5ea..9c5c8dd466 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -479,7 +479,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer res = MoveGroupInterface::plan(plan); } return bp::make_tuple(py_bindings_tools::serializeMsg(res), py_bindings_tools::serializeMsg(plan.trajectory_), - plan.planning_time_); + plan.planning_time); } py_bindings_tools::ByteString constructMotionPlanRequestPython() diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 67816c3e64..09ba2ad2e6 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -381,8 +381,8 @@ void MotionPlanningFrame::changePlanningGroupHelper() planning_display_->getMoveGroupNS().c_str()); moveit::planning_interface::MoveGroupInterface::Options opt( group, moveit::planning_interface::MoveGroupInterface::ROBOT_DESCRIPTION, planning_display_->getMoveGroupNS()); - opt.robot_model_ = robot_model; - opt.robot_description_.clear(); + opt.robot_model = robot_model; + opt.robot_description.clear(); try { #ifdef RVIZ_TF1 diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 00813ffc6e..eab7bfc328 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -159,8 +159,8 @@ bool MotionPlanningFrame::computeCartesianPlan() // Store trajectory in current_plan_ current_plan_ = std::make_shared(); - rt.getRobotTrajectoryMsg(current_plan_->trajectory_); - current_plan_->planning_time_ = (rclcpp::Clock().now() - start).seconds(); + rt.getRobotTrajectoryMsg(current_plan_->trajectory); + current_plan_->planning_time = (rclcpp::Clock().now() - start).seconds(); return success; } return false; @@ -189,7 +189,7 @@ void MotionPlanningFrame::computePlanButtonClicked() if (success) { ui_->execute_button->setEnabled(true); - ui_->result_label->setText(QString("Time: ").append(QString::number(current_plan_->planning_time_, 'f', 3))); + ui_->result_label->setText(QString("Time: ").append(QString::number(current_plan_->planning_time, 'f', 3))); } else {