From f43bc943f8d2e9ece95f44af59487c58a4b54821 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 9 Aug 2019 21:45:27 -0400 Subject: [PATCH] Refactored how depth is used to compute 3D transform (making sure the same depth image is used with same stamp and frame_id). Fixed CameraTcpServer ingoring incoming connections with Qt5. Added approx_sync parameter to ros. --- include/find_object/Camera.h | 1 + include/find_object/FindObject.h | 3 +- include/find_object/MainWindow.h | 3 +- launch/find_object_3d.launch | 1 + src/CameraTcpServer.cpp | 5 ++- src/CameraTcpServer.h | 5 +++ src/FindObject.cpp | 7 ++++- src/MainWindow.cpp | 15 +++++++-- src/ros/CameraROS.cpp | 31 ++++++++++++++----- src/ros/CameraROS.h | 19 ++++++------ src/ros/FindObjectROS.cpp | 53 +++++++++++++------------------- src/ros/FindObjectROS.h | 12 +------- src/ros/find_object_2d_node.cpp | 10 ++---- 13 files changed, 92 insertions(+), 73 deletions(-) diff --git a/include/find_object/Camera.h b/include/find_object/Camera.h index f4625a0e..c2287183 100644 --- a/include/find_object/Camera.h +++ b/include/find_object/Camera.h @@ -57,6 +57,7 @@ public: Q_SIGNALS: void imageReceived(const cv::Mat & image); + void imageReceived(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant); void finished(); public Q_SLOTS: diff --git a/include/find_object/FindObject.h b/include/find_object/FindObject.h index 98df2083..52290661 100644 --- a/include/find_object/FindObject.h +++ b/include/find_object/FindObject.h @@ -92,9 +92,10 @@ public Q_SLOTS: void addObjectAndUpdate(const cv::Mat & image, int id=0, const QString & filePath = QString()); void removeObjectAndUpdate(int id); void detect(const cv::Mat & image); // emit objectsFound() + void detect(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant); // emit objectsFound() Q_SIGNALS: - void objectsFound(const find_object::DetectionInfo &); + void objectsFound(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float); private: void clearVocabulary(); diff --git a/include/find_object/MainWindow.h b/include/find_object/MainWindow.h index e2418bae..a030cfda 100644 --- a/include/find_object/MainWindow.h +++ b/include/find_object/MainWindow.h @@ -80,6 +80,7 @@ public Q_SLOTS: void stopProcessing(); void pauseProcessing(); void update(const cv::Mat & image); + void update(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant); private Q_SLOTS: void loadSession(); @@ -113,7 +114,7 @@ private Q_SLOTS: void rectHovered(int objId); Q_SIGNALS: - void objectsFound(const find_object::DetectionInfo &); + void objectsFound(const find_object::DetectionInfo &, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant); private: bool loadSettings(const QString & path); diff --git a/launch/find_object_3d.launch b/launch/find_object_3d.launch index 660a56f0..d95da108 100644 --- a/launch/find_object_3d.launch +++ b/launch/find_object_3d.launch @@ -8,6 +8,7 @@ + diff --git a/src/CameraTcpServer.cpp b/src/CameraTcpServer.cpp index 289ee75c..ca4fadb2 100644 --- a/src/CameraTcpServer.cpp +++ b/src/CameraTcpServer.cpp @@ -100,8 +100,11 @@ quint16 CameraTcpServer::getPort() const { return this->serverPort(); } - +#if QT_VERSION >= 0x050000 +void CameraTcpServer::incomingConnection(qintptr socketDescriptor) +#else void CameraTcpServer::incomingConnection(int socketDescriptor) +#endif { QList clients = this->findChildren(); if(clients.size() >= 1) diff --git a/src/CameraTcpServer.h b/src/CameraTcpServer.h index c5a6ec4e..af1581cf 100644 --- a/src/CameraTcpServer.h +++ b/src/CameraTcpServer.h @@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef CAMERATCPCLIENT_H_ #define CAMERATCPCLIENT_H_ +#include #include #include @@ -46,7 +47,11 @@ public: quint16 getPort() const; protected: +#if QT_VERSION >= 0x050000 + virtual void incomingConnection ( qintptr socketDescriptor ); +#else virtual void incomingConnection ( int socketDescriptor ); +#endif private Q_SLOTS: void readReceivedData(); diff --git a/src/FindObject.cpp b/src/FindObject.cpp index e357c668..427c1d9d 100644 --- a/src/FindObject.cpp +++ b/src/FindObject.cpp @@ -1382,6 +1382,11 @@ private: }; void FindObject::detect(const cv::Mat & image) +{ + detect(image, "", 0.0, cv::Mat(), 0.0); +} + +void FindObject::detect(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant) { QTime time; time.start(); @@ -1411,7 +1416,7 @@ void FindObject::detect(const cv::Mat & image) if(info.objDetected_.size() > 0 || Settings::getGeneral_sendNoObjDetectedEvents()) { - Q_EMIT objectsFound(info); + Q_EMIT objectsFound(info, frameId, stamp, depth, depthConstant); } } diff --git a/src/MainWindow.cpp b/src/MainWindow.cpp index e7087ed8..3501dfef 100644 --- a/src/MainWindow.cpp +++ b/src/MainWindow.cpp @@ -287,6 +287,7 @@ MainWindow::MainWindow(FindObject * findObject, Camera * camera, QWidget * paren MainWindow::~MainWindow() { disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &))); + disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float))); disconnect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing())); camera_->stop(); qDeleteAll(objWidgets_); @@ -350,7 +351,7 @@ void MainWindow::setupTCPServer() delete tcpServer_; } tcpServer_ = new TcpServer(Settings::getGeneral_port(), this); - connect(this, SIGNAL(objectsFound(find_object::DetectionInfo)), tcpServer_, SLOT(publishDetectionInfo(find_object::DetectionInfo))); + connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float)), tcpServer_, SLOT(publishDetectionInfo(find_object::DetectionInfo))); ui_->label_ipAddress->setText(tcpServer_->getHostAddress().toString()); ui_->label_port->setNum(tcpServer_->getPort()); UINFO("Detection sent on port: %d (IP=%s)", tcpServer_->getPort(), tcpServer_->getHostAddress().toString().toStdString().c_str()); @@ -785,6 +786,7 @@ void MainWindow::hideObjectsFeatures() void MainWindow::addObjectFromScene() { disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &))); + disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float))); disconnect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing())); AddObjectDialog * dialog; bool resumeCamera = camera_->isRunning(); @@ -820,6 +822,7 @@ void MainWindow::addObjectFromScene() else { connect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)), Qt::UniqueConnection); + connect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float)), Qt::UniqueConnection); connect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing()), Qt::UniqueConnection); if(!sceneImage_.empty()) { @@ -1164,6 +1167,7 @@ void MainWindow::startProcessing() if(camera_->start()) { connect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)), Qt::UniqueConnection); + connect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float)), Qt::UniqueConnection); connect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing()), Qt::UniqueConnection); ui_->actionStop_camera->setEnabled(true); ui_->actionPause_camera->setEnabled(true); @@ -1221,6 +1225,7 @@ void MainWindow::stopProcessing() if(camera_) { disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &))); + disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float))); disconnect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing())); camera_->stop(); } @@ -1284,6 +1289,12 @@ void MainWindow::rectHovered(int objId) } void MainWindow::update(const cv::Mat & image) +{ + UERROR(""); + update(image, "", 0.0, cv::Mat(), 0.0); +} + +void MainWindow::update(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant) { if(image.empty()) { @@ -1549,7 +1560,7 @@ void MainWindow::update(const cv::Mat & image) if(info.objDetected_.size() > 0 || Settings::getGeneral_sendNoObjDetectedEvents()) { - Q_EMIT objectsFound(info); + Q_EMIT objectsFound(info, frameId, stamp, depth, depthConstant); } ui_->label_objectsDetected->setNum(info.objDetected_.size()); } diff --git a/src/ros/CameraROS.cpp b/src/ros/CameraROS.cpp index 2f9eb5ad..999645fa 100644 --- a/src/ros/CameraROS.cpp +++ b/src/ros/CameraROS.cpp @@ -35,7 +35,9 @@ using namespace find_object; CameraROS::CameraROS(bool subscribeDepth, QObject * parent) : Camera(parent), - subscribeDepth_(subscribeDepth) + subscribeDepth_(subscribeDepth), + approxSync_(0), + exactSync_(0) { ros::NodeHandle nh; // public ros::NodeHandle pnh("~"); // private @@ -51,8 +53,11 @@ CameraROS::CameraROS(bool subscribeDepth, QObject * parent) : else { int queueSize = 10; + bool approxSync = true; pnh.param("queue_size", queueSize, queueSize); + pnh.param("approx_sync", approxSync, approxSync); ROS_INFO("find_object_ros: queue_size = %d", queueSize); + ROS_INFO("find_object_ros: approx_sync = %s", approxSync?"true":"false"); ros::NodeHandle rgb_nh(nh, "rgb"); ros::NodeHandle rgb_pnh(pnh, "rgb"); @@ -66,11 +71,23 @@ CameraROS::CameraROS(bool subscribeDepth, QObject * parent) : rgbSub_.subscribe(rgb_it, rgb_nh.resolveName("image_rect_color"), 1, hintsRgb); depthSub_.subscribe(depth_it, depth_nh.resolveName("image_raw"), 1, hintsDepth); cameraInfoSub_.subscribe(depth_nh, "camera_info", 1); - sync_ = new message_filters::Synchronizer(MySyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_); - sync_->registerCallback(boost::bind(&CameraROS::imgDepthReceivedCallback, this, _1, _2, _3)); - + if(approxSync) + { + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_); + approxSync_->registerCallback(boost::bind(&CameraROS::imgDepthReceivedCallback, this, _1, _2, _3)); + } + else + { + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_); + exactSync_->registerCallback(boost::bind(&CameraROS::imgDepthReceivedCallback, this, _1, _2, _3)); + } } } +CameraROS::~CameraROS() +{ + delete approxSync_; + delete exactSync_; +} QStringList CameraROS::subscribedTopics() const { @@ -122,8 +139,7 @@ void CameraROS::imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg) image = cv_bridge::cvtColor(imgPtr, "bgr8")->image; } - Q_EMIT rosDataReceived(msg->header.frame_id, msg->header.stamp, cv::Mat(), 0.0f); - Q_EMIT imageReceived(image); + Q_EMIT imageReceived(image, QString(msg->header.frame_id.c_str()), msg->header.stamp.toSec(), cv::Mat(), 0.0f); } catch(const cv_bridge::Exception & e) { @@ -164,8 +180,7 @@ void CameraROS::imgDepthReceivedCallback( image = cv_bridge::cvtColor(imgPtr, "bgr8")->image; } - Q_EMIT rosDataReceived(rgbMsg->header.frame_id, rgbMsg->header.stamp, ptrDepth->image, depthConstant); - Q_EMIT imageReceived(image); + Q_EMIT imageReceived(image, QString(rgbMsg->header.frame_id.c_str()), rgbMsg->header.stamp.toSec(), ptrDepth->image, depthConstant); } catch(const cv_bridge::Exception & e) { diff --git a/src/ros/CameraROS.h b/src/ros/CameraROS.h index 4094d8b8..2419ec1d 100644 --- a/src/ros/CameraROS.h +++ b/src/ros/CameraROS.h @@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include @@ -49,19 +50,13 @@ class CameraROS : public find_object::Camera { Q_OBJECT public: CameraROS(bool subscribeDepth, QObject * parent = 0); - virtual ~CameraROS() {} + virtual ~CameraROS(); virtual bool start(); virtual void stop(); QStringList subscribedTopics() const; -Q_SIGNALS: - void rosDataReceived(const std::string & frameId, - const ros::Time & stamp, - const cv::Mat & depth, - float depthConstant); - private Q_SLOTS: virtual void takeImage(); @@ -83,8 +78,14 @@ private: typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, - sensor_msgs::CameraInfo> MySyncPolicy; - message_filters::Synchronizer * sync_; + sensor_msgs::CameraInfo> MyApproxSyncPolicy; + message_filters::Synchronizer * approxSync_; + + typedef message_filters::sync_policies::ExactTime< + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo> MyExactSyncPolicy; + message_filters::Synchronizer * exactSync_; }; #endif /* CAMERAROS_H_ */ diff --git a/src/ros/FindObjectROS.cpp b/src/ros/FindObjectROS.cpp index 9c046050..346fa212 100644 --- a/src/ros/FindObjectROS.cpp +++ b/src/ros/FindObjectROS.cpp @@ -37,8 +37,7 @@ using namespace find_object; FindObjectROS::FindObjectROS(QObject * parent) : FindObject(true, parent), - objFramePrefix_("object"), - depthConstant_(0.0f) + objFramePrefix_("object") { ros::NodeHandle pnh("~"); // public pnh.param("object_prefix", objFramePrefix_, objFramePrefix_); @@ -50,13 +49,13 @@ FindObjectROS::FindObjectROS(QObject * parent) : pubStamped_ = nh.advertise("objectsStamped", 1); pubInfo_ = nh.advertise("info", 1); - this->connect(this, SIGNAL(objectsFound(find_object::DetectionInfo)), this, SLOT(publish(find_object::DetectionInfo))); + this->connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float)), this, SLOT(publish(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float))); } -void FindObjectROS::publish(const find_object::DetectionInfo & info) +void FindObjectROS::publish(const find_object::DetectionInfo & info, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant) { // send tf before the message - if(info.objDetected_.size() && !depth_.empty() && depthConstant_ != 0.0f) + if(info.objDetected_.size() && !depth.empty() && depthConstant != 0.0f) { std::vector transforms; char multiSubId = 'b'; @@ -87,20 +86,20 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info) QPointF xAxis = iter->map(QPointF(3*objectWidth/4, objectHeight/2)); QPointF yAxis = iter->map(QPointF(objectWidth/2, 3*objectHeight/4)); - cv::Vec3f center3D = this->getDepth(depth_, + cv::Vec3f center3D = this->getDepth(depth, center.x()+0.5f, center.y()+0.5f, - float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f, - 1.0f/depthConstant_, 1.0f/depthConstant_); + float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f, + 1.0f/depthConstant, 1.0f/depthConstant); - cv::Vec3f axisEndX = this->getDepth(depth_, + cv::Vec3f axisEndX = this->getDepth(depth, xAxis.x()+0.5f, xAxis.y()+0.5f, - float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f, - 1.0f/depthConstant_, 1.0f/depthConstant_); + float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f, + 1.0f/depthConstant, 1.0f/depthConstant); - cv::Vec3f axisEndY = this->getDepth(depth_, + cv::Vec3f axisEndY = this->getDepth(depth, yAxis.x()+0.5f, yAxis.y()+0.5f, - float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f, - 1.0f/depthConstant_, 1.0f/depthConstant_); + float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f, + 1.0f/depthConstant, 1.0f/depthConstant); if(std::isfinite(center3D.val[0]) && std::isfinite(center3D.val[1]) && std::isfinite(center3D.val[2]) && std::isfinite(axisEndX.val[0]) && std::isfinite(axisEndX.val[1]) && std::isfinite(axisEndX.val[2]) && @@ -109,8 +108,8 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info) tf::StampedTransform transform; transform.setIdentity(); transform.child_frame_id_ = QString("%1_%2%3").arg(objFramePrefix_.c_str()).arg(id).arg(multiSuffix).toStdString(); - transform.frame_id_ = frameId_; - transform.stamp_ = stamp_; + transform.frame_id_ = frameId.toStdString(); + transform.stamp_.fromSec(stamp); transform.setOrigin(tf::Vector3(center3D.val[0], center3D.val[1], center3D.val[2])); //set rotation @@ -130,6 +129,9 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info) q *= tf::createQuaternionFromRPY(CV_PI/2.0, CV_PI/2.0, 0); transform.setRotation(q.normalized()); + ROS_WARN("%f %f %f (%f %f %f %f)", center3D.val[0], center3D.val[1], center3D.val[2], + transform.getRotation().x(), transform.getRotation().y(), transform.getRotation().z(), transform.getRotation().w()); + transforms.push_back(transform); } else @@ -223,31 +225,20 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info) if(pubStamped_.getNumSubscribers()) { // use same header as the input image (for synchronization and frame reference) - msgStamped.header.frame_id = frameId_; - msgStamped.header.stamp = stamp_; + msgStamped.header.frame_id = frameId.toStdString(); + msgStamped.header.stamp.fromSec(stamp); pubStamped_.publish(msgStamped); } if(pubInfo_.getNumSubscribers()) { // use same header as the input image (for synchronization and frame reference) - infoMsg.header.frame_id = frameId_; - infoMsg.header.stamp = stamp_; + infoMsg.header.frame_id = frameId.toStdString(); + infoMsg.header.stamp.fromSec(stamp); pubInfo_.publish(infoMsg); } } } -void FindObjectROS::setDepthData(const std::string & frameId, - const ros::Time & stamp, - const cv::Mat & depth, - float depthConstant) -{ - frameId_ = frameId; - stamp_ = stamp; - depth_ = depth; - depthConstant_ = depthConstant; -} - cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage, int x, int y, float cx, float cy, diff --git a/src/ros/FindObjectROS.h b/src/ros/FindObjectROS.h index f8ae99e1..2d0c5c63 100644 --- a/src/ros/FindObjectROS.h +++ b/src/ros/FindObjectROS.h @@ -48,12 +48,7 @@ public: virtual ~FindObjectROS() {} public Q_SLOTS: - void publish(const find_object::DetectionInfo & info); - - void setDepthData(const std::string & frameId, - const ros::Time & stamp, - const cv::Mat & depth, - float depthConstant); + void publish(const find_object::DetectionInfo & info, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant); private: cv::Vec3f getDepth(const cv::Mat & depthImage, @@ -67,11 +62,6 @@ private: ros::Publisher pubStamped_; ros::Publisher pubInfo_; - std::string frameId_; - ros::Time stamp_; - cv::Mat depth_; - float depthConstant_; - std::string objFramePrefix_; tf::TransformBroadcaster tfBroadcaster_; diff --git a/src/ros/find_object_2d_node.cpp b/src/ros/find_object_2d_node.cpp index 05c347d4..a208a1fb 100644 --- a/src/ros/find_object_2d_node.cpp +++ b/src/ros/find_object_2d_node.cpp @@ -138,12 +138,6 @@ int main(int argc, char** argv) CameraROS * camera = new CameraROS(subscribeDepth); - QObject::connect( - camera, - SIGNAL(rosDataReceived(const std::string &, const ros::Time &, const cv::Mat &, float)), - findObjectROS, - SLOT(setDepthData(const std::string &, const ros::Time &, const cv::Mat &, float))); - // Catch ctrl-c to close the gui setupQuitSignal(gui); @@ -154,9 +148,9 @@ int main(int argc, char** argv) QObject::connect( &mainWindow, - SIGNAL(objectsFound(const find_object::DetectionInfo &)), + SIGNAL(objectsFound(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float)), findObjectROS, - SLOT(publish(const find_object::DetectionInfo &))); + SLOT(publish(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float))); QStringList topics = camera->subscribedTopics(); if(topics.size() == 1)