diff --git a/include/collision_manager_alg_node.h b/include/collision_manager_alg_node.h
index e056f513aaaebed1475038bd9ef3d21181a7ad11..66c6564a3fc030eeb7eaef17200aa7315880602b 100644
--- a/include/collision_manager_alg_node.h
+++ b/include/collision_manager_alg_node.h
@@ -371,17 +371,21 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
*
* \param _covariances Laser pose covariances from ICP.
*
- * \param _odom The 2D base link pose.
+ * \param _odom The 2D base link pose on odom frames.
*
- * \param _odom_cov Its covariances.
+ * \param _odom_cov Its covariances on odom frames.
*
- * \param _odom_diff The ICP displacment on odom frame.
+ * \param _odom_diff The ICP displacement on odom frame.
+ *
+ * \param _odom_diff_blink The ICP displacement on blink frame.
+ *
+ * \param _odom_blink_cov Its covariances on blink frame.
*
* \param _front To select the laser.
*
* \return True if succeeded.
*/
- bool 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);
+ bool get_icp_odometry(const Eigen::Vector3d& _new_laser_pose, const Eigen::Matrix3d& _covariances, Eigen::Vector3d& _odom, Eigen::Matrix3d& _odom_cov, Eigen::Vector3d& _odom_diff, Eigen::Vector3d& _odom_diff_blink, Eigen::Matrix3d& _odom_blink_cov, const bool _front);
/**
* \brief Function to compare the odometry with the ICP to check that is actually a collision.
@@ -399,9 +403,9 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
*
* It checks that ICP result is good enough and calculates the pose.
*
- * \param _new_laser_pose New laser pose from ICP.
+ * \param _icp_odom_diff ICP differential odometry in odom frame.
*
- * \param _covariances Laser pose covariances from ICP.
+ * \param _icp_covariances Laser pose covariances from ICP.
*
* \param _blink_icp ICP on base link frame.
*
@@ -411,7 +415,7 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
*
* \return True if succeeded.
*/
- bool rotate_icp_to_blink(const Eigen::Vector3d& _new_laser_pose, const Eigen::Matrix3d& _covariances, Eigen::Vector3d& _blink_icp, Eigen::Matrix3d& _blink_icp_cov, const bool _front);
+ // bool icp_odometry_to_blink(const Eigen::Vector3d& _icp_odom_diff, const Eigen::Matrix3d& _icp_covariances, Eigen::Vector3d& _blink_icp, Eigen::Matrix3d& _blink_icp_cov, const bool _front);
/**
* \brief main node thread
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index f762fe2f748f24a5020cf3a2bf53d1937a55abbb..248af575b1b8547f82ff863554b56ef89143b05e 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -232,7 +232,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->tf2_broadcaster.sendTransform(this->transform_msg);
}
- 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, blink_icp, blink_icp_cov, true))
{
icp_odom_calculated = true;
// ROS_INFO_STREAM("Odom before collision " << this->odom_before_collision_);
@@ -254,23 +254,6 @@ void CollisionManagerAlgNode::mainNodeThread(void)
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;
}
- else
- {
- 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);
- for (unsigned int i=0; i<9; i++)
- 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
{
@@ -293,6 +276,21 @@ void CollisionManagerAlgNode::mainNodeThread(void)
ROS_ERROR("CollisionManagerAlgNode::mainNodeThread -> Failed to call service on topic %s",this->icp_client_.getService().c_str());
}
this->collision_msg_.front_icp_check = icp_is_collision;
+ if (icp_is_collision)
+ {
+ 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);
+ for (unsigned int i=0; i<9; i++)
+ this->relocalization_msg_.front_pose_covariance[i] = blink_cov(i);
+
+ 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);
+ }
if (this->config_.debug)
{
this->debug_front_icp_last_scan_publisher_.publish(this->debug_front_icp_last_scan_msg_);
@@ -347,7 +345,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->tf2_broadcaster.sendTransform(this->transform_msg);
}
- 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, blink_icp, blink_icp_cov, false))
{
icp_odom_calculated = true;
// ROS_INFO_STREAM("Odom before collision " << this->odom_before_collision_);
@@ -369,23 +367,6 @@ void CollisionManagerAlgNode::mainNodeThread(void)
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;
}
- else
- {
- 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);
- for (unsigned int i=0; i<9; i++)
- 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
{
@@ -408,6 +389,21 @@ void CollisionManagerAlgNode::mainNodeThread(void)
ROS_ERROR("CollisionManagerAlgNode::mainNodeThread -> Failed to call service on topic %s",this->icp_client_.getService().c_str());
}
this->collision_msg_.rear_icp_check = icp_is_collision;
+ if (icp_is_collision)
+ {
+ 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);
+ for (unsigned int i=0; i<9; i++)
+ this->relocalization_msg_.rear_pose_covariance[i] = blink_cov(i);
+
+ 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);
+ }
if (this->config_.debug)
{
this->debug_rear_icp_last_scan_publisher_.publish(this->debug_rear_icp_last_scan_msg_);
@@ -784,11 +780,12 @@ bool CollisionManagerAlgNode::get_base_link_new_pose_from_icp(const Eigen::Vecto
return true;
}
-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)
+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, Eigen::Vector3d& _odom_diff_blink, Eigen::Matrix3d& _odom_blink_cov, 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_odom_blink;
Eigen::Vector3d old_odom;
+ Eigen::Matrix3d rot_odom_blink;
ros::Time t = this->collision_start_stamp_ - ros::Duration(this->config_.scan_diff_t_from_collision);
if (!load_tf_blink_laser(_front))
@@ -832,15 +829,24 @@ bool CollisionManagerAlgNode::get_icp_odometry(const Eigen::Vector3d& _new_laser
Eigen::Transform<double,3,Eigen::Affine> odom_laser_rot(rot);
_odom_cov = odom_laser_rot.rotation()*_covariances*odom_laser_rot.rotation().transpose();
- // for (unsigned int i=0; i<9; i++)
- // _odom_cov(i) = std::fabs(_odom_cov(i))*(_covariances(i) < 0? -1: 1);
-
_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;
+ rot = Eigen::Quaternion<double>((_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation().w(),
+ (_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation().x(),
+ (_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation().y(),
+ (_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation().z());
+ Eigen::Transform<double,3,Eigen::Affine> blink_laser_rot(rot);
+ _odom_blink_cov = blink_laser_rot.rotation()*_covariances*blink_laser_rot.rotation().transpose();
+
+ rot_odom_blink << cos(old_odom[2]), sin(old_odom[2]), 0.0,
+ -sin(old_odom[2]), cos(old_odom[2]), 0.0,
+ 0.0, 0.0, 1.0;
+ _odom_diff_blink = rot_odom_blink*_odom_diff;
+
return true;
}
@@ -902,27 +908,27 @@ bool CollisionManagerAlgNode::check_odometry_diff(const Eigen::Vector3d& _icp_od
return (odom_check(0) || odom_check(1) || odom_check(2));
}
-bool CollisionManagerAlgNode::rotate_icp_to_blink(const Eigen::Vector3d& _new_laser_pose, const Eigen::Matrix3d& _covariances, Eigen::Vector3d& _blink_icp, Eigen::Matrix3d& _blink_icp_cov, const bool _front)
-{
- if (!load_tf_blink_laser(_front))
- return false;
-
- Eigen::Quaternion<double> rot((_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation().w(),
- (_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation().x(),
- (_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation().y(),
- (_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation().z());
- Eigen::Transform<double,3,Eigen::Affine> blink_laser_rot(rot);
-
- Eigen::Vector3d aux;
- aux << _new_laser_pose(0), _new_laser_pose(1), 0.0;
- _blink_icp = blink_laser_rot*aux;
- 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();
- return true;
-}
+// bool CollisionManagerAlgNode::icp_odometry_to_blink(const Eigen::Vector3d& _icp_odom_diff, const Eigen::Matrix3d& _icp_covariances, Eigen::Vector3d& _blink_icp, Eigen::Matrix3d& _blink_icp_cov, const bool _front)
+// {
+// if (!load_tf_blink_laser(_front))
+// return false;
+
+// Eigen::Quaternion<double> rot((_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation().w(),
+// (_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation().x(),
+// (_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation().y(),
+// (_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation().z());
+// Eigen::Transform<double,3,Eigen::Affine> blink_laser_rot(rot);
+
+// Eigen::Vector3d aux;
+// aux << _new_laser_pose(0), _new_laser_pose(1), 0.0;
+// _blink_icp = blink_laser_rot*aux;
+// 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()*_icp_covariances*blink_laser_rot.rotation().transpose();
+// return true;
+// }
/* [subscriber callbacks] */
void CollisionManagerAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)