From 05fa4420a7a635eee9139aa7a5a63f89a971334a Mon Sep 17 00:00:00 2001 From: Aitor Martinez Date: Wed, 3 Jan 2018 13:45:08 +0100 Subject: [PATCH] Added support for synchronized RGBD --- src/libs/comm_cpp/CMakeLists.txt | 2 + .../jderobot/comm/ros/listenerRgbd.hpp | 80 +++++++++++ .../include/jderobot/comm/ros/translators.hpp | 15 ++ src/libs/comm_cpp/src/ros/listenerRgbd.cpp | 88 ++++++++++++ src/libs/comm_cpp/src/ros/translators.cpp | 11 ++ src/libs/comm_py/comm/rgbdClient.py.ros.in | 17 +-- src/libs/comm_py/comm/ros/listenerRgbd.py | 134 ++++++++++++++++++ 7 files changed, 339 insertions(+), 8 deletions(-) create mode 100644 src/libs/comm_cpp/include/jderobot/comm/ros/listenerRgbd.hpp create mode 100644 src/libs/comm_cpp/src/ros/listenerRgbd.cpp create mode 100644 src/libs/comm_py/comm/ros/listenerRgbd.py diff --git a/src/libs/comm_cpp/CMakeLists.txt b/src/libs/comm_cpp/CMakeLists.txt index 405d0893d..0dabe7c67 100644 --- a/src/libs/comm_cpp/CMakeLists.txt +++ b/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() diff --git a/src/libs/comm_cpp/include/jderobot/comm/ros/listenerRgbd.hpp b/src/libs/comm_cpp/include/jderobot/comm/ros/listenerRgbd.hpp new file mode 100644 index 000000000..2565e9eb2 --- /dev/null +++ b/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 + */ + +#ifndef JDEROBOTCOMM_LISTENERCAMERA_H_ +#define JDEROBOTCOMM_LISTENERCAMERA_H_ + + +#include +#include +#include +//#include +#include +//#include +//#include +#include +#include +#include +#include +#include +#include +#include + +namespace Comm { + + typedef message_filters::sync_policies::ApproximateTime 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* subrgb; + message_filters::Subscriber* subd; + message_filters::Synchronizer* 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_ */ \ No newline at end of file diff --git a/src/libs/comm_cpp/include/jderobot/comm/ros/translators.hpp b/src/libs/comm_cpp/include/jderobot/comm/ros/translators.hpp index 19b96d09b..812c3e548 100644 --- a/src/libs/comm_cpp/include/jderobot/comm/ros/translators.hpp +++ b/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 + #include #include @@ -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 * diff --git a/src/libs/comm_cpp/src/ros/listenerRgbd.cpp b/src/libs/comm_cpp/src/ros/listenerRgbd.cpp new file mode 100644 index 000000000..370e1d4a0 --- /dev/null +++ b/src/libs/comm_cpp/src/ros/listenerRgbd.cpp @@ -0,0 +1,88 @@ +#include + +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" <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 (nh, topicrgb, q); + this->subd = new message_filters::Subscriber (nh, topicd, q); + this->no_cloud_sync_ = new message_filters::Synchronizer(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 \ No newline at end of file diff --git a/src/libs/comm_cpp/src/ros/translators.cpp b/src/libs/comm_cpp/src/ros/translators.cpp index 672058fbf..9419049b7 100644 --- a/src/libs/comm_cpp/src/ros/translators.cpp +++ b/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; diff --git a/src/libs/comm_py/comm/rgbdClient.py.ros.in b/src/libs/comm_py/comm/rgbdClient.py.ros.in index 172cb5b75..06a049efc 100644 --- a/src/libs/comm_py/comm/rgbdClient.py.ros.in +++ b/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 diff --git a/src/libs/comm_py/comm/ros/listenerRgbd.py b/src/libs/comm_py/comm/ros/listenerRgbd.py new file mode 100644 index 000000000..c7aa18db0 --- /dev/null +++ b/src/libs/comm_py/comm/ros/listenerRgbd.py @@ -0,0 +1,134 @@ +import rospy +import message_filters +from sensor_msgs.msg import Image as ImageROS +import threading +from jderobotTypes import Rgbd, Image + + + +def imageMsg2Image(img, bridge): + ''' + Translates from ROS Image to JderobotTypes Image. + + @param img: ROS Image to translate + @param bridge: bridge to do translation + + @type img: sensor_msgs.msg.Image + @type brige: CvBridge + + @return a JderobotTypes.Image translated from img + + ''' + image = Image() + + image.width = img.width + image.height = img.height + image.format = "RGB8" + image.timeStamp = img.header.stamp.secs + (img.header.stamp.nsecs *1e-9) + cv_image=0 + if (img.encoding == "32FC1"): + gray_img_buff = bridge.imgmsg_to_cv2(img, "32FC1") + cv_image = depthToRGB8(gray_img_buff) + else: + cv_image = bridge.imgmsg_to_cv2(img, "rgb8") + image.data = cv_image + return image + + + +def Images2Rgbd(rgb, d): + ''' + Translates from ROS Images to JderobotTypes Rgbd. + + @param rgb: ROS color Image to translate + + @param d: ROS depth image to translate + + @type rgb: ImageROS + + @type d: ImageROS + + @return a Rgbd translated from Images + + ''' + data = Rgbd() + data.color=imageMsg2Image(rgb) + data.depth=imageMsg2Image(d) + data.timeStamp = rgb.header.stamp.secs + (rgb.header.stamp.nsecs *1e-9) + return data + +class ListenerRgbd: + ''' + ROS Rgbd Subscriber. Rgbd Client to Receive Rgbd Synchronized from ROS nodes. + ''' + def __init__(self, topicrgb, topicd): + ''' + ListenerRgbd Constructor. + + @param topicrgb: ROS topic for color image to subscribe + + @param topicd: ROS topic for depth image to subscribe + + @type topic: String + + ''' + self.topicrgb = topicrgb + self.topicd = topicd + self.data = Rbgd() + self.subrgb = None + self.subd = None + self.ts = None + self.lock = threading.Lock() + self.start() + + def __callback (self, rgb, d): + ''' + Callback function to receive and save Rgbd Scans. + + @param rgb: ROS color Image to translate + + @param d: ROS depth image to translate + + @type rgb: ImageROS + + @type d: ImageROS + + ''' + data = Images2Rgbd(rgb, d) + + self.lock.acquire() + self.data = data + self.lock.release() + + def stop(self): + ''' + Stops (Unregisters) the client. + + ''' + self.subrgb.unregister() + self.subd.unregister() + + def start (self): + ''' + Starts (Subscribes) the client. + + ''' + self.subrgb = rospy.Subscriber(self.topicrgb, ImageROS) + self.subd = rospy.Subscriber(self.topicd, ImageROS) + self.ts = message_filters.ApproximateTimeSynchronizer([subrgb, subd], 1, 0.1, allow_headerless=True) + self.ts.registerCallback(self.__callback) + + def getRgbdData(self): + ''' + Returns last RgbdData. + + @return last JdeRobotTypes Rgbd saved + + ''' + self.lock.acquire() + data = self.data + self.lock.release() + + return data + +