4 : BT::ConditionNode(name, config){}
6 void PositionCondition::init(
const std::string& obj_name, tf2::Vector3& start_pos, moveit::planning_interface::PlanningSceneInterface* psi){
15 if(tf2::tf2Distance2(
start_pos_, tf2::Vector3(ref_pose.pose.position.x, ref_pose.pose.position.y, ref_pose.pose.position.z )) == 0){
16 return BT::NodeStatus::SUCCESS;
18 return BT::NodeStatus::FAILURE;
20 }
catch(std::out_of_range& oor) {
21 ROS_INFO(
"Position to check not found");
22 return BT::NodeStatus::FAILURE;