Commit 9b4fe2c0 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Fixed bug on no changing stamp when tf fails

parent aac06ebb
......@@ -82,8 +82,8 @@ flags.add("amcl_localization", bool_t, 0, "Bo
flags.add("calculate_covariance", bool_t, 0, "Boolean to calculate robot pose covariance", False)
flags.add("publish_tf_map_odom", bool_t, 0, "Boolean to publish tf from map to odom", False)
sensor_noise.add("sensor_sigma_th", double_t, 0, "Sensor angular sigma", 0.035, 0.001, 0.1)
sensor_noise.add("sensor_sigma_r", double_t, 0, "Sensor radial sigma", 0.05, 0.01, 0.2)
sensor_noise.add("sensor_sigma_th", double_t, 0, "Sensor angular sigma", 0.035, 0.001, 1.0)
sensor_noise.add("sensor_sigma_r", double_t, 0, "Sensor radial sigma", 0.05, 0.01, 1.0)
odom_noise.add("odom_fxy", double_t, 0, "Odom linear sigma factor", 0.05, 0.01, 1.0)
odom_noise.add("odom_fth", double_t, 0, "Odom angular sigma factor", 0.05, 0.01, 1.0)
......
......@@ -109,7 +109,6 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
this->estimated_sigma_(1) =this->estimated_sigma_(0);
this->estimated_sigma_(2) =this->estimated_sigma_(0);
}
this->new_estimated_pose_t_ = ros::Time::now();
}
else
{
......@@ -147,6 +146,10 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
this->last_publish_odom_ << 0.0, 0.0, 0.0;
this->publish_pose_ = false;
this->new_estimated_pose_t_ = ros::Time::now();
this->features_stamp_ = ros::Time::now();
this->tf_map_odom_ << 0.0, 0.0, 0.0;
this->robot_pose_msg_.header.frame_id = this->config_.global_frame;
this->robot_pose_msg_.pose.pose.position.z = 0.0;
for (unsigned int i = 0; i < 36; i++)
......@@ -235,6 +238,22 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
if (!this->tf2_buffer.canTransform(this->config_.odom_frame, this->config_.base_link_frame, this->new_estimated_pose_t_, ros::Duration(this->config_.tf_timeout)))
{
ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode:mainNodeThread -> Can't transform from " << this->config_.odom_frame << " to " << this->config_.base_link_frame << " at " << this->new_estimated_pose_t_);
this->new_estimated_pose_t_ = ros::Time::now();
if (this->config_.publish_tf_map_odom && !this->config_.amcl_localization)
{
this->tf_map_odom_msg.header.stamp = ros::Time::now();
this->tf_map_odom_msg.header.frame_id = this->config_.global_frame;
this->tf_map_odom_msg.child_frame_id = this->config_.odom_frame;
geometry_msgs::Transform tf;
tf.translation.x = this->tf_map_odom_(0);
tf.translation.y = this->tf_map_odom_(1);
tf.translation.z = 0.0;
tf2::Quaternion q;
q.setRPY(0.0, 0.0, this->tf_map_odom_(2));
tf.rotation = tf2::toMsg(q);
this->tf_map_odom_msg.transform = tf;
this->tf2_broadcaster.sendTransform(this->tf_map_odom_msg);
}
// this->new_estimated_pose_ = false;
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