From abb68da083b24deabcd54aa169aeab4a21988f83 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 8 Apr 2018 16:45:12 +0200 Subject: [PATCH] forward properties --- .../task_constructor/stages/compute_ik.h | 32 +++++++++++++--- core/src/stages/compute_ik.cpp | 37 +++++-------------- 2 files changed, 35 insertions(+), 34 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/compute_ik.h b/core/include/moveit/task_constructor/stages/compute_ik.h index b232d7e2..2bf6f925 100644 --- a/core/include/moveit/task_constructor/stages/compute_ik.h +++ b/core/include/moveit/task_constructor/stages/compute_ik.h @@ -54,9 +54,13 @@ namespace moveit { namespace task_constructor { namespace stages { * The wrapper reads a target_pose from the interface state of solutions provided * by the wrapped stage. This Cartesian pose (PoseStamped msg) is used as a goal * pose for inverse kinematics. + * * Usually, the end effector's parent link or the group's tip link is used as * the IK frame, which should be moved to the goal frame. However, any other * IK frame can be defined (which is linked to the tip of the group). + * + * Properties of the internally received InterfaceState can be forwarded to the + * newly generated, externally exposed InterfaceState. */ class ComputeIK : public WrapperBase { public: @@ -65,11 +69,17 @@ public: void init(const core::RobotModelConstPtr &robot_model); void onNewSolution(const SolutionBase &s) override; - void setTimeout(double timeout); - void setEndEffector(const std::string& eef); + void setTimeout(double timeout) { + setProperty("timeout", timeout); + } + void setEndEffector(const std::string& eef) { + setProperty("eef", eef); + } /// setters for IK frame - void setIKFrame(const geometry_msgs::PoseStamped &pose); + void setIKFrame(const geometry_msgs::PoseStamped &pose) { + setProperty("ik_frame", pose); + } void setIKFrame(const Eigen::Affine3d& pose, const std::string& link); template void setIKFrame(const T& p, const std::string& link) { @@ -81,7 +91,9 @@ public: } /// setters for target pose - void setTargetPose(const geometry_msgs::PoseStamped &pose); + void setTargetPose(const geometry_msgs::PoseStamped &pose) { + setProperty("target_pose", pose); + } void setTargetPose(const Eigen::Affine3d& pose, const std::string& frame = ""); template void setTargetPose(const T& p, const std::string& frame = "") { @@ -89,8 +101,16 @@ public: setTargetPose(pose, frame); } - void setMaxIKSolutions(uint32_t n); - void setIgnoreCollisions(bool flag); + void setMaxIKSolutions(uint32_t n) { + setProperty("max_ik_solutions", n); + } + void setIgnoreCollisions(bool flag) { + setProperty("ignore_collisions", flag); + } + + void setForwardedProperties(const std::set& names) { + setProperty("forward_properties", names); + } }; } } } diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 0bacbf3e..635a687f 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -61,6 +61,7 @@ ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child) p.declare("default_pose", "", "default joint pose of active group (defines cost of IK)"); p.declare("max_ik_solutions", 1); p.declare("ignore_collisions", false); + p.declare>("forward_properties", "to-be-forwarded properties from input"); // ik_frame and target_pose are read from the interface p.declare("ik_frame", "frame to be moved towards goal pose"); @@ -68,19 +69,6 @@ ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child) p.configureInitFrom(Stage::INTERFACE, {"target_pose"}); } -void ComputeIK::setTimeout(double timeout){ - setProperty("timeout", timeout); -} - -void ComputeIK::setEndEffector(const std::string &eef){ - setProperty("eef", eef); -} - -void ComputeIK::setIKFrame(const geometry_msgs::PoseStamped &pose) -{ - setProperty("ik_frame", pose); -} - void ComputeIK::setIKFrame(const Eigen::Affine3d &pose, const std::string &link) { geometry_msgs::PoseStamped pose_msg; @@ -89,11 +77,6 @@ void ComputeIK::setIKFrame(const Eigen::Affine3d &pose, const std::string &link) setIKFrame(pose_msg); } -void ComputeIK::setTargetPose(const geometry_msgs::PoseStamped &pose) -{ - setProperty("target_pose", pose); -} - void ComputeIK::setTargetPose(const Eigen::Affine3d &pose, const std::string &frame) { geometry_msgs::PoseStamped pose_msg; @@ -102,15 +85,6 @@ void ComputeIK::setTargetPose(const Eigen::Affine3d &pose, const std::string &fr setTargetPose(pose_msg); } -void ComputeIK::setMaxIKSolutions(uint32_t n){ - setProperty("max_ik_solutions", n); -} - -void ComputeIK::setIgnoreCollisions(bool flag) -{ - setProperty("ignore_collisions", flag); -} - // found IK solutions with a flag indicating validity typedef std::vector> IKSolutions; @@ -394,7 +368,14 @@ void ComputeIK::onNewSolution(const SolutionBase &s) robot_state.setJointGroupPositions(jmg, ik_solutions.back().data()); robot_state.update(); - spawn(InterfaceState(scene), std::move(solution)); + InterfaceState state(scene); + const boost::any &forwards = props.get("forward_properties"); + if (!forwards.empty()) { + auto p = s.start()->properties(); + p.exposeTo(state.properties(), boost::any_cast>(forwards)); + } + + spawn(std::move(state), std::move(solution)); } // TODO: magic constant should be a property instead ("current_seed_only", or equivalent)