diff --git a/cfg/CollisionManager.cfg b/cfg/CollisionManager.cfg
index f9f86bae3282136f0bb0f43a74fba8bf3aa3bd26..ad1f303ab71286eb60161520034a2e91d9b3bddf 100755
--- a/cfg/CollisionManager.cfg
+++ b/cfg/CollisionManager.cfg
@@ -47,15 +47,17 @@ gen.add("fixed_frame", str_t, 0, "Fixed
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)
-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_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)
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)
+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)
-icp.add("scan_buffer_size", int_t, 0, "Number of laser_scan buffered for the ICP", 3, 1, 10)
+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)
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)
diff --git a/config/params.yaml b/config/params.yaml
index 743745751a9050c711dfa4eb2079ab2ae1ca9415..ad040c2b33bde6cbaafa404082ffd6fcd146d546 100644
--- a/config/params.yaml
+++ b/config/params.yaml
@@ -7,19 +7,20 @@ err_msg_rate: 0.5
collision_timeout: 4.0
collision_acc_transition_counter_en: True
-collision_acc_th: 4.0
-#collision_acc_th: 2.0
-collision_acc_counter_limit: 12
+collision_acc_th: 5.0
+collision_acc_counter_limit: 20
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
+collision_ang_vel_counter_limit: 20
+odom_trans_th: 0.1
+odom_angular_th: 0.15
front_laser_en: True
rear_laser_en: True
-scan_buffer_size: 6
-scan_diff_t_from_collision: 0.0
+buffer_size: 10
+scan_diff_t_from_collision: 0.15
front_laser_en: True
rear_laser_en: True
-icp_min_points: 100
-icp_max_err_by_points: 1.0
+icp_min_points: 280
+icp_max_err_by_points: 0.5
diff --git a/include/collision_manager_alg_node.h b/include/collision_manager_alg_node.h
index 5d6e95ac41f2a5132ce295379615d275629dd490..ad654bb8989746260e1613dffb7e8f06a9b1e7d9 100644
--- a/include/collision_manager_alg_node.h
+++ b/include/collision_manager_alg_node.h
@@ -56,6 +56,17 @@ typedef struct {
ros::Time stamp; ///< Laser scan time stamp.
}RangesWithStamp;
+/**
+ * \struct OdomWithStamp.
+ *
+ * \brief A struct with the Odometry information and the time stamp.
+ *
+ */
+typedef struct {
+ Eigen::Vector3d odom; ///< Odometry data.
+ ros::Time stamp; ///< Time stamp.
+}OdomWithStamp;
+
/**
* \brief IRI ROS Specific Algorithm Class
*
@@ -87,9 +98,12 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
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.
bool is_rear_icp_input_data_ok_; ///<Boolean to know of all the necessary data has been saved on the service request.
- bool front_icp_succedded_; ///< Boolean to know that ICP service was called withs success.
- bool rear_icp_succedded_; ///< Boolean to know that ICP service was called withs success.
double angular_vel_; ///< Robot's angular velocity from odometry.
+ std::string odom_frame_; ///< Odometry Frame id.
+ std::list<OdomWithStamp> odom_buffer_; ///< Odometry buffer.
+ Eigen::Vector3d odom_before_collision_; ///< Odometry data before collision.
+ Eigen::Vector3d odom_after_collision_; ///< Odometry data after collision.
+ bool odom_input_is_ok_; ///< Bool to know if the wheel odometry from a collision is ok.
// [publisher attributes]
ros::Publisher collisions_publisher_;
@@ -262,13 +276,18 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
/**
* \brief Function to find the last scan before a collision.
*
- * \param _stamp The collision time stamp.
- *
* \param _front To select the laser.
*
* \return True if succeeded.
*/
- bool set_ref_ranges(ros::Time _stamp, bool _front);
+ bool set_ref_ranges(bool _front);
+
+ /**
+ * \brief Function to find the last odometry before a collision.
+ *
+ * \return True if succeeded.
+ */
+ bool set_odom_before_collision(void);
/**
* \brief Function to find the last scan after a collision.
@@ -279,6 +298,13 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
*/
bool set_last_ranges(bool _front);
+ /**
+ * \brief Function to find the last odom after a collision.
+ *
+ * \return True if succeeded.
+ */
+ bool get_last_odom(void);
+
/**
* \brief Function to load tf from base_link to fron/rear laser.
*
@@ -307,6 +333,38 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
*/
bool get_base_link_new_pose_from_icp(const Eigen::Vector3d& _new_laser_pose, const Eigen::Matrix3d& _covariances, Eigen::Vector3d& _blink_pose, Eigen::Matrix3d& _blink_cov, const bool _front);
+ /**
+ * \brief Function to get the new robot's base_link pose from ICP result.
+ *
+ * It checks that ICP result is good enough and calculates the pose.
+ *
+ * \param _new_laser_pose New laser pose from ICP.
+ *
+ * \param _covariances Laser pose covariances from ICP.
+ *
+ * \param _odom The 2D base link pose.
+ *
+ * \param _odom_cov Its covariances.
+ *
+ * \param _odom_diff The ICP displacment on odom 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);
+
+ /**
+ * \brief Function to compare the odometry with the ICP to check that is actually a collision.
+ *
+ * \param _icp_odometry ICP odometry result.
+ *
+ * \param _icp_odometry_diff To return the odometry difference.
+ *
+ * \return True if the difference is big enough.
+ */
+ bool check_odometry_diff(const Eigen::Vector3d& _icp_odometry, Eigen::Vector3d& _icp_odometry_diff);
+
/**
* \brief main node thread
*
diff --git a/msg/collision.msg b/msg/collision.msg
index b825c40dbf4b829be2903be5ff99d613cccb0c43..8983d31be5746ad8f67d3b04b1ceba96d9807087 100644
--- a/msg/collision.msg
+++ b/msg/collision.msg
@@ -1,15 +1,19 @@
Header header # Msg header.
+bool acc_trigger # If acc has trigger the collision detection.
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.
+bool front_icp_check # If front ICP checks that is a collision.
int32 front_icp_points # Front ICP laser scan matching points.
float32 front_icp_err # Front ICP error per point.
-geometry_msgs/Pose2D front_laser_disp # Front laser displacement.
-float32[9] front_covariance # Front laser displacement covariance.
+geometry_msgs/Pose2D front_odom_diff # Difference between Odometry and front laser ICP.
+float32[9] front_covariance # Front ICP odometry covariance.
+bool rear_icp_check # If rear ICP checks that is a collision.
int32 rear_icp_points # Rear ICP laser scan matching points.
float32 rear_icp_err # Rear ICP error per point.
-geometry_msgs/Pose2D rear_laser_disp # Rear laser displacement.
-float32[9] rear_covariance # Rear laser displacement covariance.
+geometry_msgs/Pose2D rear_odom_diff # Difference between Odometry and rear laser ICP.
+float32[9] rear_covariance # Rear ICP odometry covariance.
diff --git a/rviz/dogs.rviz b/rviz/dogs.rviz
index 62dc34192dccb7a70964b447b27ea6aeb9d7e129..64e35553b8bb63f55465f140e8fa8d30b7c47d82 100644
--- a/rviz/dogs.rviz
+++ b/rviz/dogs.rviz
@@ -10,8 +10,7 @@ Panels:
- /LandmarksRobotPose1/Covariance1
- /LandmarksRobotPose1/Covariance1/Position1
- /LandmarksRobotPose1/Covariance1/Orientation1
- - /Rear_icp1/rear_after_icp1
- Splitter Ratio: 0.5
+ Splitter Ratio: 0.6882352828979492
Tree Height: 728
- Class: rviz/Selection
Name: Selection
@@ -54,6 +53,10 @@ Visualization Manager:
Value: true
map:
Value: true
+ new_front_laser:
+ Value: true
+ new_rear_laser:
+ Value: true
odom:
Value: true
Marker Scale: 1
@@ -66,9 +69,11 @@ Visualization Manager:
odom:
base_link:
base_laser_front_link:
- {}
+ new_front_laser:
+ {}
base_laser_rear_link:
- {}
+ new_rear_laser:
+ {}
imu_bno055:
{}
Update Interval: 0
@@ -417,7 +422,7 @@ Visualization Manager:
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
- Color: 206; 92; 0
+ Color: 52; 101; 164
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
@@ -563,7 +568,7 @@ Visualization Manager:
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
- Color: 173; 127; 168
+ Color: 252; 233; 79
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
@@ -621,7 +626,7 @@ Visualization Manager:
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
- Color: 252; 233; 79
+ Color: 239; 41; 41
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
@@ -679,11 +684,11 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Scale: 30.226451873779297
+ Scale: 21.86640739440918
Target Frame: base_link
Value: TopDownOrtho (rviz)
- X: 0.819812536239624
- Y: 0.7132653594017029
+ X: -3.3158531188964844
+ Y: 3.8758318424224854
Saved: ~
Window Geometry:
Displays:
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index 5296f3a591a71e71c352ce1242c0f5fa8757a5c2..67d0fd03d64c6b403dcb86d20d75b4c1e7c073a7 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -36,9 +36,8 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
this->is_rear_icp_input_data_ok_ = false;
this->tf_base_link_front_laser_loaded_ = false;
this->tf_base_link_rear_laser_loaded_ = false;
- this->front_icp_succedded_ = false;
- this->rear_icp_succedded_ = false;
this->angular_vel_ = 0.0;
+ this->odom_input_is_ok_ = 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;
@@ -106,8 +105,9 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->rear_laser_scan_mutex_enter();
this->front_laser_scan_mutex_enter();
this->odom_mutex_enter();
- Eigen::Vector3d blink_acc, blink_ang_vel, rear_blink_pose, front_blink_pose, new_laser_pose;
- Eigen::Matrix3d rear_cov, front_cov, covariances;
+ Eigen::Vector3d blink_acc, blink_ang_vel, blink_pose, new_laser_pose, icp_odometry, icp_odometry_diff, diff_wheel_icp_odom;
+ Eigen::Matrix3d blink_cov, covariances, icp_odom_cov;
+ bool icp_succedded, icp_odom_calculated, icp_blink_pose_calculated, icp_is_collision;
if (this->new_imu_input_)
{
@@ -140,6 +140,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
// ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: angle estimated = " << this->front_icp_srv_.request.rot_angle_estimation);
+ this->odom_input_is_ok_ = (this->odom_input_is_ok_ && get_last_odom());
// Call the service
if (!(this->config_.front_laser_en || this->config_.rear_laser_en))
ROS_WARN("CollisionManagerAlgNode::mainNodeThread -> Collision end detected but no laser is enabled.");
@@ -157,64 +158,101 @@ void CollisionManagerAlgNode::mainNodeThread(void)
// ROS_INFO_STREAM(" error " << front_icp_srv_.response.error);
// ROS_INFO_STREAM(" error/points " << front_icp_srv_.response.error/front_icp_srv_.response.nvalid << "; max " << this->config_.icp_max_err_by_points);
+ icp_succedded = true;
+
//update collison msg data.
this->collision_msg_.front_icp_err = front_icp_srv_.response.error/front_icp_srv_.response.nvalid;
- this->collision_msg_.front_laser_disp = front_icp_srv_.response.new_laser_pose;
- for (unsigned int i=0; i<9; i++)
- this->collision_msg_.front_covariance[i] = front_icp_srv_.response.covariance[i];
+ this->collision_msg_.front_icp_points = front_icp_srv_.response.nvalid;
- 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)
+ //calculate new base_link pose in fixed frame coordenates
+ new_laser_pose << front_icp_srv_.response.new_laser_pose.x, front_icp_srv_.response.new_laser_pose.y, front_icp_srv_.response.new_laser_pose.theta;
+ covariances << front_icp_srv_.response.covariance[0], front_icp_srv_.response.covariance[1], front_icp_srv_.response.covariance[2],
+ front_icp_srv_.response.covariance[3], front_icp_srv_.response.covariance[4], front_icp_srv_.response.covariance[5],
+ front_icp_srv_.response.covariance[6], front_icp_srv_.response.covariance[7], front_icp_srv_.response.covariance[8];
+
+ icp_blink_pose_calculated = get_base_link_new_pose_from_icp(new_laser_pose, covariances, blink_pose, blink_cov, true);
+
+ //publish new laser tf to visualize the laser points.
+ if (this->config_.debug)
{
- new_laser_pose << front_icp_srv_.response.new_laser_pose.x, front_icp_srv_.response.new_laser_pose.y, front_icp_srv_.response.new_laser_pose.theta;
- covariances << front_icp_srv_.response.covariance[0], front_icp_srv_.response.covariance[1], front_icp_srv_.response.covariance[2],
- front_icp_srv_.response.covariance[3], front_icp_srv_.response.covariance[4], front_icp_srv_.response.covariance[5],
- front_icp_srv_.response.covariance[6], front_icp_srv_.response.covariance[7], front_icp_srv_.response.covariance[8];
- this->front_icp_succedded_ = true;
+ this->transform_msg.header.frame_id = this->front_laser_frame_;
+ this->transform_msg.child_frame_id = "new_front_laser";
+ this->transform_msg.header.stamp = this->imu_stamp_;
+ this->transform_msg.transform.translation.x = front_icp_srv_.response.new_laser_pose.x;
+ this->transform_msg.transform.translation.y = front_icp_srv_.response.new_laser_pose.y;
+ this->transform_msg.transform.translation.z = 0.0;
+ tf2::Quaternion q;
+ q.setRPY(0.0, 0.0, front_icp_srv_.response.new_laser_pose.theta);
+ this->transform_msg.transform.rotation = tf2::toMsg(q);
+ // this->transform_msg.transform.rotation.x = q.getRotation().x();
+ // this->transform_msg.transform.rotation.y = q.getRotation().y();
+ // this->transform_msg.transform.rotation.z = q.getRotation().z();
+ // this->transform_msg.transform.rotation.w = q.getRotation().w();
+ this->tf2_broadcaster.sendTransform(this->transform_msg);
+ }
- if (this->config_.debug)
+ 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))
{
- this->transform_msg.header.frame_id = this->front_laser_frame_;
- this->transform_msg.child_frame_id = "new_front_laser";
- this->transform_msg.header.stamp = this->imu_stamp_;
- this->transform_msg.transform.translation.x = front_icp_srv_.response.new_laser_pose.x;
- this->transform_msg.transform.translation.y = front_icp_srv_.response.new_laser_pose.y;
- this->transform_msg.transform.translation.z = 0.0;
- tf2::Quaternion q;
- q.setRPY(0.0, 0.0, front_icp_srv_.response.new_laser_pose.theta);
- this->transform_msg.transform.rotation = tf2::toMsg(q);
- // this->transform_msg.transform.rotation.x = q.getRotation().x();
- // this->transform_msg.transform.rotation.y = q.getRotation().y();
- // this->transform_msg.transform.rotation.z = q.getRotation().z();
- // this->transform_msg.transform.rotation.w = q.getRotation().w();
- this->tf2_broadcaster.sendTransform(this->transform_msg);
+ 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
+ {
+ 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;
+ icp_is_collision = false;
}
-
- this->front_icp_succedded_ = get_base_link_new_pose_from_icp(new_laser_pose, covariances, front_blink_pose, front_cov, true);
-
- this->collision_msg_.front_icp_points = front_icp_srv_.response.nvalid;
}
else
{
- this->collision_msg_.front_icp_points = 0;
- this->front_icp_succedded_ = false;
+ 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;
+ icp_is_collision = false;
}
}
else
{
- this->front_icp_succedded_ = false;
+ icp_succedded = false;
+ icp_odom_calculated = false;
+ icp_is_collision = false;
+ icp_blink_pose_calculated = false;
this->collision_msg_.front_icp_points = 0;
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 (this->config_.debug)
{
this->debug_front_icp_last_scan_publisher_.publish(this->debug_front_icp_last_scan_msg_);
this->debug_front_icp_ref_scan_publisher_.publish(this->debug_front_icp_ref_scan_msg_);
- if (this->front_icp_succedded_)
+ if (icp_succedded)
{
this->debug_front_after_icp_scan_msg_.header.frame_id = "new_front_laser";
this->debug_front_after_icp_scan_publisher_.publish(this->debug_front_after_icp_scan_msg_);
- this->debug_front_icp_pose_publisher_.publish(this->debug_front_icp_pose_msg_);
+ if (icp_blink_pose_calculated)
+ this->debug_front_icp_pose_publisher_.publish(this->debug_front_icp_pose_msg_);
}
}
}
@@ -230,64 +268,103 @@ void CollisionManagerAlgNode::mainNodeThread(void)
// ROS_INFO_STREAM(" error " << rear_icp_srv_.response.error);
// ROS_INFO_STREAM(" error/points " << rear_icp_srv_.response.error/rear_icp_srv_.response.nvalid << "; max " << this->config_.icp_max_err_by_points);
+ icp_succedded = true;
//update collison msg data.
this->collision_msg_.rear_icp_err = rear_icp_srv_.response.error/rear_icp_srv_.response.nvalid;
- this->collision_msg_.rear_laser_disp = rear_icp_srv_.response.new_laser_pose;
- for (unsigned int i=0; i<9; i++)
- this->collision_msg_.rear_covariance[i] = rear_icp_srv_.response.covariance[i];
+ this->collision_msg_.rear_icp_points = rear_icp_srv_.response.nvalid;
+ // this->collision_msg_.rear_laser_disp = rear_icp_srv_.response.new_laser_pose;
+ // for (unsigned int i=0; i<9; i++)
+ // this->collision_msg_.rear_covariance[i] = rear_icp_srv_.response.covariance[i];
+
+ //calculate new base_link pose in fixed frame coordenates
+ new_laser_pose << rear_icp_srv_.response.new_laser_pose.x, rear_icp_srv_.response.new_laser_pose.y, rear_icp_srv_.response.new_laser_pose.theta;
+ covariances << rear_icp_srv_.response.covariance[0], rear_icp_srv_.response.covariance[1], rear_icp_srv_.response.covariance[2],
+ rear_icp_srv_.response.covariance[3], rear_icp_srv_.response.covariance[4], rear_icp_srv_.response.covariance[5],
+ rear_icp_srv_.response.covariance[6], rear_icp_srv_.response.covariance[7], rear_icp_srv_.response.covariance[8];
+
+ icp_blink_pose_calculated = get_base_link_new_pose_from_icp(new_laser_pose, covariances, blink_pose, blink_cov, false);
+
+ //publish new laser tf to visualize the laser points.
+ if (this->config_.debug)
+ {
+ this->transform_msg.header.frame_id = this->rear_laser_frame_;
+ this->transform_msg.child_frame_id = "new_rear_laser";
+ this->transform_msg.header.stamp = this->imu_stamp_;
+ this->transform_msg.transform.translation.x = rear_icp_srv_.response.new_laser_pose.x;
+ this->transform_msg.transform.translation.y = rear_icp_srv_.response.new_laser_pose.y;
+ this->transform_msg.transform.translation.z = 0.0;
+ tf2::Quaternion q;
+ q.setRPY(0.0, 0.0, rear_icp_srv_.response.new_laser_pose.theta);
+ this->transform_msg.transform.rotation = tf2::toMsg(q);
+ // this->transform_msg.transform.rotation.x = q.getRotation().x();
+ // this->transform_msg.transform.rotation.y = q.getRotation().y();
+ // this->transform_msg.transform.rotation.z = q.getRotation().z();
+ // this->transform_msg.transform.rotation.w = q.getRotation().w();
+ 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)
{
- new_laser_pose << rear_icp_srv_.response.new_laser_pose.x, rear_icp_srv_.response.new_laser_pose.y, rear_icp_srv_.response.new_laser_pose.theta;
- covariances << rear_icp_srv_.response.covariance[0], rear_icp_srv_.response.covariance[1], rear_icp_srv_.response.covariance[2],
- rear_icp_srv_.response.covariance[3], rear_icp_srv_.response.covariance[4], rear_icp_srv_.response.covariance[5],
- rear_icp_srv_.response.covariance[6], rear_icp_srv_.response.covariance[7], rear_icp_srv_.response.covariance[8];
- this->rear_icp_succedded_ = true;
-
- if (this->config_.debug)
+ if (get_icp_odometry(new_laser_pose, covariances, icp_odometry, icp_odom_cov, icp_odometry_diff, true))
{
- this->transform_msg.header.frame_id = this->rear_laser_frame_;
- this->transform_msg.child_frame_id = "new_rear_laser";
- this->transform_msg.header.stamp = this->imu_stamp_;
- this->transform_msg.transform.translation.x = rear_icp_srv_.response.new_laser_pose.x;
- this->transform_msg.transform.translation.y = rear_icp_srv_.response.new_laser_pose.y;
- this->transform_msg.transform.translation.z = 0.0;
- tf2::Quaternion q;
- q.setRPY(0.0, 0.0, rear_icp_srv_.response.new_laser_pose.theta);
- this->transform_msg.transform.rotation = tf2::toMsg(q);
- // this->transform_msg.transform.rotation.x = q.getRotation().x();
- // this->transform_msg.transform.rotation.y = q.getRotation().y();
- // this->transform_msg.transform.rotation.z = q.getRotation().z();
- // this->transform_msg.transform.rotation.w = q.getRotation().w();
- this->tf2_broadcaster.sendTransform(this->transform_msg);
+ 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
+ {
+ 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;
+ icp_is_collision = false;
}
-
- this->rear_icp_succedded_ = get_base_link_new_pose_from_icp(new_laser_pose, covariances, rear_blink_pose, rear_cov, false);
-
- this->collision_msg_.rear_icp_points = rear_icp_srv_.response.nvalid;
}
else
{
- this->collision_msg_.rear_icp_points = 0;
- this->rear_icp_succedded_ = false;
+ 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;
+ icp_is_collision = false;
}
}
else
{
- this->rear_icp_succedded_ = false;
+ icp_succedded = false;
+ icp_odom_calculated = false;
+ icp_is_collision = false;
+ icp_blink_pose_calculated = false;
this->collision_msg_.rear_icp_points = 0;
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 (this->config_.debug)
{
this->debug_rear_icp_last_scan_publisher_.publish(this->debug_rear_icp_last_scan_msg_);
this->debug_rear_icp_ref_scan_publisher_.publish(this->debug_rear_icp_ref_scan_msg_);
- if (this->rear_icp_succedded_)
+ if (icp_succedded)
{
this->debug_rear_after_icp_scan_msg_.header.frame_id = "new_rear_laser";
this->debug_rear_after_icp_scan_publisher_.publish(this->debug_rear_after_icp_scan_msg_);
- this->debug_rear_icp_pose_publisher_.publish(this->debug_rear_icp_pose_msg_);
+ if (icp_blink_pose_calculated)
+ this->debug_rear_icp_pose_publisher_.publish(this->debug_rear_icp_pose_msg_);
}
}
}
@@ -336,8 +413,8 @@ void CollisionManagerAlgNode::mainNodeThread(void)
ROS_WARN("CollisionManagerAlgNode::mainNodeThread -> Collision detected but no laser is enabled.");
else
{
- this->is_front_icp_input_data_ok_ = set_ref_ranges(this->collision_start_stamp_, true);
- this->is_rear_icp_input_data_ok_ = set_ref_ranges(this->collision_start_stamp_, false);
+ this->is_front_icp_input_data_ok_ = set_ref_ranges(true);
+ this->is_rear_icp_input_data_ok_ = set_ref_ranges(false);
if (!this->is_front_icp_input_data_ok_)
this->collision_msg_.front_icp_points = 0;
@@ -345,6 +422,8 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->collision_msg_.rear_icp_points = 0;
}
+ this->odom_input_is_ok_ = set_odom_before_collision();
+
//init variables
this->in_collision_ = true;
this->max_acc_ = blink_acc;
@@ -573,6 +652,12 @@ bool CollisionManagerAlgNode::check_start_of_collision(double _acc_norm_2d, doub
{
bool acc_true = check_acc_collision_transition(_acc_norm_2d, true);
bool ang_vel_true = check_ang_vel_collision_transition(_ang_vel, true);
+
+ // update collision msg
+ if (acc_true)
+ this->collision_msg_.acc_trigger = true;
+ if (ang_vel_true)
+ this->collision_msg_.ang_vel_trigger = 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)
@@ -580,7 +665,7 @@ bool CollisionManagerAlgNode::check_start_of_collision(double _acc_norm_2d, doub
return acc_true || ang_vel_true;
}
-bool CollisionManagerAlgNode::set_ref_ranges(ros::Time _stamp, bool _front)
+bool CollisionManagerAlgNode::set_ref_ranges(bool _front)
{
if (!(_front? this->config_.front_laser_en: this->config_.rear_laser_en))
return false;
@@ -606,6 +691,24 @@ bool CollisionManagerAlgNode::set_ref_ranges(ros::Time _stamp, bool _front)
return true;
}
+bool CollisionManagerAlgNode::set_odom_before_collision(void)
+{
+ auto it = this->odom_buffer_.begin();
+ while ((it != this->odom_buffer_.end()) && (it->stamp > (this->collision_start_stamp_ - ros::Duration(this->config_.scan_diff_t_from_collision))))
+ ++it;
+ if (it == this->odom_buffer_.end())
+ {
+ ROS_WARN_STREAM("CollisionManagerAlgNode::set_odom_before_collision -> Collision detected at " << this->collision_start_stamp_ << " but there is no odometry before collision");
+ if (this->odom_buffer_.size() > 0)
+ ROS_WARN_STREAM("CollisionManagerAlgNode::set_odom_before_collision -> Odometry times: " << this->odom_buffer_.front().stamp << ", " << this->odom_buffer_.back().stamp);
+ else
+ ROS_WARN_STREAM("CollisionManagerAlgNode::set_odom_before_collision -> No Odometry buffered");
+ return false;
+ }
+ this->odom_before_collision_ = it->odom;
+ return true;
+}
+
bool CollisionManagerAlgNode::set_last_ranges(bool _front)
{
if (!(_front? this->config_.front_laser_en: this->config_.rear_laser_en))
@@ -627,13 +730,21 @@ bool CollisionManagerAlgNode::set_last_ranges(bool _front)
return false;
}
+bool CollisionManagerAlgNode::get_last_odom(void)
+{
+ if (this->odom_buffer_.size() > 0)
+ {
+ this->odom_after_collision_ = this->odom_buffer_.front().odom;
+ return true;
+ }
+ ROS_WARN_STREAM("CollisionManagerAlgNode::get_last_odom -> No odom buffered");
+ return false;
+}
+
bool CollisionManagerAlgNode::get_base_link_new_pose_from_icp(const Eigen::Vector3d& _new_laser_pose, const Eigen::Matrix3d& _covariances, Eigen::Vector3d& _blink_pose, Eigen::Matrix3d& _blink_cov, const bool _front)
{
geometry_msgs::TransformStamped tf_aux;
- tf2::Transform tf_map_laser, tf_map_new_laser, tf_laser_new_laser, tf_map_new_blink;
-
- if ((_front? !this->front_icp_succedded_: !this->rear_icp_succedded_))
- return false;
+ tf2::Transform tf_map_laser, tf_map_new_laser, tf_laser_new_laser, tf_map_new_blink, tf_laser_map;
if (!load_tf_blink_laser(_front))
return false;
@@ -657,10 +768,11 @@ bool CollisionManagerAlgNode::get_base_link_new_pose_from_icp(const Eigen::Vecto
tf_map_new_blink = tf_map_new_laser*(_front? this->tf_blink_front_laser: this->tf_blink_rear_laser).inverse();
_blink_pose << tf_map_new_blink.getOrigin().x(), tf_map_new_blink.getOrigin().y(), tf2::getYaw(tf_map_new_blink.getRotation());
- Eigen::Quaternion<double> rot(tf_map_laser.getRotation().w(), tf_map_laser.getRotation().x(), tf_map_laser.getRotation().y(), tf_map_laser.getRotation().z());
+ tf_laser_map = tf_map_laser.inverse();
+ Eigen::Quaternion<double> rot(tf_laser_map.getRotation().w(), tf_laser_map.getRotation().x(), tf_laser_map.getRotation().y(), tf_laser_map.getRotation().z());
- Eigen::Transform<double,3,Eigen::Affine> map_laser_rot(rot);
- _blink_cov = map_laser_rot*_covariances;
+ Eigen::Transform<double,3,Eigen::Affine> laser_map_rot(rot);
+ _blink_cov = laser_map_rot*_covariances;
for (unsigned int i=0; i<9; i++)
_blink_cov(i) = std::fabs(_blink_cov(i))*(_covariances(i) < 0? -1: 1);
@@ -700,6 +812,49 @@ 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)
+{
+ 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;
+
+ 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)))
+ {
+ 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_);
+ return false;
+ }
+ else
+ {
+ tf_aux = this->tf2_buffer.lookupTransform(this->odom_frame_, (_front? this->front_laser_frame_: this->rear_laser_frame_), this->imu_stamp_);
+ tf2::fromMsg(tf_aux.transform, tf_odom_laser);
+ }
+
+ tf2::Quaternion q;
+ q.setRPY(0.0, 0.0, _new_laser_pose(2));
+ tf_laser_new_laser = tf2::Transform(q, tf2::Vector3(_new_laser_pose(0), _new_laser_pose(1), 0.0));
+ tf_odom_new_laser = tf_odom_laser*tf_laser_new_laser;
+
+ tf_odom_new_blink = tf_odom_new_laser*(_front? this->tf_blink_front_laser: this->tf_blink_rear_laser).inverse();
+ _odom << tf_odom_new_blink.getOrigin().x(), tf_odom_new_blink.getOrigin().y(), tf2::getYaw(tf_odom_new_blink.getRotation());
+
+ tf_laser_odom = tf_odom_laser.inverse();
+ Eigen::Quaternion<double> rot(tf_laser_odom.getRotation().w(), tf_laser_odom.getRotation().x(), tf_laser_odom.getRotation().y(), tf_laser_odom.getRotation().z());
+
+ Eigen::Transform<double,3,Eigen::Affine> laser_odom_rot(rot);
+ _odom_cov = laser_odom_rot*_covariances;
+
+ 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);
+
+ return true;
+}
+
bool CollisionManagerAlgNode::load_tf_blink_laser(bool _front)
{
geometry_msgs::TransformStamped tf_aux;
@@ -720,6 +875,22 @@ bool CollisionManagerAlgNode::load_tf_blink_laser(bool _front)
return true;
}
+bool CollisionManagerAlgNode::check_odometry_diff(const Eigen::Vector3d& _icp_odometry, Eigen::Vector3d& _icp_odometry_diff)
+{
+ if (!this->odom_input_is_ok_)
+ {
+ ROS_WARN_STREAM("CollisionManagerAlgNode::check_odometry_diff -> No wheel odometry information has been saved. Odometry diff can't be calculated.");
+ _icp_odometry_diff << 0.0, 0.0, 0.0;
+ return false;
+ }
+ 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_);
+
+ Eigen::Matrix<bool, 3, 1> odom_check = _icp_odometry.array().abs() >= limits.array();
+ return (odom_check(0) || odom_check(1) || odom_check(2));
+}
+
/* [subscriber callbacks] */
void CollisionManagerAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
{
@@ -728,7 +899,18 @@ void CollisionManagerAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr&
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
this->odom_mutex_enter();
+ OdomWithStamp odom;
+ tf2::Transform tf;
+ tf2::fromMsg(msg->pose.pose, tf);
+ odom.stamp = msg->header.stamp;
+ 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->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;
//unlock previously blocked shared variables
@@ -786,7 +968,7 @@ void CollisionManagerAlgNode::rear_laser_scan_callback(const sensor_msgs::LaserS
RangesWithStamp r;
r.ranges = msg->ranges;
r.stamp = msg->header.stamp;
- if (this->rear_ranges_.size() >= this->config_.scan_buffer_size)
+ if (this->rear_ranges_.size() >= this->config_.buffer_size)
this->rear_ranges_.pop_back();
this->rear_ranges_.push_front(r);
}
@@ -846,7 +1028,7 @@ void CollisionManagerAlgNode::front_laser_scan_callback(const sensor_msgs::Laser
RangesWithStamp r;
r.ranges = msg->ranges;
r.stamp = msg->header.stamp;
- if (this->front_ranges_.size() >= this->config_.scan_buffer_size)
+ if (this->front_ranges_.size() >= this->config_.buffer_size)
this->front_ranges_.pop_back();
this->front_ranges_.push_front(r);
}
@@ -913,11 +1095,6 @@ void CollisionManagerAlgNode::node_config_update(Config &config, uint32_t level)
this->rear_laser_scan_mutex_enter();
this->front_laser_scan_mutex_enter();
- if (!config.front_laser_en)
- this->front_icp_succedded_ = false;
- if (!config.rear_laser_en)
- this->rear_icp_succedded_ = false;
-
if(config.rate!=this->getRate())
this->setRate(config.rate);
this->config_=config;