diff --git a/cfg/CollisionManager.cfg b/cfg/CollisionManager.cfg
index 65a796b05d1992ddf4f9068926940277cfdc52ee..1f03fe5bef34a9fefc1de3137bb6244867dd39fe 100755
--- a/cfg/CollisionManager.cfg
+++ b/cfg/CollisionManager.cfg
@@ -52,14 +52,16 @@ gen.add("watchdog_t", double_t, 0, "Maximu
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_th_end", double_t, 0, "Acceleration threshold to detect a end of 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_th_end", double_t, 0, "Angular velocity threshold to detect a end of 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.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("buffer_size", int_t, 0, "Number of laser_scan and odometry buffered for the ICP", 3, 1, 50)
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 248be99d9ab8fad33c756c571a5998d019078f2b..0244e5ce862ac2eb59f68732b1f89f88628d8203 100644
--- a/config/params.yaml
+++ b/config/params.yaml
@@ -7,12 +7,14 @@ err_msg_rate: 0.5
calculate_ang_vel: False
watchdog_t: 1.0
-collision_timeout: 5.0
+collision_timeout: 8.0
collision_acc_transition_counter_en: True
collision_acc_th: 5.0
+collision_acc_th_end: 2.0
collision_acc_counter_limit: 10
collision_ang_vel_transition_counter_en: True
collision_ang_vel_th: 0.4
+collision_ang_vel_th_end: 0.2
#collision_ang_vel_th: 0.2
collision_ang_vel_counter_limit: 10
odom_trans_th: 0.2
@@ -20,7 +22,7 @@ odom_angular_th: 0.15
front_laser_en: True
rear_laser_en: True
-buffer_size: 10
+buffer_size: 30
scan_diff_t_from_collision: 0.0
front_laser_en: True
rear_laser_en: True
diff --git a/config/range_filter.yaml b/config/range_filter.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4f398e44146809a070b92d3253e9ab2027bb9e9b
--- /dev/null
+++ b/config/range_filter.yaml
@@ -0,0 +1,9 @@
+scan_filter_chain:
+- name: range
+ type: laser_filters/LaserScanRangeFilter
+ params:
+ use_message_range_limits: false
+ lower_threshold: 2.0
+ upper_threshold: .inf
+ lower_replacement_value: .inf
+ upper_replacement_value: .inf
diff --git a/include/collision_manager_alg_node.h b/include/collision_manager_alg_node.h
index 66c6564a3fc030eeb7eaef17200aa7315880602b..0865cf76292b4a90cf22139157a16deec4f9656d 100644
--- a/include/collision_manager_alg_node.h
+++ b/include/collision_manager_alg_node.h
@@ -73,6 +73,18 @@ typedef struct {
ros::Time stamp; ///< Time stamp.
}OdomWithStamp;
+/**
+ * \struct ImuWithStamp.
+ *
+ * \brief A struct with the IMU information and the time stamp.
+ *
+ */
+typedef struct {
+ Eigen::Vector3d acc; ///< IMU accelerometer data.
+ Eigen::Vector3d ang_vel; ///< IMU angular velocity data.
+ ros::Time stamp; ///< Time stamp.
+}ImuWithStamp;
+
/**
* \brief IRI ROS Specific Algorithm Class
*
@@ -83,6 +95,7 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
bool new_imu_input_; ///< Flag to know that new input data has been received.
Eigen::Vector3d imu_local_acc_; ///< Local input linear acceleration.
Eigen::Vector3d imu_local_ang_vel_; ///< Local input angular speed.
+ std::list<ImuWithStamp> imu_buffer_; ///< IMU data buffer.
ros::Time imu_stamp_; ///< IMU stamp.
ros::Time last_imu_t_; ///< Stamp from the last imu velocity data integrated.
std::string imu_frame_; ///< IMU frame id.
@@ -234,18 +247,35 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
protected:
/**
- * \brief Function to tranform linear local imu data to base_link frame.
+ * \brief Function to tranform the current local imu data to base_link frame.
+ *
+ * \param _blink_acc The current global linear acceleration.
+ *
+ * \param _blink_ang_vel The current global angular velocity.
+ *
+ * \return True in success.
+ */
+ bool get_current_base_link_imu_data(Eigen::Vector3d& _blink_acc, Eigen::Vector3d& _blink_ang_vel);
+
+ /**
+ * \brief Function to tranform any local imu data to base_link frame.
*
* It checks if imu frame id is different from base_link frame and if not already
* saved, it saves the transfrom.
*
- * \param _blink_acc The current global linear acceleration.
+ * \param _local_acc The local linear acceleration.
*
- * \param _blink_ang_vel The current global angular velocity.
+ * \param _local_ang_vel The local angular velocity.
+ *
+ * \param _stamp The target stamp.
+ *
+ * \param _blink_acc The global linear acceleration.
+ *
+ * \param _blink_ang_vel The global angular velocity.
*
* \return True in success.
*/
- bool get_base_link_imu_data(Eigen::Vector3d& _blink_acc, Eigen::Vector3d& _blink_ang_vel);
+ bool transform_to_base_link_imu_data(const Eigen::Vector3d& _local_acc, const Eigen::Vector3d& _local_ang_vel, const ros::Time& _stamp, Eigen::Vector3d& _blink_acc, Eigen::Vector3d& _blink_ang_vel);
/**
* \brief Function to check if is an acceleration collision transition.
@@ -302,6 +332,20 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
*/
bool check_start_of_collision(double _acc_norm_2d, double _ang_vel);
+ /**
+ * \brief Function to find the stamp of the first imu data that triggers the collision.
+ *
+ * \return True if succeeded.
+ */
+ bool set_collision_start(void);
+
+ /**
+ * \brief Function to calculate the angle rotated while detecting a collision.
+ *
+ * \return The angle calculated.
+ */
+ double set_initial_angle_rotated(void);
+
/**
* \brief Function to find the last scan before a collision.
*
diff --git a/launch/collisions.launch b/launch/collisions.launch
index 7f4f9b4c249de098b0045b8fbf4ead1890353987..ec7f767e9e46e9eebf84cf5e92f0096bc7294c5b 100644
--- a/launch/collisions.launch
+++ b/launch/collisions.launch
@@ -27,9 +27,14 @@
<!-- Collision manager parameters -->
<arg name="collision_config_file" default="$(find iri_collision_manager)/config/params.yaml"/>
+ <arg name="relocalization_topic_name" default="~/icp_relocalization"/>
+
+ <!-- Laser filters parameters -->
+ <arg name="filter_config_file" default="$(find iri_collision_manager)/config/range_filter.yaml"/>
<arg name="front_laser_topic" default="/laser_front/scan"/>
<arg name="rear_laser_topic" default="/laser_rear/scan"/>
- <arg name="relocalization_topic_name" default="~/icp_relocalization"/>
+ <arg name="front_laser_topic_filtered" default="/laser_front/scan_filtered"/>
+ <arg name="rear_laser_topic_filtered" default="/laser_rear/scan_filtered"/>
<include file="$(find iri_bno055_imu_bringup)/launch/bno055_imu.launch"
if="$(arg imu_driver)">
@@ -66,14 +71,29 @@
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
<arg name="config_file" value="$(arg collision_config_file)"/>
<arg name="imu_topic" value="$(arg imu_filtered_topic)"/>
- <arg name="front_laser_topic" value="$(arg front_laser_topic)"/>
- <arg name="rear_laser_topic" value="$(arg rear_laser_topic)"/>
+ <arg name="front_laser_topic" value="$(arg front_laser_topic_filtered)"/>
+ <arg name="rear_laser_topic" value="$(arg rear_laser_topic_filtered)"/>
<arg name="icp_service_name" value="$(arg icp_service_name)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
<arg name="relocalization_topic_name" value="$(arg relocalization_topic_name)"/>
<arg name="diagnostics" value="false"/>
</include>
+ <node pkg="laser_filters"
+ type="scan_to_scan_filter_chain"
+ name="front_laser_filter">
+ <rosparam command="load" file="$(arg filter_config_file)" />
+ <remap from="/scan" to="$(arg front_laser_topic)" />
+ <remap from="/scan_filtered" to="$(arg front_laser_topic_filtered)" />
+ </node>
+
+ <node pkg="laser_filters"
+ type="scan_to_scan_filter_chain"
+ name="rear_laser_filter">
+ <rosparam command="load" file="$(arg filter_config_file)" />
+ <remap from="/scan" to="$(arg rear_laser_topic)" />
+ <remap from="/scan_filtered" to="$(arg rear_laser_topic_filtered)" />
+ </node>
<node name="rqt_reconfigure_iri_collision_manager"
pkg ="rqt_reconfigure"
diff --git a/package.xml b/package.xml
index 95af4b49a9bae9686d3fc4d26a636422847782dc..58c30a5f8135524d0b3c7a508efc476cc5dd0fc4 100644
--- a/package.xml
+++ b/package.xml
@@ -54,6 +54,7 @@
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>iri_base_algorithm</exec_depend>
+ <exec_depend>laser_filters</exec_depend>
<depend>nav_msgs</depend>
<depend>geometry_msgs</depend>
<depend>iri_laser_scan_icp</depend>
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index 4b43b72dac7f9ea44f7cb67ea5cd8148ab2cb9eb..0bb8904fbb9276f3ce494aec5712cf8de1e88474 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -152,7 +152,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
if (this->new_imu_input_)
{
- if (get_base_link_imu_data(blink_acc, blink_ang_vel))//Transform data to base_link frame coordinates
+ if (get_current_base_link_imu_data(blink_acc, blink_ang_vel))//Transform data to base_link frame coordinates
{
double acc_norm_2d = std::sqrt(std::pow(blink_acc(0),2) + std::pow(blink_acc(1),2));
if (this->in_collision_)
@@ -459,7 +459,17 @@ 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.");
- this->collision_start_stamp_ = this->imu_stamp_;
+ if (!set_collision_start())
+ {
+ ROS_ERROR_STREAM("CollisionManagerAlgNode::mainNodeThread -> Collision detected but nothing can be performed.");
+ this->new_imu_input_ = false;
+ this->odom_mutex_exit();
+ this->front_laser_scan_mutex_exit();
+ this->rear_laser_scan_mutex_exit();
+ this->imu_mutex_exit();
+ return;
+ }
+ // this->collision_start_stamp_ = this->imu_stamp_;
// Get reference scan
if (!(this->config_.front_laser_en || this->config_.rear_laser_en))
@@ -481,9 +491,10 @@ void CollisionManagerAlgNode::mainNodeThread(void)
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;
+ this->last_imu_t_ = this->imu_stamp_;
+
+ this->front_icp_srv_.request.rot_angle_estimation = set_initial_angle_rotated();
+ this->rear_icp_srv_.request.rot_angle_estimation = this->front_icp_srv_.request.rot_angle_estimation;
//update collision msg data
this->collision_msg_.header.stamp = this->imu_stamp_;
@@ -518,26 +529,54 @@ void CollisionManagerAlgNode::mainNodeThread(void)
// this->alg_.unlock();
}
-bool CollisionManagerAlgNode::get_base_link_imu_data(Eigen::Vector3d& _blink_acc, Eigen::Vector3d& _blink_ang_vel)
+bool CollisionManagerAlgNode::get_current_base_link_imu_data(Eigen::Vector3d& _blink_acc, Eigen::Vector3d& _blink_ang_vel)
+{
+ if (transform_to_base_link_imu_data(this->imu_local_acc_, this->imu_local_ang_vel_, this->imu_stamp_, _blink_acc, _blink_ang_vel))
+ return true;
+ else
+ return false;
+
+ // 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_, this->imu_stamp_, ros::Duration(this->config_.tf_timeout)))
+ // {
+ // ROS_WARN_STREAM("CollisionManagerAlgNode::get_current_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_, 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;
+ // }
+ // _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;
+}
+
+bool CollisionManagerAlgNode::transform_to_base_link_imu_data(const Eigen::Vector3d& _local_acc, const Eigen::Vector3d& _local_ang_vel, const ros::Time& _stamp, Eigen::Vector3d& _blink_acc, Eigen::Vector3d& _blink_ang_vel)
{
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_, this->imu_stamp_, ros::Duration(this->config_.tf_timeout)))
+ if (!this->tf2_buffer.canTransform(this->config_.base_link_frame, this->imu_frame_, _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 " << this->imu_stamp_);
+ ROS_WARN_STREAM("CollisionManagerAlgNode::get_current_base_link_imu_data -> Can't transform from " << this->config_.base_link_frame << " to " << this->imu_frame_ << " at " << _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_, this->imu_stamp_);
+ tf_base_link_imu_msg = this->tf2_buffer.lookupTransform(this->config_.base_link_frame, this->imu_frame_, _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;
}
- _blink_acc = this->tf_base_link_imu_*this->imu_local_acc_;
- _blink_ang_vel = this->tf_base_link_imu_*this->imu_local_ang_vel_;
+ _blink_acc = this->tf_base_link_imu_*_local_acc;
+ _blink_ang_vel = this->tf_base_link_imu_*_local_ang_vel;
return true;
}
@@ -545,14 +584,14 @@ bool CollisionManagerAlgNode::check_acc_collision_transition(double _acc_norm_2d
{
if (!this->config_.collision_acc_transition_counter_en)
return true;
- if ((_start && (_acc_norm_2d > this->config_.collision_acc_th)) || (!_start && (_acc_norm_2d < this->config_.collision_acc_th)))
+ if ((_start && (_acc_norm_2d > this->config_.collision_acc_th)) || (!_start && (_acc_norm_2d < this->config_.collision_acc_th_end)))
this->low_acc_count_++;
else
{
this->low_acc_count_ = 0;
return false;
}
- return ((_start && (_acc_norm_2d > this->config_.collision_acc_th)) || (!_start && (_acc_norm_2d < this->config_.collision_acc_th)))
+ return ((_start && (_acc_norm_2d > this->config_.collision_acc_th)) || (!_start && (_acc_norm_2d < this->config_.collision_acc_th_end)))
&& (_start || (!_start && (!this->config_.collision_acc_transition_counter_en || (this->low_acc_count_ >= this->config_.collision_acc_counter_limit))));
}
@@ -568,14 +607,14 @@ bool CollisionManagerAlgNode::check_ang_vel_collision_transition(double _ang_vel
return false;
}
- if ((_start && (diff > this->config_.collision_ang_vel_th)) || (!_start && (diff < this->config_.collision_ang_vel_th)))
+ if ((_start && (diff > this->config_.collision_ang_vel_th)) || (!_start && (diff < this->config_.collision_ang_vel_th_end)))
this->low_ang_vel_count_++;
else
{
this->low_ang_vel_count_ = 0;
return false;
}
- return ((_start && (diff > this->config_.collision_ang_vel_th)) || (!_start && (diff < this->config_.collision_ang_vel_th)))
+ return ((_start && (diff > this->config_.collision_ang_vel_th)) || (!_start && (diff < this->config_.collision_ang_vel_th_end)))
&& (_start || (!_start && (!this->config_.collision_ang_vel_transition_counter_en || (this->low_ang_vel_count_ >= this->config_.collision_ang_vel_counter_limit))));
}
@@ -611,6 +650,78 @@ bool CollisionManagerAlgNode::check_start_of_collision(double _acc_norm_2d, doub
return acc_true || ang_vel_true;
}
+bool CollisionManagerAlgNode::set_collision_start(void)
+{
+ auto it = this->imu_buffer_.begin();
+
+ if (this->imu_watchdog_.is_active())
+ {
+ ROS_WARN_STREAM("CollisionManagerAlgNode::set_collision_start -> IMU watchdog is active. ICP laser pose prior can't be calculated.");
+ return false;
+ }
+
+ if (this->collision_msg_.acc_trigger)
+ {
+ if (this->imu_buffer_.size() > this->config_.collision_acc_counter_limit)
+ std::advance(it, this->config_.collision_acc_counter_limit);
+ else
+ {
+ ROS_WARN_STREAM("CollisionManagerAlgNode::set_collision_start -> not enough IMU buffer. Please check buffer_size and collision_acc_counter_limit.");
+ ROS_WARN_STREAM("CollisionManagerAlgNode::set_collision_start -> Set collision start to current time.");
+ this->collision_start_stamp_ = this-> imu_stamp_;
+ return true;
+ }
+ }
+ else if (this->collision_msg_.ang_vel_trigger)
+ {
+ if (this->imu_buffer_.size() > this->config_.collision_acc_counter_limit)
+ std::advance(it, this->config_.collision_ang_vel_counter_limit);
+ else
+ {
+ ROS_WARN_STREAM("CollisionManagerAlgNode::set_collision_start -> not enough IMU buffer. Please check buffer_size and collision_ang_vel_counter_limit.");
+ ROS_WARN_STREAM("CollisionManagerAlgNode::set_collision_start -> Set collision start to current time.");
+ this->collision_start_stamp_ = this-> imu_stamp_;
+ return true;
+ }
+ }
+ else
+ {
+ ROS_ERROR_STREAM("CollisionManagerAlgNode::set_collision_start -> No IMU trigger defined.");
+ return false;
+ }
+
+ this->collision_start_stamp_ = it->stamp;
+ return true;
+}
+
+double CollisionManagerAlgNode::set_initial_angle_rotated(void)
+{
+ double angle = 0.0;
+ auto it = this->imu_buffer_.begin();
+ auto prev_it = this->imu_buffer_.begin();
+ Eigen::Vector3d blink_acc, blink_ang_vel;
+ if (this->imu_buffer_.size() < 2)
+ {
+ ROS_WARN_STREAM("CollisionManagerAlgNode::set_initial_angle_rotated -> no imu buffered so not initial angle rotated can be calculated.");
+ return angle;
+ }
+
+ prev_it++;
+ while ((prev_it != this->imu_buffer_.end()) && (it->stamp > (this->collision_start_stamp_ - ros::Duration(this->config_.scan_diff_t_from_collision))))
+ {
+ if (!transform_to_base_link_imu_data(it->acc, it->ang_vel, it->stamp, blink_acc, blink_ang_vel))
+ {
+ ROS_WARN_STREAM("CollisionManagerAlgNode::set_initial_angle_rotated -> Initial angle rotated calculated until " << it->stamp);
+ ROS_WARN_STREAM("CollisionManagerAlgNode::set_initial_angle_rotated -> Target stamp " << (this->collision_start_stamp_ - ros::Duration(this->config_.scan_diff_t_from_collision)));
+ break;
+ }
+ angle += blink_ang_vel(2)*(it->stamp - prev_it->stamp).toSec();
+ it++;
+ prev_it++;
+ }
+ return angle;
+}
+
bool CollisionManagerAlgNode::set_ref_ranges(bool _front)
{
if (!(_front? this->config_.front_laser_en: this->config_.rear_laser_en))
@@ -908,28 +1019,6 @@ bool CollisionManagerAlgNode::check_odometry_diff(const Eigen::Vector3d& _icp_od
return (odom_check(0) || odom_check(1) || odom_check(2));
}
-// 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)
{
@@ -1122,6 +1211,15 @@ void CollisionManagerAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg
this->imu_local_acc_ = Eigen::Vector3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z);
this->imu_local_ang_vel_ = Eigen::Vector3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z);
this->imu_stamp_ = msg->header.stamp;
+
+ ImuWithStamp imu;
+ imu.acc = this->imu_local_acc_;
+ imu.ang_vel = this->imu_local_ang_vel_;
+ imu.stamp = msg->header.stamp;
+ if (this->imu_buffer_.size() >= 3*this->config_.buffer_size)
+ this->imu_buffer_.pop_back();
+ this->imu_buffer_.push_front(imu);
+
this->imu_watchdog_.reset(ros::Duration(this->config_.watchdog_t));
// if (!this->in_collision_)
// this->collision_start_stamp_ = msg->header.stamp;