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 05a861021d81742ba571ba052f65bffe4b7cacd1..b3297cf48fd826a7fc8412c813116c046afedc1b 100644
--- a/qr_global_loc/src/qr_global_loc_alg_node.cpp
+++ b/qr_global_loc/src/qr_global_loc_alg_node.cpp
@@ -2,252 +2,255 @@
#include <urdf_model/link.h>
QrGlobalLocAlgNode::QrGlobalLocAlgNode(void) :
- algorithm_base::IriBaseAlgorithm<QrGlobalLocAlgorithm>()
+ algorithm_base::IriBaseAlgorithm<QrGlobalLocAlgorithm>()
{
- //init class attributes if necessary
- this->loop_rate_ = 10;//in [Hz]
-
- // [init publishers]
-
- // [init subscribers]
- this->initialpose_subscriber_ = this->public_node_handle_.subscribe("initialpose", 1, &QrGlobalLocAlgNode::initialpose_callback, this);
- pthread_mutex_init(&this->initialpose_mutex_,NULL);
-
- this->qr_pose_subscriber_ = this->public_node_handle_.subscribe("qr_pose", 1, &QrGlobalLocAlgNode::qr_pose_callback, this);
- pthread_mutex_init(&this->qr_pose_mutex_,NULL);
-
-
- // [init services]
-
- // [init clients]
-
- // [init action servers]
-
- // [init action clients]
+ //init class attributes if necessary
+ this->loop_rate_ = 10;//in [Hz]
+ this->iteration = 0;
+ // [init publishers]
+
+ // [init subscribers]
+ this->initialpose_subscriber_ = this->public_node_handle_.subscribe("initialpose", 1, &QrGlobalLocAlgNode::initialpose_callback, this);
+ pthread_mutex_init(&this->initialpose_mutex_,NULL);
+
+ this->qr_pose_subscriber_ = this->public_node_handle_.subscribe("qr_pose", 1, &QrGlobalLocAlgNode::qr_pose_callback, this);
+ pthread_mutex_init(&this->qr_pose_mutex_,NULL);
+
+
+ // [init services]
+
+ // [init clients]
+
+ // [init action servers]
+
+ // [init action clients]
}
QrGlobalLocAlgNode::~QrGlobalLocAlgNode(void)
{
- // [free dynamic memory]
- pthread_mutex_destroy(&this->initialpose_mutex_);
- pthread_mutex_destroy(&this->qr_pose_mutex_);
+ // [free dynamic memory]
+ pthread_mutex_destroy(&this->initialpose_mutex_);
+ pthread_mutex_destroy(&this->qr_pose_mutex_);
}
void QrGlobalLocAlgNode::mainNodeThread(void)
{
- unsigned int i=0;
- tf::Quaternion tf_quat;
- geometry_msgs::TransformStamped odom_trans;
- std::string odom_frame;
- // [fill msg structures]
-
- // [fill srv structure and make request to the server]
-
- // [fill action structure and make request to the action server]
-
- // [publish messages]
- this->alg_.lock();
- // publish the map tags transforms
- if(this->config_.publish_tf)
- {
- for(i=0;i<this->map_tags.size();i++)
+ unsigned int i=0;
+ tf::Quaternion tf_quat;
+ geometry_msgs::TransformStamped odom_trans;
+ std::string odom_frame;
+ // [fill msg structures]
+
+ // [fill srv structure and make request to the server]
+
+ // [fill action structure and make request to the action server]
+
+ // [publish messages]
+ this->alg_.lock();
+ // publish the map tags transforms
+ if(this->config_.publish_tf)
{
- 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;
- odom_trans.transform.translation.y = this->map_tags[i].pose.position.y;
- odom_trans.transform.translation.z = this->map_tags[i].pose.position.z;
- tf_quat=tf::Quaternion(this->map_tags[i].pose.rotation.x,this->map_tags[i].pose.rotation.y,this->map_tags[i].pose.rotation.z,this->map_tags[i].pose.rotation.w);
- tf::quaternionTFToMsg(tf_quat,odom_trans.transform.rotation);
- this->odom_broadcaster.sendTransform(odom_trans);
+ for(i=0;i<this->map_tags.size();i++)
+ {
+ 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;
+ odom_trans.transform.translation.y = this->map_tags[i].pose.position.y;
+ odom_trans.transform.translation.z = this->map_tags[i].pose.position.z;
+ tf_quat=tf::Quaternion(this->map_tags[i].pose.rotation.x,this->map_tags[i].pose.rotation.y,this->map_tags[i].pose.rotation.z,this->map_tags[i].pose.rotation.w);
+ tf::quaternionTFToMsg(tf_quat,odom_trans.transform.rotation);
+ this->odom_broadcaster.sendTransform(odom_trans);
+ }
}
- }
- // publish the robot transform
- if(this->config_.publish_tf)
- {
- odom_frame=this->config_.tf_prefix + "/odom";
- odom_trans.header.frame_id = this->map_frame_id;
- odom_trans.child_frame_id = odom_frame;
- odom_trans.header.stamp = ros::Time::now();
- odom_trans.transform.translation.x = this->robot_position(0);
- odom_trans.transform.translation.y = this->robot_position(1);
- odom_trans.transform.translation.z = this->robot_position(2);
- odom_trans.transform.rotation=tf::createQuaternionMsgFromRollPitchYaw(this->robot_rotation(0),this->robot_rotation(1),this->robot_rotation(2));
- this->odom_broadcaster.sendTransform(odom_trans);
- }
- this->alg_.unlock();
+ // publish the robot transform
+ if(this->config_.publish_tf)
+ {
+ odom_frame=this->config_.tf_prefix + "/odom";
+ odom_trans.header.frame_id = this->map_frame_id;
+ odom_trans.child_frame_id = odom_frame;
+ odom_trans.header.stamp = ros::Time::now();
+ odom_trans.transform.translation.x = this->robot_position(0);
+ odom_trans.transform.translation.y = this->robot_position(1);
+ odom_trans.transform.translation.z = this->robot_position(2);
+ odom_trans.transform.rotation=tf::createQuaternionMsgFromRollPitchYaw(this->robot_rotation(0),this->robot_rotation(1),this->robot_rotation(2));
+ this->odom_broadcaster.sendTransform(odom_trans);
+ }
+ this->alg_.unlock();
}
/* [subscriber callbacks] */
void QrGlobalLocAlgNode::initialpose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
- tf::Quaternion tf_quat;
- tf::Matrix3x3 tf_rotation;
-
- ROS_DEBUG("QrGlobalLocAlgNode::initialpose_callback: New Message Received");
-
- //use appropiate mutex to shared variables if necessary
- this->alg_.lock();
- //this->initialpose_mutex_enter();
- this->robot_position(0)=msg->pose.pose.position.x;
- this->robot_position(1)=msg->pose.pose.position.y;
- this->robot_position(2)=msg->pose.pose.position.z;
- tf::quaternionMsgToTF(msg->pose.pose.orientation,tf_quat);
- tf_rotation=tf::Matrix3x3(tf_quat);
- tf_rotation.getRPY(this->robot_rotation(0),this->robot_rotation(1),this->robot_rotation(2));
- this->robot_transform=this->alg_.generate_transform(this->robot_position,this->robot_rotation);
- //std::cout << msg->data << std::endl;
- //unlock previously blocked shared variables
- this->alg_.unlock();
- //this->initialpose_mutex_exit();
+ tf::Quaternion tf_quat;
+ tf::Matrix3x3 tf_rotation;
+
+ ROS_ERROR("QrGlobalLocAlgNode::initialpose_callback: New Message Received");
+
+ //use appropiate mutex to shared variables if necessary
+ this->alg_.lock();
+ //this->initialpose_mutex_enter();
+ this->robot_position(0)=msg->pose.pose.position.x;
+ this->robot_position(1)=msg->pose.pose.position.y;
+ this->robot_position(2)=msg->pose.pose.position.z;
+ tf::quaternionMsgToTF(msg->pose.pose.orientation,tf_quat);
+ tf_rotation=tf::Matrix3x3(tf_quat);
+ tf_rotation.getRPY(this->robot_rotation(0),this->robot_rotation(1),this->robot_rotation(2));
+ this->robot_transform=this->alg_.generate_transform(this->robot_position,this->robot_rotation);
+ //std::cout << msg->data << std::endl;
+ //unlock previously blocked shared variables
+ this->alg_.unlock();
+ //this->initialpose_mutex_exit();
}
void QrGlobalLocAlgNode::initialpose_mutex_enter(void)
{
- pthread_mutex_lock(&this->initialpose_mutex_);
+ pthread_mutex_lock(&this->initialpose_mutex_);
}
void QrGlobalLocAlgNode::initialpose_mutex_exit(void)
{
- pthread_mutex_unlock(&this->initialpose_mutex_);
+ pthread_mutex_unlock(&this->initialpose_mutex_);
}
void QrGlobalLocAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_pose_array::ConstPtr& msg)
{
- geometry_msgs::TransformStamped odom_trans;
- tf::StampedTransform tf_transform;
- tf::Quaternion tf_quat;
- tf::Matrix3x3 tf_rotation;
- tf::Vector3 tf_position;
- Eigen::Matrix4d body_transform,qr_transform,cam1_transform,cam2_transform,map_transform;
- std::vector< Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d> > total_transforms;
- Eigen::Vector3d position,rotation;
- std::string odom_frame;
- unsigned int i=0,j=0;
- bool tf_exists;
-
- ROS_DEBUG("QrGlobalLocAlgNode::qr_pose_callback: New Message Received");
- //use appropiate mutex to shared variables if necessary
- //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
- this->tf_listener_.lookupTransform(odom_frame,msg->header.frame_id,msg->header.stamp,tf_transform);
- tf_rotation=tf_transform.getBasis();
- tf_rotation.getRPY(rotation(0),rotation(1),rotation(2));
- tf_position=tf_transform.getOrigin();
- position << tf_position.getX(), tf_position.getY(), tf_position.getZ();
- body_transform=this->alg_.generate_transform(position,rotation);
- // 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++)
+ geometry_msgs::TransformStamped odom_trans;
+ tf::StampedTransform tf_transform;
+ tf::Quaternion tf_quat;
+ tf::Matrix3x3 tf_rotation;
+ tf::Vector3 tf_position;
+ Eigen::Matrix4d body_transform,qr_transform,cam1_transform,cam2_transform,map_transform;
+ std::vector< Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d> > total_transforms;
+ Eigen::Vector3d position,rotation;
+ Eigen::Vector3d estimated_position, estimated_rotation;
+ std::string odom_frame;
+ double smooth = 0.3;
+ unsigned int i=0,j=0;
+ bool tf_exists;
+
+ ROS_DEBUG("QrGlobalLocAlgNode::qr_pose_callback: New Message Received");
+ //use appropiate mutex to shared variables if necessary
+ //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)
{
- 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);
- tf_rotation.getRPY(rotation(0),rotation(1),rotation(2));
- qr_transform=this->alg_.generate_transform(position,rotation);
- map_transform=cam1_transform*qr_transform*cam2_transform;
- if(this->config_.publish_tf)
- {
- odom_trans.header.frame_id = msg->header.frame_id;
- odom_trans.child_frame_id = msg->tags[i].tag_id + "_cam";
- odom_trans.header.stamp = ros::Time::now();
- this->alg_.get_transform_info(map_transform,position,rotation);
- odom_trans.transform.translation.x = position(0);
- odom_trans.transform.translation.y = position(1);
- odom_trans.transform.translation.z = position(2);
- odom_trans.transform.rotation=tf::createQuaternionMsgFromRollPitchYaw(rotation(0),rotation(1),rotation(2));
- this->odom_broadcaster.sendTransform(odom_trans);
- }
- for(j=0;j<this->map_tags.size();j++)
- {
- if(msg->tags[i].tag_id==this->map_tags[j].name)
+ // camera transform
+ //Saves transform form cam_link to odom and saves in tf_transfom.
+ this->tf_listener_.lookupTransform(odom_frame,msg->header.frame_id,msg->header.stamp,tf_transform);
+ tf_rotation=tf_transform.getBasis();
+ tf_rotation.getRPY(rotation(0),rotation(1),rotation(2));
+ tf_position=tf_transform.getOrigin();
+ position << tf_position.getX(), tf_position.getY(), tf_position.getZ();
+ body_transform=this->alg_.generate_transform(position,rotation);
+ // 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++)
{
- 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);
+ 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);
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");
+ qr_transform=this->alg_.generate_transform(position,rotation);
+ map_transform=cam1_transform*qr_transform*cam2_transform;
+ if(this->config_.publish_tf)
+ {
+ odom_trans.header.frame_id = msg->header.frame_id;
+ odom_trans.child_frame_id = msg->tags[i].tag_id + "_cam";
+ odom_trans.header.stamp = ros::Time::now();
+ this->alg_.get_transform_info(map_transform,position,rotation);
+ odom_trans.transform.translation.x = position(0);
+ odom_trans.transform.translation.y = position(1);
+ odom_trans.transform.translation.z = position(2);
+ odom_trans.transform.rotation=tf::createQuaternionMsgFromRollPitchYaw(rotation(0),rotation(1),rotation(2));
+ this->odom_broadcaster.sendTransform(odom_trans);
+ }
+ for(j=0;j<this->map_tags.size();j++)
+ {
+ if(msg->tags[i].tag_id==this->map_tags[j].name)
+ {
+ 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
+ total_transforms.push_back(map_transform*cam2_transform.inverse()*qr_transform.inverse()*cam1_transform.inverse()*body_transform.inverse());
}
- }
- // compute the total transform odom map
- total_transforms.push_back(map_transform*cam2_transform.inverse()*qr_transform.inverse()*cam1_transform.inverse()*body_transform.inverse());
- }
- // average all the transforms
- if(total_transforms.size()>0)
- {
- this->robot_position(0)=0.0;
- this->robot_position(1)=0.0;
- this->robot_position(2)=0.0;
- this->robot_rotation(0)=0.0;
- this->robot_rotation(1)=0.0;
- this->robot_rotation(2)=0.0;
- for(i=0;i<total_transforms.size();i++)
- {
- this->alg_.get_transform_info(total_transforms[i],position,rotation);
- this->robot_position(0)+=position(0);
- this->robot_position(1)+=position(1);
- this->robot_position(2)+=position(2);
- this->robot_rotation(0)+=rotation(0);
- this->robot_rotation(1)+=rotation(1);
- this->robot_rotation(2)+=rotation(2);
- }
- this->robot_position(0)/=total_transforms.size();
- this->robot_position(1)/=total_transforms.size();
- this->robot_position(2)/=total_transforms.size();
- this->robot_rotation(0)/=total_transforms.size();
- this->robot_rotation(1)/=total_transforms.size();
- this->robot_rotation(2)/=total_transforms.size();
- this->robot_transform=this->alg_.generate_transform(this->robot_position,this->robot_rotation);
+ // average all the transforms
+ if(total_transforms.size()>0)
+ {
+ //We are averaging the readings and then running a first odrder exponential filter using the current and past averages.
+ // That's OK for static sensors but shall not be used on long term moving robots.
+ // This should be a wheighted average based on the quality of the reading and then exponentialy filtered using only recent
+ // averages (depending on the frequency of the data), not the whole set of averages.
+
+ //After some tests, it seems that the results are not bad for the global exponential filter.
+
+ estimated_position = Eigen::Vector3d::Zero();
+ estimated_rotation = Eigen::Vector3d::Zero();
+ for(i = 0; i < total_transforms.size(); i++)
+ {
+ this->alg_.get_transform_info(total_transforms[i], position, rotation);
+ estimated_position += position;
+ estimated_rotation += rotation;
+ }
+ estimated_position /= total_transforms.size();
+ estimated_rotation /= total_transforms.size();
+
+ this->robot_position = smooth*(estimated_position) + (1.0 - smooth)*this->robot_position;
+ this->robot_rotation = smooth*(estimated_rotation) + (1.0 - smooth)*this->robot_rotation;
+ this->robot_transform = this->alg_.generate_transform(this->robot_position, this->robot_rotation);
+
+
+ }
}
- }
- //std::cout << msg->data << std::endl;
- //unlock previously blocked shared variables
- this->alg_.unlock();
- //this->qr_pose_mutex_exit();
+ //std::cout << msg->data << std::endl;
+ //unlock previously blocked shared variables
+ this->alg_.unlock();
+ //this->qr_pose_mutex_exit();
}
void QrGlobalLocAlgNode::qr_pose_mutex_enter(void)
{
- pthread_mutex_lock(&this->qr_pose_mutex_);
+ pthread_mutex_lock(&this->qr_pose_mutex_);
}
void QrGlobalLocAlgNode::qr_pose_mutex_exit(void)
{
- pthread_mutex_unlock(&this->qr_pose_mutex_);
+ pthread_mutex_unlock(&this->qr_pose_mutex_);
}
@@ -259,75 +262,75 @@ void QrGlobalLocAlgNode::qr_pose_mutex_exit(void)
void QrGlobalLocAlgNode::node_config_update(Config &config, uint32_t level)
{
- std::vector< boost::shared_ptr<urdf::Link> > links;
- boost::shared_ptr<urdf::Joint> parent_joint;
- TQRTag new_map_tag;
- unsigned int i=0;
-
- this->alg_.lock();
- if(config.qr_map_file!="" && this->config_.qr_map_file!=config.qr_map_file)
- {
- if(!this->map_model.initFile(config.qr_map_file))
- ROS_ERROR("Failed to parse urdf file");
- else
+ std::vector< boost::shared_ptr<urdf::Link> > links;
+ boost::shared_ptr<urdf::Joint> parent_joint;
+ TQRTag new_map_tag;
+ unsigned int i=0;
+
+ this->alg_.lock();
+ if(config.qr_map_file!="" && this->config_.qr_map_file!=config.qr_map_file)
{
- this->map_tags.clear();
- ROS_INFO("Map file successfully loaded");
- /* process all link of the Map file */
- this->map_model.getLinks(links);
- for(i=0;i<links.size();i++)
- {
- new_map_tag.name=links[i]->name;
- // get the transform from the world frame
- 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);
- }
+ if(!this->map_model.initFile(config.qr_map_file))
+ ROS_ERROR("Failed to parse urdf file");
else
{
- ROS_INFO("Main frame ID: %s",links[i]->name.c_str());
- this->map_frame_id=links[i]->name;
+ this->map_tags.clear();
+ ROS_INFO("Map file successfully loaded");
+ /* process all link of the Map file */
+ this->map_model.getLinks(links);
+ for(i=0;i<links.size();i++)
+ {
+ new_map_tag.name=links[i]->name;
+ // get the transform from the world frame
+ 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);
+ }
+ else
+ {
+ ROS_INFO("Main frame ID: %s",links[i]->name.c_str());
+ this->map_frame_id=links[i]->name;
+ }
+ }
}
- }
}
- }
- else if(config.qr_map_param!="" && this->config_.qr_map_param!=config.qr_map_param)
- {
- if(!this->map_model.initParam(config.qr_map_param))
- ROS_ERROR("Failed to parse urdf parameter");
- else
+ else if(config.qr_map_param!="" && this->config_.qr_map_param!=config.qr_map_param)
{
- this->map_tags.clear();
- ROS_INFO("Map file successfully loaded");
- /* process all link of the Map file */
- this->map_model.getLinks(links);
- for(i=0;i<links.size();i++)
- {
- new_map_tag.name=links[i]->name;
- // get the transform from the world frame
- 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);
- }
+ if(!this->map_model.initParam(config.qr_map_param))
+ ROS_ERROR("Failed to parse urdf parameter");
else
{
- ROS_INFO("Main frame ID: %s",links[i]->name.c_str());
- this->map_frame_id=links[i]->name;
+ this->map_tags.clear();
+ ROS_INFO("Map file successfully loaded");
+ /* process all link of the Map file */
+ this->map_model.getLinks(links);
+ for(i=0;i<links.size();i++)
+ {
+ new_map_tag.name=links[i]->name;
+ // get the transform from the world frame
+ 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);
+ }
+ else
+ {
+ ROS_INFO("Main frame ID: %s",links[i]->name.c_str());
+ this->map_frame_id=links[i]->name;
+ }
+ }
}
- }
}
- }
- this->loop_rate_=config.publish_rate;
- this->config_=config;
- this->alg_.unlock();
+ this->loop_rate_=config.publish_rate;
+ this->config_=config;
+ this->alg_.unlock();
}
void QrGlobalLocAlgNode::addNodeDiagnostics(void)
@@ -337,5 +340,5 @@ void QrGlobalLocAlgNode::addNodeDiagnostics(void)
/* main function */
int main(int argc,char *argv[])
{
- return algorithm_base::main<QrGlobalLocAlgNode>(argc, argv, "qr_global_loc_alg_node");
+ return algorithm_base::main<QrGlobalLocAlgNode>(argc, argv, "qr_global_loc_alg_node");
}