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) if(info.objDetected_.size() && !depth_.empty() && depthConstant_ != 0.0f)
{ {
std::vector<tf::StampedTransform> transforms; std::vector<tf::StampedTransform> transforms;
char multiSubId = 'b';
int previousId = -1;
QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin(); QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
for(QMultiMap<int, QTransform>::const_iterator iter=info.objDetected_.constBegin(); for(QMultiMap<int, QTransform>::const_iterator iter=info.objDetected_.constBegin();
iter!=info.objDetected_.constEnd(); iter!=info.objDetected_.constEnd();
@ -66,6 +68,17 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info)
float objectWidth = iterSizes->width(); float objectWidth = iterSizes->width();
float objectHeight = iterSizes->height(); float objectHeight = iterSizes->height();
QString multiSuffix;
if(id == previousId)
{
multiSuffix = QString("_") + multiSubId++;
}
else
{
multiSubId = 'b';
}
previousId = id;
// Find center of the object // Find center of the object
QPointF center = iter->map(QPointF(objectWidth/2, objectHeight/2)); QPointF center = iter->map(QPointF(objectWidth/2, objectHeight/2));
QPointF xAxis = iter->map(QPointF(3*objectWidth/4, 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; tf::StampedTransform transform;
transform.setIdentity(); 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.frame_id_ = frameId_;
transform.stamp_ = stamp_; transform.stamp_ = stamp_;
transform.setOrigin(tf::Vector3(center3D.val[0], center3D.val[1], center3D.val[2])); 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()) if(msg->objects.data.size())
{ {
char multiSubId = 'b';
int previousId = -1;
for(unsigned int i=0; i<msg->objects.data.size(); i+=12) for(unsigned int i=0; i<msg->objects.data.size(); i+=12)
{ {
// get data // get data
int id = (int)msg->objects.data[i]; 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 pose;
tf::StampedTransform poseCam; tf::StampedTransform poseCam;
@ -73,12 +88,12 @@ public:
} }
// Here "pose" is the position of the object "id" in "/map" frame. // 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]", ROS_INFO("%s [x,y,z] [x,y,z,w] in \"%s\" frame: [%f,%f,%f] [%f,%f,%f,%f]",
id, mapFrameId_.c_str(), objectFrameId.c_str(), mapFrameId_.c_str(),
pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z(), pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z(),
pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z(), pose.getRotation().w()); 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]", ROS_INFO("%s [x,y,z] [x,y,z,w] in \"%s\" frame: [%f,%f,%f] [%f,%f,%f,%f]",
id, msg->header.frame_id.c_str(), objectFrameId.c_str(), msg->header.frame_id.c_str(),
poseCam.getOrigin().x(), poseCam.getOrigin().y(), poseCam.getOrigin().z(), poseCam.getOrigin().x(), poseCam.getOrigin().y(), poseCam.getOrigin().z(),
poseCam.getRotation().x(), poseCam.getRotation().y(), poseCam.getRotation().z(), poseCam.getRotation().w()); poseCam.getRotation().x(), poseCam.getRotation().y(), poseCam.getRotation().z(), poseCam.getRotation().w());
} }