diff --git a/cfg/CollisionManager.cfg b/cfg/CollisionManager.cfg
index ad1f303ab71286eb60161520034a2e91d9b3bddf..c7a799778a85f5645e1be139c7452f7433e3f6e9 100755
--- a/cfg/CollisionManager.cfg
+++ b/cfg/CollisionManager.cfg
@@ -46,6 +46,7 @@ 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("calculate_ang_vel", bool_t, 0, "Flag to calculate robot angular vel from odometry", False)
collision_detection.add("collision_timeout", double_t, 0, "Timeout to detect an end of collision", 2.0, 1.0, 30.0)
collision_detection.add("collision_acc_transition_counter_en", bool_t, 0, "Enable the collision counter for the acc detection", False)
diff --git a/config/params.yaml b/config/params.yaml
index ad040c2b33bde6cbaafa404082ffd6fcd146d546..e953581b8ba8b44fb956ed3407bf3043222590f7 100644
--- a/config/params.yaml
+++ b/config/params.yaml
@@ -4,6 +4,7 @@ fixed_frame: map
base_link_frame: base_link
tf_timeout: 0.2
err_msg_rate: 0.5
+calculate_ang_vel: True
collision_timeout: 4.0
collision_acc_transition_counter_en: True
@@ -13,13 +14,13 @@ collision_ang_vel_transition_counter_en: True
collision_ang_vel_th: 0.4
#collision_ang_vel_th: 0.2
collision_ang_vel_counter_limit: 20
-odom_trans_th: 0.1
+odom_trans_th: 0.2
odom_angular_th: 0.15
front_laser_en: True
rear_laser_en: True
buffer_size: 10
-scan_diff_t_from_collision: 0.15
+scan_diff_t_from_collision: 0.1
front_laser_en: True
rear_laser_en: True
icp_min_points: 280
diff --git a/include/collision_manager_alg_node.h b/include/collision_manager_alg_node.h
index ad654bb8989746260e1613dffb7e8f06a9b1e7d9..6f5bee94ad7b3cf56598d9857d8d978db4007dcd 100644
--- a/include/collision_manager_alg_node.h
+++ b/include/collision_manager_alg_node.h
@@ -92,6 +92,7 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
ros::Time collision_start_stamp_; ///< Time stamp of a collision initial detection.
// 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.
+ double max_ang_vel_diff_; ///< Maximum angular velocity difference between the odometry and the IMU.
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.
@@ -210,13 +211,13 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
* It checks if imu frame id is different from base_link frame and if not already
* saved, it saves the transfrom.
*
- * \param _global_acc The current global linear acceleration.
+ * \param _blink_acc The current global linear acceleration.
*
- * \param _global_ang_vel The current global angular velocity.
+ * \param _blink_ang_vel The current global angular velocity.
*
* \return True in success.
*/
- bool get_base_link_imu_data(Eigen::Vector3d& _global_acc, Eigen::Vector3d& _global_ang_vel);
+ bool get_base_link_imu_data(Eigen::Vector3d& _blink_acc, Eigen::Vector3d& _blink_ang_vel);
/**
* \brief Function to check if is an acceleration collision transition.
diff --git a/msg/collision.msg b/msg/collision.msg
index 8983d31be5746ad8f67d3b04b1ceba96d9807087..6040b91f69cd90d20730d705cea71f13004f8eaf 100644
--- a/msg/collision.msg
+++ b/msg/collision.msg
@@ -4,7 +4,7 @@ float32 acc_peak # Acceleration peak at collision
float32 acc_angle # Estimation of the collision angle from accelerometer data.
bool ang_vel_trigger # If angular velocity has trigger the collision detection.
-float32 ang_vel_diff # Angular velocity difference between IMU data and odometry.
+float32 ang_vel_diff_peak # Angular velocity difference peak between IMU data and odometry.
bool front_icp_check # If front ICP checks that is a collision.
int32 front_icp_points # Front ICP laser scan matching points.
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index 67d0fd03d64c6b403dcb86d20d75b4c1e7c073a7..3d7750a8261d35324bc2a04a705497f22e725989 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -1,4 +1,5 @@
#include "collision_manager_alg_node.h"
+#include <math.h>
CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
algorithm_base::IriBaseAlgorithm<CollisionManagerAlgorithm>()
@@ -124,18 +125,17 @@ void CollisionManagerAlgNode::mainNodeThread(void)
collision_angle -= 2*M_PI;
while (collision_angle < -M_PI)
collision_angle += 2*M_PI;
- ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision end detected.");
- ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: acc = " << max_acc_norm_2d << "; angle = " << collision_angle);
+ // ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision end detected.");
+ // ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: acc = " << max_acc_norm_2d << "; angle = " << collision_angle);
//update collision msg data
this->collision_msg_.acc_peak = max_acc_norm_2d;
this->collision_msg_.acc_angle = collision_angle;
+ 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();
-
- //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);
@@ -191,33 +191,26 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->tf2_broadcaster.sendTransform(this->transform_msg);
}
- if (front_icp_srv_.response.nvalid > this->config_.icp_min_points && front_icp_srv_.response.error/front_icp_srv_.response.nvalid < this->config_.icp_max_err_by_points)
+ if (get_icp_odometry(new_laser_pose, covariances, icp_odometry, icp_odom_cov, icp_odometry_diff, true))
{
- if (get_icp_odometry(new_laser_pose, covariances, icp_odometry, icp_odom_cov, icp_odometry_diff, true))
- {
- icp_odom_calculated = true;
- ROS_INFO_STREAM("Odom before collision " << this->odom_before_collision_);
- ROS_INFO_STREAM("Odom after collision " << this->odom_after_collision_);
- ROS_INFO_STREAM("New laser pose " << new_laser_pose);
- ROS_INFO_STREAM("ICP odom " << icp_odometry);
- ROS_INFO_STREAM("ICP odom diff " << icp_odometry_diff);
- icp_is_collision = check_odometry_diff(icp_odometry_diff, diff_wheel_icp_odom);
-
- // Update collision msg
- this->collision_msg_.front_odom_diff.x = diff_wheel_icp_odom(0);
- this->collision_msg_.front_odom_diff.y = diff_wheel_icp_odom(1);
- this->collision_msg_.front_odom_diff.theta = diff_wheel_icp_odom(2);
- for (unsigned int i=0; i<9; i++)
- this->collision_msg_.front_covariance[i] = icp_odom_cov(i);
- }
- else
+ icp_odom_calculated = true;
+ // ROS_INFO_STREAM("Odom before collision " << this->odom_before_collision_);
+ // ROS_INFO_STREAM("Odom after collision " << this->odom_after_collision_);
+ // ROS_INFO_STREAM("New laser pose " << new_laser_pose);
+ // ROS_INFO_STREAM("ICP odom " << icp_odometry);
+ // ROS_INFO_STREAM("ICP odom diff " << icp_odometry_diff);
+ // ROS_INFO_STREAM("odom diff " << this->odom_after_collision_ - this->odom_before_collision_);
+ icp_is_collision = check_odometry_diff(icp_odometry_diff, diff_wheel_icp_odom);
+
+ // Update collision msg
+ this->collision_msg_.front_odom_diff.x = diff_wheel_icp_odom(0);
+ this->collision_msg_.front_odom_diff.y = diff_wheel_icp_odom(1);
+ this->collision_msg_.front_odom_diff.theta = diff_wheel_icp_odom(2);
+ for (unsigned int i=0; i<9; i++)
+ this->collision_msg_.front_covariance[i] = icp_odom_cov(i);
+ if (!(front_icp_srv_.response.nvalid > this->config_.icp_min_points && front_icp_srv_.response.error/front_icp_srv_.response.nvalid < this->config_.icp_max_err_by_points))
{
- this->collision_msg_.front_odom_diff.x = 0.0;
- this->collision_msg_.front_odom_diff.y = 0.0;
- this->collision_msg_.front_odom_diff.theta = 0.0;
- for (unsigned int i=0; i<9; i++)
- this->collision_msg_.front_covariance[i] = 0.0;
- icp_odom_calculated = false;
+ ROS_WARN("CollisionManagerAlgNode::mainNodeThread -> Not enough points or too much error on front ICP. Not valid to check if is a collision");
icp_is_collision = false;
}
}
@@ -303,33 +296,26 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->tf2_broadcaster.sendTransform(this->transform_msg);
}
- if (rear_icp_srv_.response.nvalid > this->config_.icp_min_points && rear_icp_srv_.response.error/rear_icp_srv_.response.nvalid < this->config_.icp_max_err_by_points)
+ if (get_icp_odometry(new_laser_pose, covariances, icp_odometry, icp_odom_cov, icp_odometry_diff, true))
{
- if (get_icp_odometry(new_laser_pose, covariances, icp_odometry, icp_odom_cov, icp_odometry_diff, true))
- {
- icp_odom_calculated = true;
- ROS_INFO_STREAM("Odom before collision " << this->odom_before_collision_);
- ROS_INFO_STREAM("Odom after collision " << this->odom_after_collision_);
- ROS_INFO_STREAM("New laser pose " << new_laser_pose);
- ROS_INFO_STREAM("ICP odom " << icp_odometry);
- ROS_INFO_STREAM("ICP odom diff " << icp_odometry_diff);
- icp_is_collision = check_odometry_diff(icp_odometry_diff, diff_wheel_icp_odom);
-
- // Update collision msg
- this->collision_msg_.rear_odom_diff.x = diff_wheel_icp_odom(0);
- this->collision_msg_.rear_odom_diff.y = diff_wheel_icp_odom(1);
- this->collision_msg_.rear_odom_diff.theta = diff_wheel_icp_odom(2);
- for (unsigned int i=0; i<9; i++)
- this->collision_msg_.rear_covariance[i] = icp_odom_cov(i);
- }
- else
+ icp_odom_calculated = true;
+ // ROS_INFO_STREAM("Odom before collision " << this->odom_before_collision_);
+ // ROS_INFO_STREAM("Odom after collision " << this->odom_after_collision_);
+ // ROS_INFO_STREAM("New laser pose " << new_laser_pose);
+ // ROS_INFO_STREAM("ICP odom " << icp_odometry);
+ // ROS_INFO_STREAM("ICP odom diff " << icp_odometry_diff);
+ // ROS_INFO_STREAM("odom diff " << this->odom_after_collision_ - this->odom_before_collision_);
+ icp_is_collision = check_odometry_diff(icp_odometry_diff, diff_wheel_icp_odom);
+
+ // Update collision msg
+ this->collision_msg_.rear_odom_diff.x = diff_wheel_icp_odom(0);
+ this->collision_msg_.rear_odom_diff.y = diff_wheel_icp_odom(1);
+ this->collision_msg_.rear_odom_diff.theta = diff_wheel_icp_odom(2);
+ for (unsigned int i=0; i<9; i++)
+ this->collision_msg_.rear_covariance[i] = icp_odom_cov(i);
+ if (!(rear_icp_srv_.response.nvalid > this->config_.icp_min_points && rear_icp_srv_.response.error/rear_icp_srv_.response.nvalid < this->config_.icp_max_err_by_points))
{
- this->collision_msg_.rear_odom_diff.x = 0.0;
- this->collision_msg_.rear_odom_diff.y = 0.0;
- this->collision_msg_.rear_odom_diff.theta = 0.0;
- for (unsigned int i=0; i<9; i++)
- this->collision_msg_.rear_covariance[i] = 0.0;
- icp_odom_calculated = false;
+ ROS_WARN("CollisionManagerAlgNode::mainNodeThread -> Not enough points or too much error on rear ICP. Not valid to check if is a collision");
icp_is_collision = false;
}
}
@@ -370,9 +356,6 @@ void CollisionManagerAlgNode::mainNodeThread(void)
}
// fuse poses?? call service to modify SLAM graph
}
- ROS_INFO_STREAM("");
- ROS_INFO_STREAM("");
- ROS_INFO_STREAM("");
//restart variables
this->in_collision_ = false;
this->low_acc_count_ = 0;
@@ -388,6 +371,8 @@ void CollisionManagerAlgNode::mainNodeThread(void)
//Update max_acc
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));
//Update angle rotated
this->front_icp_srv_.request.rot_angle_estimation += blink_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
@@ -405,7 +390,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
}
else if (check_start_of_collision(acc_norm_2d, std::fabs(blink_ang_vel(2))))
{
- ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision start detected.");
+ // ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision start detected.");
this->collision_start_stamp_ = this->imu_stamp_;
// Get reference scan
@@ -427,13 +412,13 @@ void CollisionManagerAlgNode::mainNodeThread(void)
//init variables
this->in_collision_ = true;
this->max_acc_ = blink_acc;
+ this->max_ang_vel_diff_ = std::fabs(blink_ang_vel(2) - this->angular_vel_);
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;
//update collision msg data
this->collision_msg_.header.stamp = this->imu_stamp_;
- this->collision_msg_.ang_vel_diff = std::fabs(blink_ang_vel(2));
}
}
this->new_imu_input_ = false;
@@ -583,26 +568,26 @@ void CollisionManagerAlgNode::mainNodeThread(void)
// this->alg_.unlock();
}
-bool CollisionManagerAlgNode::get_base_link_imu_data(Eigen::Vector3d& _global_acc, Eigen::Vector3d& _global_ang_vel)
+bool CollisionManagerAlgNode::get_base_link_imu_data(Eigen::Vector3d& _blink_acc, Eigen::Vector3d& _blink_ang_vel)
{
- if (this->config_.base_link_frame != this->imu_frame_ || !this->tf_base_link_imu_loaded_)
+ if (!this->tf_base_link_imu_loaded_)
{
geometry_msgs::TransformStamped tf_base_link_imu_msg;
- if (!this->tf2_buffer.canTransform(this->config_.base_link_frame, this->imu_frame_, ros::Time::now(), ros::Duration(this->config_.tf_timeout)))
+ if (!this->tf2_buffer.canTransform(this->config_.base_link_frame, this->imu_frame_, this->imu_stamp_, ros::Duration(this->config_.tf_timeout)))
{
- ROS_WARN_STREAM("CollisionManagerAlgNode::get_base_link_imu_data -> Can't transform from " << this->config_.base_link_frame << " to " << this->imu_frame_ << " at " << ros::Time::now());
+ ROS_WARN_STREAM("CollisionManagerAlgNode::get_base_link_imu_data -> Can't transform from " << this->config_.base_link_frame << " to " << this->imu_frame_ << " at " << this->imu_stamp_);
this->tf_base_link_imu_loaded_ = false;
return false;
}
- tf_base_link_imu_msg = this->tf2_buffer.lookupTransform(this->config_.base_link_frame, this->imu_frame_, ros::Time::now());
+ tf_base_link_imu_msg = this->tf2_buffer.lookupTransform(this->config_.base_link_frame, this->imu_frame_, this->imu_stamp_);
Eigen::Quaternion<double> rot(
tf_base_link_imu_msg.transform.rotation.w, tf_base_link_imu_msg.transform.rotation.x, tf_base_link_imu_msg.transform.rotation.y, tf_base_link_imu_msg.transform.rotation.z);
this->tf_base_link_imu_ = Eigen::Transform<double,3,Eigen::Affine>(rot);
this->tf_base_link_imu_loaded_ = true;
}
- _global_acc = this->tf_base_link_imu_*this->imu_local_acc_;
- _global_ang_vel = this->tf_base_link_imu_*this->imu_local_ang_vel_;
+ _blink_acc = this->tf_base_link_imu_*this->imu_local_acc_;
+ _blink_ang_vel = this->tf_base_link_imu_*this->imu_local_ang_vel_;
return true;
}
@@ -656,8 +641,12 @@ bool CollisionManagerAlgNode::check_start_of_collision(double _acc_norm_2d, doub
// update collision msg
if (acc_true)
this->collision_msg_.acc_trigger = true;
+ else
+ this->collision_msg_.acc_trigger = false;
if (ang_vel_true)
this->collision_msg_.ang_vel_trigger = true;
+ else
+ this->collision_msg_.ang_vel_trigger = false;
// 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)
@@ -745,18 +734,19 @@ bool CollisionManagerAlgNode::get_base_link_new_pose_from_icp(const Eigen::Vecto
{
geometry_msgs::TransformStamped tf_aux;
tf2::Transform tf_map_laser, tf_map_new_laser, tf_laser_new_laser, tf_map_new_blink, tf_laser_map;
+ ros::Time t = this->collision_start_stamp_ - ros::Duration(this->config_.scan_diff_t_from_collision);
if (!load_tf_blink_laser(_front))
return false;
- if (!this->tf2_buffer.canTransform(this->config_.fixed_frame, (_front? this->front_laser_frame_: this->rear_laser_frame_), this->imu_stamp_, ros::Duration(this->config_.tf_timeout)))
+ if (!this->tf2_buffer.canTransform(this->config_.fixed_frame, (_front? this->front_laser_frame_: this->rear_laser_frame_), t, ros::Duration(this->config_.tf_timeout)))
{
- ROS_WARN_STREAM("CollisionManagerAlgNode::get_base_link_new_pose_from_icp -> Can't transform from " << this->config_.fixed_frame << " to " << (_front? this->front_laser_frame_: this->rear_laser_frame_) << " at " << this->imu_stamp_);
+ ROS_WARN_STREAM("CollisionManagerAlgNode::get_base_link_new_pose_from_icp -> Can't transform from " << this->config_.fixed_frame << " to " << (_front? this->front_laser_frame_: this->rear_laser_frame_) << " at " << t);
return false;
}
else
{
- tf_aux = this->tf2_buffer.lookupTransform(this->config_.fixed_frame, (_front? this->front_laser_frame_: this->rear_laser_frame_), this->imu_stamp_);
+ tf_aux = this->tf2_buffer.lookupTransform(this->config_.fixed_frame, (_front? this->front_laser_frame_: this->rear_laser_frame_), t);
tf2::fromMsg(tf_aux.transform, tf_map_laser);
}
@@ -815,19 +805,35 @@ bool CollisionManagerAlgNode::get_base_link_new_pose_from_icp(const Eigen::Vecto
bool CollisionManagerAlgNode::get_icp_odometry(const Eigen::Vector3d& _new_laser_pose, const Eigen::Matrix3d& _covariances, Eigen::Vector3d& _odom, Eigen::Matrix3d& _odom_cov, Eigen::Vector3d& _odom_diff, const bool _front)
{
geometry_msgs::TransformStamped tf_aux;
- tf2::Transform tf_odom_laser, tf_odom_new_laser, tf_laser_new_laser, tf_odom_new_blink, tf_laser_odom, tf_odom_new_laser_diff, rot_odom_laser;
+ tf2::Transform tf_odom_laser, tf_odom_new_laser, tf_laser_new_laser, tf_odom_new_blink, tf_laser_odom, tf_odom_blink;
+ Eigen::Vector3d old_odom;
+ ros::Time t = this->collision_start_stamp_ - ros::Duration(this->config_.scan_diff_t_from_collision);
if (!load_tf_blink_laser(_front))
return false;
- if (!this->tf2_buffer.canTransform(this->odom_frame_, (_front? this->front_laser_frame_: this->rear_laser_frame_), this->imu_stamp_, ros::Duration(this->config_.tf_timeout)))
+ if (!this->tf2_buffer.canTransform(this->odom_frame_, this->config_.base_link_frame, t, ros::Duration(this->config_.tf_timeout)))
+ {
+ ROS_WARN_STREAM("CollisionManagerAlgNode::get_icp_odometry -> Can't transform from " << this->odom_frame_ << " to " << this->config_.base_link_frame << " at " << t);
+ return false;
+ }
+ else
+ {
+ tf_aux = this->tf2_buffer.lookupTransform(this->odom_frame_, this->config_.base_link_frame, t);
+ tf2::fromMsg(tf_aux.transform, tf_odom_blink);
+ }
+ old_odom << tf_odom_blink.getOrigin().x(), tf_odom_blink.getOrigin().y(), tf2::getYaw(tf_odom_blink.getRotation());
+
+ // ROS_INFO_STREAM("ICP odom before " << old_odom);
+
+ if (!this->tf2_buffer.canTransform(this->odom_frame_, (_front? this->front_laser_frame_: this->rear_laser_frame_), t, ros::Duration(this->config_.tf_timeout)))
{
- ROS_WARN_STREAM("CollisionManagerAlgNode::get_icp_odometry -> Can't transform from " << this->odom_frame_ << " to " << (_front? this->front_laser_frame_: this->rear_laser_frame_) << " at " << this->imu_stamp_);
+ ROS_WARN_STREAM("CollisionManagerAlgNode::get_icp_odometry -> Can't transform from " << this->odom_frame_ << " to " << (_front? this->front_laser_frame_: this->rear_laser_frame_) << " at " << t);
return false;
}
else
{
- tf_aux = this->tf2_buffer.lookupTransform(this->odom_frame_, (_front? this->front_laser_frame_: this->rear_laser_frame_), this->imu_stamp_);
+ tf_aux = this->tf2_buffer.lookupTransform(this->odom_frame_, (_front? this->front_laser_frame_: this->rear_laser_frame_), t);
tf2::fromMsg(tf_aux.transform, tf_odom_laser);
}
@@ -848,9 +854,11 @@ bool CollisionManagerAlgNode::get_icp_odometry(const Eigen::Vector3d& _new_laser
for (unsigned int i=0; i<9; i++)
_odom_cov(i) = std::fabs(_odom_cov(i))*(_covariances(i) < 0? -1: 1);
- rot_odom_laser = tf2::Transform(tf_odom_laser.getRotation(), tf2::Vector3(0.0, 0.0, 0.0));
- tf_odom_new_laser_diff = rot_odom_laser*tf_laser_new_laser;
- _odom_diff << tf_odom_new_laser_diff.getOrigin().x(), tf_odom_new_laser_diff.getOrigin().y(), _new_laser_pose(2);
+ _odom_diff = _odom - old_odom;
+ while (_odom_diff(2) >= M_PI)
+ _odom_diff(2) -= 2*M_PI;
+ while (_odom_diff(2) < -M_PI)
+ _odom_diff(2) += 2*M_PI;
return true;
}
@@ -886,8 +894,12 @@ bool CollisionManagerAlgNode::check_odometry_diff(const Eigen::Vector3d& _icp_od
Eigen::Vector3d limits;
limits << this->config_.odom_trans_th, this->config_.odom_trans_th, this->config_.odom_angular_th;
_icp_odometry_diff = _icp_odometry - (this->odom_after_collision_ - this->odom_before_collision_);
+ while (_icp_odometry_diff(2) >= M_PI)
+ _icp_odometry_diff(2) -= 2*M_PI;
+ while (_icp_odometry_diff(2) < -M_PI)
+ _icp_odometry_diff(2) += 2*M_PI;
- Eigen::Matrix<bool, 3, 1> odom_check = _icp_odometry.array().abs() >= limits.array();
+ Eigen::Matrix<bool, 3, 1> odom_check = _icp_odometry_diff.array().abs() >= limits.array();
return (odom_check(0) || odom_check(1) || odom_check(2));
}
@@ -906,10 +918,29 @@ void CollisionManagerAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr&
odom.odom(0) = msg->pose.pose.position.x;
odom.odom(1) = msg->pose.pose.position.y;
odom.odom(2) = tf2::getYaw(tf.getRotation());
+ if (this->config_.calculate_ang_vel)
+ {
+ double diff_ang = odom.odom(2) - this->odom_buffer_.front().odom(2);
+ while (diff_ang >= M_PI)
+ diff_ang -= 2*M_PI;
+ while (diff_ang < -M_PI)
+ diff_ang += 2*M_PI;
+ this->angular_vel_ = (diff_ang)/(odom.stamp - this->odom_buffer_.front().stamp).toSec();
+ }
+ else
+ this->angular_vel_ = msg->twist.twist.angular.z;
+ // if (std::fabs(this->angular_vel_) > 0.5)
+ // {
+ // ROS_INFO_STREAM("last odom: stamp " << this->odom_buffer_.front().stamp);
+ // ROS_INFO_STREAM("last odom: odom " << this->odom_buffer_.front().odom);
+ // ROS_INFO_STREAM("current odom: stamp " << odom.stamp);
+ // ROS_INFO_STREAM("current odom: odom " << odom.odom);
+ // ROS_INFO_STREAM("ang vel = " << this->angular_vel_);
+ // }
+
if (this->odom_buffer_.size() >= this->config_.buffer_size)
this->odom_buffer_.pop_back();
this->odom_buffer_.push_front(odom);
- this->angular_vel_ = msg->twist.twist.angular.z;
this->odom_frame_ = msg->header.frame_id;
//std::cout << msg->data << std::endl;