diff --git a/gb_world_model/CMakeLists.txt b/gb_world_model/CMakeLists.txt index 43b83ec..0cbf637 100644 --- a/gb_world_model/CMakeLists.txt +++ b/gb_world_model/CMakeLists.txt @@ -44,6 +44,9 @@ list(APPEND plugin_libs gb_get_placing_pose_bt_node) add_library(gb_select_pick_bt_node SHARED src/behavior_tree_nodes/SelectPick.cpp) list(APPEND plugin_libs gb_select_pick_bt_node) +add_library(gb_search_object_bt_node SHARED src/behavior_tree_nodes/GetSearchObject.cpp) +list(APPEND plugin_libs gb_search_object_bt_node) + add_library(gb_is_object_perceived_bt_node SHARED src/behavior_tree_nodes/isObjectPerceived.cpp) list(APPEND plugin_libs gb_is_object_perceived_bt_node) diff --git a/gb_world_model/include/gb_world_model/behavior_tree_nodes/GetSearchObject.hpp b/gb_world_model/include/gb_world_model/behavior_tree_nodes/GetSearchObject.hpp new file mode 100644 index 0000000..e381ec6 --- /dev/null +++ b/gb_world_model/include/gb_world_model/behavior_tree_nodes/GetSearchObject.hpp @@ -0,0 +1,56 @@ +// Copyright 2019 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GB_BEHAVIOR_TREE__BEHAVIOR_TREE_NODES__GETSEARCHOBJECT_HPP_ +#define GB_BEHAVIOR_TREE__BEHAVIOR_TREE_NODES__GETSEARCHOBJECT_HPP_ + +#include +#include "std_msgs/msg/string.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" + +namespace gb_world_model +{ + +class GetSearchObject : public BT::ActionNodeBase +{ +public: + explicit GetSearchObject( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + void halt(); + BT::NodeStatus tick(); + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::OutputPort("object_id") + }); + } + + void messageCB(const std_msgs::msg::String::SharedPtr msg); + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr message_sub_; + +}; + +} // namespace namespace gb_world_model + +#endif // GB_BEHAVIOR_TREE__BEHAVIOR_TREE_NODES__GETSEARCHOBJECT_HPP_ diff --git a/gb_world_model/include/gb_world_model/behavior_tree_nodes/SelectPick.hpp b/gb_world_model/include/gb_world_model/behavior_tree_nodes/SelectPick.hpp index c07be1a..c098acb 100644 --- a/gb_world_model/include/gb_world_model/behavior_tree_nodes/SelectPick.hpp +++ b/gb_world_model/include/gb_world_model/behavior_tree_nodes/SelectPick.hpp @@ -37,7 +37,9 @@ class SelectPick : public BT::ActionNodeBase static BT::PortsList providedPorts() { - return BT::PortsList({}); + return { + BT::InputPort("target") + }; } private: diff --git a/gb_world_model/src/behavior_tree_nodes/GetSearchObject.cpp b/gb_world_model/src/behavior_tree_nodes/GetSearchObject.cpp new file mode 100644 index 0000000..d0e93b2 --- /dev/null +++ b/gb_world_model/src/behavior_tree_nodes/GetSearchObject.cpp @@ -0,0 +1,73 @@ +// Copyright 2019 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "ros2_knowledge_graph/GraphNode.hpp" +#include "gb_world_model/behavior_tree_nodes/GetSearchObject.hpp" +#include "ros2_knowledge_graph/graph_utils.hpp" + +#include "behaviortree_cpp_v3/behavior_tree.h" + +namespace gb_world_model +{ + +GetSearchObject::GetSearchObject( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(xml_tag_name, conf) +{ + node_ = config().blackboard->get("node"); + message_sub_ = node_->create_subscription( + "/message", 1, std::bind(&GetSearchObject::messageCB, this, _1)); +} + +void +GetSearchObject::halt() +{ +} + + +void +GetSearchObject::messageCB(const std_msgs::msg::String::SharedPtr msg) +{ + received_msg_ = (msg->data); +} + + +BT::NodeStatus +GetSearchObject::tick() +{ + + + if (received_msg_ == "") + { + return BT::NodeStatus::RUNNING; + } + else + { + setOutput("object_id", received_msg_); + return BT::NodeStatus::SUCCESS; + return BT::NodeStatus::SUCCESS; + } + +} + +} // namespace gb_world_model + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("GetSearchObject"); +} diff --git a/gb_world_model/src/behavior_tree_nodes/SelectPick.cpp b/gb_world_model/src/behavior_tree_nodes/SelectPick.cpp index 4d423a0..3e796e8 100644 --- a/gb_world_model/src/behavior_tree_nodes/SelectPick.cpp +++ b/gb_world_model/src/behavior_tree_nodes/SelectPick.cpp @@ -45,25 +45,40 @@ SelectPick::tick() std::string object_id; auto edges_by_data = graph_->get_edges_from_node_by_data(robot_, "perceived"); + + std::string target; + getInput("target", target); + if (edges_by_data.size() == 0) { std::cerr << " [SelectPick] Error: I found [" << edges_by_data.size() << "] perceived edges from jarvis node. Retrying..." << std::endl; - for (auto edge : edges_by_data) - { - std::cerr << " [" << ros2_knowledge_graph::to_string(edge) << "] " << std::endl; - } return BT::NodeStatus::RUNNING; } + if (target=="any") + { + object_id = edges_by_data[0].target_node_id; + } + else + { + object_id = target; + } + for (auto edge : edges_by_data) { - object_id = edge.target_node_id; + if (target == edge.target_node_id) + { + auto edge_pick_obj = ros2_knowledge_graph::new_edge( + robot_, object_id, "pick"); + graph_->update_edge(edge_pick_obj); + return BT::NodeStatus::SUCCESS; + } + } - auto edge_pick_obj = ros2_knowledge_graph::new_edge( - robot_, object_id, "pick"); - graph_->update_edge(edge_pick_obj); - return BT::NodeStatus::SUCCESS; + std::cerr << " [SelectPick] Error: Requested Object [" << target << "] wasn't among detections..." << std::endl; + return BT::NodeStatus::RUNNING; + } } // namespace gb_world_model diff --git a/gb_world_model/src/behavior_tree_nodes/isObjectPerceived.cpp b/gb_world_model/src/behavior_tree_nodes/isObjectPerceived.cpp index d77f225..acd51ac 100644 --- a/gb_world_model/src/behavior_tree_nodes/isObjectPerceived.cpp +++ b/gb_world_model/src/behavior_tree_nodes/isObjectPerceived.cpp @@ -43,6 +43,9 @@ isObjectPerceived::tick() { rclcpp::spin_some(node_); std::string object_id; + std::string target; + getInput("target", target); + auto edges_by_data = graph_->get_edges_from_node_by_data(robot_, "perceived"); if (edges_by_data.size() == 0) @@ -50,12 +53,27 @@ isObjectPerceived::tick() RCLCPP_INFO(node_->get_logger(), "isObjectPerceived returns false, scanning..."); return BT::NodeStatus::RUNNING; } - else + + if (target=="any") { RCLCPP_INFO(node_->get_logger(), "isObjectPerceived returns TRUE"); return BT::NodeStatus::SUCCESS; } -} + + //TODO:: make sure that string comparison is right!!!!!! + for (auto edge : edges_by_data) + { + if (target == edge.target_node_id) + { + RCLCPP_INFO(node_->get_logger(), "isObjectPerceived returns TRUE"); + return BT::NodeStatus::SUCCESS; + } + } + + // edges do not match target ... + return BT::NodeStatus::RUNNING; + + } } // namespace gb_world_model