From 46d56b32979dfc51296dc1caa5ccccbc2cc2fbdc Mon Sep 17 00:00:00 2001
From: Louis-Guillaume DUBOIS <contact@lgdubois.fr>
Date: Thu, 23 Apr 2015 19:35:16 +0200
Subject: [PATCH] corrige r g b dans random_pcl_publisher & pcl_displayer

---
 hand_control/CMakeLists.txt               |  5 +++
 hand_control/launch/essai-filtre.launch   |  4 +-
 hand_control/src/filtre.cpp               | 11 ------
 hand_control/src/pcl_displayer.cpp        | 48 +++++++++++++++++++++++
 hand_control/src/random_pcl_publisher.cpp | 16 +++-----
 5 files changed, 61 insertions(+), 23 deletions(-)
 create mode 100644 hand_control/src/pcl_displayer.cpp

diff --git a/hand_control/CMakeLists.txt b/hand_control/CMakeLists.txt
index 49d245f..8e9baba 100644
--- a/hand_control/CMakeLists.txt
+++ b/hand_control/CMakeLists.txt
@@ -1,6 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(hand_control)
 
+#set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -116,6 +117,10 @@ add_dependencies(filtre ${catkin_EXPORTED_TARGETS})
 add_executable(random_pcl_publisher src/random_pcl_publisher.cpp)
 target_link_libraries(random_pcl_publisher ${catkin_LIBRARIES})
 add_dependencies(random_pcl_publisher ${catkin_EXPORTED_TARGETS})
+
+add_executable(pcl_displayer src/pcl_displayer.cpp)
+target_link_libraries(pcl_displayer ${catkin_LIBRARIES})
+add_dependencies(pcl_displayer ${catkin_EXPORTED_TARGETS})
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
 # add_dependencies(hand_control_node hand_control_generate_messages_cpp)
diff --git a/hand_control/launch/essai-filtre.launch b/hand_control/launch/essai-filtre.launch
index 280b9b4..8a3c7ff 100644
--- a/hand_control/launch/essai-filtre.launch
+++ b/hand_control/launch/essai-filtre.launch
@@ -1,12 +1,12 @@
 <launch>
 
   <node name="filtre_z" pkg="hand_control" type="filtre">
-    <remap from="/filtre/input" to="random_pcl"/>
+    <remap from="/filtre/input" to="/random_pcl"/>
   </node>
   <param name="/filtre/zmax" value="100." type="double" />
 
   <node name="publisher" pkg="hand_control" type="random_pcl_publisher" >
-    <remap from="/random/output" to="random_pcl"/>
+    <remap from="/random/output" to="/random_pcl"/>
   </node>
   <param name="/random/freq" value="1." type="double" />
   <param name="/random/length" value="5" type="int" />
diff --git a/hand_control/src/filtre.cpp b/hand_control/src/filtre.cpp
index c1bff2a..9f77ba0 100644
--- a/hand_control/src/filtre.cpp
+++ b/hand_control/src/filtre.cpp
@@ -25,17 +25,6 @@ class Callback {
       // publication
       publisher.publish(pcl);
       ROS_INFO("PointCloud published");
-      ROS_INFO("filtered cloud :");
-      for(int i = 0; i < pcl->points.size(); ++i)
-      {
-        ROS_INFO("\nx : %f\ny : %f\nz : %f\nr : %d\ng : %d\nb : %d",
-                pcl->points[i].x,
-                pcl->points[i].y,
-                pcl->points[i].z,
-                pcl->points[i].r,
-                pcl->points[i].g,
-                pcl->points[i].b);
-      }
     }
 
     Callback(ros::Publisher& pub) : publisher(pub), z_filtre()
diff --git a/hand_control/src/pcl_displayer.cpp b/hand_control/src/pcl_displayer.cpp
new file mode 100644
index 0000000..ae9d134
--- /dev/null
+++ b/hand_control/src/pcl_displayer.cpp
@@ -0,0 +1,48 @@
+#include <ros/ros.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <iostream>
+#include <string>
+#include <sstream>
+
+typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
+
+class Callback {
+  public:
+    void
+      operator()(const PointCloud::ConstPtr& msg)
+      {
+        std::ostringstream stream;
+        stream << "PointCloud published :" << std::endl;
+        for(int i = 0; i < msg->points.size(); ++i)
+        {
+          pcl::PointXYZRGB p = msg->points[i];
+          stream << std::endl
+            << "point # " << i << std::endl
+            << "x : " << p.x << std::endl
+            << "y : " << p.y << std::endl
+            << "z : " << p.z << std::endl
+            << "r : " << (int) p.r << std::endl
+            << "g : " << (int) p.g << std::endl
+            << "b : " << (int) p.b << std::endl;
+        }
+          ROS_INFO("%s", stream.str().c_str());
+      }
+};
+
+  int
+main(int argc, char** argv)
+{
+  ros::init(argc, argv, "pcl_displayer");
+  ros::NodeHandle node("pcl_displayer");
+
+  // initialisation
+  ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, Callback());
+
+  // démarrage
+  ROS_INFO("node started");
+  ros::spin();
+  ROS_INFO("exit");
+  return 0;
+}
diff --git a/hand_control/src/random_pcl_publisher.cpp b/hand_control/src/random_pcl_publisher.cpp
index 1d2d2c6..63c2acd 100644
--- a/hand_control/src/random_pcl_publisher.cpp
+++ b/hand_control/src/random_pcl_publisher.cpp
@@ -12,7 +12,8 @@ typedef pcl::common::UniformGenerator<float> UGenerator;
 class Generator
 {
   public:
-    Generator(int len, double m, double M) : length(len), min(m), max(M), cgen()
+    Generator(int len, double m, double M)
+    : length(len), min(m), max(M), cgen()
     {
       UGenerator::Parameters params(min, max, -1);
       cgen.setParameters(params);
@@ -23,16 +24,11 @@ class Generator
     {
       PointCloud::Ptr pcl(new PointCloud());
       cgen.fill(length, length, *pcl);
-      ROS_INFO("random cloud :");
-      for(int i = 0; i < pcl->points.size(); ++i)
+      for (int i = 0; i < pcl->points.size(); ++i)
       {
-        ROS_INFO("\nx : %f\ny : %f\nz : %f\nr : %d\ng : %d\nb : %d",
-                pcl->points[i].x,
-                pcl->points[i].y,
-                pcl->points[i].z,
-                pcl->points[i].r,
-                pcl->points[i].g,
-                pcl->points[i].b);
+        pcl->points[i].r = (uint8_t) 255;
+        pcl->points[i].g = (uint8_t) 255;
+        pcl->points[i].b = (uint8_t) 0;
       }
       return pcl;
     }