mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
add effect description as debug output
This commit is contained in:
parent
73b7475cdb
commit
725f57e7fd
@ -141,10 +141,13 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
|
|||||||
plan.plan_components_.emplace_back();
|
plan.plan_components_.emplace_back();
|
||||||
plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components_.back();
|
plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components_.back();
|
||||||
|
|
||||||
exec_traj.description_ = std::to_string(i+1) + "/"
|
// define individual variable for use in closure below
|
||||||
+ std::to_string(solution.sub_trajectory.size())
|
const std::string description = std::to_string(i+1) + "/"
|
||||||
+ " - subsolution " + std::to_string(sub_traj.id)
|
+ std::to_string(solution.sub_trajectory.size())
|
||||||
+ " of stage " + std::to_string(sub_traj.stage_id);
|
+ " - subsolution " + std::to_string(sub_traj.id)
|
||||||
|
+ " of stage " + std::to_string(sub_traj.stage_id);
|
||||||
|
|
||||||
|
exec_traj.description_ = description;
|
||||||
|
|
||||||
const moveit::core::JointModelGroup* group = nullptr;
|
const moveit::core::JointModelGroup* group = nullptr;
|
||||||
{
|
{
|
||||||
@ -162,9 +165,10 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
|
|||||||
exec_traj.trajectory_->setRobotTrajectoryMsg(state, sub_traj.trajectory);
|
exec_traj.trajectory_->setRobotTrajectoryMsg(state, sub_traj.trajectory);
|
||||||
|
|
||||||
/* TODO add action feedback and markers */
|
/* TODO add action feedback and markers */
|
||||||
exec_traj.effect_on_success_ = [this,sub_traj](const plan_execution::ExecutableMotionPlan*){
|
exec_traj.effect_on_success_ = [this,sub_traj,description](const plan_execution::ExecutableMotionPlan*){
|
||||||
if(!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)){
|
if(!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)){
|
||||||
planning_scene_monitor::LockedPlanningSceneRW scene(context_->planning_scene_monitor_);
|
planning_scene_monitor::LockedPlanningSceneRW scene(context_->planning_scene_monitor_);
|
||||||
|
ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description );
|
||||||
return scene->usePlanningSceneMsg(sub_traj.scene_diff);
|
return scene->usePlanningSceneMsg(sub_traj.scene_diff);
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
@ -172,7 +176,7 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
|
|||||||
|
|
||||||
if(!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff.robot_state) &&
|
if(!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff.robot_state) &&
|
||||||
!moveit::core::robotStateMsgToRobotState(sub_traj.scene_diff.robot_state, state, true)){
|
!moveit::core::robotStateMsgToRobotState(sub_traj.scene_diff.robot_state, state, true)){
|
||||||
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "invalid intermediate robot state in scene diff of SubTrajectory " << exec_traj.description_);
|
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "invalid intermediate robot state in scene diff of SubTrajectory " << description);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user