diff --git a/src/adc_landmarks_slam_solver_alg_node.cpp b/src/adc_landmarks_slam_solver_alg_node.cpp index 0e9420f80ad455211085938898ea9678ebb97ad3..391181bb12c3d219b943e74d8d71e89202a58b57 100644 --- a/src/adc_landmarks_slam_solver_alg_node.cpp +++ b/src/adc_landmarks_slam_solver_alg_node.cpp @@ -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();