diff --git a/src/ros/FindObjectROS.cpp b/src/ros/FindObjectROS.cpp index e0c20b80..092ade9c 100644 --- a/src/ros/FindObjectROS.cpp +++ b/src/ros/FindObjectROS.cpp @@ -56,6 +56,8 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info) if(info.objDetected_.size() && !depth_.empty() && depthConstant_ != 0.0f) { std::vector transforms; + char multiSubId = 'b'; + int previousId = -1; QMultiMap::const_iterator iterSizes=info.objDetectedSizes_.constBegin(); for(QMultiMap::const_iterator iter=info.objDetected_.constBegin(); iter!=info.objDetected_.constEnd(); @@ -66,6 +68,17 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info) float objectWidth = iterSizes->width(); float objectHeight = iterSizes->height(); + QString multiSuffix; + if(id == previousId) + { + multiSuffix = QString("_") + multiSubId++; + } + else + { + multiSubId = 'b'; + } + previousId = id; + // Find center of the object QPointF center = iter->map(QPointF(objectWidth/2, objectHeight/2)); QPointF xAxis = iter->map(QPointF(3*objectWidth/4, objectHeight/2)); @@ -92,7 +105,7 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info) { tf::StampedTransform transform; transform.setIdentity(); - transform.child_frame_id_ = QString("%1_%2").arg(objFramePrefix_.c_str()).arg(id).toStdString(); + 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.setOrigin(tf::Vector3(center3D.val[0], center3D.val[1], center3D.val[2])); diff --git a/src/ros/tf_example_node.cpp b/src/ros/tf_example_node.cpp index 3962adc9..73097a4c 100644 --- a/src/ros/tf_example_node.cpp +++ b/src/ros/tf_example_node.cpp @@ -51,11 +51,26 @@ public: { if(msg->objects.data.size()) { + char multiSubId = 'b'; + int previousId = -1; for(unsigned int i=0; iobjects.data.size(); i+=12) { // get data int id = (int)msg->objects.data[i]; - std::string objectFrameId = QString("%1_%2").arg(objFramePrefix_.c_str()).arg(id).toStdString(); // "object_1", "object_2" + + QString multiSuffix; + if(id == previousId) + { + multiSuffix = QString("_") + multiSubId++; + } + else + { + multiSubId = 'b'; + } + previousId = id; + + // "object_1", "object_1_b", "object_1_c", "object_2" + std::string objectFrameId = QString("%1_%2%3").arg(objFramePrefix_.c_str()).arg(id).arg(multiSuffix).toStdString(); tf::StampedTransform pose; tf::StampedTransform poseCam; @@ -73,12 +88,12 @@ public: } // Here "pose" is the position of the object "id" in "/map" frame. - ROS_INFO("Object_%d [x,y,z] [x,y,z,w] in \"%s\" frame: [%f,%f,%f] [%f,%f,%f,%f]", - id, mapFrameId_.c_str(), + ROS_INFO("%s [x,y,z] [x,y,z,w] in \"%s\" frame: [%f,%f,%f] [%f,%f,%f,%f]", + objectFrameId.c_str(), mapFrameId_.c_str(), pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z(), pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z(), pose.getRotation().w()); - ROS_INFO("Object_%d [x,y,z] [x,y,z,w] in \"%s\" frame: [%f,%f,%f] [%f,%f,%f,%f]", - id, msg->header.frame_id.c_str(), + ROS_INFO("%s [x,y,z] [x,y,z,w] in \"%s\" frame: [%f,%f,%f] [%f,%f,%f,%f]", + objectFrameId.c_str(), msg->header.frame_id.c_str(), poseCam.getOrigin().x(), poseCam.getOrigin().y(), poseCam.getOrigin().z(), poseCam.getRotation().x(), poseCam.getRotation().y(), poseCam.getRotation().z(), poseCam.getRotation().w()); }