Skip to content
Snippets Groups Projects
Commit 7812a7bd authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

ICP incremental odometry from base link calculated

parent defc2a10
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/collision_manager_alg_node.h
+ 11
− 7
View file @ 7812a7bd
......@@ -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
......
src/collision_manager_alg_node.cpp
+ 67
− 61
View file @ 7812a7bd
......@@ -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)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment