asd

src/point_processing_node/main.cpp

@@ -3,47 +3,59 @@
 #include <geometry_msgs/Pose.h>
 #include <chrono>
 #include <comon/clockhelper.hpp>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/statistical_outlier_removal.h>
 
-int main(int argc, char** argv){
-    // Init ROS stuff
-    ros::init(argc, argv, "crs_tf_publisher");
+// Less typing. We are using a pointcloud that contains XYZ and addtionally RGBA data.
+using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;
+
+// We are going to publish clouds ... Therefore we need a publisher!
+ros::Publisher publisher;
+
+// We are going to receive clouds ... Therefore we need a subscriber!
+ros::Subscriber subscriber;
 
-    // Create Transform Broadcaster
-    tf::TransformBroadcaster br;
 
-    // Init the clock helper
-    ClockHelper helper;
-    helper.init();
-    helper.setCycleTime(std::chrono::seconds(3));
+// The callback function. It gets called whenever we receive a new point loucd.
+void callback(CloudRGBA::ConstPtr msgPtr) {
+    // Print some information
+    ROS_INFO("Received a cloud with %i points", msgPtr->size());
 
-    // Specify an update rate
-    ros::Rate rate(3);
 
-    // Main loop
-    while(ros::ok()) {
-        // Get the roll value for the current time point
-        auto roll = helper.getRoll();
+    // Init voxelgrid filter and apply
+    pcl::VoxelGrid<pcl::PointXYZRGBA> voxelFilter;
+    voxelFilter.setInputCloud (msgPtr);
+    voxelFilter.setLeafSize (0.01f, 0.01f, 0.01f);
+    auto filteredPtr = boost::make_shared<CloudRGBA>();
+    voxelFilter.filter(*filteredPtr);
 
-        // Specify the origin for our transformation
-        auto origin = tf::Vector3(0, 0, 0);
+    // Set the header of the message. Just some shabby details ...
+    filteredPtr->header.frame_id = "filtered_frame";
+    pcl_conversions::toPCL(ros::Time::now(), filteredPtr->header.stamp);
 
-        // Specify the rotation for our transformation
-        tf::Quaternion q;
-        q.setRPY(0, 0, roll);
+    // Publish cloud
+    publisher.publish(filteredPtr);
+
+    // Print some information
+    ROS_INFO("Published a cloud with %i points", filteredPtr->size());
+}
+
+int main(int argc, char** argv){
+    // Init ROS stuff
+    ros::init(argc, argv, "crs_tf_publisher");
+    auto node = ros::NodeHandle();
 
-        // Bundle all information into a transformation object
-        tf::Transform transform;
-        transform.setOrigin(origin);
-        transform.setRotation(q);
+    // Init subscriber and set topic
+    subscriber = node.subscribe<CloudRGBA>("/kinect2/sd/points", 1, callback);
 
-        // Publish the transformation
-        std::string destinationFrame = "map";
-        std::string sourceFrame = "kinect2_ir_optical_frame";
-        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), destinationFrame, sourceFrame));
+    // Init publisher and set topic
+    publisher = node.advertise<CloudRGBA>("/crs/points", 5);
 
-        // Sleep a little bit
-        rate.sleep();
-    }
+    // Run!
+    ros::spin();
 
     return 0;
-};
\ No newline at end of file
+};

src/point_processing_node_student/main.cpp

@@ -7,8 +7,9 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/statistical_outlier_removal.h>
 
-// Less typing
+// Less typing. We are using a pointcloud that contains XYZ and addtionally RGBA data.
 using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;
 
 // We are going to publish clouds ... Therefore we need a publisher!
@@ -25,15 +26,17 @@ void callback(CloudRGBA::ConstPtr msgPtr) {
 
 
     // Init voxelgrid filter and apply
-    pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
-    sor.setInputCloud (msgPtr);
-    sor.setLeafSize (0.01f, 0.01f, 0.01f);
+    pcl::VoxelGrid<pcl::PointXYZRGBA> voxelFilter;
+    voxelFilter.setInputCloud (msgPtr);
+
+    /* ######### <MISSING> ########## */
+
     auto filteredPtr = boost::make_shared<CloudRGBA>();
-    sor.filter(*filteredPtr);
+    voxelFilter.filter(*filteredPtr);
 
 
     // Set the header of the message. Just some shabby details ...
-    filteredPtr->header.frame_id = "filtered_frame";
+    filteredPtr->header.frame_id = ""; /* ######### <MISSING> ########## */
     pcl_conversions::toPCL(ros::Time::now(), filteredPtr->header.stamp);
 
     // Publish cloud
@@ -48,14 +51,20 @@ int main(int argc, char** argv){
     ros::init(argc, argv, "crs_tf_publisher");
     auto node = ros::NodeHandle();
 
+    std::string subscribeTopic;
+    /* ######### <MISSING> ########## */
+
+    std::string advertiseTopic;
+    /* ######### <MISSING> ########## */
+
+
     // Init subscriber and set topic
-    subscriber = node.subscribe<CloudRGBA>("/kinect2/sd/points", 1, callback);
+    subscriber = node.subscribe<CloudRGBA>(subscribeTopic, 1, callback);
 
     // Init publisher and set topic
-    publisher = node.advertise<CloudRGBA>("/crs/points", 5);
+    publisher = node.advertise<CloudRGBA>(advertiseTopic, 5);
 
-    // Run!
-    ros::spin();
+    /* ######### <MISSING> ########## */
 
     return 0;
-};
\ No newline at end of file
+};

src/tf_publisher_node_student/main.cpp

@@ -36,8 +36,6 @@ int main(int argc, char** argv){
         /* ######### <MISSING> ########## */
         /* ######### <MISSING> ########## */
 
-
-
         // Publish the transformation
         std::string destinationFrame;
         /* ######### <MISSING> ########## */
@@ -52,4 +50,4 @@ int main(int argc, char** argv){
     }
 
     return 0;
-};
\ No newline at end of file
+};