Commit 71a43d80 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Cleaned code

parent b962b289
......@@ -463,51 +463,6 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
// Initialize the topic message structure
//this->landmarks_localization_pose_PoseWithCovarianceStamped_msg_.data = my_var;
/*
//tf2_listener example BEGIN
try{
std::string target_frame = "child_frame";
std::string source_frame = "parent_frame";
ros::Time time = ros::Time::now();
ros::Duration timeout = ros::Duration(0.1);
this->alg_.unlock();
bool tf2_exists = this->tf2_buffer.canTransform(target_frame, source_frame, time, timeout);
this->alg_.lock();
if(tf2_exists){
geometry_msgs::PoseStamped stamped_pose_in;
stamped_pose_in.header.stamp = time;
stamped_pose_in.header.frame_id = source_frame;
stamped_pose_in.pose.position.x = 1.0;
stamped_pose_in.pose.position.y = 0.0;
stamped_pose_in.pose.position.z = 0.0;
tf2::Quaternion quat_tf;
quat_tf.setRPY(0.0, 0.0, 1.5709);
geometry_msgs::Quaternion quat_msg;
tf2::convert(quat_tf, quat_msg);
stamped_pose_in.pose.orientation = quat_msg;
double yaw, pitch, roll;
tf2::Matrix3x3(tf2::Quaternion(quat_msg.x, quat_msg.y,quat_msg.z, quat_msg.w)).getEulerYPR(yaw, pitch, roll);
ROS_INFO("Original pose in '%s' frame, with (x,y,z)=[%f,%f,%f], (yaw,pitch,roll)=[%f,%f,%f]", stamped_pose_in.header.frame_id.c_str(), stamped_pose_in.pose.position.x, stamped_pose_in.pose.position.y, stamped_pose_in.pose.position.z, yaw, pitch, roll);
geometry_msgs::PoseStamped stamped_pose_out;
geometry_msgs::TransformStamped transform;
transform = this->tf2_buffer.lookupTransform(target_frame, source_frame, time);
quat_msg = transform.transform.rotation;
tf2::Matrix3x3(tf2::Quaternion(quat_msg.x, quat_msg.y,quat_msg.z, quat_msg.w)).getEulerYPR(yaw, pitch, roll);
ROS_INFO("Found transform betwen frames (%s-->%s) with (x,y,z)=[%f,%f,%f], (yaw,pitch,roll)=[%f,%f,%f]", source_frame.c_str(), target_frame.c_str(), transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z, yaw, pitch, roll);
tf2::doTransform(stamped_pose_in, stamped_pose_out, transform);
quat_msg = stamped_pose_out.pose.orientation;
tf2::Matrix3x3(tf2::Quaternion(quat_msg.x, quat_msg.y,quat_msg.z, quat_msg.w)).getEulerYPR(yaw, pitch, roll);
ROS_INFO("Transformed pose in '%s' frame, with (x,y,z)=[%f,%f,%f], (yaw,pitch,roll)=[%f,%f,%f]", stamped_pose_out.header.frame_id.c_str(), stamped_pose_out.pose.position.x, stamped_pose_out.pose.position.y, stamped_pose_out.pose.position.z, yaw, pitch, roll);
ROS_INFO("---");
}else{
ROS_WARN("No transform found from '%s' to '%s'", source_frame.c_str(), target_frame.c_str()); }
}catch (tf2::TransformException &ex){
ROS_ERROR("TF2 Exception: %s",ex.what()); }
//tf2_listener example END
*/
// [fill srv structure and make request to the server]
// [fill action structure and make request to the action server]
......@@ -522,25 +477,6 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
// Uncomment the following line to publish the topic message
//this->landmarks_localization_pose_publisher_.publish(this->landmarks_localization_pose_PoseWithCovarianceStamped_msg_);
/*
//tf2_broadcaster example BEGIN
this->transform_msg.header.stamp = ros::Time::now();
this->transform_msg.header.frame_id = "parent_frame";
this->transform_msg.child_frame_id = "child_frame";
geometry_msgs::Transform t;
t.translation.x = 0.0;
t.translation.y = 0.0;
t.translation.z = 0.0;
tf2::Quaternion q;
q.setRPY(0.0, 0.0, 0.0);
t.rotation = tf2::toMsg(q);
this->transform_msg.transform = t;
this->tf2_broadcaster.sendTransform(this->transform_msg);
//tf2_broadcaster example END
*/
// this->alg_.unlock();
this->estimated_pose_mutex_exit();
this->features_mutex_exit();
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment