New behaviours + pose estimation behaviour tree

behaviour_library/CMakeLists.txt

@@ -15,13 +15,15 @@ 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)
 
 set(THIS_PACKAGE_DEPS
     behaviortree_ros2::behaviortree_ros2
     ${btcpp_ros2_interfaces_TARGETS}
+    ${geometry_msgs_TARGETS}
     ${sensor_msgs_TARGETS}
     ${robot_swiss_knife_msgs_TARGETS}
 )
@@ -35,13 +37,48 @@ 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
 ###########################################
 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,15 +94,25 @@ 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}
 )
 
 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

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.

behaviour_library/behaviour_trees/object-pose-estimation-tree.xml

@@ -0,0 +1,17 @@
+<root BTCPP_format="4">
+    <BehaviorTree ID="ObjectPoseEstimation">
+        <Sequence>
+            <RetryUntilSuccessful num_attempts="-1">
+                <ImageSubBehaviour name="image_subscriber" topic_name="/head_front_camera/rgb/image_raw" latest_image="{latest_image}" />
+            </RetryUntilSuccessful>
+            <CheckVisibilityBehaviour name="visibility_checker" service_name="check_objects_visible" latest_image="{latest_image}" object_categories="person" visible_objects="{visible_objects}" />
+            <SegmentObjectsBehaviour name="object_segmenter" service_name="segment_objects" latest_image="{latest_image}" object_categories="person" segmented_objects="{segmented_objects}" />
+            <RetryUntilSuccessful num_attempts="-1">
+                <PointCloudSubBehaviour name="cloud_subscriber" topic_name="/head_front_camera/depth/rgb/points" latest_point_cloud="{latest_point_cloud}" />
+            </RetryUntilSuccessful>
+            <ExtractROI3DPointsBehaviour name="roi_extractor" service_name="extract_3d_rois" latest_point_cloud="{latest_point_cloud}" segmented_objects="{segmented_objects}" object_clouds="{object_clouds}" />
+            <CalculatePoseBehaviour name="pose_calculator" service_name="calculate_pose_from_cloud" object_clouds="{object_clouds}" object_of_interest="person" calculated_pose="{calculated_pose}" />
+            <PosePubBehaviour name="pose_publisher" topic_name="object_pose" object_pose="{calculated_pose}" />
+        </Sequence>
+    </BehaviorTree>
+</root>

behaviour_library/include/behaviour_library/calculate_pose_behaviour.hpp

@@ -0,0 +1,53 @@
+#ifndef CALCULATE_POSE_BEHAVIOUR_HPP
+#define CALCULATE_POSE_BEHAVIOUR_HPP
+
+#include <vector>
+#include <map>
+#include <behaviortree_ros2/bt_service_node.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#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<robot_swiss_knife_msgs::srv::CalculatePoseFromCloud>
+{
+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<std::string, sensor_msgs::msg::PointCloud2>)
+     *     * 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

behaviour_library/include/behaviour_library/check_visibility_behaviour.hpp

@@ -44,6 +44,8 @@ class CheckVisibilityBehaviour : public RosServiceNode<robot_swiss_knife_msgs::s
      * Registers a node failure.
      */
     virtual NodeStatus onFailure(ServiceNodeErrorCode error) override;
+private:
+    RosNodeParams setCustomParams(RosNodeParams params);
 };
 
 #endif

behaviour_library/include/behaviour_library/cloud_sub_behaviour.hpp

@@ -0,0 +1,35 @@
+#ifndef CLOUD_SUB_BEHAVIOUR_HPP
+#define CLOUD_SUB_BEHAVIOUR_HPP
+
+#include <behaviortree_ros2/bt_topic_sub_node.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+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<sensor_msgs::msg::PointCloud2>
+{
+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<sensor_msgs::msg::PointCloud2>& last_msg) override;
+};
+
+#endif

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 <vector>
+#include <map>
+#include <behaviortree_ros2/bt_service_node.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#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<robot_swiss_knife_msgs::srv::ExtractROI3DPoints>
+{
+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<std::string, sensor_msgs::msg::Image>)
+     * - Output ports:
+     *     * object_clouds (std::map<std::string, sensor_msgs::msg::PointCloud2>)
+     */
+    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<std::string> object_names;
+};
+
+#endif

behaviour_library/include/behaviour_library/pose_pub_behaviour.hpp

@@ -0,0 +1,34 @@
+#ifndef POSE_PUB_BEHAVIOUR_HPP
+#define POSE_PUB_BEHAVIOUR_HPP
+
+#include <behaviortree_ros2/bt_topic_pub_node.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+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<geometry_msgs::msg::PoseStamped>
+{
+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

behaviour_library/include/behaviour_library/segment_objects_behaviour.hpp

@@ -0,0 +1,52 @@
+#ifndef SEGMENT_OBJECTS_BEHAVIOUR_HPP
+#define SEGMENT_OBJECTS_BEHAVIOUR_HPP
+
+#include <vector>
+#include <map>
+#include <behaviortree_ros2/bt_service_node.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#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<robot_swiss_knife_msgs::srv::SegmentObjects>
+{
+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<std::string>)
+     * - Output ports:
+     *     * segmented_objects (std::map<std::string, sensor_msgs::msg::Image>)
+     */
+    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