[issue #913] added support for RGBD sensors in Ice to comm

src/interfaces/slice/jderobot/rgbd.ice

@@ -28,6 +28,7 @@ module jderobot{
     struct rgbData {
         ImageData color;
         ImageData depth;
+        Time timeStamp; 				/**< TimeStamp of Data */
     };
 
 

src/libs/comm_cpp/CMakeLists.txt

@@ -46,7 +46,9 @@ set(HEADERS
     include/jderobot/comm/motorsClient.hpp
     include/jderobot/comm/interfaces/motorsClient.hpp
     include/jderobot/comm/ice/motorsIceClient.hpp
-    include/jderobot/comm/ros/publisherMotors.hpp
+    include/jderobot/comm/rgbdClient.hpp
+    include/jderobot/comm/interfaces/rgbdClient.hpp
+    include/jderobot/comm/ice/rgbdIceClient.hpp
 )
 
 set(SOURCES
@@ -59,6 +61,8 @@ set(SOURCES
     src/ice/pose3dIceClient.cpp
     src/motorsClient.cpp
     src/ice/motorsIceClient.cpp
+    src/rgbdClient.cpp
+    src/ice/rgbdIceClient.cpp
 )
 
 
@@ -70,6 +74,7 @@ IF(JDEROBOTCOM_ROS)
         include/jderobot/comm/ros/listenerLaser.hpp
         include/jderobot/comm/ros/listenerCamera.hpp
         include/jderobot/comm/ros/listenerPose.hpp
+        include/jderobot/comm/ros/publisherMotors.hpp
     )
 
     set(SOURCES

src/libs/comm_cpp/include/jderobot/comm/ice/rgbdIceClient.hpp

@@ -0,0 +1,71 @@
+/*
+ *  Copyright (C) 1997-2013 JDE Developers TeamkinectViewer.camRGB
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/.
+ *
+ *  Author : Jose María Cañas <jmplaza@gsyc.es>
+			Francisco Miguel Rivas Montero <franciscomiguel.rivas@urjc.es>
+
+ */
+
+#ifndef JDEROBOTCOMM_RGBDICECLIENT_H_
+#define JDEROBOTCOMM_RGBDICECLIENT_H_
+
+#include <IceUtil/IceUtil.h>
+#include <Ice/Ice.h>
+#include <jderobot/rgbd.h>
+#include <logger/Logger.h>
+#include <jderobotutil/utils/CameraUtils.h>
+#include <jderobot/types/rgbd.h>
+#include <jderobot/comm/communicator.hpp>
+#include <jderobot/comm/interfaces/rgbdClient.hpp>
+
+namespace Comm {
+
+class RgbdIceClient: public IceUtil::Thread, public Comm::RgbdClient {
+public:
+	RgbdIceClient(Comm::Communicator* jdrc, std::string prefix);
+	virtual ~RgbdIceClient();
+	virtual void run();
+
+	//callbacks
+	virtual JdeRobotTypes::Rgbd getRgbd();
+	virtual int getRefreshRate();
+
+
+	void pause();
+	void resume();
+	void stop_thread();
+	bool getPause(){return pauseStatus;};
+
+	jderobot::ImageFormat getImageFormat();
+	void setImageFormat (std::string format);
+
+
+private:
+	jderobot::rgbdPrx prx;
+	long long int cycle;
+
+	IceUtil::Mutex controlMutex;
+	std::string prefix;
+
+	bool pauseStatus;
+
+	IceUtil::Cond semWait;
+	std::string mImageFormat;
+
+};
+
+} /* namespace Comm */
+#endif /* JDEROBOTCOMM_RGBDICECLIENT_H_ */

src/libs/comm_cpp/include/jderobot/comm/interfaces/rgbdClient.hpp

@@ -0,0 +1,46 @@
+/*
+ *  Copyright (C) 1997-2016 JDE Developers Team
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/.
+ *  Authors :
+ *       Aitor Martinez Fernandez <aitor.martinez.fernandez@gmail.com>
+ */
+
+#ifndef JDEROBOTCOMM_RGBDCLIENT_INTERFACE_H
+#define JDEROBOTCOMM_RGBDCLIENT_INTERFACE_H
+
+#include <jderobot/types/rgbd.h>
+
+
+namespace Comm {
+
+	/**
+	 * @brief RgbdClient class.
+	 * This class is a Interface to seprate communications from tools. 
+	 * With this, the tools don't need know which communicator (ROS or ICE) are using because both use the same interface.
+	 *
+	 */
+	class RgbdClient {
+	public:
+		virtual JdeRobotTypes::Rgbd getRgbd() = 0;
+		virtual int getRefreshRate() = 0;
+		bool on = false;
+	protected:
+		JdeRobotTypes::Rgbd rgbd;
+		int refreshRate;
+	};
+
+} //NS
+
+#endif // JDEROBOTCOMM_RGBDCLIENT_INTERFACE_H

src/libs/comm_cpp/include/jderobot/comm/rgbdClient.hpp

@@ -0,0 +1,49 @@
+/*
+ *  Copyright (C) 1997-2017 JDE Developers Team
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/.
+ *  Authors :
+ *       Aitor Martinez Fernandez <aitor.martinez.fernandez@gmail.com>
+ */
+
+#ifndef JDEROBOTCOMM_RGBDCLIENT_H
+#define JDEROBOTCOMM_RGBDCLIENT_H
+
+#include <jderobot/types/rgbd.h>
+#include <Ice/Communicator.h>
+#include <jderobot/comm/communicator.hpp>
+#include <jderobot/comm/interfaces/rgbdClient.hpp>
+
+
+
+
+
+namespace Comm {
+
+	/**
+	 * @brief make a RgbdClient using propierties
+	 *
+	 *
+	 * @param communicator that contains properties
+	 * @param prefix of client Propierties (example: "kobukiViewer.Rgbd")
+	 * 
+	 *
+	 * @return null if propierties are wrong
+	 */
+	RgbdClient* getRgbdClient(Comm::Communicator* jdrc, std::string prefix);
+
+
+} //NS
+
+#endif // JDEROBOTCOMM_RGBDCLIENT_H

src/libs/comm_cpp/include/jderobot/comm/ros/translators.hpp

@@ -21,7 +21,7 @@
 #define JDEROBOTCOMM_TRANSLATORSROS_H_
 
 #include <ros/ros.h>
-
+#include <cv.h>
 
 #include <jderobot/types/laserData.h>
 #include <sensor_msgs/LaserScan.h>
@@ -56,7 +56,7 @@ namespace Comm {
 	JdeRobotTypes::LaserData translate_laser_messages(const sensor_msgs::LaserScanConstPtr& scan);
 
 	/**
-	 * @brief translate ROS Image messages to JdeRobot Iaage
+	 * @brief translate ROS Image messages to JdeRobot Image
 	 *
 	 *
 	 * @param ROS Image Message
@@ -66,14 +66,25 @@ namespace Comm {
 	 */
 	JdeRobotTypes::Image translate_image_messages(const sensor_msgs::ImageConstPtr& image_msg);
 
+	/**
+	 * @brief Translates from 32FC1 Image format to RGB. Inf values are represented by NaN, when converting to RGB, NaN passed to 0 
+	 *
+	 *
+	 * @param ROS Image Message
+	 * 
+	 *
+	 * @return Image translated from ROS Message 
+	 */
+	void depthToRGB(const cv::Mat& float_img, cv::Mat& rgb_img);
+
 	/**
 	 * @brief translate Jderobot CMDVel to ROS Twist messages
 	 *
 	 *
-	 * @param Jderobot CMDVel type
+	 * @param float_img, image in 32FC1
+	 * @param rgb_img, container for image in RGB
 	 * 
 	 *
-	 * @return Twist message translated from JdeRobot CMDVel 
 	 */
 	geometry_msgs::Twist translate_twist_messages(JdeRobotTypes::CMDVel cmdvel );