From 8f54ddd88075ede841f16535e6ad2e4965d22180 Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Mon, 7 Feb 2022 15:49:25 +0100
Subject: [PATCH] Added rotation of the ICP estimated angle depending on the tf
between base link and laser
---
cfg/CollisionManager.cfg | 4 ++--
config/params.yaml | 2 +-
include/collision_manager_alg_node.h | 2 ++
src/collision_manager_alg_node.cpp | 19 ++++++++++++++++++-
4 files changed, 23 insertions(+), 4 deletions(-)
diff --git a/cfg/CollisionManager.cfg b/cfg/CollisionManager.cfg
index c7a7997..7f8308b 100755
--- a/cfg/CollisionManager.cfg
+++ b/cfg/CollisionManager.cfg
@@ -55,8 +55,8 @@ collision_detection.add("collision_acc_counter_limit", int_t,
collision_detection.add("collision_ang_vel_transition_counter_en", bool_t, 0, "Enable the collision counter", False)
collision_detection.add("collision_ang_vel_th", double_t, 0, "Angular velocity threshold to detect a collision", 0.3, 0.1, 1)
collision_detection.add("collision_ang_vel_counter_limit", int_t, 0, "Number of low acc imu meassures to detect an end of collision", 3, 1, 30)
-collision_detection.add("odom_trans_th", double_t, 0, "Max translational difference between wheels and ICP odometries", 0.1, 0.01, 0.3)
-collision_detection.add("odom_angular_th", double_t, 0, "Max angular difference between wheels and ICP odometries in rad", 0.15, 0.05, 0.35)
+collision_detection.add("odom_trans_th", double_t, 0, "Max translational difference between wheels and ICP odometries", 0.1, 0.01, 0.5)
+collision_detection.add("odom_angular_th", double_t, 0, "Max angular difference between wheels and ICP odometries in rad", 0.15, 0.05, 0.7)
icp.add("buffer_size", int_t, 0, "Number of laser_scan and odometry buffered for the ICP", 3, 1, 30)
icp.add("scan_diff_t_from_collision", double_t, 0, "Time to subtract from collision time stamp to ensure a clean scan", 0.0, 0.0, 0.5)
diff --git a/config/params.yaml b/config/params.yaml
index e953581..b63bc17 100644
--- a/config/params.yaml
+++ b/config/params.yaml
@@ -4,7 +4,7 @@ fixed_frame: map
base_link_frame: base_link
tf_timeout: 0.2
err_msg_rate: 0.5
-calculate_ang_vel: True
+calculate_ang_vel: False
collision_timeout: 4.0
collision_acc_transition_counter_en: True
diff --git a/include/collision_manager_alg_node.h b/include/collision_manager_alg_node.h
index 6f5bee9..30f030b 100644
--- a/include/collision_manager_alg_node.h
+++ b/include/collision_manager_alg_node.h
@@ -86,8 +86,10 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
Eigen::Transform<double,3,Eigen::Affine> tf_base_link_imu_; ///< Transform from base_link frame to IMU frame.
bool tf_base_link_front_laser_loaded_; ///< Flag to know that tf_base_link_front_laser has been loaded before.
tf2::Transform tf_blink_front_laser; ///< Transform from base_link to front laser.
+ bool z_axis_blink_front_laser_inv_; ///< Flag to know if the z axis of base link and front laser have the same orientation.
bool tf_base_link_rear_laser_loaded_; ///< Flag to know that tf_base_link_rear_laser has been loaded before.
tf2::Transform tf_blink_rear_laser; ///< Transform from base_link to rear laser.
+ bool z_axis_blink_rear_laser_inv_; ///< Flag to know if the z axis of base link and rear laser have the same orientation.
bool in_collision_; ///< Flag to know that we are receiving collision meassures.
ros::Time collision_start_stamp_; ///< Time stamp of a collision initial detection.
// ros::Time collision_end_stamp_; ///< Time stamp at the end of collision.
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index b5e1d9b..8aaedf5 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -39,6 +39,8 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
this->tf_base_link_rear_laser_loaded_ = false;
this->angular_vel_ = 0.0;
this->odom_input_is_ok_ = false;
+ this->z_axis_blink_front_laser_inv_ = false;
+ this->z_axis_blink_rear_laser_inv_ = false;
this->debug_rear_icp_pose_msg_.header.frame_id = this->config_.fixed_frame;
this->debug_rear_icp_pose_msg_.pose.pose.position.z = 0.0;
@@ -134,9 +136,13 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->collision_msg_.ang_vel_diff_peak = this->max_ang_vel_diff_;
//update rot_angle_estimated
- // TODO: rotate angle with tf between blink and laser
this->front_icp_srv_.request.rot_angle_estimation += blink_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
this->rear_icp_srv_.request.rot_angle_estimation = this->front_icp_srv_.request.rot_angle_estimation;
+ // rotate angle with tf between blink and laser
+ if (this->z_axis_blink_front_laser_inv_)
+ this->front_icp_srv_.request.rot_angle_estimation *=-1.0;
+ if (this->z_axis_blink_rear_laser_inv_)
+ this->rear_icp_srv_.request.rot_angle_estimation *=-1.0;
// ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: angle estimated = " << this->front_icp_srv_.request.rot_angle_estimation);
@@ -874,6 +880,17 @@ bool CollisionManagerAlgNode::load_tf_blink_laser(bool _front)
tf_aux = this->tf2_buffer.lookupTransform(this->config_.base_link_frame, (_front? this->front_laser_frame_: this->rear_laser_frame_), this->imu_stamp_);
tf2::fromMsg(tf_aux.transform, (_front? this->tf_blink_front_laser: this->tf_blink_rear_laser));
(_front? this->tf_base_link_front_laser_loaded_: this->tf_base_link_rear_laser_loaded_) = true;
+
+ // check if z axis from base link and laser have the same orientation
+ Eigen::Vector3d z, aux;
+ z << 0.0, 0.0, 1.0;
+ Eigen::Quaternion<double> rot(tf_aux.transform.rotation.w, tf_aux.transform.rotation.x, tf_aux.transform.rotation.y, tf_aux.transform.rotation.z);
+ Eigen::Transform<double,3,Eigen::Affine> blink_laser(rot);
+ aux = blink_laser*z;
+ if (aux(2) > 0)
+ (_front? this->z_axis_blink_front_laser_inv_: this->z_axis_blink_rear_laser_inv_) = false;
+ else
+ (_front? this->z_axis_blink_front_laser_inv_: this->z_axis_blink_rear_laser_inv_) = true;
}
return true;
}
--
GitLab