Added the "_det" suffix to the reference frame of the qr_detector detection frames.

Added information on the parent link to the map information to properly reconstruct the map.
Transfromed the tag position into the mab base link before using it.

qr_detector/src/qr_detector_driver_node.cpp

@@ -91,7 +91,7 @@ void QrDetectorDriverNode::image_callback(const sensor_msgs::Image::ConstPtr& ms
       this->camera_pose_tag_pose_array_msg_.tags[i].orientation.z   = poses[i][6];      
       transform.setOrigin(tf::Vector3(poses[i][0],poses[i][1],poses[i][2]));
       transform.setRotation(tf::Quaternion(poses[i][4],poses[i][5],poses[i][6],poses[i][3]));
-      this->tag_pose_br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),msg->header.frame_id,tag_ids[i]));
+      this->tag_pose_br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),msg->header.frame_id,tag_ids[i]+"_det"));
     }
     this->camera_pose_publisher_.publish(this->camera_pose_tag_pose_array_msg_);
   }

qr_global_loc/include/qr_global_loc_alg_node.h

@@ -43,6 +43,7 @@
 
 typedef struct {
   std::string name;
+  std::string parent;
   urdf::Pose pose;
 }TQRTag;
 

qr_global_loc/src/qr_global_loc_alg_node.cpp

@@ -52,7 +52,7 @@ void QrGlobalLocAlgNode::mainNodeThread(void)
   {
     for(i=0;i<this->map_tags.size();i++)
     {
-      odom_trans.header.frame_id = this->map_frame_id;
+      odom_trans.header.frame_id = this->map_tags[i].parent;
       odom_trans.child_frame_id = this->map_tags[i].name;
       odom_trans.header.stamp = ros::Time::now();
       odom_trans.transform.translation.x = this->map_tags[i].pose.position.x;
@@ -129,11 +129,11 @@ void QrGlobalLocAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_pose_a
 
   ROS_DEBUG("QrGlobalLocAlgNode::qr_pose_callback: New Message Received");
   //use appropiate mutex to shared variables if necessary
-  this->alg_.lock();
   //this->qr_pose_mutex_enter();
  
   odom_frame=this->config_.tf_prefix + "/odom";
   tf_exists=this->tf_listener_.waitForTransform(odom_frame,msg->header.frame_id,msg->header.stamp,ros::Duration(1), ros::Duration(0.01));
+  this->alg_.lock();
   if(tf_exists)
   {
     // camera transform
@@ -146,13 +146,14 @@ void QrGlobalLocAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_pose_a
     // QR detection library transfrom
     position << 0.0,0.0,0.0;
     rotation << -1.5707 , 0.0 , -1.5707;
+    //rotation << 0.0 , 0.0 , 0.0;
     cam1_transform=this->alg_.generate_transform(position,rotation);
     position << 0.0,0.0,0.0;
     rotation << 0.0 , 0.0 , 3.14159;
+    //rotation << 0.0 , 0.0 , 0;
     cam2_transform=this->alg_.generate_transform(position,rotation);
     for(i=0;i<msg->tags.size();i++)
     {
-      // QR transform 
       position << msg->tags[i].position.x , msg->tags[i].position.y , msg->tags[i].position.z;
       tf::quaternionMsgToTF(msg->tags[i].orientation,tf_quat);
       tf_rotation=tf::Matrix3x3(tf_quat);
@@ -175,10 +176,31 @@ void QrGlobalLocAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_pose_a
       {
         if(msg->tags[i].tag_id==this->map_tags[j].name)
         {
-          position << this->map_tags[j].pose.position.x , this->map_tags[j].pose.position.y , this->map_tags[j].pose.position.z; 
-          this->map_tags[j].pose.rotation.getRPY(rotation(0),rotation(1),rotation(2));
-          map_transform=this->alg_.generate_transform(position,rotation);
-          break;
+          this->alg_.unlock();
+          tf_exists=this->tf_listener_.waitForTransform(this->map_frame_id,msg->tags[i].tag_id,msg->header.stamp,ros::Duration(1), ros::Duration(0.01));
+          this->alg_.lock();
+          if(tf_exists)
+          {
+            geometry_msgs::PoseStamped pose;
+            pose.header.stamp=msg->header.stamp;
+            pose.header.frame_id=msg->tags[i].tag_id;
+            pose.pose.position.x=0.0;
+            pose.pose.position.y=0.0;
+            pose.pose.position.z=0.0;
+            pose.pose.orientation.x=0.0;
+            pose.pose.orientation.y=0.0;
+            pose.pose.orientation.z=0.0;
+            pose.pose.orientation.w=1.0;
+            this->tf_listener_.transformPose(this->map_frame_id,pose,pose);
+            position << pose.pose.position.x , pose.pose.position.y , pose.pose.position.z;
+            tf::quaternionMsgToTF(pose.pose.orientation,tf_quat);
+            tf_rotation=tf::Matrix3x3(tf_quat);
+            tf_rotation.getRPY(rotation(0),rotation(1),rotation(2));
+            map_transform=this->alg_.generate_transform(position,rotation);
+            break;
+          }
+          else
+            ROS_ERROR_STREAM("Transform between " << this->map_frame_id << " and " << msg->tags[i].tag_id << " does not exist");
         }
       } 
       // compute the total transform odom map
@@ -260,6 +282,7 @@ void QrGlobalLocAlgNode::node_config_update(Config &config, uint32_t level)
         parent_joint=links[i]->parent_joint;
         if(parent_joint!=NULL)
         {
+          new_map_tag.parent=parent_joint->parent_link_name;
           new_map_tag.pose=parent_joint->parent_to_joint_origin_transform;
           ROS_INFO("Processed a tag named %s",new_map_tag.name.c_str());
           this->map_tags.push_back(new_map_tag);
@@ -289,6 +312,7 @@ void QrGlobalLocAlgNode::node_config_update(Config &config, uint32_t level)
         parent_joint=links[i]->parent_joint;
         if(parent_joint!=NULL)
         {
+          new_map_tag.parent=parent_joint->parent_link_name;
           new_map_tag.pose=parent_joint->parent_to_joint_origin_transform;
           ROS_INFO("Processed a tag named %s",new_map_tag.name.c_str());
           this->map_tags.push_back(new_map_tag);