add perception filter

rbs_perception/include/rbs_perception/perception_filter.hpp

@@ -0,0 +1,53 @@
+#include <rclcpp/rclcpp.hpp>
+#include <tf2/transform_datatypes.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include "tf2_ros/static_transform_broadcaster.h"
+
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/transforms.hpp>
+#include <pcl/filters/voxel_grid.h>
+
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+
+namespace rbs_perception
+{
+    class PCFilter : public rclcpp::Node
+    {
+        public:
+            explicit PCFilter();
+        private:
+            // Publish
+            void pub_callback(rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher,
+                               pcl::PointCloud<pcl::PointXYZ> point_cloud);
+            void make_transforms(std::string child);
+            rclcpp::TimerBase::SharedPtr timer_;
+            rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
+            size_t count_;
+            // Sub
+            void sub_callback(const sensor_msgs::msg::PointCloud2 msg);
+            rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscriber_;
+            geometry_msgs::msg::TransformStamped standform;
+            pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
+            std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+            std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
+            std::unique_ptr<tf2_ros::TransformBroadcaster> br;
+            std::string world_frame = "world";
+            std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
+    };
+}
+
+int main(int argc, char *argv[])
+{
+    /*
+     * INITIALIZE ROS NODE
+     */
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<rbs_perception::PCFilter>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}
+

rbs_perception/src/perception_filter.cpp

@@ -0,0 +1,68 @@
+#include "rbs_perception/perception_filter.hpp"
+
+
+rbs_perception::PCFilter::PCFilter() : Node("pc_filter", rclcpp::NodeOptions())
+{
+    publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("rgbd_camera/filtered_points", 1);
+    subscriber_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
+        "rgbd_camera/points", 1, std::bind(&PCFilter::sub_callback, this, std::placeholders::_1)
+    );
+    tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+    //this->make_transforms("ur5e/wrist_3_link/rgbd_camera");
+}
+
+void rbs_perception::PCFilter::sub_callback(const sensor_msgs::msg::PointCloud2 msg)
+{
+    try
+    {
+        standform = tf_buffer_->lookupTransform(msg.header.frame_id, world_frame,
+                                                    tf2::TimePointZero, tf2::durationFromSec(3));
+
+    }
+    catch (const tf2::TransformException & ex)
+    {
+        RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
+    }
+    sensor_msgs::msg::PointCloud2 transformed_cloud;
+    pcl_ros::transformPointCloud(world_frame, standform, msg, transformed_cloud);
+    pcl::PointCloud<pcl::PointXYZ> cloud;
+    pcl::fromROSMsg(transformed_cloud, cloud);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>(cloud));
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxel_filtered(new pcl::PointCloud<pcl::PointXYZ>());
+    voxel_filter.setInputCloud(cloud_ptr);
+    voxel_filter.setLeafSize(0.02, 0.02, 0.02);
+    voxel_filter.filter(*cloud_voxel_filtered);
+    pub_callback(publisher_, *cloud_voxel_filtered);
+}
+
+void rbs_perception::PCFilter::pub_callback(rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher,
+                               pcl::PointCloud<pcl::PointXYZ> point_cloud)
+{
+    sensor_msgs::msg::PointCloud2::SharedPtr pc2_cloud(new sensor_msgs::msg::PointCloud2);
+    pcl::toROSMsg(point_cloud, *pc2_cloud);
+    pc2_cloud->header.frame_id = world_frame;
+    pc2_cloud->header.stamp = this->get_clock()->now();
+    publisher->publish(*pc2_cloud);
+}
+
+void rbs_perception::PCFilter::make_transforms(std::string child)
+{
+    geometry_msgs::msg::TransformStamped t;
+
+    t.header.stamp = this->get_clock()->now();
+    t.header.frame_id = "world";
+    t.child_frame_id = child;
+
+    t.transform.translation.x = -1.66893e-10;
+    t.transform.translation.y = 0.3029;
+    t.transform.translation.z = 1.1294;
+    t.transform.rotation.x = -0.000281656;
+    t.transform.rotation.y = -0.000281432;
+    t.transform.rotation.z = 0.706825;
+    t.transform.rotation.w = 0.707388;
+
+    tf_static_broadcaster_->sendTransform(t);
+}
+
+