position_condition.cpp
Go to the documentation of this file.
2 
3 PositionCondition::PositionCondition(const std::string& name, const BT::NodeConfiguration& config)
4  : BT::ConditionNode(name, config){}
5 
6 void PositionCondition::init(const std::string& obj_name, tf2::Vector3& start_pos, moveit::planning_interface::PlanningSceneInterface* psi){
7  obj_name_ = obj_name;
8  psi_ = psi;
9  start_pos_ = start_pos;
10 }
11 
12 BT::NodeStatus PositionCondition::tick(){
13  try {
14  auto ref_pose = psi_->getObjects().at(obj_name_);
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;
17  } else {
18  return BT::NodeStatus::FAILURE;
19  }
20  } catch(std::out_of_range& oor) {
21  ROS_INFO("Position to check not found");
22  return BT::NodeStatus::FAILURE;
23  }
24 }
PositionCondition::start_pos_
tf2::Vector3 start_pos_
Start position.
Definition: position_condition.h:54
PositionCondition::tick
virtual BT::NodeStatus tick() override
tick function
Definition: position_condition.cpp:12
PositionCondition::psi_
moveit::planning_interface::PlanningSceneInterface * psi_
PSI.
Definition: position_condition.h:53
position_condition.h
PositionCondition::PositionCondition
PositionCondition(const std::string &name, const BT::NodeConfiguration &config)
PositionCondition constructor.
Definition: position_condition.cpp:3
PositionCondition::init
void init(const std::string &obj_name, tf2::Vector3 &start_pos, moveit::planning_interface::PlanningSceneInterface *psi)
Initialize Node.
Definition: position_condition.cpp:6
PositionCondition::obj_name_
std::string obj_name_
Object name.
Definition: position_condition.h:52


multi_cell_builder
Author(s): Matteo Anedda
autogenerated on Sun Apr 9 2023 23:59:51