just commit

asd

src/er1_driver_node/main.cpp

@@ -72,7 +72,8 @@ int main(int argc, char** argv) {
     ros::Subscriber sub = node.subscribe("er1_motor_commands", 1000, &callback);
 
     // Connect to robot. You have to secify which robot you are using!
-	EvolutionRobotID id = EvolutionRobotID::ER1Small;
+
+	EvolutionRobotID id = EvolutionRobotID::ER1Medium;
     robot.connect(id);
 
     // Enter ROS main loop.

src/point_processing_node/main.cpp

@@ -3,47 +3,67 @@
 #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>
 
-int main(int argc, char** argv){
-    // Init ROS stuff
-    ros::init(argc, argv, "crs_tf_publisher");
 
-    // Create Transform Broadcaster
-    tf::TransformBroadcaster br;
+// Less typing
+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;
+
+
+// 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());
+
+
+    // Init voxelgrid filter and apply
+    pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
+    sor.setInputCloud(msgPtr);
+    sor.setLeafSize(0.01f, 0.01f, 0.01f);
+    auto filteredPtr = boost::make_shared<CloudRGBA>();
+    sor.filter(*filteredPtr);
+
 
-    // Init the clock helper
-    ClockHelper helper;
-    helper.init();
-    helper.setCycleTime(std::chrono::seconds(3));
+    // 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 an update rate
-    ros::Rate rate(3);
+    // Publish cloud
+    publisher.publish(filteredPtr);
 
-    // Main loop
-    while(ros::ok()) {
-        // Get the roll value for the current time point
-        auto roll = helper.getRoll();
+    // 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();
 
-        // Specify the origin for our transformation
-        auto origin = tf::Vector3(0, 0, 0);
+    // Init subscriber and set topic
+    subscriber = node.subscribe<CloudRGBA>("/kinect2/sd/points", 1, callback);
 
-        // Specify the rotation for our transformation
-        tf::Quaternion q;
-        q.setRPY(0, 0, roll);
+    // Init publisher and set topic
 
-        // Bundle all information into a transformation object
-        tf::Transform transform;
-        transform.setOrigin(origin);
-        transform.setRotation(q);
+    publisher = node.advertise<CloudRGBA>("/crs/filtered_vg", 5);
 
-        // Publish the transformation
-        std::string destinationFrame = "map";
-        std::string sourceFrame = "kinect2_ir_optical_frame";
-        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), destinationFrame, sourceFrame));
 
-        // 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!
@@ -24,20 +25,23 @@ void callback(CloudRGBA::ConstPtr msgPtr) {
     ROS_INFO("Received a cloud with %i points", msgPtr->size());
 
 
-    // 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
-    publisher.publish(filteredPtr);
+
 
     // Print some information
     ROS_INFO("Published a cloud with %i points", filteredPtr->size());
@@ -45,17 +49,28 @@ void callback(CloudRGBA::ConstPtr msgPtr) {
 
 int main(int argc, char** argv){
     // Init ROS stuff
-    ros::init(argc, argv, "crs_tf_publisher");
+    ros::init(argc, argv, "crs_cloud_processor");
     auto node = ros::NodeHandle();
 
+
+    /* ######### <MISSING> ########## */
+    /* ######### <MISSING> ########## */
+
+    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
+};