diff --git a/cfg/CollisionManager.cfg b/cfg/CollisionManager.cfg
index c7a799778a85f5645e1be139c7452f7433e3f6e9..7f8308bcb5e3a2109060aaf4e398a57c3eaf4bc9 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 e953581b8ba8b44fb956ed3407bf3043222590f7..b63bc17eef7b3cd9c1c52b1592160f58dd734d2d 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 6f5bee94ad7b3cf56598d9857d8d978db4007dcd..30f030b097c1faa832aae7d23d59e9f658d3e940 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 b5e1d9b70ff52dca17bc257e19c910a1774564be..8aaedf57b4c65fe2da5630a3dbe6abd72ff77441 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;
}