[Jderobot-admin] jderobot-r916 - in trunk/src/components/pluginsGazebo/kinect: . build

lr.morales en jderobot.org lr.morales en jderobot.org
Lun Mayo 13 02:36:04 CEST 2013


Author: lr.morales
Date: 2013-05-13 02:35:04 +0200 (Mon, 13 May 2013)
New Revision: 916

Added:
   trunk/src/components/pluginsGazebo/kinect/build/
   trunk/src/components/pluginsGazebo/kinect/build/CMakeLists.txt
   trunk/src/components/pluginsGazebo/kinect/build/clean.sh
Modified:
   trunk/src/components/pluginsGazebo/kinect/CMakeLists.txt
   trunk/src/components/pluginsGazebo/kinect/kinect.cfg
   trunk/src/components/pluginsGazebo/kinect/kinectPlugin.cc
   trunk/src/components/pluginsGazebo/kinect/kinectPlugin.h
Log:
A?\195?\177adida capacidad de diezmado configurable a la nube de puntos (PointCloud.LeafSize).
Modificada estructura del cmake para concordar con el resto del ?\195?\161rbol de componentes.



Modified: trunk/src/components/pluginsGazebo/kinect/CMakeLists.txt
===================================================================
--- trunk/src/components/pluginsGazebo/kinect/CMakeLists.txt	2013-05-12 21:05:23 UTC (rev 915)
+++ trunk/src/components/pluginsGazebo/kinect/CMakeLists.txt	2013-05-13 00:35:04 UTC (rev 916)
@@ -1,32 +1,25 @@
-
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
-  pkg_check_modules(GAZEBO gazebo)
-endif()
-
-find_package(PCL 1.2 REQUIRED)
-find_package(OpenCV REQUIRED)
-
 include_directories( 
-	${GAZEBO_INCLUDE_DIRS} 
-	${PCL_INCLUDE_DIRS} 
-	/usr/local/include/jderobot
-	
+	${GAZEBO_INCLUDE_DIRS}
+    ${INTERFACES_CPP_DIR}
+    ${LIBS_DIR}
+	${GEARBOX_INCLUDE_DIRS}
+    ${CMAKE_CURRENT_SOURCE_DIR}
 	) 
+
 link_directories( 
 	${GAZEBO_LIBRARY_DIRS} 
-	${PCL_LIBRARY_DIRS} 
 	)
 
-set( CMAKE_CXX_FLAGS "-lIce -lIceUtil" ) # Opciones para el compilador
+set( CMAKE_CXX_FLAGS "-Wall" ) # Opciones para el compilador
 
 add_library(kinectPlugin SHARED kinectPlugin.cc)
 
 target_link_libraries(kinectPlugin 
+    ${INTERFACES_CPP_DIR}/jderobot/libJderobotInterfaces.so
+    ${LIBS_DIR}/jderobotice/libjderobotice.so
+    ${LIBS_DIR}/jderobotutil/libjderobotutil.so
 	${GAZEBO_libraries} 
-	${PCL_LIBRARIES} 
-	${OpenCV_LIBS} 
-		/usr/local/lib/jderobot/libJderobotInterfaces.so
-
+    ${PCL_LIBRARIES}  
+    ${OpenCV_LIBRARIES}
+    ${ZeroCIce_LIBRARIES}
 )

Added: trunk/src/components/pluginsGazebo/kinect/build/CMakeLists.txt
===================================================================
--- trunk/src/components/pluginsGazebo/kinect/build/CMakeLists.txt	                        (rev 0)
+++ trunk/src/components/pluginsGazebo/kinect/build/CMakeLists.txt	2013-05-13 00:35:04 UTC (rev 916)
@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+SET( INTERFACES_CPP_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../interfaces/cpp) # Directorio con las interfaces ICE en C++
+SET( LIBS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../libs) # Directorio donde se encuentran las librerias propias de jderobot
+SET( LIBS_NEEDED   jderobotice jderobotutil) # Librerias de las que depende el componente
+SET( DEPS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../../Deps) # Directorio donde se encuentran las dependencias
+
+include (FindPkgConfig)
+if (PKG_CONFIG_FOUND)
+  pkg_check_modules(GAZEBO gazebo)
+endif()
+
+find_package(PCL 1.2 REQUIRED)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+
+# FIND AND CHECK OTHER DEPENDENCES
+include(${DEPS_DIR}/ice/CMakeLists.txt)
+#include(${DEPS_DIR}/pcl/CMakeLists.txt)
+include(${DEPS_DIR}/opencv/CMakeLists.txt)
+include(${DEPS_DIR}/gearbox/CMakeLists.txt)
+
+add_subdirectory (${CMAKE_CURRENT_SOURCE_DIR}/../../../.. ${CMAKE_CURRENT_SOURCE_DIR}/../../../..)
+add_subdirectory (${CMAKE_CURRENT_SOURCE_DIR}/.. ${CMAKE_CURRENT_SOURCE_DIR}/..)
+
+

Added: trunk/src/components/pluginsGazebo/kinect/build/clean.sh
===================================================================
--- trunk/src/components/pluginsGazebo/kinect/build/clean.sh	                        (rev 0)
+++ trunk/src/components/pluginsGazebo/kinect/build/clean.sh	2013-05-13 00:35:04 UTC (rev 916)
@@ -0,0 +1,17 @@
+#!/bin/bash
+
+make clean
+#rm ../cmake_install.cmake
+rm -r CMakeFiles
+rm -r ../CMakeFiles
+mkdir ../temp
+mv CMakeLists.txt ../temp
+mv clean.sh ../temp
+#mv cmake_uninstall.cmake.in ../temp
+rm *
+mv ../temp/CMakeLists.txt .
+mv ../temp/clean.sh .
+#mv ../temp/cmake_uninstall.cmake.in .
+rm -r ../temp
+cd ../../../../../
+./clean_repository 


Property changes on: trunk/src/components/pluginsGazebo/kinect/build/clean.sh
___________________________________________________________________
Added: svn:executable
   + *

Modified: trunk/src/components/pluginsGazebo/kinect/kinect.cfg
===================================================================
--- trunk/src/components/pluginsGazebo/kinect/kinect.cfg	2013-05-12 21:05:23 UTC (rev 915)
+++ trunk/src/components/pluginsGazebo/kinect/kinect.cfg	2013-05-13 00:35:04 UTC (rev 916)
@@ -1,2 +1,3 @@
 #without registry
 Kinect.Endpoints=default -h localhost -p 9998
+PointCloud.LeafSize=17

Modified: trunk/src/components/pluginsGazebo/kinect/kinectPlugin.cc
===================================================================
--- trunk/src/components/pluginsGazebo/kinect/kinectPlugin.cc	2013-05-12 21:05:23 UTC (rev 915)
+++ trunk/src/components/pluginsGazebo/kinect/kinectPlugin.cc	2013-05-13 00:35:04 UTC (rev 916)
@@ -142,9 +142,9 @@
 	point.g      = 0;
 	point.b      = 0;
     
-	for(int x = 0 ; x < _width ; x++){
-		for(int y = 0; y < _height; y++){
-			int indice = y*_width + x;
+	for(unsigned int x = 0 ; x < _width ; x++){
+		for(unsigned int y = 0; y < _height; y++){
+			unsigned int indice = y*_width + x;
 
 		  	if (_height>1) 
 				yAngle = atan2( (double)x - 0.5*(double)(width-1), fl);
@@ -166,7 +166,7 @@
 				point.g      = 0;
 				point.b      = 0;
 			}else{
-				int indice = y*imageRGB.step + x*imageRGB.channels();
+				unsigned int indice = y*imageRGB.step + x*imageRGB.channels();
 				point.r      = imageRGB.data[indice];
 				point.g      = imageRGB.data[indice+1];
 				point.b      = imageRGB.data[indice+2];
@@ -186,7 +186,7 @@
 	
 	pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
 	sor.setInputCloud (cloud);
-	sor.setLeafSize (0.01, 0.01, 0.01);
+	sor.setLeafSize (this->leafSize, this->leafSize, this->leafSize);
 
 	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZRGBA>);
 
@@ -235,6 +235,13 @@
 
 }
 
+void DepthCameraPlugin::SetLeafSize(const float size)
+{
+	pthread_mutex_lock (&kinect->mutex);
+    this->leafSize = size;
+	pthread_mutex_unlock (&kinect->mutex);
+}
+
    class KinectI: virtual public jderobot::pointCloud{
    public:
 		KinectI (std::string propertyPrefix, gazebo::DepthCameraPlugin* kinect):prefix(propertyPrefix)
@@ -252,8 +259,8 @@
 			
             if(cloud.points.size()){
                KData->p.resize(cloud.points.size());
-               int index = 0;
-               for(int i = 0; i < cloud.points.size(); i++){
+               unsigned int index = 0;
+               for(unsigned int i = 0; i < cloud.points.size(); i++){
                   KData->p[index].x = cloud.points[i].x;
                   KData->p[index].y = cloud.points[i].y;
                   KData->p[index].z = cloud.points[i].z;
@@ -313,7 +320,7 @@
 		}
 
 		virtual std::string startCameraStreaming(const Ice::Current&){
-
+            return ("N/A");
 		}
 
 		virtual void stopCameraStreaming(const Ice::Current&) {
@@ -462,7 +469,7 @@
 		}
 
 		virtual std::string startCameraStreaming(const Ice::Current&){
-
+            return ("N/A");
 		}
 
 		virtual void stopCameraStreaming(const Ice::Current&) {
@@ -587,11 +594,22 @@
         std::string Endpoints = prop->getProperty("Kinect.Endpoints");
         std::cout << "Kinect Endpoints > " << Endpoints << std::endl;
         
+
+        std::stringstream cnvrt(prop->getPropertyWithDefault("PointCloud.LeafSize","100"));
+        float size;
+        cnvrt >> size;
+        if (cnvrt.fail()) {
+         std::cout << "Couldn't read PointCloud.LeafSize property, setting to default" << std::endl;
+         size=100;
+        }else if(size <= 0.001){
+         std::cout << "PointCloud.LeafSize property out of bounds, setting to default" << std::endl;
+         size=100;
+        }
+        kinect->SetLeafSize(1./size);
+
         Ice::ObjectAdapterPtr adapter =
             ic->createObjectAdapterWithEndpoints("Kinect", Endpoints);
 
-
-
         Ice::ObjectPtr object = new KinectI(std::string("pointcloud1"),  kinect);
         Ice::ObjectPtr object2 = new CameraI(std::string("cameraRGB"),  kinect);
         Ice::ObjectPtr object3 = new CameraII(std::string("cameraDepth"),  kinect);
@@ -615,5 +633,6 @@
         }
     }
 
+    return NULL;
 }
 

Modified: trunk/src/components/pluginsGazebo/kinect/kinectPlugin.h
===================================================================
--- trunk/src/components/pluginsGazebo/kinect/kinectPlugin.h	2013-05-12 21:05:23 UTC (rev 915)
+++ trunk/src/components/pluginsGazebo/kinect/kinectPlugin.h	2013-05-13 00:35:04 UTC (rev 916)
@@ -72,7 +72,10 @@
                               unsigned int _width, unsigned int _height,
                               unsigned int _depth, const std::string &_format);
 
+    public: virtual void SetLeafSize(const float size);
+
     protected: unsigned int width, height, depth;
+    protected: float leafSize;
     protected: std::string format;
 
     protected: sensors::DepthCameraSensorPtr parentSensor;



More information about the Jderobot-admin mailing list