Merge branch 'master' into testbranch

src/er1_driver_node/main.cpp

@@ -72,7 +72,7 @@ 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::ER1Medium;
+	EvolutionRobotID id = EvolutionRobotID::ER1Small;
     robot.connect(id);
 
     // Enter ROS main loop.

src/point_processing_node/main.cpp

@@ -3,59 +3,47 @@
 #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>
-
-// 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());
 
+int main(int argc, char** argv){
+    // Init ROS stuff
+    ros::init(argc, argv, "crs_tf_publisher");
 
-    // 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);
+    // Create Transform Broadcaster
+    tf::TransformBroadcaster br;
 
+    // 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());
-}
+        // Specify the origin for our transformation
+        auto origin = tf::Vector3(0, 0, 0);
 
-int main(int argc, char** argv){
-    // Init ROS stuff
-    ros::init(argc, argv, "crs_tf_publisher");
-    auto node = ros::NodeHandle();
+        // Specify the rotation for our transformation
+        tf::Quaternion q;
+        q.setRPY(0, 0, roll);
 
-    // Init subscriber and set topic
-    subscriber = node.subscribe<CloudRGBA>("/kinect2/sd/points", 1, callback);
+        // Bundle all information into a transformation object
+        tf::Transform transform;
+        transform.setOrigin(origin);
+        transform.setRotation(q);
 
-    // Init publisher and set topic
-    publisher = node.advertise<CloudRGBA>("/crs/points", 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));
 
-    // Run!
-    ros::spin();
+        // Sleep a little bit
+        rate.sleep();
+    }
 
     return 0;
 };
\ No newline at end of file

src/point_processing_node_student/main.cpp

@@ -24,20 +24,20 @@ void callback(CloudRGBA::ConstPtr msgPtr) {
     ROS_INFO("Received a cloud with %i points", msgPtr->size());
 
 
-    auto filteredPtr = boost::make_shared<CloudRGBA>();
-
     // Init voxelgrid filter and apply
     pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
-    /* ######### <MISSING> ########## */
-    /* ######### <MISSING> ########## */
-    /* ######### <MISSING> ########## */
+    sor.setInputCloud (msgPtr);
+    sor.setLeafSize (0.01f, 0.01f, 0.01f);
+    auto filteredPtr = boost::make_shared<CloudRGBA>();
+    sor.filter(*filteredPtr);
 
 
     // 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);
 
-
+    // Publish cloud
+    publisher.publish(filteredPtr);
 
     // Print some information
     ROS_INFO("Published a cloud with %i points", filteredPtr->size());
@@ -45,11 +45,14 @@ void callback(CloudRGBA::ConstPtr msgPtr) {
 
 int main(int argc, char** argv){
     // Init ROS stuff
-    ros::init(argc, argv, "crs_cloud_processor");
+    ros::init(argc, argv, "crs_tf_publisher");
     auto node = ros::NodeHandle();
 
-    /* ######### <MISSING> ########## */
-    /* ######### <MISSING> ########## */
+    // Init subscriber and set topic
+    subscriber = node.subscribe<CloudRGBA>("/kinect2/sd/points", 1, callback);
+
+    // Init publisher and set topic
+    publisher = node.advertise<CloudRGBA>("/crs/points", 5);
 
     // Run!
     ros::spin();

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
+};