diff --git a/behaviour_library/CMakeLists.txt b/behaviour_library/CMakeLists.txt index 3769a4d..786d3ea 100644 --- a/behaviour_library/CMakeLists.txt +++ b/behaviour_library/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(behaviortree_ros2 REQUIRED) find_package(btcpp_ros2_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(robot_swiss_knife_msgs REQUIRED) find_package(move_base_skill REQUIRED) @@ -22,6 +23,7 @@ find_package(move_base_skill REQUIRED) set(THIS_PACKAGE_DEPS behaviortree_ros2::behaviortree_ros2 ${btcpp_ros2_interfaces_TARGETS} + ${geometry_msgs_TARGETS} ${sensor_msgs_TARGETS} ${robot_swiss_knife_msgs_TARGETS} ) @@ -35,6 +37,20 @@ add_library(image_sub_plugin SHARED src/image_sub_behaviour.cpp) target_compile_definitions(image_sub_plugin PRIVATE BT_PLUGIN_EXPORT) target_link_libraries(image_sub_plugin ${THIS_PACKAGE_DEPS}) +##################################### +# Point cloud subscription behaviour +##################################### +add_library(cloud_sub_plugin SHARED src/cloud_sub_behaviour.cpp) +target_compile_definitions(cloud_sub_plugin PRIVATE BT_PLUGIN_EXPORT) +target_link_libraries(cloud_sub_plugin ${THIS_PACKAGE_DEPS}) + +################################################## +# Behaviour for publishing a pose stamped message +################################################## +add_library(pose_pub_plugin SHARED src/pose_pub_behaviour.cpp) +target_compile_definitions(pose_pub_plugin PRIVATE BT_PLUGIN_EXPORT) +target_link_libraries(pose_pub_plugin ${THIS_PACKAGE_DEPS}) + ########################################### # Behaviour for checking object visibility ########################################### @@ -42,6 +58,27 @@ add_library(check_visibility_plugin SHARED src/check_visibility_behaviour.cpp) target_compile_definitions(check_visibility_plugin PRIVATE BT_PLUGIN_EXPORT) target_link_libraries(check_visibility_plugin ${THIS_PACKAGE_DEPS}) +################################ +# Object segmentation behaviour +################################ +add_library(segment_objects_plugin SHARED src/segment_objects_behaviour.cpp) +target_compile_definitions(segment_objects_plugin PRIVATE BT_PLUGIN_EXPORT) +target_link_libraries(segment_objects_plugin ${THIS_PACKAGE_DEPS}) + +####################################### +# ROI point cloud extraction behaviour +####################################### +add_library(extract_roi_3d_points_plugin SHARED src/extract_roi_3d_points_behaviour.cpp) +target_compile_definitions(extract_roi_3d_points_plugin PRIVATE BT_PLUGIN_EXPORT) +target_link_libraries(extract_roi_3d_points_plugin ${THIS_PACKAGE_DEPS}) + +############################# +# Pose calculation behaviour +############################# +add_library(calculate_pose_plugin SHARED src/calculate_pose_behaviour.cpp) +target_compile_definitions(calculate_pose_plugin PRIVATE BT_PLUGIN_EXPORT) +target_link_libraries(calculate_pose_plugin ${THIS_PACKAGE_DEPS}) + ######################################################## # Behaviour for moving a robot's base between locations ######################################################## @@ -57,7 +94,12 @@ target_link_libraries(example_tree ${THIS_PACKAGE_DEPS}) install(TARGETS image_sub_plugin + cloud_sub_plugin + pose_pub_plugin check_visibility_plugin + segment_objects_plugin + extract_roi_3d_points_plugin + calculate_pose_plugin move_base_plugin example_tree DESTINATION lib/${PROJECT_NAME} @@ -65,7 +107,12 @@ install(TARGETS install(TARGETS image_sub_plugin + cloud_sub_plugin + pose_pub_plugin check_visibility_plugin + segment_objects_plugin + extract_roi_3d_points_plugin + calculate_pose_plugin move_base_plugin LIBRARY DESTINATION share/${PROJECT_NAME}/bt_plugins ARCHIVE DESTINATION share/${PROJECT_NAME}/bt_plugins diff --git a/behaviour_library/README.md b/behaviour_library/README.md index a424bda..7139b8a 100644 --- a/behaviour_library/README.md +++ b/behaviour_library/README.md @@ -19,6 +19,19 @@ By default, this launches an example tree executor, which is currently the only ## Short descriptions of available behaviours -* `ImageSubBehaviour`: Subscribes to image messages. Saves a received image message on the `latest_image` output port. +### Publisher / subscriber behaviours + +* `ImageSubBehaviour`: Subscribes to `sensor_msgs/msg/Image` messages. Saves a received image message on the `latest_image` output port. +* `PointCloudSubBehaviour`: Subscribes to `sensor_msgs/msg/PointCloud2` messages. Saves a received image message on the `latest_point_cloud` output port. +* `PosePubBehaviour`: Publishes `geometry_msgs/msg/PoseStamped` messages. Reads the pose to be published from the `object_pose` input port. + +### Service interface behaviours + * `CheckVisibilityBehaviour`: Acts as a client of the [object visibility checking component](https://github.com/alex-mitrevski/robot-swiss-army-knife/tree/main/perception/object_visibility_checker). Thus, expects an image (input port `latest_image`) and target object categories (input port `object_categories`) to be available on the blackboard. Saves the result on the `visible_objects` output port. +* `SegmentObjectsBehaviour`: Acts as a client of the [object segmentation component](https://github.com/alex-mitrevski/robot-swiss-army-knife/tree/main/perception/semantic_segmentation). Thus, expects an image (input port `latest_image`) and target object categories (input port `object_categories`) to be available on the blackboard. Saves the result on the `segmented_objects` output port. +* `ExtractROI3DPointsBehaviour`: Acts as a client of the [ROI cloud extraction component](https://github.com/alex-mitrevski/robot-swiss-army-knife/tree/main/perception/point_cloud_utils). Thus, expects a point cloud (input port `latest_point_cloud`) and segmented objects as produced by the `SegmentObjectsBehaviour` (input port `segmented_objects`) to be available on the blackboard. Saves the result on the `object_clouds` output port. +* `CalculatePoseBehaviour`: Acts as a client of the [cloud-based pose calculation component](https://github.com/alex-mitrevski/robot-swiss-army-knife/tree/main/perception/point_cloud_utils). Expects object points clouds as produced by the `ExtractROI3DPointsBehaviour` (input port `object_clouds`) and the name of the object of interest (input port `object_of_interest`) to be available on the blackboard. Saves the result on the `calculated_pose` output port. If the name is not available in `object_clouds` directly, looks for an existing name that contains the passed name (e.g. if the value passed on `object_of_interest` is `cup`, but `object_clouds` has an object `cup_0`, it will calculate the pose of `cup_0`). + +### Skill behaviours + * `MoveBaseBehaviour`: Acts as a client of the [move base skill](https://github.com/alex-mitrevski/robot-swiss-army-knife/tree/main/skills/move_base_skill). Expects a skill goal type (input port `goal_type`) and navigation goals (input port `goal_poses` for goals specified as a list of `geometry_msgs/msg/PoseStamped` messages). Does not have any output ports. \ No newline at end of file diff --git a/behaviour_library/behaviour_trees/object-pose-estimation-tree.xml b/behaviour_library/behaviour_trees/object-pose-estimation-tree.xml new file mode 100644 index 0000000..16dfe1f --- /dev/null +++ b/behaviour_library/behaviour_trees/object-pose-estimation-tree.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behaviour_library/include/behaviour_library/calculate_pose_behaviour.hpp b/behaviour_library/include/behaviour_library/calculate_pose_behaviour.hpp new file mode 100644 index 0000000..51b75df --- /dev/null +++ b/behaviour_library/include/behaviour_library/calculate_pose_behaviour.hpp @@ -0,0 +1,53 @@ +#ifndef CALCULATE_POSE_BEHAVIOUR_HPP +#define CALCULATE_POSE_BEHAVIOUR_HPP + +#include +#include +#include +#include +#include +#include "robot_swiss_knife_msgs/srv/calculate_pose_from_cloud.hpp" + +using namespace BT; + +/** + * A behaviour for interacting with a service of type robot_swiss_knife_msgs::srv::CalculatePoseFromCloud. + * Based on the samples in https://github.com/BehaviorTree/BehaviorTree.ROS2 + * + * @author Alex Mitrevski + * @contact alemitr@chalmers.se + */ +class CalculatePoseBehaviour : public RosServiceNode +{ +public: + CalculatePoseBehaviour(const std::string& name, const NodeConfig& conf, const RosNodeParams& params); + + /** + * Sets the ports of the behaviour, which correspond to the service request and response: + * - Input ports: + * * object_clouds (std::map) + * * object_of_interest (std::string) + * - Output ports: + * * calculated_pose (geometry_msgs::msg::PoseStamped) + */ + static PortsList providedPorts(); + + /** + * Sets the service request based on the blackboard contents. + */ + virtual bool setRequest(Request::SharedPtr& request) override; + + /** + * Updates the blackboard based on the received response. + */ + virtual NodeStatus onResponseReceived(const Response::SharedPtr& response) override; + + /** + * Registers a node failure. + */ + virtual NodeStatus onFailure(ServiceNodeErrorCode error) override; +private: + RosNodeParams setCustomParams(RosNodeParams params); +}; + +#endif \ No newline at end of file diff --git a/behaviour_library/include/behaviour_library/check_visibility_behaviour.hpp b/behaviour_library/include/behaviour_library/check_visibility_behaviour.hpp index 27ab2b1..4b1835c 100644 --- a/behaviour_library/include/behaviour_library/check_visibility_behaviour.hpp +++ b/behaviour_library/include/behaviour_library/check_visibility_behaviour.hpp @@ -44,6 +44,8 @@ class CheckVisibilityBehaviour : public RosServiceNode +#include + +using namespace BT; + +/** + * A behaviour that subscribes to a point cloud and writes the result to the blackboard. + * Based on the samples in https://github.com/BehaviorTree/BehaviorTree.ROS2 + * + * @author Alex Mitrevski + * @contact alemitr@chalmers.se + */ +class PointCloudSubBehaviour : public RosTopicSubNode +{ +public: + PointCloudSubBehaviour(const std::string& name, const NodeConfig& conf, const RosNodeParams& params); + + /** + * Sets the ports of the behaviour: + * - Output ports: + * * latest_cloud (sensor_msgs::msg::PointCloud2) + */ + static PortsList providedPorts(); + + /** + * Updates the blackboard with the received message and returns SUCCESS. + * If no message has been received, returns FAILURE. + */ + virtual NodeStatus onTick(const std::shared_ptr& last_msg) override; +}; + +#endif \ No newline at end of file diff --git a/behaviour_library/include/behaviour_library/extract_roi_3d_points_behaviour.hpp b/behaviour_library/include/behaviour_library/extract_roi_3d_points_behaviour.hpp new file mode 100644 index 0000000..b1ae486 --- /dev/null +++ b/behaviour_library/include/behaviour_library/extract_roi_3d_points_behaviour.hpp @@ -0,0 +1,55 @@ +#ifndef EXTRACT_ROI_3D_POINTS_BEHAVIOUR_HPP +#define EXTRACT_ROI_3D_POINTS_BEHAVIOUR_HPP + +#include +#include +#include +#include +#include +#include "robot_swiss_knife_msgs/srv/extract_roi3_d_points.hpp" + +using namespace BT; + +/** + * A behaviour for interacting with a service of type robot_swiss_knife_msgs::srv::ExtractROI3DPoints. + * Based on the samples in https://github.com/BehaviorTree/BehaviorTree.ROS2 + * + * @author Alex Mitrevski + * @contact alemitr@chalmers.se + */ +class ExtractROI3DPointsBehaviour : public RosServiceNode +{ +public: + ExtractROI3DPointsBehaviour(const std::string& name, const NodeConfig& conf, const RosNodeParams& params); + + /** + * Sets the ports of the behaviour, which correspond to the service request and response: + * - Input ports: + * * latest_point_cloud (sensor_msgs::msg::PointCloud2) + * * segmented_objects (std::map) + * - Output ports: + * * object_clouds (std::map) + */ + static PortsList providedPorts(); + + /** + * Sets the service request based on the blackboard contents. + */ + virtual bool setRequest(Request::SharedPtr& request) override; + + /** + * Updates the blackboard based on the received response. + */ + virtual NodeStatus onResponseReceived(const Response::SharedPtr& response) override; + + /** + * Registers a node failure. + */ + virtual NodeStatus onFailure(ServiceNodeErrorCode error) override; +private: + RosNodeParams setCustomParams(RosNodeParams params); + + std::vector object_names; +}; + +#endif \ No newline at end of file diff --git a/behaviour_library/include/behaviour_library/pose_pub_behaviour.hpp b/behaviour_library/include/behaviour_library/pose_pub_behaviour.hpp new file mode 100644 index 0000000..8b4e2c3 --- /dev/null +++ b/behaviour_library/include/behaviour_library/pose_pub_behaviour.hpp @@ -0,0 +1,34 @@ +#ifndef POSE_PUB_BEHAVIOUR_HPP +#define POSE_PUB_BEHAVIOUR_HPP + +#include +#include + +using namespace BT; + +/** + * A behaviour that publishes a PoseStamped message that is saved on the blackboard. + * Based on the samples in https://github.com/BehaviorTree/BehaviorTree.ROS2 + * + * @author Alex Mitrevski + * @contact alemitr@chalmers.se + */ +class PosePubBehaviour : public RosTopicPubNode +{ +public: + PosePubBehaviour(const std::string& name, const NodeConfig& conf, const RosNodeParams& params); + + /** + * Sets the ports of the behaviour: + * - Input ports: + * * object_pose (geometry_msgs::msg::PoseStamped) + */ + static PortsList providedPorts(); + + /** + * Sets the message based on the blackboard contents. + */ + virtual bool setMessage(geometry_msgs::msg::PoseStamped& pose_stamped_msg) override; +}; + +#endif \ No newline at end of file diff --git a/behaviour_library/include/behaviour_library/segment_objects_behaviour.hpp b/behaviour_library/include/behaviour_library/segment_objects_behaviour.hpp new file mode 100644 index 0000000..96e83c3 --- /dev/null +++ b/behaviour_library/include/behaviour_library/segment_objects_behaviour.hpp @@ -0,0 +1,52 @@ +#ifndef SEGMENT_OBJECTS_BEHAVIOUR_HPP +#define SEGMENT_OBJECTS_BEHAVIOUR_HPP + +#include +#include +#include +#include +#include "robot_swiss_knife_msgs/srv/segment_objects.hpp" + +using namespace BT; + +/** + * A behaviour for interacting with a service of type robot_swiss_knife_msgs::srv::SegmentObjects. + * Based on the samples in https://github.com/BehaviorTree/BehaviorTree.ROS2 + * + * @author Alex Mitrevski + * @contact alemitr@chalmers.se + */ +class SegmentObjectsBehaviour : public RosServiceNode +{ +public: + SegmentObjectsBehaviour(const std::string& name, const NodeConfig& conf, const RosNodeParams& params); + + /** + * Sets the ports of the behaviour, which correspond to the service request and response: + * - Input ports: + * * latest_image (sensor_msgs::msg::Image) + * * object_categories (std::vector) + * - Output ports: + * * segmented_objects (std::map) + */ + static PortsList providedPorts(); + + /** + * Sets the service request based on the blackboard contents. + */ + virtual bool setRequest(Request::SharedPtr& request) override; + + /** + * Updates the blackboard based on the received response. + */ + virtual NodeStatus onResponseReceived(const Response::SharedPtr& response) override; + + /** + * Registers a node failure. + */ + virtual NodeStatus onFailure(ServiceNodeErrorCode error) override; +private: + RosNodeParams setCustomParams(RosNodeParams params); +}; + +#endif \ No newline at end of file diff --git a/behaviour_library/src/calculate_pose_behaviour.cpp b/behaviour_library/src/calculate_pose_behaviour.cpp new file mode 100644 index 0000000..1744816 --- /dev/null +++ b/behaviour_library/src/calculate_pose_behaviour.cpp @@ -0,0 +1,71 @@ +#include "behaviour_library/calculate_pose_behaviour.hpp" +#include + +CalculatePoseBehaviour::CalculatePoseBehaviour(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) +: RosServiceNode(name, conf, params) +{} + +PortsList CalculatePoseBehaviour::providedPorts() +{ + return providedBasicPorts({ + InputPort>("object_clouds"), + InputPort("object_of_interest"), + OutputPort("calculated_pose") + }); +} + +bool CalculatePoseBehaviour::setRequest(Request::SharedPtr& request) +{ + std::map object_clouds; + this->getInput("object_clouds", object_clouds); + + std::string object_of_interest; + this->getInput("object_of_interest", object_of_interest); + + bool object_found = false; + if (object_clouds.find(object_of_interest) != object_clouds.end()) + { + request->point_cloud = object_clouds[object_of_interest]; + object_found = true; + } + else + { + RCLCPP_ERROR(this->logger(), "Received unknown object %s", object_of_interest.c_str()); + for (const auto &kv : object_clouds) + { + if (kv.first.find(object_of_interest) != std::string::npos) + { + RCLCPP_INFO(this->logger(), "Calculating pose of known object %s", kv.first.c_str()); + request->point_cloud = kv.second; + object_found = true; + break; + } + } + } + + return object_found; +} + +NodeStatus CalculatePoseBehaviour::onResponseReceived(const Response::SharedPtr& response) +{ + if (response->calculation_successful) + { + this->setOutput("calculated_pose", response->pose); + return NodeStatus::SUCCESS; + } + return NodeStatus::FAILURE; +} + +NodeStatus CalculatePoseBehaviour::onFailure(ServiceNodeErrorCode error) +{ + RCLCPP_ERROR(this->logger(), "Error: %d", error); + return NodeStatus::FAILURE; +} + +RosNodeParams CalculatePoseBehaviour::setCustomParams(RosNodeParams params) +{ + params.server_timeout = std::chrono::milliseconds(10000); + return params; +} + +CreateRosNodePlugin(CalculatePoseBehaviour, "CalculatePoseBehaviour"); \ No newline at end of file diff --git a/behaviour_library/src/check_visibility_behaviour.cpp b/behaviour_library/src/check_visibility_behaviour.cpp index 94aeeb7..0066e30 100644 --- a/behaviour_library/src/check_visibility_behaviour.cpp +++ b/behaviour_library/src/check_visibility_behaviour.cpp @@ -23,8 +23,24 @@ bool CheckVisibilityBehaviour::setRequest(Request::SharedPtr& request) NodeStatus CheckVisibilityBehaviour::onResponseReceived(const Response::SharedPtr& response) { - this->setOutput("visible_objects", response->objects_visible); - return NodeStatus::SUCCESS; + std::vector object_categories; + this->getInput("object_categories", object_categories); + + std::vector visible_objects; + for (unsigned int i=0; iobjects_visible[i]) + { + visible_objects.push_back(object_categories[i]); + } + } + this->setOutput("visible_objects", visible_objects); + + if (visible_objects.size() > 0) + { + return NodeStatus::SUCCESS; + } + return NodeStatus::FAILURE; } NodeStatus CheckVisibilityBehaviour::onFailure(ServiceNodeErrorCode error) @@ -33,4 +49,10 @@ NodeStatus CheckVisibilityBehaviour::onFailure(ServiceNodeErrorCode error) return NodeStatus::FAILURE; } +RosNodeParams CheckVisibilityBehaviour::setCustomParams(RosNodeParams params) +{ + params.server_timeout = std::chrono::milliseconds(10000); + return params; +} + CreateRosNodePlugin(CheckVisibilityBehaviour, "CheckVisibilityBehaviour"); \ No newline at end of file diff --git a/behaviour_library/src/cloud_sub_behaviour.cpp b/behaviour_library/src/cloud_sub_behaviour.cpp new file mode 100644 index 0000000..b64dc14 --- /dev/null +++ b/behaviour_library/src/cloud_sub_behaviour.cpp @@ -0,0 +1,25 @@ +#include "behaviour_library/cloud_sub_behaviour.hpp" +#include + +PointCloudSubBehaviour::PointCloudSubBehaviour(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) +: RosTopicSubNode(name, conf, params, rclcpp::SensorDataQoS()) +{} + +PortsList PointCloudSubBehaviour::providedPorts() +{ + return providedBasicPorts({ + OutputPort("latest_point_cloud") + }); +} + +NodeStatus PointCloudSubBehaviour::onTick(const std::shared_ptr& last_msg) +{ + if(last_msg) + { + this->setOutput("latest_point_cloud", *last_msg); + return NodeStatus::SUCCESS; + } + return NodeStatus::FAILURE; +} + +CreateRosNodePlugin(PointCloudSubBehaviour, "PointCloudSubBehaviour"); \ No newline at end of file diff --git a/behaviour_library/src/extract_roi_3d_points_behaviour.cpp b/behaviour_library/src/extract_roi_3d_points_behaviour.cpp new file mode 100644 index 0000000..2369b6d --- /dev/null +++ b/behaviour_library/src/extract_roi_3d_points_behaviour.cpp @@ -0,0 +1,57 @@ +#include "behaviour_library/extract_roi_3d_points_behaviour.hpp" +#include + +ExtractROI3DPointsBehaviour::ExtractROI3DPointsBehaviour(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) +: RosServiceNode(name, conf, params) +{} + +PortsList ExtractROI3DPointsBehaviour::providedPorts() +{ + return providedBasicPorts({ + InputPort("latest_point_cloud"), + InputPort>("segmented_objects"), + OutputPort>("object_clouds") + }); +} + +bool ExtractROI3DPointsBehaviour::setRequest(Request::SharedPtr& request) +{ + this->getInput("latest_point_cloud", request->point_cloud); + + std::map segmented_objects; + this->getInput("segmented_objects", segmented_objects); + + this->object_names.clear(); + for (const auto &segmented_object_data : segmented_objects) + { + this->object_names.push_back(segmented_object_data.first); + request->object_masks.push_back(segmented_object_data.second); + } + + return true; +} + +NodeStatus ExtractROI3DPointsBehaviour::onResponseReceived(const Response::SharedPtr& response) +{ + std::map roi_output; + for (unsigned int i=0; iobject_clouds.size(); i++) + { + roi_output[this->object_names[i]] = response->object_clouds[i]; + } + this->setOutput("object_clouds", roi_output); + return NodeStatus::SUCCESS; +} + +NodeStatus ExtractROI3DPointsBehaviour::onFailure(ServiceNodeErrorCode error) +{ + RCLCPP_ERROR(this->logger(), "Error: %d", error); + return NodeStatus::FAILURE; +} + +RosNodeParams ExtractROI3DPointsBehaviour::setCustomParams(RosNodeParams params) +{ + params.server_timeout = std::chrono::milliseconds(10000); + return params; +} + +CreateRosNodePlugin(ExtractROI3DPointsBehaviour, "ExtractROI3DPointsBehaviour"); \ No newline at end of file diff --git a/behaviour_library/src/image_sub_behaviour.cpp b/behaviour_library/src/image_sub_behaviour.cpp index c6fb71f..c11ed6b 100644 --- a/behaviour_library/src/image_sub_behaviour.cpp +++ b/behaviour_library/src/image_sub_behaviour.cpp @@ -2,13 +2,13 @@ #include ImageSubBehaviour::ImageSubBehaviour(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) -: RosTopicSubNode(name, conf, params) +: RosTopicSubNode(name, conf, params, rclcpp::SensorDataQoS()) {} PortsList ImageSubBehaviour::providedPorts() { return providedBasicPorts({ - OutputPort>("latest_image") + OutputPort("latest_image") }); } diff --git a/behaviour_library/src/pose_pub_behaviour.cpp b/behaviour_library/src/pose_pub_behaviour.cpp new file mode 100644 index 0000000..75f01fb --- /dev/null +++ b/behaviour_library/src/pose_pub_behaviour.cpp @@ -0,0 +1,21 @@ +#include "behaviour_library/pose_pub_behaviour.hpp" +#include + +PosePubBehaviour::PosePubBehaviour(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) +: RosTopicPubNode(name, conf, params) +{} + +PortsList PosePubBehaviour::providedPorts() +{ + return providedBasicPorts({ + InputPort("object_pose") + }); +} + +bool PosePubBehaviour::setMessage(geometry_msgs::msg::PoseStamped& pose_stamped_msg) +{ + this->getInput("object_pose", pose_stamped_msg); + return true; +} + +CreateRosNodePlugin(PosePubBehaviour, "PosePubBehaviour"); \ No newline at end of file diff --git a/behaviour_library/src/segment_objects_behaviour.cpp b/behaviour_library/src/segment_objects_behaviour.cpp new file mode 100644 index 0000000..efdc7ea --- /dev/null +++ b/behaviour_library/src/segment_objects_behaviour.cpp @@ -0,0 +1,62 @@ +#include "behaviour_library/segment_objects_behaviour.hpp" +#include + +SegmentObjectsBehaviour::SegmentObjectsBehaviour(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) +: RosServiceNode(name, conf, setCustomParams(params)) +{} + +PortsList SegmentObjectsBehaviour::providedPorts() +{ + return providedBasicPorts({ + InputPort("latest_image"), + InputPort>("object_categories"), + OutputPort>("segmented_objects") + }); +} + +bool SegmentObjectsBehaviour::setRequest(Request::SharedPtr& request) +{ + this->getInput("latest_image", request->image); + this->getInput("object_categories", request->object_categories); + return true; +} + +NodeStatus SegmentObjectsBehaviour::onResponseReceived(const Response::SharedPtr& response) +{ + if (response->mask_categories.size() == 0) + { + return NodeStatus::FAILURE; + } + + std::map segmentation_output; + std::map category_obj_counters; + for (unsigned int i=0; imask_categories.size(); i++) + { + if (category_obj_counters.find(response->mask_categories[i]) == category_obj_counters.end()) + { + category_obj_counters[response->mask_categories[i]] = 0; + } + else + { + category_obj_counters[response->mask_categories[i]] += 1; + } + std::string obj_name = response->mask_categories[i] + "_" + std::to_string(category_obj_counters[response->mask_categories[i]]); + segmentation_output[obj_name] = response->masks[i]; + } + this->setOutput("segmented_objects", segmentation_output); + return NodeStatus::SUCCESS; +} + +NodeStatus SegmentObjectsBehaviour::onFailure(ServiceNodeErrorCode error) +{ + RCLCPP_ERROR(this->logger(), "Error: %d", error); + return NodeStatus::FAILURE; +} + +RosNodeParams SegmentObjectsBehaviour::setCustomParams(RosNodeParams params) +{ + params.server_timeout = std::chrono::milliseconds(10000); + return params; +} + +CreateRosNodePlugin(SegmentObjectsBehaviour, "SegmentObjectsBehaviour"); \ No newline at end of file