From 5c308d1f28aa6c9532d1c05f6ba3e6ccef3260dc Mon Sep 17 00:00:00 2001 From: Peter David Fagan Date: Fri, 17 Feb 2023 20:11:02 +0000 Subject: [PATCH] remove underscore from public members in MotionPlanResponse (#426) Required to align with changes introduced by https://github.com/ros-planning/moveit2/pull/1939 Co-authored-by: JafarAbdi --- .../src/execute_task_solution_capability.cpp | 20 +++++++++---------- .../src/execute_task_solution_capability.h | 6 +++--- core/src/solvers/pipeline_planner.cpp | 4 ++-- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index d7cfb449..54faa61e 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -99,7 +99,7 @@ void ExecuteTaskSolutionCapability::initialize() { } void ExecuteTaskSolutionCapability::goalCallback( - const std::shared_ptr> goal_handle) { + const std::shared_ptr>& goal_handle) { auto result = std::make_shared(); const auto& goal = goal_handle->get_goal(); @@ -126,7 +126,7 @@ void ExecuteTaskSolutionCapability::goalCallback( } rclcpp_action::CancelResponse ExecuteTaskSolutionCapability::preemptCallback( - const std::shared_ptr> /*goal_handle*/) { + const std::shared_ptr>& /*goal_handle*/) { if (context_->plan_execution_) context_->plan_execution_->stop(); return rclcpp_action::CancelResponse::ACCEPT; @@ -142,16 +142,16 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr state = scene->getCurrentState(); } - plan.plan_components_.reserve(solution.sub_trajectory.size()); + plan.plan_components.reserve(solution.sub_trajectory.size()); for (size_t i = 0; i < solution.sub_trajectory.size(); ++i) { const moveit_task_constructor_msgs::msg::SubTrajectory& sub_traj = solution.sub_trajectory[i]; - plan.plan_components_.emplace_back(); - plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components_.back(); + plan.plan_components.emplace_back(); + plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components.back(); // define individual variable for use in closure below const std::string description = std::to_string(i + 1) + "/" + std::to_string(solution.sub_trajectory.size()); - exec_traj.description_ = description; + exec_traj.description = description; const moveit::core::JointModelGroup* group = nullptr; { @@ -168,12 +168,12 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr RCLCPP_DEBUG(LOGGER, "Using JointModelGroup '%s' for execution", group->getName().c_str()); } } - exec_traj.trajectory_ = std::make_shared(model, group); - exec_traj.trajectory_->setRobotTrajectoryMsg(state, sub_traj.trajectory); + exec_traj.trajectory = std::make_shared(model, group); + exec_traj.trajectory->setRobotTrajectoryMsg(state, sub_traj.trajectory); /* TODO add action feedback and markers */ - exec_traj.effect_on_success_ = [this, sub_traj, - description](const plan_execution::ExecutableMotionPlan* /*plan*/) { + exec_traj.effect_on_success = [this, sub_traj, + description](const plan_execution::ExecutableMotionPlan* /*plan*/) { if (!moveit::core::isEmpty(sub_traj.scene_diff)) { RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description); return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff); diff --git a/capabilities/src/execute_task_solution_capability.h b/capabilities/src/execute_task_solution_capability.h index dbc8cb47..3248b06a 100644 --- a/capabilities/src/execute_task_solution_capability.h +++ b/capabilities/src/execute_task_solution_capability.h @@ -63,14 +63,14 @@ private: bool constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution, plan_execution::ExecutableMotionPlan& plan); - void goalCallback(const std::shared_ptr> goal_handle); + void goalCallback(const std::shared_ptr>& goal_handle); rclcpp_action::CancelResponse - preemptCallback(const std::shared_ptr> goal_handle); + preemptCallback(const std::shared_ptr>& goal_handle); /** Always accept the goal */ rclcpp_action::GoalResponse handleNewGoal(const rclcpp_action::GoalUUID& /*uuid*/, - const ExecuteTaskSolutionAction::Goal::ConstSharedPtr /*goal*/) const { + const ExecuteTaskSolutionAction::Goal::ConstSharedPtr& /*goal*/) const { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 408b1597..47bafa8e 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -180,7 +180,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, ::planning_interface::MotionPlanResponse res; bool success = planner_->generatePlan(from, req, res); - result = res.trajectory_; + result = res.trajectory; return success; } @@ -205,7 +205,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co ::planning_interface::MotionPlanResponse res; bool success = planner_->generatePlan(from, req, res); - result = res.trajectory_; + result = res.trajectory; return success; } } // namespace solvers