From 7a33accddd0c4bc3b63da9305deb79cc0cd607fb Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 21 Nov 2018 14:11:28 -0500 Subject: [PATCH] Added new msg DetectionInfo (#71) --- CMakeLists.txt | 1 + msg/DetectionInfo.msg | 12 ++++++ src/ros/FindObjectROS.cpp | 84 ++++++++++++++++++++++++++++++++------- src/ros/FindObjectROS.h | 1 + 4 files changed, 83 insertions(+), 15 deletions(-) create mode 100644 msg/DetectionInfo.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 0a9ef324..78d87ce4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -296,6 +296,7 @@ ELSE() add_message_files( FILES ObjectsStamped.msg + DetectionInfo.msg ) ## Generate added messages and services with any dependencies listed here diff --git a/msg/DetectionInfo.msg b/msg/DetectionInfo.msg new file mode 100644 index 00000000..bba88ae1 --- /dev/null +++ b/msg/DetectionInfo.msg @@ -0,0 +1,12 @@ + +Header header + +# All arrays should have the same size +std_msgs/Int32[] ids +std_msgs/Int32[] widths +std_msgs/Int32[] heights +std_msgs/String[] filePaths +std_msgs/Int32[] inliers +std_msgs/Int32[] outliers +# 3x3 homography matrix: [h11, h12, h13, h21, h22, h23, h31, h32, h33] (h31 = dx and h32 = dy, see QTransform) +std_msgs/Float32MultiArray[] homographies diff --git a/src/ros/FindObjectROS.cpp b/src/ros/FindObjectROS.cpp index 841dc5e3..9c046050 100644 --- a/src/ros/FindObjectROS.cpp +++ b/src/ros/FindObjectROS.cpp @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "find_object_2d/ObjectsStamped.h" +#include "find_object_2d/DetectionInfo.h" #include @@ -36,7 +37,8 @@ using namespace find_object; FindObjectROS::FindObjectROS(QObject * parent) : FindObject(true, parent), - objFramePrefix_("object") + objFramePrefix_("object"), + depthConstant_(0.0f) { ros::NodeHandle pnh("~"); // public pnh.param("object_prefix", objFramePrefix_, objFramePrefix_); @@ -46,6 +48,7 @@ FindObjectROS::FindObjectROS(QObject * parent) : pub_ = nh.advertise("objects", 1); pubStamped_ = nh.advertise("objectsStamped", 1); + pubInfo_ = nh.advertise("info", 1); this->connect(this, SIGNAL(objectsFound(find_object::DetectionInfo)), this, SLOT(publish(find_object::DetectionInfo))); } @@ -144,30 +147,74 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info) } } - if(pub_.getNumSubscribers() || pubStamped_.getNumSubscribers()) + if(pub_.getNumSubscribers() || pubStamped_.getNumSubscribers() || pubInfo_.getNumSubscribers()) { std_msgs::Float32MultiArray msg; find_object_2d::ObjectsStamped msgStamped; + find_object_2d::DetectionInfo infoMsg; + if(pubInfo_.getNumSubscribers()) + { + infoMsg.ids.resize(info.objDetected_.size()); + infoMsg.widths.resize(info.objDetected_.size()); + infoMsg.heights.resize(info.objDetected_.size()); + infoMsg.filePaths.resize(info.objDetected_.size()); + infoMsg.inliers.resize(info.objDetected_.size()); + infoMsg.outliers.resize(info.objDetected_.size()); + infoMsg.homographies.resize(info.objDetected_.size()); + } msg.data = std::vector(info.objDetected_.size()*12); msgStamped.objects.data = std::vector(info.objDetected_.size()*12); + + ROS_ASSERT(info.objDetected_.size() == info.objDetectedSizes_.size() && + info.objDetected_.size() == info.objDetectedFilePaths_.size() && + info.objDetected_.size() == info.objDetectedInliersCount_.size() && + info.objDetected_.size() == info.objDetectedOutliersCount_.size()); + + int infoIndex=0; int i=0; QMultiMap::const_iterator iterSizes=info.objDetectedSizes_.constBegin(); + QMultiMap::const_iterator iterFilePaths=info.objDetectedFilePaths_.constBegin(); + QMultiMap::const_iterator iterInliers=info.objDetectedInliersCount_.constBegin(); + QMultiMap::const_iterator iterOutliers=info.objDetectedOutliersCount_.constBegin(); for(QMultiMap::const_iterator iter=info.objDetected_.constBegin(); iter!=info.objDetected_.constEnd(); - ++iter, ++iterSizes) + ++iter, ++iterSizes, ++iterFilePaths, ++infoIndex, ++iterInliers, ++iterOutliers) { - msg.data[i] = msgStamped.objects.data[i] = iter.key(); ++i; - msg.data[i] = msgStamped.objects.data[i] = iterSizes->width(); ++i; - msg.data[i] = msgStamped.objects.data[i] = iterSizes->height(); ++i; - msg.data[i] = msgStamped.objects.data[i] = iter->m11(); ++i; - msg.data[i] = msgStamped.objects.data[i] = iter->m12(); ++i; - msg.data[i] = msgStamped.objects.data[i] = iter->m13(); ++i; - msg.data[i] = msgStamped.objects.data[i] = iter->m21(); ++i; - msg.data[i] = msgStamped.objects.data[i] = iter->m22(); ++i; - msg.data[i] = msgStamped.objects.data[i] = iter->m23(); ++i; - msg.data[i] = msgStamped.objects.data[i] = iter->m31(); ++i;// dx - msg.data[i] = msgStamped.objects.data[i] = iter->m32(); ++i;// dy - msg.data[i] = msgStamped.objects.data[i] = iter->m33(); ++i; + if(pub_.getNumSubscribers() || pubStamped_.getNumSubscribers()) + { + msg.data[i] = msgStamped.objects.data[i] = iter.key(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iterSizes->width(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iterSizes->height(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iter->m11(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iter->m12(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iter->m13(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iter->m21(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iter->m22(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iter->m23(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iter->m31(); ++i;// dx + msg.data[i] = msgStamped.objects.data[i] = iter->m32(); ++i;// dy + msg.data[i] = msgStamped.objects.data[i] = iter->m33(); ++i; + } + + if(pubInfo_.getNumSubscribers()) + { + infoMsg.ids[infoIndex].data = iter.key(); + infoMsg.widths[infoIndex].data = iterSizes->width(); + infoMsg.heights[infoIndex].data = iterSizes->height(); + infoMsg.filePaths[infoIndex].data = iterFilePaths.value().toStdString(); + infoMsg.inliers[infoIndex].data = iterInliers.value(); + infoMsg.outliers[infoIndex].data = iterOutliers.value(); + infoMsg.homographies[infoIndex].data.resize(9); + infoMsg.homographies[infoIndex].data[0] = iter->m11(); + infoMsg.homographies[infoIndex].data[1] = iter->m12(); + infoMsg.homographies[infoIndex].data[2] = iter->m13(); + infoMsg.homographies[infoIndex].data[3] = iter->m21(); + infoMsg.homographies[infoIndex].data[4] = iter->m22(); + infoMsg.homographies[infoIndex].data[5] = iter->m23(); + infoMsg.homographies[infoIndex].data[6] = iter->m31(); + infoMsg.homographies[infoIndex].data[7] = iter->m32(); + infoMsg.homographies[infoIndex].data[8] = iter->m33(); + } } if(pub_.getNumSubscribers()) { @@ -180,6 +227,13 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info) msgStamped.header.stamp = 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_; + pubInfo_.publish(infoMsg); + } } } diff --git a/src/ros/FindObjectROS.h b/src/ros/FindObjectROS.h index b4ff48ca..f8ae99e1 100644 --- a/src/ros/FindObjectROS.h +++ b/src/ros/FindObjectROS.h @@ -65,6 +65,7 @@ private: private: ros::Publisher pub_; ros::Publisher pubStamped_; + ros::Publisher pubInfo_; std::string frameId_; ros::Time stamp_;