diff --git a/CMakeLists.txt b/CMakeLists.txt index e7e2e7e1d6149a896ccf25e67b77aabbd2ecc3f5..ddeacf261bed3a53bb4c9e3ef2a585fab5e64e84 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(catkin REQUIRED COMPONENTS tf2_ros geometry_msgs sensor_msgs + std_msgs iri_ros_tools ) @@ -102,7 +103,7 @@ set(head_helpers_name head_helpers) catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} ${arithmetic_helpers_name} ${geometry_helpers_name} ${common_helpers_name} ${head_helpers_name} - CATKIN_DEPENDS roscpp dynamic_reconfigure diagnostic_updater behaviortree_cpp_v3 iri_behaviortree tf2_ros geometry_msgs sensor_msgs iri_ros_tools + CATKIN_DEPENDS roscpp dynamic_reconfigure diagnostic_updater behaviortree_cpp_v3 iri_behaviortree tf2_ros geometry_msgs sensor_msgs std_msgs iri_ros_tools # DEPENDS system_lib DEPENDS Boost ) diff --git a/include/iri_base_bt_client/common_bt_helpers.h b/include/iri_base_bt_client/common_bt_helpers.h index 56bebb06d17b45d2138d287d8afddccd65ae5acb..cd7b8f1fccc8d7bfb05464273efb4a80434f124d 100644 --- a/include/iri_base_bt_client/common_bt_helpers.h +++ b/include/iri_base_bt_client/common_bt_helpers.h @@ -3,6 +3,7 @@ #include "iri_bt_factory.h" #include <iri_ros_tools/timeout.h> +#include <std_msgs/String.h> class CommonBTHelpers { @@ -147,6 +148,12 @@ class CommonBTHelpers * */ BT::NodeStatus erase_timeout(BT::TreeNode& _self); + + BT::NodeStatus create_string_publisher(BT::TreeNode& _self); + + BT::NodeStatus publish_string(BT::TreeNode& _self); + + BT::NodeStatus delete_string_publisher(BT::TreeNode& _self); }; #endif diff --git a/package.xml b/package.xml index e74fd570b8fa7ff02304d006998bc70b979f693e..11ba9659ec6b99dab64a9a9c8eb4ffa6468031a5 100644 --- a/package.xml +++ b/package.xml @@ -20,6 +20,7 @@ <depend>iri_behaviortree</depend> <depend>tf2_ros</depend> <depend>sensor_msgs</depend> + <depend>std_msgs</depend> <depend>geometry_msgs</depend> <depend>iri_ros_tools</depend> diff --git a/src/common_bt_helpers.cpp b/src/common_bt_helpers.cpp index 02e9322ec5d249ec6dea684cd33d162cce49383b..e2748d6fae0ba63c2728d9409c421ad5b3330f97 100644 --- a/src/common_bt_helpers.cpp +++ b/src/common_bt_helpers.cpp @@ -17,6 +17,9 @@ void CommonBTHelpers::init(IriBehaviorTreeFactory &factory) BT::PortsList start_timeout_ports = {BT::InputPort<std::string>("tout_name"), BT::InputPort<double>("duration")}; BT::PortsList stop_timeout_ports = {BT::InputPort<std::string>("tout_name")}; BT::PortsList get_timed_out_ports = {BT::InputPort<std::string>("tout_name"), BT::OutputPort<bool>("timed_out")}; + BT::PortsList create_publisher_ports = {BT::InputPort<std::string>("topic_name"), BT::OutputPort<ros::Publisher *>("publisher")}; + BT::PortsList publish_ports = {BT::InputPort<ros::Publisher *>("publisher"),BT::InputPort<std::string>("string_value")}; + BT::PortsList delete_publisher_ports = {BT::BidirectionalPort<ros::Publisher *>("publisher")}; factory.registerIriAsyncAction("RUNNING", std::bind(&CommonBTHelpers::RUNNING, this)); factory.registerSimpleAction("print_msg", std::bind(&CommonBTHelpers::print_msg, this, std::placeholders::_1), print_msg_ports); @@ -26,6 +29,9 @@ void CommonBTHelpers::init(IriBehaviorTreeFactory &factory) factory.registerSimpleAction("stop_timeout", std::bind(&CommonBTHelpers::stop_timeout, this, std::placeholders::_1), stop_timeout_ports); factory.registerSimpleAction("get_timed_out", std::bind(&CommonBTHelpers::get_timed_out, this, std::placeholders::_1), get_timed_out_ports); factory.registerSimpleAction("erase_timeout", std::bind(&CommonBTHelpers::erase_timeout, this, std::placeholders::_1), stop_timeout_ports); + factory.registerSimpleAction("create_string_publisher", std::bind(&CommonBTHelpers::create_string_publisher, this, std::placeholders::_1), create_publisher_ports); + factory.registerSimpleAction("publish_string", std::bind(&CommonBTHelpers::publish_string, this, std::placeholders::_1), publish_ports); + factory.registerSimpleAction("delete_string_publisher", std::bind(&CommonBTHelpers::delete_string_publisher, this, std::placeholders::_1), delete_publisher_ports); } BT::NodeStatus CommonBTHelpers::RUNNING() @@ -233,3 +239,62 @@ BT::NodeStatus CommonBTHelpers::erase_timeout(BT::TreeNode& _self) return BT::NodeStatus::SUCCESS; } + +BT::NodeStatus CommonBTHelpers::create_string_publisher(BT::TreeNode& _self) +{ + BT::Optional<std::string> topic_name_p = _self.getInput<std::string>("topic_name"); + + if(!topic_name_p) + { + ROS_ERROR_STREAM("CommonBTHelpers::create_string_publisher (" << _self.name() << ") -> Incorrect or missing input. It needs the following input ports:" + << " topic_name (sdt::string)"); + return BT::NodeStatus::FAILURE; + } + std::string topic_name=topic_name_p.value(); + ros::NodeHandle nh("~"); + + ros::Publisher *string_publisher = new ros::Publisher(nh.advertise<std_msgs::String>(topic_name,1)); + _self.setOutput("publisher", string_publisher); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus CommonBTHelpers::publish_string(BT::TreeNode& _self) +{ + BT::Optional<ros::Publisher *> publisher_p = _self.getInput<ros::Publisher *>("publisher"); + BT::Optional<std::string> string_value_p = _self.getInput<std::string>("string_value"); + + if(!publisher_p || !string_value_p) + { + ROS_ERROR_STREAM("CommonBTHelpers::delete_string_publisher (" << _self.name() << ") -> Incorrect or missing input. It needs the following input ports:" + << " publisher (ros::publisher *) string_value (std::string)"); + return BT::NodeStatus::FAILURE; + } + ros::Publisher *string_publisher=publisher_p.value(); + std_msgs::String msg; + msg.data=string_value_p.value(); + + if(string_publisher!=NULL) + { + string_publisher->publish(msg); + return BT::NodeStatus::SUCCESS; + } + else + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus CommonBTHelpers::delete_string_publisher(BT::TreeNode& _self) +{ + BT::Optional<ros::Publisher *> publisher_p = _self.getInput<ros::Publisher *>("publisher"); + + if(!publisher_p) + { + ROS_ERROR_STREAM("CommonBTHelpers::delete_string_publisher (" << _self.name() << ") -> Incorrect or missing input. It needs the following input ports:" + << " publisher (ros::publisher *)"); + return BT::NodeStatus::FAILURE; + } + ros::Publisher *string_publisher=publisher_p.value(); + delete string_publisher; + string_publisher=NULL; + _self.setOutput("publisher", string_publisher); + return BT::NodeStatus::SUCCESS; +}