diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9da7ada79af9d84edd233e19b73331bd3f53484f..f12edbd8c242a6190f1dc446152c24fc1832875b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm iri_laser_scan_icp tf2_ros sensor_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm geometry_msgs iri_laser_scan_icp tf2_ros sensor_msgs)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -61,7 +61,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS iri_base_algorithm iri_laser_scan_icp tf2_ros sensor_msgs
+ CATKIN_DEPENDS iri_base_algorithm geometry_msgs iri_laser_scan_icp tf2_ros sensor_msgs
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
@@ -96,6 +96,7 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
# Add message headers dependencies
# ********************************************************************
# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} geometry_msgs_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} iri_laser_scan_icp_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)
# ********************************************************************
diff --git a/README.md b/README.md
index c7330e1cf0f4b43d2fe1de39ab8e8f6984a00520..460d086ce6df4bb3abbd9e107957c5874e0d5c7e 100644
--- a/README.md
+++ b/README.md
@@ -10,6 +10,8 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal).
# ROS Interface
### Topic publishers
+ - ~**debug_rear_icp_pose** (geometry_msgs/PoseWithCovariance.msg)
+ - ~**debug_front_icp_pose** (geometry_msgs/PoseWithCovariance.msg)
- ~**debug_rear_icp_last_scan** (sensor_msgs/LaserScan.msg): Debug topic to visulize rear ICP matching laser scan.
- ~**debug_rear_icp_ref_scan** (sensor_msgs/LaserScan.msg): Debug topic to visulize rear ICP reference laser scan.
- ~**debug_front_icp_last_scan** (sensor_msgs/LaserScan.msg): Debug topic to visulize front ICP matching laser scan.
diff --git a/cfg/CollisionManager.cfg b/cfg/CollisionManager.cfg
index 571b56f2f73163a86d88bd7c421b048a0a66136c..ce786f941664a46b1e337164fbaa021d900a45e2 100755
--- a/cfg/CollisionManager.cfg
+++ b/cfg/CollisionManager.cfg
@@ -41,7 +41,8 @@ gen = ParameterGenerator()
gen.add("rate", double_t, 0, "Main loop rate (Hz)", 10.0, 0.1, 1000.0)
gen.add("debug", bool_t, 0, "Bool to show debug information", False)
gen.add("err_msg_rate", double_t, 0, "Rate to publish err messages", 0.5, 0.1, 1.0)
-gen.add("fixed_frame", str_t, 0, "Fixed frame", "base_link")
+gen.add("fixed_frame", str_t, 0, "Fixed frame", "map")
+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)
gen.add("collision_transition_counter_en", bool_t, 0, "Enable the collision counter", False)
gen.add("collision_timeout", double_t, 0, "Timeout to detect an end of collision", 2.0, 1.0, 10.0)
diff --git a/config/params.yaml b/config/params.yaml
index c0ef8289a92bbd3d524a6123c1ded55a1224fa34..1b7cbd0a7e94918bd9f236c1561472ad76506cd4 100644
--- a/config/params.yaml
+++ b/config/params.yaml
@@ -1,11 +1,12 @@
rate: 240
debug: True
fixed_frame: base_link
+base_link_frame: base_link
tf_timeout: 0.2
err_msg_rate: 0.5
collision_transition_counter_en: True
collision_timeout: 4.0
-collision_acc_th: 3.3
+collision_acc_th: 2.5
collision_counter_limit: 12
front_laser_en: True
rear_laser_en: True
@@ -13,5 +14,5 @@ scan_buffer_size: 6
scan_diff_t_from_collision: 0.0
front_laser_en: True
rear_laser_en: True
-icp_min_points: 300
-icp_max_err_by_points: 0.1
+icp_min_points: 100
+icp_max_err_by_points: 1.0
diff --git a/include/collision_manager_alg_node.h b/include/collision_manager_alg_node.h
index 6962a861d0c864f3ddeccfb148aa02da946d0610..52b38f455c6c6f7ef29f65f4e96bf8d9b79675c1 100644
--- a/include/collision_manager_alg_node.h
+++ b/include/collision_manager_alg_node.h
@@ -28,8 +28,10 @@
#include <iri_base_algorithm/iri_base_algorithm.h>
#include "collision_manager_alg.h"
#include <Eigen/Eigen>
+#include <tf2/utils.h>
// [publisher subscriber headers]
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/LaserScan.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
@@ -64,19 +66,33 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
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.
- bool tf_loaded_; ///< Flag to know that tf_fixed_frame_imu has been loaded before.
- Eigen::Transform<double,3,Eigen::Affine> tf_fixed_frame_imu_; ///< Transform from fixed frame to IMU frame.
+ std::string front_laser_frame_; ///< Front laser frame id.
+ std::string rear_laser_frame_; ///< Rear laser frame id.
+ bool tf_base_link_imu_loaded_; ///< Flag to know that tf_base_link_imu has been loaded before.
+ 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 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 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.
- Eigen::Vector3d max_acc_; ///< The linear accelerations meassured from fized frame reference at maximum acceleration module.
+ Eigen::Vector3d max_acc_; ///< The linear accelerations meassured from base_link frame reference at maximum acceleration module.
int low_acc_count_; ///< Low acceleration meassures counter to detect the end of a collision.
std::list<RangesWithStamp> front_ranges_; ///< Front laser ranges buffer.
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.
// [publisher attributes]
+ ros::Publisher debug_rear_icp_pose_publisher_;
+ geometry_msgs::PoseWithCovarianceStamped debug_rear_icp_pose_msg_;
+
+ ros::Publisher debug_front_icp_pose_publisher_;
+ geometry_msgs::PoseWithCovarianceStamped debug_front_icp_pose_msg_;
+
ros::Publisher debug_rear_icp_last_scan_publisher_;
sensor_msgs::LaserScan debug_rear_icp_last_scan_msg_;
@@ -152,9 +168,9 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
protected:
/**
- * \brief Function to tranform linear local imu data to fixed frame.
+ * \brief Function to tranform linear local imu data to base_link frame.
*
- * It checks if imu frame id is different from fixed frame and if not already
+ * It checks if imu frame id is different from base_link frame and if not already
* saved, it saves the transfrom.
*
* \param _global_acc The current global linear acceleration.
@@ -163,7 +179,7 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
*
* \return True in success.
*/
- bool get_global_imu_data(Eigen::Vector3d& _global_acc, Eigen::Vector3d& _global_ang_vel);
+ bool get_base_link_imu_data(Eigen::Vector3d& _global_acc, Eigen::Vector3d& _global_ang_vel);
/**
* \brief Function to check if is a collision transition.
@@ -221,6 +237,34 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
*/
bool set_last_ranges(bool _front);
+ /**
+ * \brief Function to load tf from base_link to fron/rear laser.
+ *
+ * \param _front To select the laser.
+ *
+ * \return True if succeeded.
+ */
+ bool load_tf_blink_laser(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 _blink_pose The 2D base link pose.
+ *
+ * \param _blink_cov Its covariances.
+ *
+ * \param _front To select the laser.
+ *
+ * \return True if succeeded.
+ */
+ 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 main node thread
*
diff --git a/package.xml b/package.xml
index 25cbf1e01b2ae0628835aa5a229275d621092ff3..568b88c3364e2744f951e69a9d5e93a6771aeb2e 100644
--- a/package.xml
+++ b/package.xml
@@ -52,6 +52,7 @@
<build_depend>iri_base_algorithm</build_depend>
<build_export_depend>iri_base_algorithm</build_export_depend>
<exec_depend>iri_base_algorithm</exec_depend>
+ <depend>geometry_msgs</depend>
<depend>iri_laser_scan_icp</depend>
<depend>tf2_ros</depend>
<depend>sensor_msgs</depend>
diff --git a/rviz/dogs.rviz b/rviz/dogs.rviz
index ebb3c42ef104d0c7dfe5332c51b1d6e2854f4507..e6c138beb56928c9189c11310a425029d57d93ad 100644
--- a/rviz/dogs.rviz
+++ b/rviz/dogs.rviz
@@ -463,6 +463,36 @@ Visualization Manager:
Use Fixed Frame: true
Use rainbow: true
Value: true
+ - Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Class: rviz/PoseWithCovariance
+ Color: 252; 233; 79
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: true
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Name: front_icp_pose
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Shape: Arrow
+ Topic: /iri_collision_manager/debug_front_icp_pose
+ Unreliable: false
+ Value: true
Enabled: true
Name: Front_icp
- Class: rviz/Group
@@ -523,6 +553,36 @@ Visualization Manager:
Use Fixed Frame: true
Use rainbow: true
Value: true
+ - Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Class: rviz/PoseWithCovariance
+ Color: 173; 127; 168
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: true
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Name: rear_icp_pose
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Shape: Arrow
+ Topic: /iri_collision_manager/debug_rear_icp_pose
+ Unreliable: false
+ Value: true
Enabled: true
Name: Rear_icp
Enabled: true
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index dc287f2e4a5e30caef4ba6b5029170fd496d8a0c..af20bdafb8e7db206ffe55ea566d8c6e7202454e 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -12,20 +12,45 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
else
this->setRate(this->config_.rate);
+ if(!this->private_node_handle_.getParam("base_link_frame", this->config_.base_link_frame))
+ {
+ ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'base_link' not found. Setting it to base_link.");
+ this->config_.base_link_frame = "base_link";
+ }
+
if(!this->private_node_handle_.getParam("fixed_frame", this->config_.fixed_frame))
{
- ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'fixed_frame' not found. Setting it to map.");
+ ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'base_link' not found. Setting it to map.");
this->config_.fixed_frame = "map";
}
this->new_imu_input_ = false;
this->imu_frame_ = "";
- this->tf_loaded_ = false;
+ this->front_laser_frame_ = "";
+ this->rear_laser_frame_ = "";
+ this->tf_base_link_imu_loaded_ = false;
this->in_collision_ = false;
this->low_acc_count_ = 0;
this->is_front_icp_input_data_ok_ = false;
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->debug_rear_icp_pose_msg_.header.frame_id = this->config_.fixed_frame;
+ this->debug_rear_icp_pose_msg_.pose.pose.position.z = 0.0;
+ for (unsigned int i = 0; i < 36; i++)
+ this->debug_rear_icp_pose_msg_.pose.covariance[i] = 0.0;
+
+ this->debug_front_icp_pose_msg_.header.frame_id = this->config_.fixed_frame;
+ this->debug_front_icp_pose_msg_.pose.pose.position.z = 0.0;
+ for (unsigned int i = 0; i < 36; i++)
+ this->debug_front_icp_pose_msg_.pose.covariance[i] = 0.0;
+
// [init publishers]
+ this->debug_rear_icp_pose_publisher_ = this->private_node_handle_.advertise<geometry_msgs::PoseWithCovarianceStamped>("debug_rear_icp_pose", 1);
+ this->debug_front_icp_pose_publisher_ = this->private_node_handle_.advertise<geometry_msgs::PoseWithCovarianceStamped>("debug_front_icp_pose", 1);
this->debug_rear_icp_last_scan_publisher_ = this->private_node_handle_.advertise<sensor_msgs::LaserScan>("debug_rear_icp_last_scan", 1);
this->debug_rear_icp_ref_scan_publisher_ = this->private_node_handle_.advertise<sensor_msgs::LaserScan>("debug_rear_icp_ref_scan", 1);
this->debug_front_icp_last_scan_publisher_ = this->private_node_handle_.advertise<sensor_msgs::LaserScan>("debug_front_icp_last_scan", 1);
@@ -69,11 +94,12 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->imu_mutex_enter();
this->rear_laser_scan_mutex_enter();
this->front_laser_scan_mutex_enter();
+ Eigen::Vector3d global_acc, global_ang_vel, rear_blink_pose, front_blink_pose, new_laser_pose;
+ Eigen::Matrix3d rear_cov, front_cov, covariances;
if (this->new_imu_input_)
{
- Eigen::Vector3d global_acc, global_ang_vel;
- if (get_global_imu_data(global_acc, global_ang_vel))//Transform data to fixed frame coordinates
+ if (get_base_link_imu_data(global_acc, global_ang_vel))//Transform data to fixed frame coordinates
{
double acc_norm_2d = std::sqrt(std::pow(global_acc(0),2) + std::pow(global_acc(1),2));
if (this->in_collision_)
@@ -110,13 +136,29 @@ void CollisionManagerAlgNode::mainNodeThread(void)
ROS_INFO_STREAM(" nvalid " << front_icp_srv_.response.nvalid << "; min " << this->config_.icp_min_points);
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);
+ 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)
+ {
+ 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->front_icp_succedded_ = get_base_link_new_pose_from_icp(new_laser_pose, covariances, front_blink_pose, front_cov, true);
+ }
+ else
+ this->front_icp_succedded_ = false;
}
else
+ {
+ this->front_icp_succedded_ = false;
ROS_ERROR("CollisionManagerAlgNode::mainNodeThread -> Failed to call service on topic %s",this->icp_client_.getService().c_str());
+ }
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_)
+ this->debug_front_icp_pose_publisher_.publish(this->debug_front_icp_pose_msg_);
}
}
if (this->is_rear_icp_input_data_ok_ && set_last_ranges(false))
@@ -130,15 +172,33 @@ void CollisionManagerAlgNode::mainNodeThread(void)
ROS_INFO_STREAM(" nvalid " << rear_icp_srv_.response.nvalid << "; min " << this->config_.icp_min_points);
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);
+
+ 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;
+ this->rear_icp_succedded_ = get_base_link_new_pose_from_icp(new_laser_pose, covariances, rear_blink_pose, rear_cov, false);
+ }
+ else
+ this->rear_icp_succedded_ = false;
}
else
+ {
+ this->rear_icp_succedded_ = false;
ROS_ERROR("CollisionManagerAlgNode::mainNodeThread -> Failed to call service on topic %s",this->icp_client_.getService().c_str());
+ }
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_)
+ this->debug_rear_icp_pose_publisher_.publish(this->debug_rear_icp_pose_msg_);
}
}
+ // fuse poses?? call service to modify SLAM graph
}
ROS_INFO_STREAM("");
ROS_INFO_STREAM("");
@@ -194,6 +254,12 @@ void CollisionManagerAlgNode::mainNodeThread(void)
}
// [fill msg structures]
+ // Initialize the topic message structure
+ //this->debug_rear_icp_pose_PoseWithCovariance_msg_.data = my_var;
+
+ // Initialize the topic message structure
+ //this->debug_front_icp_pose_PoseWithCovariance_msg_.data = my_var;
+
// Initialize the topic message structure
//this->debug_rear_icp_last_scan_LaserScan_msg_.data = my_var;
@@ -269,6 +335,12 @@ void CollisionManagerAlgNode::mainNodeThread(void)
// [fill action structure and make request to the action server]
// [publish messages]
+ // Uncomment the following line to publish the topic message
+ //this->debug_rear_icp_pose_publisher_.publish(this->debug_rear_icp_pose_PoseWithCovariance_msg_);
+
+ // Uncomment the following line to publish the topic message
+ //this->debug_front_icp_pose_publisher_.publish(this->debug_front_icp_pose_PoseWithCovariance_msg_);
+
// Uncomment the following line to publish the topic message
//this->debug_rear_icp_last_scan_publisher_.publish(this->debug_rear_icp_last_scan_LaserScan_msg_);
@@ -288,26 +360,26 @@ void CollisionManagerAlgNode::mainNodeThread(void)
// this->alg_.unlock();
}
-bool CollisionManagerAlgNode::get_global_imu_data(Eigen::Vector3d& _global_acc, Eigen::Vector3d& _global_ang_vel)
+bool CollisionManagerAlgNode::get_base_link_imu_data(Eigen::Vector3d& _global_acc, Eigen::Vector3d& _global_ang_vel)
{
- if (this->config_.fixed_frame != this->imu_frame_ || !this->tf_loaded_)
+ if (this->config_.base_link_frame != this->imu_frame_ || !this->tf_base_link_imu_loaded_)
{
- geometry_msgs::TransformStamped tf_fixed_frame_imu_msg;
- if (!this->tf2_buffer.canTransform(this->config_.fixed_frame, this->imu_frame_, ros::Time::now(), ros::Duration(this->config_.tf_timeout)))
+ geometry_msgs::TransformStamped tf_base_link_imu_msg;
+ if (!this->tf2_buffer.canTransform(this->config_.base_link_frame, this->imu_frame_, ros::Time::now(), ros::Duration(this->config_.tf_timeout)))
{
- ROS_WARN_STREAM("CollisionManagerAlgNode::get_global_imu_data -> Can't transform from " << this->config_.fixed_frame << " to " << this->imu_frame_ << " at " << ros::Time::now());
- this->tf_loaded_ = false;
+ ROS_WARN_STREAM("CollisionManagerAlgNode::get_base_link_imu_data -> Can't transform from " << this->config_.base_link_frame << " to " << this->imu_frame_ << " at " << ros::Time::now());
+ this->tf_base_link_imu_loaded_ = false;
return false;
}
- tf_fixed_frame_imu_msg = this->tf2_buffer.lookupTransform(this->config_.fixed_frame, this->imu_frame_, ros::Time::now());
+ tf_base_link_imu_msg = this->tf2_buffer.lookupTransform(this->config_.base_link_frame, this->imu_frame_, ros::Time::now());
Eigen::Quaternion<double> rot(
- tf_fixed_frame_imu_msg.transform.rotation.w, tf_fixed_frame_imu_msg.transform.rotation.x, tf_fixed_frame_imu_msg.transform.rotation.y, tf_fixed_frame_imu_msg.transform.rotation.z);
+ 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_fixed_frame_imu_ = Eigen::Transform<double,3,Eigen::Affine>(rot);
- this->tf_loaded_ = true;
+ this->tf_base_link_imu_ = Eigen::Transform<double,3,Eigen::Affine>(rot);
+ this->tf_base_link_imu_loaded_ = true;
}
- _global_acc = this->tf_fixed_frame_imu_*this->imu_local_acc_;
- _global_ang_vel = this->tf_fixed_frame_imu_*this->imu_local_ang_vel_;
+ _global_acc = this->tf_base_link_imu_*this->imu_local_acc_;
+ _global_ang_vel = this->tf_base_link_imu_*this->imu_local_ang_vel_;
return true;
}
@@ -382,6 +454,96 @@ bool CollisionManagerAlgNode::set_last_ranges(bool _front)
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;
+
+ if (!load_tf_blink_laser(_front))
+ return false;
+
+ if (!this->tf2_buffer.canTransform(this->config_.fixed_frame, (_front? this->front_laser_frame_: this->rear_laser_frame_), this->imu_stamp_, ros::Duration(this->config_.tf_timeout)))
+ {
+ ROS_WARN_STREAM("CollisionManagerAlgNode::get_base_link_new_pose_from_icp -> Can't transform from " << this->config_.fixed_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->config_.fixed_frame, (_front? this->front_laser_frame_: this->rear_laser_frame_), this->imu_stamp_);
+ tf2::fromMsg(tf_aux.transform, tf_map_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_map_new_laser = tf_map_laser*tf_laser_new_laser;
+
+ 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.getOrigin().w(), tf_map_laser.getOrigin().x(), tf_map_laser.getOrigin().y(), tf_map_laser.getOrigin().z());
+
+ Eigen::Transform<double,3,Eigen::Affine> map_laser_rot(rot);
+ _blink_cov = map_laser_rot*_covariances;
+
+ if (this->config_.debug)
+ {
+ if (_front)
+ {
+ this->debug_front_icp_pose_msg_.header.stamp = this->imu_stamp_;
+ tf2::toMsg(tf_map_new_blink, this->debug_front_icp_pose_msg_.pose.pose);
+ this->debug_front_icp_pose_msg_.pose.covariance[0] = _blink_cov(0);
+ this->debug_front_icp_pose_msg_.pose.covariance[1] = _blink_cov(1);
+ this->debug_front_icp_pose_msg_.pose.covariance[5] = _blink_cov(2);
+ this->debug_front_icp_pose_msg_.pose.covariance[6] = _blink_cov(3);
+ this->debug_front_icp_pose_msg_.pose.covariance[7] = _blink_cov(4);
+ this->debug_front_icp_pose_msg_.pose.covariance[11] = _blink_cov(5);
+ this->debug_front_icp_pose_msg_.pose.covariance[30] = _blink_cov(6);
+ this->debug_front_icp_pose_msg_.pose.covariance[31] = _blink_cov(7);
+ this->debug_front_icp_pose_msg_.pose.covariance[35] = _blink_cov(8);
+ }
+ else
+ {
+ this->debug_rear_icp_pose_msg_.header.stamp = this->imu_stamp_;
+ tf2::toMsg(tf_map_new_blink, this->debug_rear_icp_pose_msg_.pose.pose);
+ this->debug_rear_icp_pose_msg_.pose.covariance[0] = _blink_cov(0);
+ this->debug_rear_icp_pose_msg_.pose.covariance[1] = _blink_cov(1);
+ this->debug_rear_icp_pose_msg_.pose.covariance[5] = _blink_cov(2);
+ this->debug_rear_icp_pose_msg_.pose.covariance[6] = _blink_cov(3);
+ this->debug_rear_icp_pose_msg_.pose.covariance[7] = _blink_cov(4);
+ this->debug_rear_icp_pose_msg_.pose.covariance[11] = _blink_cov(5);
+ this->debug_rear_icp_pose_msg_.pose.covariance[30] = _blink_cov(6);
+ this->debug_rear_icp_pose_msg_.pose.covariance[31] = _blink_cov(7);
+ this->debug_rear_icp_pose_msg_.pose.covariance[35] = _blink_cov(8);
+ }
+
+ }
+ return true;
+}
+
+bool CollisionManagerAlgNode::load_tf_blink_laser(bool _front)
+{
+ geometry_msgs::TransformStamped tf_aux;
+ if ((_front? this->tf_base_link_front_laser_loaded_: this->tf_base_link_rear_laser_loaded_))
+ return true;
+ if (!this->tf2_buffer.canTransform(this->config_.base_link_frame, (_front? this->front_laser_frame_: this->rear_laser_frame_), this->imu_stamp_, ros::Duration(this->config_.tf_timeout)))
+ {
+ ROS_WARN_STREAM("CollisionManagerAlgNode::load_tf_blink_laser -> Can't transform from " << this->config_.base_link_frame << " to " << (_front? this->front_laser_frame_: this->rear_laser_frame_) << " at " << this->imu_stamp_);
+ // this->tf_base_link_front_laser_loaded_ = false;
+ return false;
+ }
+ else
+ {
+ 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;
+ }
+ return true;
+}
+
/* [subscriber callbacks] */
void CollisionManagerAlgNode::rear_laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
@@ -392,6 +554,7 @@ void CollisionManagerAlgNode::rear_laser_scan_callback(const sensor_msgs::LaserS
this->rear_laser_scan_mutex_enter();
if (this->config_.rear_laser_en)
{
+ this->rear_laser_frame_ = msg->header.frame_id;
rear_icp_srv_.request.angle_min = msg->angle_min;
rear_icp_srv_.request.angle_max = msg->angle_max;
rear_icp_srv_.request.angle_increment = msg->angle_increment;
@@ -445,6 +608,7 @@ void CollisionManagerAlgNode::front_laser_scan_callback(const sensor_msgs::Laser
this->front_laser_scan_mutex_enter();
if (this->config_.front_laser_en)
{
+ this->front_laser_frame_ = msg->header.frame_id;
front_icp_srv_.request.angle_min = msg->angle_min;
front_icp_srv_.request.angle_max = msg->angle_max;
front_icp_srv_.request.angle_increment = msg->angle_increment;
@@ -536,6 +700,11 @@ 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;