mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
fixup! Reset joint values in scene diff for execution (#504)
This commit is contained in:
parent
06b7b77e9e
commit
2f06438a86
@ -237,6 +237,9 @@ void SubTrajectory::appendTo(moveit_task_constructor_msgs::msg::Solution& msg, I
|
|||||||
trajectory()->getRobotTrajectoryMsg(t.trajectory);
|
trajectory()->getRobotTrajectoryMsg(t.trajectory);
|
||||||
|
|
||||||
this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
|
this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
|
||||||
|
// reset JointStates (joints are already handled in trajectories)
|
||||||
|
t.scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState();
|
||||||
|
t.scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState();
|
||||||
}
|
}
|
||||||
|
|
||||||
double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const {
|
double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const {
|
||||||
|
|||||||
@ -281,13 +281,6 @@ moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
|
|||||||
moveit_task_constructor_msgs::action::ExecuteTaskSolution::Goal goal;
|
moveit_task_constructor_msgs::action::ExecuteTaskSolution::Goal goal;
|
||||||
s.toMsg(goal.solution, pimpl()->introspection_.get());
|
s.toMsg(goal.solution, pimpl()->introspection_.get());
|
||||||
|
|
||||||
// Reset joint values in scene diff to prevent any execution capability from
|
|
||||||
// modifying the planning scene joint values
|
|
||||||
for (auto& sub_trajectory : goal.solution.sub_trajectory) {
|
|
||||||
sub_trajectory.scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState();
|
|
||||||
sub_trajectory.scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState();
|
|
||||||
}
|
|
||||||
|
|
||||||
moveit_msgs::msg::MoveItErrorCodes error_code;
|
moveit_msgs::msg::MoveItErrorCodes error_code;
|
||||||
error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
|
error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
|
||||||
auto goal_handle_future = ac->async_send_goal(goal);
|
auto goal_handle_future = ac->async_send_goal(goal);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user