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

Updated to ICP server

parent 90aa1848
No related branches found
No related tags found
No related merge requests found
...@@ -6,7 +6,7 @@ find_package(catkin REQUIRED) ...@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ******************************************************************** # ********************************************************************
# Add catkin additional components here # Add catkin additional components here
# ******************************************************************** # ********************************************************************
find_package(catkin REQUIRED COMPONENTS iri_base_algorithm tf2_ros sensor_msgs) find_package(catkin REQUIRED COMPONENTS iri_base_algorithm iri_laser_scan_icp tf2_ros sensor_msgs)
## System dependencies are found with CMake's conventions ## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system) # find_package(Boost REQUIRED COMPONENTS system)
...@@ -61,7 +61,7 @@ catkin_package( ...@@ -61,7 +61,7 @@ catkin_package(
# ******************************************************************** # ********************************************************************
# Add ROS and IRI ROS run time dependencies # Add ROS and IRI ROS run time dependencies
# ******************************************************************** # ********************************************************************
CATKIN_DEPENDS iri_base_algorithm tf2_ros sensor_msgs CATKIN_DEPENDS iri_base_algorithm iri_laser_scan_icp tf2_ros sensor_msgs
# ******************************************************************** # ********************************************************************
# Add system and labrobotica run time dependencies here # Add system and labrobotica run time dependencies here
# ******************************************************************** # ********************************************************************
...@@ -96,6 +96,7 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) ...@@ -96,6 +96,7 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
# Add message headers dependencies # Add message headers dependencies
# ******************************************************************** # ********************************************************************
# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp) # add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} iri_laser_scan_icp_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp) add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)
# ******************************************************************** # ********************************************************************
# Add dynamic reconfigure dependencies # Add dynamic reconfigure dependencies
......
...@@ -10,17 +10,27 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal). ...@@ -10,17 +10,27 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal).
# ROS Interface # ROS Interface
### Topic subscribers ### Topic subscribers
- ~**rear_laser_scan** (sensor_msgs/LaserScan.msg): Rear laser scan input data.
- ~**front_laser_scan** (sensor_msgs/LaserScan.msg): Front laser scan input data.
- /**tf** (tf/tfMessage): Robot tf. - /**tf** (tf/tfMessage): Robot tf.
- ~**imu** (sensor_msgs/Imu.msg): Imu input data. - ~**imu** (sensor_msgs/Imu.msg): Imu input data.
### Service clients
- ~**icp** (iri_laser_scan_icp/icp.srv): ICP server.
### Parameters ### Parameters
- ~**rate** (Double; default: 10.0; min: 0.1; max: 1000) The main node thread loop rate in Hz. - ~**rate** (Double; default: 10.0; min: 0.1; max: 1000): The main node thread loop rate in Hz.
- ~**fixed_frame** (String; default: base_link) Fixed frame id. - ~**fixed_frame** (String; default: base_link): Fixed frame id.
- ~**tf_timeout** (Double; default: 0.2; min: 0.1; max: 2.0) Timeout to find a transform. - ~**tf_timeout** (Double; default: 0.2; min: 0.1; max: 2.0): Timeout to find a transform.
- ~**err_msg_rate** (Double; default: 0.5; min: 0.1; max: 1.0) Rate to publish error messages. - ~**err_msg_rate** (Double; default: 0.5; min: 0.1; max: 1.0): Rate to publish error messages.
- ~**collision_acc_th** (Double; default: 9.8; min: 0.1; max: 160.0) Threshold to detect a collision. - ~**collision_transition_counter_en** (Boolean; default: False): Enable the collision counter.
- ~**collision_transition_counter_en** (Boolean; default: False) Enable the collision counter. - ~**collision_timeout** (Double; default: 4.0; min: 1.0; max: 10.0): Timeout to detect an end of collision.
- ~**collision_counter_limit** (Integer; default: 6; min: 1; max: 30) Number of low acc imu meassures to detect an end of collision. - ~**collision_acc_th** (Double; default: 9.8; min: 0.1; max: 160.0): Threshold to detect a collision.
- ~**collision_counter_limit** (Integer; default: 6; min: 1; max: 30): Number of low acc imu meassures to detect an end of collision.
- ~**front_laser_en** (Boolean; default: True): Enable front laser for ICP.
- ~**rear_laser_en** (Boolean; default: True): Enable rear laser for ICP.
- ~**scan_buffer_size** (Integer; default: 3; min: 1; max: 10): Number of laser_scan buffered for the ICP.
- ~**scan_diff_t_from_collision** (Double; default: 0.0; min: 0.0; max: 0.5): Time to subtract from collision time stamp to ensure a clean scan.
## Installation ## Installation
......
...@@ -39,12 +39,17 @@ gen = ParameterGenerator() ...@@ -39,12 +39,17 @@ gen = ParameterGenerator()
# Name Type Reconf.level Description Default Min Max # Name Type Reconf.level Description Default Min Max
gen.add("rate", double_t, 0, "Main loop rate (Hz)", 10.0, 0.1, 1000.0) gen.add("rate", double_t, 0, "Main loop rate (Hz)", 10.0, 0.1, 1000.0)
gen.add("collision_acc_th", double_t, 0, "Threshold to detect a collision", 9.8, 0.1, 160.0)
gen.add("err_msg_rate", double_t, 0, "Rate to publish err messages", 0.5, 0.1, 1.0) 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", "base_link")
gen.add("tf_timeout", double_t, 0, "Timeout to find a transform", 0.2, 0.1, 2.0) 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_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)
gen.add("collision_acc_th", double_t, 0, "Threshold to detect a collision", 9.8, 0.1, 160.0)
gen.add("collision_counter_limit", int_t, 0, "Number of low acc imu meassures to detect an end of collision", 6, 1, 30) gen.add("collision_counter_limit", int_t, 0, "Number of low acc imu meassures to detect an end of collision", 6, 1, 30)
gen.add("scan_buffer_size", int_t, 0, "Number of laser_scan buffered for the ICP", 3, 1, 10)
gen.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)
gen.add("front_laser_en", bool_t, 0, "Enable front laser for ICP", True)
gen.add("rear_laser_en", bool_t, 0, "Enable rear laser for ICP", True)
exit(gen.generate(PACKAGE, "CollisionManagerAlgorithm", "CollisionManager")) exit(gen.generate(PACKAGE, "CollisionManagerAlgorithm", "CollisionManager"))
\ No newline at end of file
rate: 240 rate: 240
fixed_frame: imu_bno055
tf_timeout: 0.2 tf_timeout: 0.2
collision_acc_th: 5.0 err_msg_rate: 0.5
collision_transition_counter_en: True collision_transition_counter_en: True
collision_timeout: 4.0
collision_acc_th: 5.0
collision_counter_limit: 12 collision_counter_limit: 12
err_msg_rate: 0.5 front_laser_en: True
fixed_frame: imu_bno055 rear_laser_en: True
scan_buffer_size: 6
scan_diff_t_from_collision: 0.0
...@@ -30,14 +30,27 @@ ...@@ -30,14 +30,27 @@
#include <Eigen/Eigen> #include <Eigen/Eigen>
// [publisher subscriber headers] // [publisher subscriber headers]
#include <sensor_msgs/LaserScan.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h> #include <tf2_ros/transform_listener.h>
#include <sensor_msgs/Imu.h> #include <sensor_msgs/Imu.h>
// [service client headers] // [service client headers]
#include <iri_laser_scan_icp/icp.h>
// [action server client headers] // [action server client headers]
/**
* \struct RangesWithStamp.
*
* \brief A struct with the ranges and the time stamp info.
*
*/
typedef struct {
std::vector<float> ranges; ///< Laser scan ranges data.
ros::Time stamp; ///< Laser scan time stamp.
}RangesWithStamp;
/** /**
* \brief IRI ROS Specific Algorithm Class * \brief IRI ROS Specific Algorithm Class
* *
...@@ -47,17 +60,37 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio ...@@ -47,17 +60,37 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
private: private:
bool new_imu_input_; ///< Flag to know that new input data has been received. 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_acc_; ///< Local input linear acceleration.
Eigen::Vector3d imu_local_ang_vel_; ///< Local input angular speed.
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. std::string imu_frame_; ///< IMU frame id.
bool tf_loaded_; ///< Flag to know that tf_fixed_frame_imu has been loaded before. 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. Eigen::Transform<double,3,Eigen::Affine> tf_fixed_frame_imu_; ///< Transform from fixed frame to IMU frame.
bool in_collision_; ///< Flag to know that we are receiving collision meassures. 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_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 fized frame reference at maximum acceleration module.
int low_acc_count_; ///< Low acceleration meassures counter to detect the end of a collision. 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.
// [publisher attributes] // [publisher attributes]
// [subscriber attributes] // [subscriber attributes]
ros::Subscriber rear_laser_scan_subscriber_;
void rear_laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
pthread_mutex_t rear_laser_scan_mutex_;
void rear_laser_scan_mutex_enter(void);
void rear_laser_scan_mutex_exit(void);
ros::Subscriber front_laser_scan_subscriber_;
void front_laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
pthread_mutex_t front_laser_scan_mutex_;
void front_laser_scan_mutex_enter(void);
void front_laser_scan_mutex_exit(void);
tf2_ros::Buffer tf2_buffer; tf2_ros::Buffer tf2_buffer;
tf2_ros::TransformListener tf2_listener; tf2_ros::TransformListener tf2_listener;
...@@ -72,6 +105,10 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio ...@@ -72,6 +105,10 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
// [service attributes] // [service attributes]
// [client attributes] // [client attributes]
ros::ServiceClient icp_client_;
iri_laser_scan_icp::icp front_icp_srv_;
iri_laser_scan_icp::icp rear_icp_srv_;
// [action server attributes] // [action server attributes]
...@@ -103,16 +140,18 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio ...@@ -103,16 +140,18 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
protected: protected:
/** /**
* \brief Function to tranform linear local acceleration to fixed frame. * \brief Function to tranform linear local imu data to fixed 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 fixed frame and if not already
* saved, it saves the transfrom. * saved, it saves the transfrom.
* *
* \param _global_acc The current global linear acceleration. * \param _global_acc The current global linear acceleration.
* *
* \param _global_ang_vel The current global angular velocity.
*
* \return True in success. * \return True in success.
*/ */
bool get_global_acc(Eigen::Vector3d& _global_acc); bool get_global_imu_data(Eigen::Vector3d& _global_acc, Eigen::Vector3d& _global_ang_vel);
/** /**
* \brief Function to check if is a collision transition. * \brief Function to check if is a collision transition.
...@@ -150,6 +189,26 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio ...@@ -150,6 +189,26 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
*/ */
bool check_start_of_collision(double _acc_norm_2d); bool check_start_of_collision(double _acc_norm_2d);
/**
* \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);
/**
* \brief Function to find the last scan after a collision.
*
* \param _front To select the laser.
*
* \return True if succeeded.
*/
bool set_last_ranges(bool _front);
/** /**
* \brief main node thread * \brief main node thread
* *
......
...@@ -52,6 +52,7 @@ ...@@ -52,6 +52,7 @@
<build_depend>iri_base_algorithm</build_depend> <build_depend>iri_base_algorithm</build_depend>
<build_export_depend>iri_base_algorithm</build_export_depend> <build_export_depend>iri_base_algorithm</build_export_depend>
<exec_depend>iri_base_algorithm</exec_depend> <exec_depend>iri_base_algorithm</exec_depend>
<depend>iri_laser_scan_icp</depend>
<depend>tf2_ros</depend> <depend>tf2_ros</depend>
<depend>sensor_msgs</depend> <depend>sensor_msgs</depend>
<depend>iri_bno055_imu_bringup</depend> <depend>iri_bno055_imu_bringup</depend>
......
...@@ -23,10 +23,17 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) : ...@@ -23,10 +23,17 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
this->tf_loaded_ = false; this->tf_loaded_ = false;
this->in_collision_ = false; this->in_collision_ = false;
this->low_acc_count_ = 0; this->low_acc_count_ = 0;
this->is_front_icp_input_data_ok_ = false;
this->is_rear_icp_input_data_ok_ = false;
// [init publishers] // [init publishers]
// [init subscribers] // [init subscribers]
this->rear_laser_scan_subscriber_ = this->private_node_handle_.subscribe("rear_laser_scan", 1, &CollisionManagerAlgNode::rear_laser_scan_callback, this);
pthread_mutex_init(&this->rear_laser_scan_mutex_,NULL);
this->front_laser_scan_subscriber_ = this->private_node_handle_.subscribe("front_laser_scan", 1, &CollisionManagerAlgNode::front_laser_scan_callback, this);
pthread_mutex_init(&this->front_laser_scan_mutex_,NULL);
this->imu_subscriber_ = this->private_node_handle_.subscribe("imu", 1, &CollisionManagerAlgNode::imu_callback, this); this->imu_subscriber_ = this->private_node_handle_.subscribe("imu", 1, &CollisionManagerAlgNode::imu_callback, this);
pthread_mutex_init(&this->imu_mutex_,NULL); pthread_mutex_init(&this->imu_mutex_,NULL);
...@@ -34,6 +41,8 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) : ...@@ -34,6 +41,8 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
// [init services] // [init services]
// [init clients] // [init clients]
icp_client_ = this->private_node_handle_.serviceClient<iri_laser_scan_icp::icp>("icp");
// [init action servers] // [init action servers]
...@@ -43,6 +52,8 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) : ...@@ -43,6 +52,8 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
CollisionManagerAlgNode::~CollisionManagerAlgNode(void) CollisionManagerAlgNode::~CollisionManagerAlgNode(void)
{ {
// [free dynamic memory] // [free dynamic memory]
pthread_mutex_destroy(&this->rear_laser_scan_mutex_);
pthread_mutex_destroy(&this->front_laser_scan_mutex_);
pthread_mutex_destroy(&this->imu_mutex_); pthread_mutex_destroy(&this->imu_mutex_);
} }
...@@ -52,11 +63,13 @@ void CollisionManagerAlgNode::mainNodeThread(void) ...@@ -52,11 +63,13 @@ void CollisionManagerAlgNode::mainNodeThread(void)
// this->alg_.lock(); // this->alg_.lock();
// ROS_DEBUG("CollisionManagerAlgNode::mainNodeThread"); // ROS_DEBUG("CollisionManagerAlgNode::mainNodeThread");
this->imu_mutex_enter(); this->imu_mutex_enter();
this->rear_laser_scan_mutex_enter();
this->front_laser_scan_mutex_enter();
if (this->new_imu_input_) if (this->new_imu_input_)
{ {
Eigen::Vector3d global_acc; Eigen::Vector3d global_acc, global_ang_vel;
if (get_global_acc(global_acc)) if (get_global_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)); double acc_norm_2d = std::sqrt(std::pow(global_acc(0),2) + std::pow(global_acc(1),2));
if (this->in_collision_) if (this->in_collision_)
...@@ -71,17 +84,90 @@ void CollisionManagerAlgNode::mainNodeThread(void) ...@@ -71,17 +84,90 @@ void CollisionManagerAlgNode::mainNodeThread(void)
collision_angle += 2*M_PI; collision_angle += 2*M_PI;
ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision end detected."); ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision end detected.");
ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: acc = " << max_acc_norm_2d << "; angle = " << collision_angle); ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: acc = " << max_acc_norm_2d << "; angle = " << collision_angle);
//update rot_angle_estimated
this->front_icp_srv_.request.rot_angle_estimation += global_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
this->rear_icp_srv_.request.rot_angle_estimation = -this->front_icp_srv_.request.rot_angle_estimation;
// 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.");
else
{
if (this->is_front_icp_input_data_ok_ && set_last_ranges(true))
{
if (icp_client_.call(front_icp_srv_))
{
ROS_INFO("CollisionManagerAlgNode::mainNodeThread -> Front icp_client_ received a response from service server");
ROS_INFO_STREAM(" valid " << (int)front_icp_srv_.response.valid);
ROS_INFO_STREAM(" new_laser_pose " << front_icp_srv_.response.new_laser_pose);
ROS_INFO_STREAM(" covariance " << 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]);
ROS_INFO_STREAM(" nvalid " << front_icp_srv_.response.nvalid);
ROS_INFO_STREAM(" error " << front_icp_srv_.response.error);
}
else
ROS_ERROR("CollisionManagerAlgNode::mainNodeThread -> Failed to call service on topic %s",this->icp_client_.getService().c_str());
}
if (this->is_rear_icp_input_data_ok_ && set_last_ranges(false))
{
if (icp_client_.call(rear_icp_srv_))
{
ROS_INFO("CollisionManagerAlgNode::mainNodeThread -> Rear icp_client_ received a response from service server");
ROS_INFO_STREAM(" valid " << (int)rear_icp_srv_.response.valid);
ROS_INFO_STREAM(" new_laser_pose " << rear_icp_srv_.response.new_laser_pose);
ROS_INFO_STREAM(" covariance " << 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]);
ROS_INFO_STREAM(" nvalid " << rear_icp_srv_.response.nvalid);
ROS_INFO_STREAM(" error " << rear_icp_srv_.response.error);
}
else
ROS_ERROR("CollisionManagerAlgNode::mainNodeThread -> Failed to call service on topic %s",this->icp_client_.getService().c_str());
}
}
//restart variables
this->in_collision_ = false; this->in_collision_ = false;
this->low_acc_count_ = 0; this->low_acc_count_ = 0;
this->is_front_icp_input_data_ok_ = false;
this->is_rear_icp_input_data_ok_ = false;
}
else
{
//Update max_acc
if (acc_norm_2d > max_acc_norm_2d)
this->max_acc_ = global_acc;
//Update angle rotated
this->front_icp_srv_.request.rot_angle_estimation += global_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
this->last_imu_t_ = this->imu_stamp_;
//Check collision timeout
if ((ros::Time::now() - this->collision_start_stamp_) > ros::Duration(this->config_.collision_timeout))
{
ROS_WARN_STREAM("CollisionManagerAlgNode::mainNodeThread -> No end of collision detected after " << this->config_.collision_timeout << " seconds.");
this->in_collision_ = false;
this->low_acc_count_ = 0;
}
} }
else if (acc_norm_2d > max_acc_norm_2d)
this->max_acc_ = global_acc;
} }
else if (check_start_of_collision(acc_norm_2d)) else if (check_start_of_collision(acc_norm_2d))
{ {
ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision start detected."); ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision start detected.");
this->collision_start_stamp_ = this->imu_stamp_;
// Get reference scan
if (!(this->config_.front_laser_en || this->config_.rear_laser_en))
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);
}
//init variables
this->in_collision_ = true; this->in_collision_ = true;
this->max_acc_ = global_acc; this->max_acc_ = global_acc;
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->new_imu_input_ = false; this->new_imu_input_ = false;
...@@ -134,23 +220,38 @@ void CollisionManagerAlgNode::mainNodeThread(void) ...@@ -134,23 +220,38 @@ void CollisionManagerAlgNode::mainNodeThread(void)
// [fill srv structure and make request to the server] // [fill srv structure and make request to the server]
//icp_srv_.request.data = my_var;
//ROS_INFO("CollisionManagerAlgNode:: Calling service icp_client_!");
//if (icp_client_.call(icp_srv_))
//{
// ROS_INFO("CollisionManagerAlgNode:: icp_client_ received a response from service server");
//// ROS_INFO("CollisionManagerAlgNode:: Response: %s", icp_srv_.response.somestringfield.c_str());
//}
//else
//{
// ROS_INFO("CollisionManagerAlgNode:: Failed to call service on topic %s",this->icp_client_.getService().c_str());
//}
// [fill action structure and make request to the action server] // [fill action structure and make request to the action server]
// [publish messages] // [publish messages]
this->front_laser_scan_mutex_exit();
this->rear_laser_scan_mutex_exit();
this->imu_mutex_exit(); this->imu_mutex_exit();
// this->alg_.unlock(); // this->alg_.unlock();
} }
bool CollisionManagerAlgNode::get_global_acc(Eigen::Vector3d& _global_acc) bool CollisionManagerAlgNode::get_global_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_.fixed_frame != this->imu_frame_ || !this->tf_loaded_)
{ {
geometry_msgs::TransformStamped tf_fixed_frame_imu_msg; 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))) if (!this->tf2_buffer.canTransform(this->config_.fixed_frame, this->imu_frame_, ros::Time::now(), ros::Duration(this->config_.tf_timeout)))
{ {
ROS_WARN_STREAM("CollisionManagerAlgNode::get_global_acc -> Can't transform from " << this->config_.fixed_frame << " to " << this->imu_frame_ << " at " << ros::Time::now()); 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; this->tf_loaded_ = false;
return false; return false;
} }
...@@ -162,6 +263,7 @@ bool CollisionManagerAlgNode::get_global_acc(Eigen::Vector3d& _global_acc) ...@@ -162,6 +263,7 @@ bool CollisionManagerAlgNode::get_global_acc(Eigen::Vector3d& _global_acc)
this->tf_loaded_ = true; this->tf_loaded_ = true;
} }
_global_acc = this->tf_fixed_frame_imu_*this->imu_local_acc_; _global_acc = this->tf_fixed_frame_imu_*this->imu_local_acc_;
_global_ang_vel = this->tf_fixed_frame_imu_*this->imu_local_ang_vel_;
return true; return true;
} }
...@@ -191,7 +293,121 @@ bool CollisionManagerAlgNode::check_start_of_collision(double _acc_norm_2d) ...@@ -191,7 +293,121 @@ bool CollisionManagerAlgNode::check_start_of_collision(double _acc_norm_2d)
return check_collision_transition(_acc_norm_2d, true); return check_collision_transition(_acc_norm_2d, true);
} }
bool CollisionManagerAlgNode::set_ref_ranges(ros::Time _stamp, bool _front)
{
if (!(_front? this->config_.front_laser_en: this->config_.rear_laser_en))
return false;
auto it = (_front? this->front_ranges_.begin(): this->rear_ranges_.begin());
while ((it != (_front? this->front_ranges_.end(): this->rear_ranges_.end())) && (it->stamp > (this->collision_start_stamp_ - ros::Duration(this->config_.scan_diff_t_from_collision))))
++it;
if (it == (_front? this->front_ranges_.end(): this->rear_ranges_.end()))
{
ROS_WARN_STREAM("CollisionManagerAlgNode::set_ref_ranges -> Collision detected at " << this->collision_start_stamp_ << " but there is no "
<< (_front? "front ": "rear ") << "scan before collision.");
if ((_front? this->front_ranges_: this->rear_ranges_).size() > 0)
ROS_WARN_STREAM("CollisionManagerAlgNode::set_ref_ranges -> scan times: " << (_front? this->front_ranges_: this->rear_ranges_).front().stamp << ", " << (_front? this->front_ranges_: this->rear_ranges_).back().stamp);
else
ROS_WARN_STREAM("CollisionManagerAlgNode::set_ref_ranges -> No scan buffered");
return false;
}
(_front? this->front_icp_srv_: this->rear_icp_srv_).request.ref_ranges = it->ranges;
return true;
}
bool CollisionManagerAlgNode::set_last_ranges(bool _front)
{
if (!(_front? this->config_.front_laser_en: this->config_.rear_laser_en))
return false;
if ((_front? this->front_ranges_: this->rear_ranges_).size() > 0)
{
(_front? this->front_icp_srv_: this->rear_icp_srv_).request.last_ranges = (_front? this->front_ranges_: this->rear_ranges_).front().ranges;
return true;
}
ROS_WARN_STREAM("CollisionManagerAlgNode::set_last_ranges -> No scan buffered");
return false;
}
/* [subscriber callbacks] */ /* [subscriber callbacks] */
void CollisionManagerAlgNode::rear_laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
// ROS_INFO("CollisionManagerAlgNode::rear_laser_scan_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
this->rear_laser_scan_mutex_enter();
if (this->config_.rear_laser_en)
{
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;
rear_icp_srv_.request.scan_time = msg->scan_time;
rear_icp_srv_.request.range_min = msg->range_min;
rear_icp_srv_.request.range_max = msg->range_max;
RangesWithStamp r;
r.ranges = msg->ranges;
r.stamp = msg->header.stamp;
if (this->rear_ranges_.size() >= this->config_.scan_buffer_size)
this->rear_ranges_.pop_back();
this->rear_ranges_.push_front(r);
}
//std::cout << msg->data << std::endl;
//unlock previously blocked shared variables
//this->alg_.unlock();
this->rear_laser_scan_mutex_exit();
}
void CollisionManagerAlgNode::rear_laser_scan_mutex_enter(void)
{
pthread_mutex_lock(&this->rear_laser_scan_mutex_);
}
void CollisionManagerAlgNode::rear_laser_scan_mutex_exit(void)
{
pthread_mutex_unlock(&this->rear_laser_scan_mutex_);
}
void CollisionManagerAlgNode::front_laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
// ROS_INFO("CollisionManagerAlgNode::front_laser_scan_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
this->front_laser_scan_mutex_enter();
if (this->config_.front_laser_en)
{
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;
front_icp_srv_.request.scan_time = msg->scan_time;
front_icp_srv_.request.range_min = msg->range_min;
front_icp_srv_.request.range_max = msg->range_max;
RangesWithStamp r;
r.ranges = msg->ranges;
r.stamp = msg->header.stamp;
if (this->front_ranges_.size() >= this->config_.scan_buffer_size)
this->front_ranges_.pop_back();
this->front_ranges_.push_front(r);
}
//std::cout << msg->data << std::endl;
//unlock previously blocked shared variables
//this->alg_.unlock();
this->front_laser_scan_mutex_exit();
}
void CollisionManagerAlgNode::front_laser_scan_mutex_enter(void)
{
pthread_mutex_lock(&this->front_laser_scan_mutex_);
}
void CollisionManagerAlgNode::front_laser_scan_mutex_exit(void)
{
pthread_mutex_unlock(&this->front_laser_scan_mutex_);
}
void CollisionManagerAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg) void CollisionManagerAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{ {
// ROS_INFO("CollisionManagerAlgNode::imu_callback: New Message Received"); // ROS_INFO("CollisionManagerAlgNode::imu_callback: New Message Received");
...@@ -202,8 +418,12 @@ void CollisionManagerAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg ...@@ -202,8 +418,12 @@ void CollisionManagerAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg
this->new_imu_input_ = true; this->new_imu_input_ = true;
this->imu_frame_ = msg->header.frame_id; this->imu_frame_ = msg->header.frame_id;
this->imu_local_acc_ = Eigen::Vector3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z); this->imu_local_acc_ = Eigen::Vector3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z);
if (!this->in_collision_) this->imu_local_ang_vel_ = Eigen::Vector3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z);
this->collision_start_stamp_ = msg->header.stamp; this->imu_stamp_ = msg->header.stamp;
// if (!this->in_collision_)
// this->collision_start_stamp_ = msg->header.stamp;
// if (this->in_collision_)
// this->collision_end_stamp_ = msg->header.stamp;
//std::cout << msg->data << std::endl; //std::cout << msg->data << std::endl;
//unlock previously blocked shared variables //unlock previously blocked shared variables
//this->alg_.unlock(); //this->alg_.unlock();
...@@ -230,9 +450,17 @@ void CollisionManagerAlgNode::imu_mutex_exit(void) ...@@ -230,9 +450,17 @@ void CollisionManagerAlgNode::imu_mutex_exit(void)
void CollisionManagerAlgNode::node_config_update(Config &config, uint32_t level) void CollisionManagerAlgNode::node_config_update(Config &config, uint32_t level)
{ {
this->alg_.lock(); this->alg_.lock();
this->imu_mutex_enter();
this->rear_laser_scan_mutex_enter();
this->front_laser_scan_mutex_enter();
if(config.rate!=this->getRate()) if(config.rate!=this->getRate())
this->setRate(config.rate); this->setRate(config.rate);
this->config_=config; this->config_=config;
this->front_laser_scan_mutex_exit();
this->rear_laser_scan_mutex_exit();
this->imu_mutex_exit();
this->alg_.unlock(); this->alg_.unlock();
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment