mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
modified ExecuteTaskSolution.action
This commit is contained in:
parent
387fa5c087
commit
99a7a9bc9a
@ -103,7 +103,7 @@ void ExecuteTaskSolutionCapability::goalCallback(const moveit_task_constructor_m
|
|||||||
}
|
}
|
||||||
|
|
||||||
plan_execution::ExecutableMotionPlan plan;
|
plan_execution::ExecutableMotionPlan plan;
|
||||||
if(!constructMotionPlan(goal->task_solution, plan))
|
if(!constructMotionPlan(goal->solution, plan))
|
||||||
result.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
|
result.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
|
||||||
else {
|
else {
|
||||||
ROS_INFO_NAMED("ExecuteTaskSolution", "Executing TaskSolution");
|
ROS_INFO_NAMED("ExecuteTaskSolution", "Executing TaskSolution");
|
||||||
@ -125,7 +125,8 @@ void ExecuteTaskSolutionCapability::preemptCallback() {
|
|||||||
context_->plan_execution_->stop();
|
context_->plan_execution_->stop();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution, plan_execution::ExecutableMotionPlan& plan) {
|
bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution,
|
||||||
|
plan_execution::ExecutableMotionPlan& plan) {
|
||||||
robot_model::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel();
|
robot_model::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel();
|
||||||
|
|
||||||
robot_state::RobotState state(model);
|
robot_state::RobotState state(model);
|
||||||
|
@ -1,5 +1,5 @@
|
|||||||
# Task solution to execute
|
# Task solution to execute
|
||||||
Solution task_solution
|
Solution solution
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
@ -8,5 +8,6 @@ moveit_msgs/MoveItErrorCodes error_code
|
|||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
# current stage/subtrajectory during execution
|
# finished subtrajectory id / number
|
||||||
string state
|
uint32 sub_id
|
||||||
|
uint32 sub_no
|
||||||
|
Loading…
Reference in New Issue
Block a user