apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.

Browse Source
This commit is contained in:
matlabbe 2019-08-09 21:45:27 -04:00
parent 413cb6820d
commit f43bc943f8
13 changed files with 92 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/find_object/Camera.h
View File

@ -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:

3
include/find_object/FindObject.h
View File

@ -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();

3
include/find_object/MainWindow.h
View File

@ -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);

1
launch/find_object_3d.launch
View File

@ -8,6 +8,7 @@
<param name="subscribe_depth" value="true" type="bool"/>
<param name="objects_path" value="" type="str"/>
<param name="object_prefix" value="object" type="str"/>
<param name="approx_sync" value="true" type="bool"/>
<remap from="rgb/image_rect_color" to="camera/rgb/image_rect_color"/>
<remap from="depth_registered/image_raw" to="camera/depth_registered/image_raw"/>

5
src/CameraTcpServer.cpp
View File

@ -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<QTcpSocket*> clients = this->findChildren<QTcpSocket*>();
if(clients.size() >= 1)

5
src/CameraTcpServer.h
View File

@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef CAMERATCPCLIENT_H_
#define CAMERATCPCLIENT_H_
#include <QtGlobal>
#include <QtNetwork/QTcpServer>
#include <opencv2/opencv.hpp>
@ -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();

7
src/FindObject.cpp
View File

@ -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);
}
}

15
src/MainWindow.cpp
View File

@ -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());
}

31
src/ros/CameraROS.cpp
View File

@ -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,10 +71,22 @@ 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>(MySyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_);
sync_->registerCallback(boost::bind(&CameraROS::imgDepthReceivedCallback, this, _1, _2, _3));
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_);
approxSync_->registerCallback(boost::bind(&CameraROS::imgDepthReceivedCallback, this, _1, _2, _3));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(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)
{

19
src/ros/CameraROS.h
View File

@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
@ -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<MySyncPolicy> * sync_;
sensor_msgs::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
typedef message_filters::sync_policies::ExactTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
};
#endif /* CAMERAROS_H_ */

53
src/ros/FindObjectROS.cpp
View File

@ -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<find_object_2d::ObjectsStamped>("objectsStamped", 1);
pubInfo_ = nh.advertise<find_object_2d::DetectionInfo>("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<tf::StampedTransform> 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,

12
src/ros/FindObjectROS.h
View File

@ -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_;

10
src/ros/find_object_2d_node.cpp
View File

@ -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)