From 0830d5540bd31eeb6639ffd6094425efe896eab8 Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Mon, 4 Apr 2022 11:09:24 +0200
Subject: [PATCH] Fixed bug on relocalization odometry angle calculation
---
config/params.yaml | 4 ++--
src/collision_manager_alg_node.cpp | 24 ++++++++++++------------
2 files changed, 14 insertions(+), 14 deletions(-)
diff --git a/config/params.yaml b/config/params.yaml
index bd2e5d9..621fb7c 100644
--- a/config/params.yaml
+++ b/config/params.yaml
@@ -1,4 +1,4 @@
-rate: 120
+rate: 150
debug: True
fixed_frame: map
base_link_frame: base_link
@@ -7,7 +7,7 @@ err_msg_rate: 0.5
calculate_ang_vel: True
watchdog_t: 1.0
-collision_timeout: 4.0
+collision_timeout: 5.0
collision_acc_transition_counter_en: True
collision_acc_th: 5.0
collision_acc_counter_limit: 20
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index db49edd..f762fe2 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -152,7 +152,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
if (this->new_imu_input_)
{
- if (get_base_link_imu_data(blink_acc, blink_ang_vel))//Transform data to fixed frame coordinates
+ if (get_base_link_imu_data(blink_acc, blink_ang_vel))//Transform data to base_link frame coordinates
{
double acc_norm_2d = std::sqrt(std::pow(blink_acc(0),2) + std::pow(blink_acc(1),2));
if (this->in_collision_)
@@ -256,7 +256,6 @@ void CollisionManagerAlgNode::mainNodeThread(void)
}
else
{
- this->relocalization_msg_.front_icp_check = true;
this->relocalization_msg_.front_blink_pose.x = blink_pose(0);
this->relocalization_msg_.front_blink_pose.y = blink_pose(1);
this->relocalization_msg_.front_blink_pose.theta = blink_pose(2);
@@ -264,14 +263,13 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->relocalization_msg_.front_pose_covariance[i] = blink_cov(i);
if (rotate_icp_to_blink(new_laser_pose, covariances, blink_icp, blink_icp_cov, true))
{
+ this->relocalization_msg_.front_icp_check = true;
this->relocalization_msg_.front_icp_odom.x = blink_icp(0);
this->relocalization_msg_.front_icp_odom.y = blink_icp(1);
this->relocalization_msg_.front_icp_odom.theta = blink_icp(2);
for (unsigned int i=0; i<9; i++)
this->relocalization_msg_.front_odom_covariance[i] = blink_icp_cov(i);
}
- else
- this->relocalization_msg_.front_icp_check = false;
}
}
else
@@ -373,7 +371,6 @@ void CollisionManagerAlgNode::mainNodeThread(void)
}
else
{
- this->relocalization_msg_.rear_icp_check = true;
this->relocalization_msg_.rear_blink_pose.x = blink_pose(0);
this->relocalization_msg_.rear_blink_pose.y = blink_pose(1);
this->relocalization_msg_.rear_blink_pose.theta = blink_pose(2);
@@ -381,14 +378,13 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->relocalization_msg_.rear_pose_covariance[i] = blink_cov(i);
if (rotate_icp_to_blink(new_laser_pose, covariances, blink_icp, blink_icp_cov, false))
{
+ this->relocalization_msg_.rear_icp_check = true;
this->relocalization_msg_.rear_icp_odom.x = blink_icp(0);
this->relocalization_msg_.rear_icp_odom.y = blink_icp(1);
this->relocalization_msg_.rear_icp_odom.theta = blink_icp(2);
for (unsigned int i=0; i<9; i++)
this->relocalization_msg_.rear_odom_covariance[i] = blink_icp_cov(i);
}
- else
- this->relocalization_msg_.rear_icp_check = false;
}
}
else
@@ -437,7 +433,10 @@ void CollisionManagerAlgNode::mainNodeThread(void)
//publish collision.
this->collisions_publisher_.publish(this->collision_msg_);
- this->relocalization_publisher_.publish(this->relocalization_msg_);
+ if (this->relocalization_msg_.front_icp_check || this->relocalization_msg_.rear_icp_check)
+ this->relocalization_publisher_.publish(this->relocalization_msg_);
+ else
+ ROS_WARN_STREAM("CollisionManagerAlgNode::mainNodeThread -> Posible collision detected but relocalization can't be calculated.");
}
else
{
@@ -445,7 +444,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
if (acc_norm_2d > max_acc_norm_2d)
this->max_acc_ = blink_acc;
if (std::fabs(blink_ang_vel(2) - this->angular_vel_) > this->max_ang_vel_diff_)
- this->max_ang_vel_diff_ = std::fabs(blink_ang_vel(2));
+ this->max_ang_vel_diff_ = std::fabs(blink_ang_vel(2) - this->angular_vel_);
//Update angle rotated
this->front_icp_srv_.request.rot_angle_estimation += blink_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
@@ -917,10 +916,11 @@ bool CollisionManagerAlgNode::rotate_icp_to_blink(const Eigen::Vector3d& _new_la
Eigen::Vector3d aux;
aux << _new_laser_pose(0), _new_laser_pose(1), 0.0;
_blink_icp = blink_laser_rot*aux;
- _blink_icp(2) = _new_laser_pose(2) + tf2::getYaw((_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation());
+ if ((_front? this->z_axis_blink_front_laser_inv_: this->z_axis_blink_rear_laser_inv_))
+ _blink_icp(2) = _new_laser_pose(2)*-1.0;
+ else
+ _blink_icp(2) = _new_laser_pose(2);
_blink_icp_cov = blink_laser_rot.rotation()*_covariances*blink_laser_rot.rotation().transpose();
- ROS_INFO_STREAM("input " << _new_laser_pose);
- ROS_INFO_STREAM("output " << _blink_icp);
return true;
}
--
GitLab