From 5d734fc7b84a498ac0f610e9472148d785ecf908 Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Thu, 16 Dec 2021 10:02:08 +0100
Subject: [PATCH] Added angular velocity collision detector
---
README.md | 10 +++-
cfg/CollisionManager.cfg | 27 ++++++----
config/params.yaml | 13 +++--
include/collision_manager_alg_node.h | 30 +++++++++--
src/collision_manager_alg_node.cpp | 79 ++++++++++++++++++++--------
5 files changed, 116 insertions(+), 43 deletions(-)
diff --git a/README.md b/README.md
index 460d086..4f96e4d 100644
--- a/README.md
+++ b/README.md
@@ -26,15 +26,21 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal).
- ~**icp** (iri_laser_scan_icp/icp.srv): ICP server.
### Parameters
+
+##### Generals
- ~**rate** (Double; default: 10.0; min: 0.1; max: 1000): The main node thread loop rate in Hz.
- ~**debug** (Boolean; default: False): Boolean to publish debug information.
- ~**fixed_frame** (String; default: base_link): Fixed frame id.
- ~**tf_timeout** (Double; default: 0.2; min: 0.1; max: 2.0): Timeout to find a transform.
- ~**err_msg_rate** (Double; default: 0.5; min: 0.1; max: 1.0): Rate to publish error messages.
-- ~**collision_transition_counter_en** (Boolean; default: False): Enable the collision counter.
+
+##### Collision detection
- ~**collision_timeout** (Double; default: 4.0; min: 1.0; max: 10.0): Timeout to detect an end of collision.
+- ~**collision_acc_transition_counter_en** (Boolean; default: False): Enable the collision counter.
- ~**collision_acc_th** (Double; default: 9.8; min: 0.1; max: 160.0): Threshold to detect a collision.
-- ~**collision_counter_limit** (Integer; default: 6; min: 1; max: 30): Number of low acc imu meassures to detect an end of collision.
+- ~**collision_acc_counter_limit** (Integer; default: 6; min: 1; max: 30): Number of low acc imu meassures to detect an end of collision.
+
+##### ICP data
- ~**front_laser_en** (Boolean; default: True): Enable front laser for ICP.
- ~**rear_laser_en** (Boolean; default: True): Enable rear laser for ICP.
- ~**scan_buffer_size** (Integer; default: 3; min: 1; max: 10): Number of laser_scan buffered for the ICP.
diff --git a/cfg/CollisionManager.cfg b/cfg/CollisionManager.cfg
index ce786f9..f9f86ba 100755
--- a/cfg/CollisionManager.cfg
+++ b/cfg/CollisionManager.cfg
@@ -36,6 +36,8 @@ PACKAGE='iri_collision_manager'
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
+collision_detection = gen.add_group("Collision detection parameters")
+icp = gen.add_group("ICP data parameters")
# Name Type Reconf.level Description Default Min Max
gen.add("rate", double_t, 0, "Main loop rate (Hz)", 10.0, 0.1, 1000.0)
@@ -44,16 +46,21 @@ gen.add("err_msg_rate", double_t, 0, "Rate t
gen.add("fixed_frame", str_t, 0, "Fixed frame", "map")
gen.add("base_link_frame", str_t, 0, "Base_link frame", "base_link")
gen.add("tf_timeout", double_t, 0, "Timeout to find a transform", 0.2, 0.1, 2.0)
-gen.add("collision_transition_counter_en", bool_t, 0, "Enable the collision counter", False)
-gen.add("collision_timeout", double_t, 0, "Timeout to detect an end of collision", 2.0, 1.0, 10.0)
-gen.add("collision_acc_th", double_t, 0, "Threshold to detect a collision", 9.8, 0.1, 160.0)
-gen.add("collision_counter_limit", int_t, 0, "Number of low acc imu meassures to detect an end of collision", 6, 1, 30)
-gen.add("scan_buffer_size", int_t, 0, "Number of laser_scan buffered for the ICP", 3, 1, 10)
-gen.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)
-gen.add("front_laser_en", bool_t, 0, "Enable front laser for ICP", True)
-gen.add("rear_laser_en", bool_t, 0, "Enable rear laser for ICP", True)
-gen.add("icp_min_points", int_t, 0, "Minimum ICP points thershold", 300, 0, 2000)
-gen.add("icp_max_err_by_points", double_t, 0, "Maximum ICP average error per point", 0.1, 0.0, 10.0)
+
+collision_detection.add("collision_timeout", double_t, 0, "Timeout to detect an end of collision", 2.0, 1.0, 10.0)
+collision_detection.add("collision_acc_transition_counter_en", bool_t, 0, "Enable the collision counter for the acc detection", False)
+collision_detection.add("collision_acc_th", double_t, 0, "Acceleration threshold to detect a collision", 9.8, 0.1, 160.0)
+collision_detection.add("collision_acc_counter_limit", int_t, 0, "Number of low acc imu meassures to detect an end of collision", 6, 1, 30)
+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)
+
+icp.add("scan_buffer_size", int_t, 0, "Number of laser_scan buffered for the ICP", 3, 1, 10)
+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)
+icp.add("front_laser_en", bool_t, 0, "Enable front laser for ICP", True)
+icp.add("rear_laser_en", bool_t, 0, "Enable rear laser for ICP", True)
+icp.add("icp_min_points", int_t, 0, "Minimum ICP points thershold", 300, 0, 2000)
+icp.add("icp_max_err_by_points", double_t, 0, "Maximum ICP average error per point", 0.1, 0.0, 10.0)
exit(gen.generate(PACKAGE, "CollisionManagerAlgorithm", "CollisionManager"))
\ No newline at end of file
diff --git a/config/params.yaml b/config/params.yaml
index 1d24aa7..47cf45a 100644
--- a/config/params.yaml
+++ b/config/params.yaml
@@ -4,10 +4,17 @@ fixed_frame: map
base_link_frame: base_link
tf_timeout: 0.2
err_msg_rate: 0.5
-collision_transition_counter_en: True
+
collision_timeout: 4.0
-collision_acc_th: 2.5
-collision_counter_limit: 12
+collision_acc_transition_counter_en: True
+collision_acc_th: 3.0
+#collision_acc_th: 2.0
+collision_acc_counter_limit: 12
+collision_ang_vel_transition_counter_en: True
+collision_ang_vel_th: 0.4
+#collision_ang_vel_th: 0.2
+collision_ang_vel_counter_limit: 4
+
front_laser_en: True
rear_laser_en: True
scan_buffer_size: 6
diff --git a/include/collision_manager_alg_node.h b/include/collision_manager_alg_node.h
index 52b38f4..1993cbd 100644
--- a/include/collision_manager_alg_node.h
+++ b/include/collision_manager_alg_node.h
@@ -79,6 +79,7 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
// ros::Time collision_end_stamp_; ///< Time stamp at the end of collision.
Eigen::Vector3d max_acc_; ///< The linear accelerations meassured from base_link frame reference at maximum acceleration module.
int low_acc_count_; ///< Low acceleration meassures counter to detect the end of a collision.
+ int low_ang_vel_count_; ///< Low angular velocities meassures counter to detect the end of a collision.
std::list<RangesWithStamp> front_ranges_; ///< Front laser ranges buffer.
std::list<RangesWithStamp> rear_ranges_; ///< Rear laser ranges buffer.
bool is_front_icp_input_data_ok_; ///<Boolean to know of all the necessary data has been saved on the service request.
@@ -182,7 +183,7 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
bool get_base_link_imu_data(Eigen::Vector3d& _global_acc, Eigen::Vector3d& _global_ang_vel);
/**
- * \brief Function to check if is a collision transition.
+ * \brief Function to check if is an acceleration collision transition.
*
* It checks if the current acceleration meassured is behind or above the threshold
* (depending on _start param) updates the counter and checks if the limit has been reached.
@@ -193,18 +194,35 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
*
* \return True if the conditions has been met.
*/
- bool check_collision_transition(double _acc_norm_2d, bool _start);
+ bool check_acc_collision_transition(double _acc_norm_2d, bool _start);
+
+ /**
+ * \brief Function to check if is an acceleration collision transition.
+ *
+ * It checks if the current acceleration meassured is behind or above the threshold
+ * (depending on _start param) updates the counter and checks if the limit has been reached.
+ *
+ * \param _ang_vel The current angular velocity.
+ *
+ * \param _start If it must check the star transition.
+ *
+ * \return True if the conditions has been met.
+ */
+ bool check_ang_vel_collision_transition(double _ang_vel, bool _start);
/**
* \brief Function to check the end of a collision.
*
- * It calls check_collision_transition with _start to false.
+ * It calls check_acc_collision_transition and check_ang_vel_collision_transition
+ * with _start to false.
*
* \param _acc_norm_2d The current global linear acceleration norm.
*
+ * \param _ang_vel The current angular velocity.
+ *
* \return True if the condition has been met.
*/
- bool check_end_of_collision(double _acc_norm_2d);
+ bool check_end_of_collision(double _acc_norm_2d, double _ang_vel);
/**
* \brief Function to check the end of a collision.
@@ -213,9 +231,11 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
*
* \param _acc_norm_2d The current global linear acceleration norm.
*
+ * \param _ang_vel The current angular velocity.
+ *
* \return True if the condition has been met.
*/
- bool check_start_of_collision(double _acc_norm_2d);
+ bool check_start_of_collision(double _acc_norm_2d, double _ang_vel);
/**
* \brief Function to find the last scan before a collision.
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index 536e18b..1d163d6 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -31,6 +31,7 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
this->tf_base_link_imu_loaded_ = false;
this->in_collision_ = false;
this->low_acc_count_ = 0;
+ this->low_ang_vel_count_ = 0;
this->is_front_icp_input_data_ok_ = false;
this->is_rear_icp_input_data_ok_ = false;
this->tf_base_link_front_laser_loaded_ = false;
@@ -94,18 +95,18 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->imu_mutex_enter();
this->rear_laser_scan_mutex_enter();
this->front_laser_scan_mutex_enter();
- Eigen::Vector3d global_acc, global_ang_vel, rear_blink_pose, front_blink_pose, new_laser_pose;
+ Eigen::Vector3d blink_acc, blink_ang_vel, rear_blink_pose, front_blink_pose, new_laser_pose;
Eigen::Matrix3d rear_cov, front_cov, covariances;
if (this->new_imu_input_)
{
- if (get_base_link_imu_data(global_acc, global_ang_vel))//Transform data to fixed frame coordinates
+ if (get_base_link_imu_data(blink_acc, blink_ang_vel))//Transform data to fixed frame coordinates
{
- double acc_norm_2d = std::sqrt(std::pow(global_acc(0),2) + std::pow(global_acc(1),2));
+ double acc_norm_2d = std::sqrt(std::pow(blink_acc(0),2) + std::pow(blink_acc(1),2));
if (this->in_collision_)
{
double max_acc_norm_2d = std::sqrt(std::pow(this->max_acc_(0),2) + std::pow(this->max_acc_(1),2));
- if (check_end_of_collision(acc_norm_2d))
+ if (check_end_of_collision(acc_norm_2d, std::fabs(blink_ang_vel(2))))
{
double collision_angle = std::atan2(this->max_acc_(1), this->max_acc_(0));
while (collision_angle >= M_PI)
@@ -116,8 +117,12 @@ void CollisionManagerAlgNode::mainNodeThread(void)
ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: acc = " << max_acc_norm_2d << "; angle = " << collision_angle);
//update rot_angle_estimated
- this->front_icp_srv_.request.rot_angle_estimation += global_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
+ this->front_icp_srv_.request.rot_angle_estimation += blink_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
+
+ //rotate angle when fixed tf
+
this->rear_icp_srv_.request.rot_angle_estimation = this->front_icp_srv_.request.rot_angle_estimation;
+
ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: angle estimated = " << this->front_icp_srv_.request.rot_angle_estimation);
// Call the service
@@ -206,6 +211,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
//restart variables
this->in_collision_ = false;
this->low_acc_count_ = 0;
+ this->low_ang_vel_count_ = 0;
this->is_front_icp_input_data_ok_ = false;
this->is_rear_icp_input_data_ok_ = false;
}
@@ -213,10 +219,10 @@ void CollisionManagerAlgNode::mainNodeThread(void)
{
//Update max_acc
if (acc_norm_2d > max_acc_norm_2d)
- this->max_acc_ = global_acc;
+ this->max_acc_ = blink_acc;
//Update angle rotated
- this->front_icp_srv_.request.rot_angle_estimation += global_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
+ this->front_icp_srv_.request.rot_angle_estimation += blink_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
this->last_imu_t_ = this->imu_stamp_;
//Check collision timeout
@@ -225,10 +231,11 @@ void CollisionManagerAlgNode::mainNodeThread(void)
ROS_WARN_STREAM("CollisionManagerAlgNode::mainNodeThread -> No end of collision detected after " << this->config_.collision_timeout << " seconds.");
this->in_collision_ = false;
this->low_acc_count_ = 0;
+ this->low_ang_vel_count_ = 0;
}
}
}
- else if (check_start_of_collision(acc_norm_2d))
+ else if (check_start_of_collision(acc_norm_2d, std::fabs(blink_ang_vel(2))))
{
ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision start detected.");
this->collision_start_stamp_ = this->imu_stamp_;
@@ -244,7 +251,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
//init variables
this->in_collision_ = true;
- this->max_acc_ = global_acc;
+ this->max_acc_ = blink_acc;
this->last_imu_t_ = this->collision_start_stamp_;
this->front_icp_srv_.request.rot_angle_estimation = 0.0;
this->rear_icp_srv_.request.rot_angle_estimation = 0.0;
@@ -383,30 +390,56 @@ bool CollisionManagerAlgNode::get_base_link_imu_data(Eigen::Vector3d& _global_ac
return true;
}
-bool CollisionManagerAlgNode::check_collision_transition(double _acc_norm_2d, bool _start)
+bool CollisionManagerAlgNode::check_acc_collision_transition(double _acc_norm_2d, bool _start)
{
- if (this->config_.collision_transition_counter_en)
+ if (!this->config_.collision_acc_transition_counter_en)
+ return true;
+ if ((_start && (_acc_norm_2d > this->config_.collision_acc_th)) || (!_start && (_acc_norm_2d < this->config_.collision_acc_th)))
+ this->low_acc_count_++;
+ else
{
- if ((_start && (_acc_norm_2d > this->config_.collision_acc_th)) || (!_start && (_acc_norm_2d < this->config_.collision_acc_th)))
- this->low_acc_count_++;
- else
- {
- this->low_acc_count_ = 0;
- return false;
- }
+ this->low_acc_count_ = 0;
+ return false;
}
return ((_start && (_acc_norm_2d > this->config_.collision_acc_th)) || (!_start && (_acc_norm_2d < this->config_.collision_acc_th)))
- && (_start || (!_start && (!this->config_.collision_transition_counter_en || (this->low_acc_count_ >= this->config_.collision_counter_limit))));
+ && (_start || (!_start && (!this->config_.collision_acc_transition_counter_en || (this->low_acc_count_ >= this->config_.collision_acc_counter_limit))));
+}
+
+bool CollisionManagerAlgNode::check_ang_vel_collision_transition(double _ang_vel, bool _start)
+{
+ if (!this->config_.collision_ang_vel_transition_counter_en)
+ return true;
+ if ((_start && (_ang_vel > this->config_.collision_ang_vel_th)) || (!_start && (_ang_vel < this->config_.collision_ang_vel_th)))
+ this->low_ang_vel_count_++;
+ else
+ {
+ this->low_ang_vel_count_ = 0;
+ return false;
+ }
+ return ((_start && (_ang_vel > this->config_.collision_ang_vel_th)) || (!_start && (_ang_vel < this->config_.collision_ang_vel_th)))
+ && (_start || (!_start && (!this->config_.collision_ang_vel_transition_counter_en || (this->low_ang_vel_count_ >= this->config_.collision_ang_vel_counter_limit))));
}
-bool CollisionManagerAlgNode::check_end_of_collision(double _acc_norm_2d)
+bool CollisionManagerAlgNode::check_end_of_collision(double _acc_norm_2d, double _ang_vel)
{
- return check_collision_transition(_acc_norm_2d, false);
+ bool acc_true = check_acc_collision_transition(_acc_norm_2d, false);
+ bool ang_vel_true = check_ang_vel_collision_transition(_ang_vel, false);
+ if (acc_true)
+ ROS_INFO_STREAM("CollisionManagerAlgNode::check_end_of_collision -> (END) Acc true. acc: " << _acc_norm_2d << "; vel: " << _ang_vel);
+ if (ang_vel_true)
+ ROS_INFO_STREAM("CollisionManagerAlgNode::check_end_of_collision -> (END) Angular vel true. acc: " << _acc_norm_2d << "; vel: " << _ang_vel);
+ return acc_true && ang_vel_true;
}
-bool CollisionManagerAlgNode::check_start_of_collision(double _acc_norm_2d)
+bool CollisionManagerAlgNode::check_start_of_collision(double _acc_norm_2d, double _ang_vel)
{
- return check_collision_transition(_acc_norm_2d, true);
+ bool acc_true = check_acc_collision_transition(_acc_norm_2d, true);
+ bool ang_vel_true = check_ang_vel_collision_transition(_ang_vel, true);
+ if (acc_true)
+ ROS_INFO_STREAM("CollisionManagerAlgNode::check_start_of_collision -> (START) Acc true. acc: " << _acc_norm_2d << "; vel: " << _ang_vel);
+ if (ang_vel_true)
+ ROS_INFO_STREAM("CollisionManagerAlgNode::check_start_of_collision -> (START) Angular vel true. acc: " << _acc_norm_2d << "; vel: " << _ang_vel);
+ return acc_true || ang_vel_true;
}
bool CollisionManagerAlgNode::set_ref_ranges(ros::Time _stamp, bool _front)
--
GitLab