[issue #1002 ]Added support for synchronized RGBD

src/libs/comm_cpp/CMakeLists.txt

@@ -96,6 +96,7 @@ IF(JDEROBOTCOM_ROS)
         include/jderobot/comm/ros/listenerPose.hpp
         include/jderobot/comm/ros/publisherMotors.hpp
         include/jderobot/comm/ros/listenerBumper.hpp
+        include/jderobot/comm/ros/listenerRgbd.hpp
     )
 
     set(SOURCES
@@ -106,6 +107,7 @@ IF(JDEROBOTCOM_ROS)
         src/ros/listenerPose.cpp
         src/ros/publisherMotors.cpp
         src/ros/listenerBumper.cpp
+        src/ros/listenerRgbd.cpp
     )
 
 ENDIF()

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

@@ -0,0 +1,80 @@
+/*
+ *  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_LISTENERCAMERA_H_
+#define JDEROBOTCOMM_LISTENERCAMERA_H_
+
+
+#include <ros/ros.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+//#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+//#include <image_transport/image_transport.h>
+//#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/Image.h>
+#include <boost/thread/thread.hpp>
+#include <jderobot/types/rgbd.h>
+#include <jderobot/comm/interfaces/rgbdClient.hpp>
+#include <jderobot/comm/ros/translators.hpp>
+#include <boost/bind.hpp>
+#include <time.h>
+
+namespace Comm {
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, 
+                                                        sensor_msgs::Image> MySyncPolicy;
+
+	class ListenerRgbd: public Comm::RgbdClient {
+
+	public:
+		ListenerRgbd(int argc, char** argv, std::string nodeName, std::string topicrgb, std::string topicd);
+		~ListenerRgbd();
+
+		void start();
+		void stop();
+		virtual JdeRobotTypes::Rgbd getRgbd();
+		virtual int getRefreshRate();
+
+
+	private:
+		pthread_mutex_t mutex;
+		message_filters::Subscriber<sensor_msgs::Image>* subrgb;      
+		message_filters::Subscriber<sensor_msgs::Image>* subd;           
+		message_filters::Synchronizer<Comm::MySyncPolicy>* no_cloud_sync_;
+		std::string topicrgb;
+		std::string topicd;
+		std::string nodeName;
+
+		int cont = 0; //used to count Frames per seconds
+		time_t timer; // used to save time for FPS
+
+		ros::AsyncSpinner* spinner;
+
+
+		void callback (const sensor_msgs::ImageConstPtr& rgb, const sensor_msgs::ImageConstPtr& d);
+
+
+
+
+
+	};//class
+
+} //NS
+#endif /* JDEROBOTCOMM_LISTENERCAMERA_H_ */

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

@@ -37,6 +37,8 @@
 #include "cv_bridge/cv_bridge.h"
 #include "sensor_msgs/image_encodings.h"
 
+#include <jderobot/types/rgbd.h>
+
 
 #include <jderobot/types/cmdvel.h>
 #include <geometry_msgs/Twist.h>
@@ -80,6 +82,19 @@ namespace Comm {
 	 */
 	JdeRobotTypes::Image translate_image_messages(const sensor_msgs::ImageConstPtr& image_msg);
 
+
+	/**
+	 * @brief translate ROS images messages to JdeRobot Rgbd
+	 *
+	 *
+	 * @param ROS Image Message
+	 * @param ROS Image Message
+	 * 
+	 *
+	 * @return Rgbd translated from ROS Messages 
+	 */
+	JdeRobotTypes::Rgbd translate_rgbd(const sensor_msgs::ImageConstPtr& rgb,const sensor_msgs::ImageConstPtr& d);
+
 	/**
 	 * @brief Translates from 32FC1 Image format to RGB. Inf values are represented by NaN, when converting to RGB, NaN passed to 0 
 	 *

src/libs/comm_cpp/src/ros/listenerRgbd.cpp

@@ -0,0 +1,88 @@
+#include <jderobot/comm/ros/listenerRgbd.hpp>
+
+namespace Comm {
+
+	ListenerRgbd::ListenerRgbd(int argc, char** argv, std::string nodeName, std::string topicrgb, std::string topicd){
+		pthread_mutex_init(&mutex, NULL);
+		if ("" == topicrgb || "" == topicd ){
+			this->on = false;
+			std::cerr <<"Invalid camera topic" <<std::endl;
+		}else{
+			this->on = true;
+			this->topicrgb = topicrgb;
+			this->topicd = topicd;
+			this->nodeName = nodeName;
+
+			const std::string name = std::string(this->nodeName);
+
+			time(&timer);
+			int a = 0;
+			ros::init(a, nullptr, name);
+			ros::NodeHandle nh;
+			int q = 1; //queue size
+			this->subrgb = new message_filters::Subscriber<sensor_msgs::Image> (nh, topicrgb, q);
+			this->subd = new message_filters::Subscriber<sensor_msgs::Image> (nh, topicd, q);
+			this->no_cloud_sync_ = new message_filters::Synchronizer<Comm::MySyncPolicy>(Comm::MySyncPolicy(q),  *subrgb, *subd);
+
+
+			this->no_cloud_sync_->registerCallback(boost::bind(&ListenerRgbd::callback, this, _1, _2));
+			std::cout << "listen from "+ this->topicrgb << " and " << this->topicd << std::endl;
+
+			this->spinner = new ros::AsyncSpinner(1);
+		}
+	}
+
+
+
+	ListenerRgbd::~ListenerRgbd(){
+		this->stop();
+	}
+
+	void 
+	ListenerRgbd::start(){
+		this->spinner->start();
+	}
+
+	void 
+	ListenerRgbd::stop(){
+		this->spinner->stop();
+		ros::shutdown();
+	}
+
+	void 
+	ListenerRgbd::callback(const sensor_msgs::ImageConstPtr& rgb, const sensor_msgs::ImageConstPtr& d){
+		this->cont++;
+		time_t now;
+		time(&now);
+		pthread_mutex_lock(&mutex);
+		this->rgbd = Comm::translate_rgbd(rgb, d);
+		if (difftime(this->timer, now)>=1){
+			this->refreshRate = this->cont;
+			this->cont = 0;
+			this->timer = now;
+		}
+		pthread_mutex_unlock(&mutex);
+
+	}
+
+	JdeRobotTypes::Rgbd  ListenerRgbd::getRgbd(){
+		JdeRobotTypes::Rgbd img;
+		pthread_mutex_lock(&mutex);
+		img = this->rgbd;
+		pthread_mutex_unlock(&mutex);
+		return img;
+	}
+
+	int ListenerRgbd::getRefreshRate(){
+
+		int rr;
+		pthread_mutex_lock(&mutex);
+		rr = this->refreshRate;
+		pthread_mutex_unlock(&mutex);
+
+		return rr;
+	}
+
+
+
+}//NS

src/libs/comm_cpp/src/ros/translators.cpp

@@ -99,6 +99,17 @@ namespace Comm {
 		return img;
 	}
 
+
+	JdeRobotTypes::Rgbd 
+	translate_rgbd(const sensor_msgs::ImageConstPtr& rgb,const sensor_msgs::ImageConstPtr& d){
+		JdeRobotTypes::Rgbd rgbd;
+		rgbd.timeStamp = rgb->header.stamp.sec + (rgb->header.stamp.nsec *1e-9);
+		rgbd.color = translate_image_messages(rgb);
+		rgbd.depth = translate_image_messages(d);
+		return rgbd;
+
+	}
+
 	geometry_msgs::Twist 
 	translate_twist_messages(JdeRobotTypes::CMDVel cmdvel ){
 		geometry_msgs::Twist vel;

src/libs/comm_py/comm/rgbdClient.py.ros.in

@@ -3,8 +3,8 @@ import Ice
 import rospy
 from .ice.rgbdIceClient import RgbdIceClient
 
-#if (sys.version_info[0] == 2):
-#    from .ros.listenerRgbd import ListenerRgbd
+if (sys.version_info[0] == 2):
+    from .ros.listenerRgbd import ListenerRgbd
 
 def __getRgbdIceClient(jdrc, prefix):
     '''
@@ -38,12 +38,13 @@ def __getListenerRgbd(jdrc, prefix):
 
     '''
     if (sys.version_info[0] == 2):
-        #print("Receiving " + prefix + " Rgbd from ROS messages")
-        #topic = jdrc.getConfig().getProperty(prefix+".Topic")
-        #client = ListenerRgbd(topic)
-        #return client
-        print(prefix + ": This Interface doesn't support ROS msg")
-        return None
+        print("Receiving " + prefix + " Rgbd from ROS messages")
+        topicrgb = jdrc.getConfig().getProperty(prefix+".Topicrgb")
+        topicd = jdrc.getConfig().getProperty(prefix+".Topicd")
+        client = ListenerRgbd(topicrgb, topicd)
+        return client
+        #print(prefix + ": This Interface doesn't support ROS msg")
+        #return None
     else:
         print(prefix + ": ROS msg are diabled for python "+ sys.version_info[0])
         return None