mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
moved implementations of property setters to header
This commit is contained in:
parent
17e487be06
commit
9e19f2187b
@ -57,17 +57,32 @@ public:
|
|||||||
bool computeForward(const InterfaceState& from) override;
|
bool computeForward(const InterfaceState& from) override;
|
||||||
bool computeBackward(const InterfaceState& to) override;
|
bool computeBackward(const InterfaceState& to) override;
|
||||||
|
|
||||||
void setGroup(const std::string& group);
|
void setGroup(const std::string& group) {
|
||||||
void setLink(const std::string& link);
|
setProperty("group", group);
|
||||||
|
}
|
||||||
|
void setLink(const std::string& link) {
|
||||||
|
setProperty("link", link);
|
||||||
|
}
|
||||||
|
|
||||||
/// move link to given pose
|
/// move link to given pose
|
||||||
void setGoal(const geometry_msgs::PoseStamped& pose);
|
void setGoal(const geometry_msgs::PoseStamped& pose) {
|
||||||
|
setProperty("pose", pose);
|
||||||
|
}
|
||||||
|
|
||||||
/// move link to given point, keeping current orientation
|
/// move link to given point, keeping current orientation
|
||||||
void setGoal(const geometry_msgs::PointStamped& point);
|
void setGoal(const geometry_msgs::PointStamped& point) {
|
||||||
|
setProperty("point", point);
|
||||||
|
}
|
||||||
|
|
||||||
/// move joint model group to given named pose
|
/// move joint model group to given named pose
|
||||||
void setGoal(const std::string& named_joint_pose);
|
void setGoal(const std::string& named_joint_pose) {
|
||||||
|
setProperty("named_joint_pose", named_joint_pose);
|
||||||
|
}
|
||||||
|
|
||||||
/// move joints specified in msg to their target values
|
/// move joints specified in msg to their target values
|
||||||
void setGoal(const moveit_msgs::RobotState& robot_state);
|
void setGoal(const moveit_msgs::RobotState& robot_state) {
|
||||||
|
setProperty("joint_pose", robot_state);
|
||||||
|
}
|
||||||
|
|
||||||
void setPathConstraints(moveit_msgs::Constraints path_constraints){
|
void setPathConstraints(moveit_msgs::Constraints path_constraints){
|
||||||
setProperty("path_constraints", std::move(path_constraints));
|
setProperty("path_constraints", std::move(path_constraints));
|
||||||
|
|||||||
@ -62,34 +62,6 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
|
|||||||
"constraints to maintain during trajectory");
|
"constraints to maintain during trajectory");
|
||||||
}
|
}
|
||||||
|
|
||||||
void MoveTo::setGroup(const std::string& group){
|
|
||||||
setProperty("group", group);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MoveTo::setLink(const std::string& link){
|
|
||||||
setProperty("link", link);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MoveTo::setGoal(const geometry_msgs::PoseStamped &pose)
|
|
||||||
{
|
|
||||||
setProperty("pose", pose);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MoveTo::setGoal(const geometry_msgs::PointStamped &point)
|
|
||||||
{
|
|
||||||
setProperty("point", point);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MoveTo::setGoal(const std::string &named_joint_pose)
|
|
||||||
{
|
|
||||||
setProperty("named_joint_pose", named_joint_pose);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MoveTo::setGoal(const moveit_msgs::RobotState& robot_state)
|
|
||||||
{
|
|
||||||
setProperty("joint_pose", robot_state);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model)
|
void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
PropagatingEitherWay::init(robot_model);
|
PropagatingEitherWay::init(robot_model);
|
||||||
@ -115,7 +87,7 @@ bool MoveTo::getJointStateGoal(moveit::core::RobotState& state,
|
|||||||
if (!goal.empty()) {
|
if (!goal.empty()) {
|
||||||
const std::string& named_joint_pose = boost::any_cast<std::string>(goal);
|
const std::string& named_joint_pose = boost::any_cast<std::string>(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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user