Revision history [back]

I rectified the error by :

 tf::TransformListener listener;
tf::StampedTransform transform_cd;
tf::Transform  transform_tc,transform_td;
try
{
  for(unsigned int i=0; i<tag_detection_array.detections.size(); i++) {
    geometry_msgs::PoseStamped pose;
    pose.pose = tag_detection_array.detections[i].pose.pose.pose;
    pose.header = tag_detection_array.detections[i].pose.header;
    tf::Stamped<tf::Transform> tag_transform, tf_td;
    tf::poseStampedMsgToTF(pose, tag_transform);
    listener.lookupTransform("camera", "drone",  tag_transform.stamp_, transform_cd);
    transform_tc = tag_transform.inverse();
    transform_td = transform_tc*transform_cd;
    tf_td.setData(transform_td);
    tf::Matrix3x3(transform_td.getRotation()).getRPY(uavRollENU, uavPitchENU, uavYawENU);
    if(pose.pose.position.z>0.1 && fabs(uavPitchENU) < 30/57.2958 && fabs(uavRollENU) < 30/57.2958)
    {
      tf_pub_.sendTransform(tf::StampedTransform(tag_transform,
                                              tag_transform.stamp_,
                                              camera_tf_frame_,
                                              detection_names[i]));
      tf::poseStampedTFToMsg(tf_td, pose);