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)