diff --git a/qr_detector/src/qr_detector_driver_node.cpp b/qr_detector/src/qr_detector_driver_node.cpp
index 3d8e76aaa66d4fd1b42968945640a041931f9080..b5831053f937ffdf2727c7561cadb651934128c3 100644
--- a/qr_detector/src/qr_detector_driver_node.cpp
+++ b/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_);
}
diff --git a/qr_global_loc/include/qr_global_loc_alg_node.h b/qr_global_loc/include/qr_global_loc_alg_node.h
index 0fd296b875fba2288bdd890bf9f095364f71ebc1..8857bb41475fdde97dd90b67119182e1d3112d1c 100644
--- a/qr_global_loc/include/qr_global_loc_alg_node.h
+++ b/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;
diff --git a/qr_global_loc/src/qr_global_loc_alg_node.cpp b/qr_global_loc/src/qr_global_loc_alg_node.cpp
index 84685aa067e1cc0bf9d8f142005bafea2a8730bd..aae0b2907fde4c8934e60e3fa34a1a9924171801 100644
--- a/qr_global_loc/src/qr_global_loc_alg_node.cpp
+++ b/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);