3636
3737#include " execute_task_solution_capability.h"
3838
39- #include < moveit/task_constructor/moveit_compat.h>
40-
39+ #include < moveit/moveit_cpp/moveit_cpp.h>
4140#include < moveit/plan_execution/plan_execution.h>
4241#include < moveit/trajectory_processing/trajectory_tools.h>
4342#include < moveit/kinematic_constraints/utils.h>
4443#include < moveit/move_group/capability_names.h>
4544#include < moveit/robot_state/conversions.h>
46- #if MOVEIT_HAS_MESSAGE_CHECKS
4745#include < moveit/utils/message_checks.h>
48- #endif
46+ #include < moveit/moveit_cpp/moveit_cpp.h>
47+
48+ #include < boost/algorithm/string/join.hpp>
4949
5050namespace {
5151
@@ -80,66 +80,71 @@ const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::Rob
8080}
8181} // namespace
8282
83+ static const rclcpp::Logger LOGGER = rclcpp::get_logger(" moveit_task_constructor_visualization.execute_task_solution"
84+
8385namespace move_group {
8486
8587ExecuteTaskSolutionCapability::ExecuteTaskSolutionCapability () : MoveGroupCapability(" ExecuteTaskSolution"
8688
8789void ExecuteTaskSolutionCapability::initialize () {
8890 // configure the action server
89- as_.reset (new actionlib::SimpleActionServer<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>(
90- root_node_handle_, " execute_task_solution"
91- std::bind (&ExecuteTaskSolutionCapability::goalCallback, this , std::placeholders::_1), false ));
92- as_->registerPreemptCallback (std::bind (&ExecuteTaskSolutionCapability::preemptCallback, this ));
93- as_->start ();
91+ as_ = rclcpp_action::create_server<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
92+ context_->moveit_cpp_ ->getNode (), " execute_task_solution"
93+ ActionServerType::GoalCallback (std::bind (&ExecuteTaskSolutionCapability::handleNewGoal, this ,
94+ std::placeholders::_1, std::placeholders::_2)),
95+ ActionServerType::CancelCallback (
96+ std::bind (&ExecuteTaskSolutionCapability::preemptCallback, this , std::placeholders::_1)),
97+ ActionServerType::AcceptedCallback (
98+ std::bind (&ExecuteTaskSolutionCapability::goalCallback, this , std::placeholders::_1)));
9499}
95100
96101void ExecuteTaskSolutionCapability::goalCallback (
97- const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal ) {
98- moveit_task_constructor_msgs::ExecuteTaskSolutionResult result ;
102+ const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle ) {
103+ auto result = std::make_shared< moveit_task_constructor_msgs::action::ExecuteTaskSolution::Result>() ;
99104
105+ const auto & goal = goal_handle->get_goal ();
100106 if (!context_->plan_execution_ ) {
101- const std::string response = " Cannot execute solution. ~allow_trajectory_execution was set to false"
102- result.error_code .val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
103- as_->setAborted (result, response);
107+ result->error_code .val = moveit_msgs::msg::MoveItErrorCodes::CONTROL_FAILED;
108+ goal_handle->abort (result);
104109 return ;
105110 }
106111
107112 plan_execution::ExecutableMotionPlan plan;
108113 if (!constructMotionPlan (goal->solution , plan))
109- result. error_code .val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
114+ result-> error_code .val = moveit_msgs::msg ::MoveItErrorCodes::INVALID_MOTION_PLAN;
110115 else {
111- ROS_INFO_NAMED ( " ExecuteTaskSolution " " Executing TaskSolution"
112- result. error_code = context_->plan_execution_ ->executeAndMonitor (plan);
116+ RCLCPP_INFO (LOGGER , " Executing TaskSolution"
117+ result-> error_code = context_->plan_execution_ ->executeAndMonitor (plan);
113118 }
114119
115- const std::string response = context_->plan_execution_ ->getErrorCodeString (result.error_code );
116-
117- if (result.error_code .val == moveit_msgs::MoveItErrorCodes::SUCCESS)
118- as_->setSucceeded (result, response);
119- else if (result.error_code .val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
120- as_->setPreempted (result, response);
120+ if (result->error_code .val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
121+ goal_handle->succeed (result);
122+ else if (result->error_code .val == moveit_msgs::msg::MoveItErrorCodes::PREEMPTED)
123+ goal_handle->canceled (result);
121124 else
122- as_-> setAborted (result, response );
125+ goal_handle-> abort (result);
123126}
124127
125- void ExecuteTaskSolutionCapability::preemptCallback () {
128+ rclcpp_action::CancelResponse ExecuteTaskSolutionCapability::preemptCallback (
129+ const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle) {
126130 if (context_->plan_execution_ )
127131 context_->plan_execution_ ->stop ();
132+ return rclcpp_action::CancelResponse::ACCEPT;
128133}
129134
130- bool ExecuteTaskSolutionCapability::constructMotionPlan (const moveit_task_constructor_msgs::Solution& solution,
135+ bool ExecuteTaskSolutionCapability::constructMotionPlan (const moveit_task_constructor_msgs::msg:: Solution& solution,
131136 plan_execution::ExecutableMotionPlan& plan) {
132- robot_model ::RobotModelConstPtr model = context_->planning_scene_monitor_ ->getRobotModel ();
137+ moveit::core ::RobotModelConstPtr model = context_->planning_scene_monitor_ ->getRobotModel ();
133138
134- robot_state ::RobotState state (model);
139+ moveit::core ::RobotState state (model);
135140 {
136141 planning_scene_monitor::LockedPlanningSceneRO scene (context_->planning_scene_monitor_ );
137142 state = scene->getCurrentState ();
138143 }
139144
140145 plan.plan_components_ .reserve (solution.sub_trajectory .size ());
141146 for (size_t i = 0 ; i < solution.sub_trajectory .size (); ++i) {
142- const moveit_task_constructor_msgs::SubTrajectory& sub_traj = solution.sub_trajectory [i];
147+ const moveit_task_constructor_msgs::msg:: SubTrajectory& sub_traj = solution.sub_trajectory [i];
143148
144149 plan.plan_components_ .emplace_back ();
145150 plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components_ .back ();
@@ -156,12 +161,11 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
156161 if (!joint_names.empty ()) {
157162 group = findJointModelGroup (*model, joint_names);
158163 if (!group) {
159- ROS_ERROR_STREAM_NAMED ( " ExecuteTaskSolution " " Could not find JointModelGroup that actuates {"
160- << boost::algorithm::join (joint_names, " , " " }"
164+ RCLCPP_ERROR_STREAM (LOGGER , " Could not find JointModelGroup that actuates {"
165+ << boost::algorithm::join (joint_names, " , " " }"
161166 return false ;
162167 }
163- ROS_DEBUG_NAMED (" ExecuteTaskSolution" " Using JointModelGroup '%s' for execution"
164- group->getName ().c_str ());
168+ RCLCPP_DEBUG (LOGGER, " Using JointModelGroup '%s' for execution" getName ().c_str ());
165169 }
166170 }
167171 exec_traj.trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(model, group);
@@ -170,25 +174,16 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
170174 /* TODO add action feedback and markers */
171175 exec_traj.effect_on_success_ = [this , sub_traj,
172176 description](const plan_execution::ExecutableMotionPlan* /* plan*/
173- #if MOVEIT_HAS_MESSAGE_CHECKS
174177 if (!moveit::core::isEmpty (sub_traj.scene_diff )) {
175- #else
176- if (!planning_scene::PlanningScene::isEmpty (sub_traj.scene_diff )) {
177- #endif
178- ROS_DEBUG_STREAM_NAMED (" ExecuteTaskSolution" " apply effect of "
178+ RCLCPP_DEBUG_STREAM (LOGGER, " apply effect of "
179179 return context_->planning_scene_monitor_ ->newPlanningSceneMessage (sub_traj.scene_diff );
180180 }
181181 return true ;
182182 };
183183
184- #if MOVEIT_HAS_MESSAGE_CHECKS
185184 if (!moveit::core::isEmpty (sub_traj.scene_diff .robot_state ) &&
186- #else
187- if (!planning_scene::PlanningScene::isEmpty (sub_traj.scene_diff .robot_state ) &&
188- #endif
189185 !moveit::core::robotStateMsgToRobotState (sub_traj.scene_diff .robot_state , state, true )) {
190- ROS_ERROR_STREAM_NAMED (" ExecuteTaskSolution"
191- " invalid intermediate robot state in scene diff of SubTrajectory "
186+ RCLCPP_ERROR_STREAM (LOGGER, " invalid intermediate robot state in scene diff of SubTrajectory "
192187 return false ;
193188 }
194189 }
0 commit comments