From c2660e2922cec3f3f7f64a8594f178bd8c19d9fd Mon Sep 17 00:00:00 2001 From: Alopez <alopez@iri.upc.edu> Date: Thu, 2 Dec 2021 17:04:08 +0100 Subject: [PATCH] Updated to ICP server --- CMakeLists.txt | 5 +- README.md | 24 ++- cfg/CollisionManager.cfg | 7 +- config/params.yaml | 11 +- include/collision_manager_alg_node.h | 63 ++++++- package.xml | 1 + src/collision_manager_alg_node.cpp | 246 ++++++++++++++++++++++++++- 7 files changed, 333 insertions(+), 24 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 528813e..9da7ada 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 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 # 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 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 # ******************************************************************** @@ -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} iri_laser_scan_icp_generate_messages_cpp) add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp) # ******************************************************************** # Add dynamic reconfigure dependencies diff --git a/README.md b/README.md index d1a747f..fc5650a 100644 --- a/README.md +++ b/README.md @@ -10,17 +10,27 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal). # ROS Interface ### 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. - ~**imu** (sensor_msgs/Imu.msg): Imu input data. +### Service clients + - ~**icp** (iri_laser_scan_icp/icp.srv): ICP server. + ### Parameters -- ~**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. -- ~**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. -- ~**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_counter_limit** (Integer; default: 6; min: 1; max: 30) Number of low acc imu meassures to detect an end of collision. +- ~**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. +- ~**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. +- ~**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_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 diff --git a/cfg/CollisionManager.cfg b/cfg/CollisionManager.cfg index 43d5837..5051dde 100755 --- a/cfg/CollisionManager.cfg +++ b/cfg/CollisionManager.cfg @@ -39,12 +39,17 @@ gen = ParameterGenerator() # 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("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("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("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("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")) \ No newline at end of file diff --git a/config/params.yaml b/config/params.yaml index cfd4239..0f975fe 100644 --- a/config/params.yaml +++ b/config/params.yaml @@ -1,7 +1,12 @@ rate: 240 +fixed_frame: imu_bno055 tf_timeout: 0.2 -collision_acc_th: 5.0 +err_msg_rate: 0.5 collision_transition_counter_en: True +collision_timeout: 4.0 +collision_acc_th: 5.0 collision_counter_limit: 12 -err_msg_rate: 0.5 -fixed_frame: imu_bno055 +front_laser_en: True +rear_laser_en: True +scan_buffer_size: 6 +scan_diff_t_from_collision: 0.0 diff --git a/include/collision_manager_alg_node.h b/include/collision_manager_alg_node.h index c92ec90..e16cb4d 100644 --- a/include/collision_manager_alg_node.h +++ b/include/collision_manager_alg_node.h @@ -30,14 +30,27 @@ #include <Eigen/Eigen> // [publisher subscriber headers] +#include <sensor_msgs/LaserScan.h> #include <tf2_geometry_msgs/tf2_geometry_msgs.h> #include <tf2_ros/transform_listener.h> #include <sensor_msgs/Imu.h> // [service client headers] +#include <iri_laser_scan_icp/icp.h> // [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 * @@ -47,17 +60,37 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio private: 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. + 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. 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. 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] // [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::TransformListener tf2_listener; @@ -72,6 +105,10 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio // [service 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] @@ -103,16 +140,18 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio 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 * saved, it saves the transfrom. * * \param _global_acc The current global linear acceleration. * + * \param _global_ang_vel The current global angular velocity. + * * \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. @@ -150,6 +189,26 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio */ 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 * diff --git a/package.xml b/package.xml index db0289d..25cbf1e 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>iri_laser_scan_icp</depend> <depend>tf2_ros</depend> <depend>sensor_msgs</depend> <depend>iri_bno055_imu_bringup</depend> diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp index e54c4e6..0b52573 100644 --- a/src/collision_manager_alg_node.cpp +++ b/src/collision_manager_alg_node.cpp @@ -23,10 +23,17 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) : this->tf_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; // [init publishers] // [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); pthread_mutex_init(&this->imu_mutex_,NULL); @@ -34,6 +41,8 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) : // [init services] // [init clients] + icp_client_ = this->private_node_handle_.serviceClient<iri_laser_scan_icp::icp>("icp"); + // [init action servers] @@ -43,6 +52,8 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) : CollisionManagerAlgNode::~CollisionManagerAlgNode(void) { // [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_); } @@ -52,11 +63,13 @@ void CollisionManagerAlgNode::mainNodeThread(void) // this->alg_.lock(); // ROS_DEBUG("CollisionManagerAlgNode::mainNodeThread"); this->imu_mutex_enter(); + this->rear_laser_scan_mutex_enter(); + this->front_laser_scan_mutex_enter(); if (this->new_imu_input_) { - Eigen::Vector3d global_acc; - if (get_global_acc(global_acc)) + Eigen::Vector3d global_acc, global_ang_vel; + 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)); if (this->in_collision_) @@ -71,17 +84,90 @@ void CollisionManagerAlgNode::mainNodeThread(void) collision_angle += 2*M_PI; ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision end detected."); 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->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)) { 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->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; @@ -134,23 +220,38 @@ void CollisionManagerAlgNode::mainNodeThread(void) // [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] // [publish messages] + this->front_laser_scan_mutex_exit(); + this->rear_laser_scan_mutex_exit(); this->imu_mutex_exit(); // 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_) { 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))) { - 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; return false; } @@ -162,6 +263,7 @@ bool CollisionManagerAlgNode::get_global_acc(Eigen::Vector3d& _global_acc) this->tf_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_; return true; } @@ -191,7 +293,121 @@ bool CollisionManagerAlgNode::check_start_of_collision(double _acc_norm_2d) 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] */ +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) { // ROS_INFO("CollisionManagerAlgNode::imu_callback: New Message Received"); @@ -202,8 +418,12 @@ void CollisionManagerAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg this->new_imu_input_ = true; 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); - if (!this->in_collision_) - this->collision_start_stamp_ = msg->header.stamp; + 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; + // 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; //unlock previously blocked shared variables //this->alg_.unlock(); @@ -230,9 +450,17 @@ void CollisionManagerAlgNode::imu_mutex_exit(void) void CollisionManagerAlgNode::node_config_update(Config &config, uint32_t level) { this->alg_.lock(); + this->imu_mutex_enter(); + this->rear_laser_scan_mutex_enter(); + this->front_laser_scan_mutex_enter(); + if(config.rate!=this->getRate()) this->setRate(config.rate); this->config_=config; + + this->front_laser_scan_mutex_exit(); + this->rear_laser_scan_mutex_exit(); + this->imu_mutex_exit(); this->alg_.unlock(); } -- GitLab