mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
fixup! MoveTo: store goal as any type
This commit is contained in:
parent
ca9d50e7d8
commit
c70db65578
@ -81,7 +81,9 @@ bool MoveTo::getJointStateGoal(const boost::any& goal,
|
|||||||
if (!state.setToDefaultValues(jmg, named_joint_pose))
|
if (!state.setToDefaultValues(jmg, named_joint_pose))
|
||||||
throw InitStageException(*this, "Unknown joint pose: " + named_joint_pose);
|
throw InitStageException(*this, "Unknown joint pose: " + named_joint_pose);
|
||||||
return true;
|
return true;
|
||||||
|
} catch (const boost::bad_any_cast&) {}
|
||||||
|
|
||||||
|
try {
|
||||||
// try RobotState
|
// try RobotState
|
||||||
const moveit_msgs::RobotState& msg = boost::any_cast<moveit_msgs::RobotState>(goal);
|
const moveit_msgs::RobotState& msg = boost::any_cast<moveit_msgs::RobotState>(goal);
|
||||||
if (!msg.is_diff)
|
if (!msg.is_diff)
|
||||||
@ -98,9 +100,7 @@ bool MoveTo::getJointStateGoal(const boost::any& goal,
|
|||||||
|
|
||||||
moveit::core::robotStateMsgToRobotState(msg, state, false);
|
moveit::core::robotStateMsgToRobotState(msg, state, false);
|
||||||
return true;
|
return true;
|
||||||
} catch (const boost::bad_any_cast&) {
|
} catch (const boost::bad_any_cast&) {}
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user