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

ROS: fixed TF when multiDetection is enabled (#54)

Browse Source
This commit is contained in:
matlabbe 2018-05-29 14:53:05 -04:00
parent 890ad0fe0a
commit 2cb9120f74
2 changed files with 34 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/ros/FindObjectROS.cpp
View File

@ -56,6 +56,8 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info)
if(info.objDetected_.size() && !depth_.empty() && depthConstant_ != 0.0f)
{
std::vector<tf::StampedTransform> transforms;
char multiSubId = 'b';
int previousId = -1;
QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
for(QMultiMap<int, QTransform>::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]));

25
src/ros/tf_example_node.cpp
View File

@ -51,11 +51,26 @@ public:
{
if(msg->objects.data.size())
{
char multiSubId = 'b';
int previousId = -1;
for(unsigned int i=0; i<msg->objects.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());
}