From cdf7cdc1727c7eb4e29c9276b19e26fb8e506484 Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Mon, 17 Jan 2022 15:15:34 +0100
Subject: [PATCH] Added collison.msg msg publication
---
CMakeLists.txt | 23 ++---
README.md | 2 +
include/collision_manager_alg_node.h | 12 +++
launch/collisions.launch | 2 +
launch/node.launch | 2 +
launch/test.launch | 1 +
msg/collision.msg | 15 +++
package.xml | 3 +
src/collision_manager_alg_node.cpp | 133 ++++++++++++++++++++++-----
9 files changed, 157 insertions(+), 36 deletions(-)
create mode 100644 msg/collision.msg
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f12edbd..407363d 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 geometry_msgs iri_laser_scan_icp tf2_ros sensor_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm message_generation nav_msgs geometry_msgs iri_laser_scan_icp tf2_ros sensor_msgs)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -21,11 +21,10 @@ find_package(Eigen3 REQUIRED)
# Add topic, service and action definition here
# ********************************************************************
## Generate messages in the 'msg' folder
-# add_message_files(
-# FILES
-# Message1.msg
-# Message2.msg
-# )
+add_message_files(
+ FILES
+ collision.msg
+)
## Generate services in the 'srv' folder
# add_service_files(
@@ -42,10 +41,11 @@ find_package(Eigen3 REQUIRED)
# )
## Generate added messages and services with any dependencies listed here
-# generate_messages(
-# DEPENDENCIES
-# std_msgs # Or other packages containing msgs
-# )
+generate_messages(
+ DEPENDENCIES
+ std_msgs
+ geometry_msgs # Or other packages containing msgs
+)
# ********************************************************************
# Add the dynamic reconfigure file
@@ -61,7 +61,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS iri_base_algorithm geometry_msgs iri_laser_scan_icp tf2_ros sensor_msgs
+ CATKIN_DEPENDS iri_base_algorithm message_runtime nav_msgs 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} nav_msgs_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 b98464c..9d7ed60 100644
--- a/README.md
+++ b/README.md
@@ -10,6 +10,7 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal).
# ROS Interface
### Topic publishers
+ - ~**collisions** (iri_collision_manager/collision.msg)
- ~**debug_rear_after_icp_scan** (sensor_msgs/LaserScan.msg)
- ~**debug_front_after_icp_scan** (sensor_msgs/LaserScan.msg)
- /**tf** (tf/tfMessage)
@@ -20,6 +21,7 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal).
- ~**debug_front_icp_last_scan** (sensor_msgs/LaserScan.msg): Debug topic to visulize front ICP matching laser scan.
- ~**debug_front_icp_ref_scan** (sensor_msgs/LaserScan.msg): Debug topic to visulize front ICP reference laser scan.
### Topic subscribers
+ - ~**odom** (nav_msgs/Odometry.msg)
- ~**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.
diff --git a/include/collision_manager_alg_node.h b/include/collision_manager_alg_node.h
index b690da1..5d6e95a 100644
--- a/include/collision_manager_alg_node.h
+++ b/include/collision_manager_alg_node.h
@@ -31,6 +31,8 @@
#include <tf2/utils.h>
// [publisher subscriber headers]
+#include <iri_collision_manager/collision.h>
+#include <nav_msgs/Odometry.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/LaserScan.h>
@@ -87,8 +89,12 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
bool is_rear_icp_input_data_ok_; ///<Boolean to know of all the necessary data has been saved on the service request.
bool front_icp_succedded_; ///< Boolean to know that ICP service was called withs success.
bool rear_icp_succedded_; ///< Boolean to know that ICP service was called withs success.
+ double angular_vel_; ///< Robot's angular velocity from odometry.
// [publisher attributes]
+ ros::Publisher collisions_publisher_;
+ iri_collision_manager::collision collision_msg_;
+
ros::Publisher debug_rear_after_icp_scan_publisher_;
sensor_msgs::LaserScan debug_rear_after_icp_scan_msg_;
@@ -118,6 +124,12 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
// [subscriber attributes]
+ ros::Subscriber odom_subscriber_;
+ void odom_callback(const nav_msgs::Odometry::ConstPtr& msg);
+ pthread_mutex_t odom_mutex_;
+ void odom_mutex_enter(void);
+ void odom_mutex_exit(void);
+
ros::Subscriber rear_laser_scan_subscriber_;
void rear_laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
pthread_mutex_t rear_laser_scan_mutex_;
diff --git a/launch/collisions.launch b/launch/collisions.launch
index 883b15a..3b15a7a 100644
--- a/launch/collisions.launch
+++ b/launch/collisions.launch
@@ -27,6 +27,7 @@
<arg name="collision_config_file" default="$(find iri_collision_manager)/config/params.yaml"/>
<arg name="front_laser_topic" default="/laser_front/scan"/>
<arg name="rear_laser_topic" default="/laser_rear/scan"/>
+ <arg name="odom_topic" default="/odom"/>
<include file="$(find iri_bno055_imu_bringup)/launch/bno055_imu.launch"
if="$(arg imu_driver)">
@@ -65,6 +66,7 @@
<arg name="front_laser_topic" value="$(arg front_laser_topic)"/>
<arg name="rear_laser_topic" value="$(arg rear_laser_topic)"/>
<arg name="icp_service_name" value="$(arg icp_service_name)"/>
+ <arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
diff --git a/launch/node.launch b/launch/node.launch
index 7e94558..b1a6d37 100644
--- a/launch/node.launch
+++ b/launch/node.launch
@@ -10,6 +10,7 @@
<arg name="front_laser_topic" default="~/front_laser_scan"/>
<arg name="rear_laser_topic" default="~/rear_laser_scan"/>
<arg name="icp_service_name" default="~/icp"/>
+ <arg name="odom_topic" default="~/odom"/>
<node name="$(arg node_name)"
pkg ="iri_collision_manager"
@@ -22,6 +23,7 @@
<remap from="~/front_laser_scan" to="$(arg front_laser_topic)"/>
<remap from="~/rear_laser_scan" to="$(arg rear_laser_topic)"/>
<remap from="~/icp" to="$(arg icp_service_name)"/>
+ <remap from="~/odom" to="$(arg odom_topic)"/>
</node>
</launch>
diff --git a/launch/test.launch b/launch/test.launch
index ffe8b8f..5ce5b40 100644
--- a/launch/test.launch
+++ b/launch/test.launch
@@ -19,6 +19,7 @@
<arg name="front_laser_topic" value="/front_laser_scan"/>
<arg name="rear_laser_topic" value="/rear_laser_scan"/>
<arg name="icp_service_name" value="~/icp"/>
+ <arg name="odom_topic" value="/odom"/>
</include>
<node name="rqt_reconfigure_iri_collision_manager"
diff --git a/msg/collision.msg b/msg/collision.msg
new file mode 100644
index 0000000..b825c40
--- /dev/null
+++ b/msg/collision.msg
@@ -0,0 +1,15 @@
+Header header # Msg header.
+float32 acc_peak # Acceleration peak at collision
+float32 acc_angle # Estimation of the collision angle from accelerometer data.
+
+float32 ang_vel_diff # Angular velocity difference between IMU data and odometry.
+
+int32 front_icp_points # Front ICP laser scan matching points.
+float32 front_icp_err # Front ICP error per point.
+geometry_msgs/Pose2D front_laser_disp # Front laser displacement.
+float32[9] front_covariance # Front laser displacement covariance.
+
+int32 rear_icp_points # Rear ICP laser scan matching points.
+float32 rear_icp_err # Rear ICP error per point.
+geometry_msgs/Pose2D rear_laser_disp # Rear laser displacement.
+float32[9] rear_covariance # Rear laser displacement covariance.
diff --git a/package.xml b/package.xml
index 568b88c..401ef1d 100644
--- a/package.xml
+++ b/package.xml
@@ -51,7 +51,10 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>iri_base_algorithm</build_depend>
<build_export_depend>iri_base_algorithm</build_export_depend>
+ <build_depend>message_generation</build_depend>
+ <exec_depend>message_runtime</exec_depend>
<exec_depend>iri_base_algorithm</exec_depend>
+ <depend>nav_msgs</depend>
<depend>geometry_msgs</depend>
<depend>iri_laser_scan_icp</depend>
<depend>tf2_ros</depend>
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index 2742391..6c6c656 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -38,6 +38,7 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
this->tf_base_link_rear_laser_loaded_ = false;
this->front_icp_succedded_ = false;
this->rear_icp_succedded_ = false;
+ this->angular_vel_ = 0.0;
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;
@@ -49,7 +50,10 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
for (unsigned int i = 0; i < 36; i++)
this->debug_front_icp_pose_msg_.pose.covariance[i] = 0.0;
+ this->collision_msg_.header.frame_id = "iri_collision_manager";
+
// [init publishers]
+ this->collisions_publisher_ = this->private_node_handle_.advertise<iri_collision_manager::collision>("collisions", 1);
this->debug_rear_after_icp_scan_publisher_ = this->private_node_handle_.advertise<sensor_msgs::LaserScan>("debug_rear_after_icp_scan", 1);
this->debug_front_after_icp_scan_publisher_ = this->private_node_handle_.advertise<sensor_msgs::LaserScan>("debug_front_after_icp_scan", 1);
this->debug_rear_icp_pose_publisher_ = this->private_node_handle_.advertise<geometry_msgs::PoseWithCovarianceStamped>("debug_rear_icp_pose", 1);
@@ -60,6 +64,9 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
this->debug_front_icp_ref_scan_publisher_ = this->private_node_handle_.advertise<sensor_msgs::LaserScan>("debug_front_icp_ref_scan", 1);
// [init subscribers]
+ this->odom_subscriber_ = this->private_node_handle_.subscribe("odom", 1, &CollisionManagerAlgNode::odom_callback, this);
+ pthread_mutex_init(&this->odom_mutex_,NULL);
+
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);
@@ -84,6 +91,7 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
CollisionManagerAlgNode::~CollisionManagerAlgNode(void)
{
// [free dynamic memory]
+ pthread_mutex_destroy(&this->odom_mutex_);
pthread_mutex_destroy(&this->rear_laser_scan_mutex_);
pthread_mutex_destroy(&this->front_laser_scan_mutex_);
pthread_mutex_destroy(&this->imu_mutex_);
@@ -97,6 +105,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->imu_mutex_enter();
this->rear_laser_scan_mutex_enter();
this->front_laser_scan_mutex_enter();
+ this->odom_mutex_enter();
Eigen::Vector3d blink_acc, blink_ang_vel, rear_blink_pose, front_blink_pose, new_laser_pose;
Eigen::Matrix3d rear_cov, front_cov, covariances;
@@ -118,6 +127,10 @@ void CollisionManagerAlgNode::mainNodeThread(void)
ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision end detected.");
ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: acc = " << max_acc_norm_2d << "; angle = " << collision_angle);
+ //update collision msg data
+ this->collision_msg_.acc_peak = max_acc_norm_2d;
+ this->collision_msg_.acc_angle = collision_angle;
+
//update rot_angle_estimated
this->front_icp_srv_.request.rot_angle_estimation += blink_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
@@ -125,7 +138,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->rear_icp_srv_.request.rot_angle_estimation = this->front_icp_srv_.request.rot_angle_estimation;
- ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: angle estimated = " << this->front_icp_srv_.request.rot_angle_estimation);
+ // ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: angle estimated = " << this->front_icp_srv_.request.rot_angle_estimation);
// Call the service
if (!(this->config_.front_laser_en || this->config_.rear_laser_en))
@@ -136,13 +149,20 @@ void CollisionManagerAlgNode::mainNodeThread(void)
{
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 << "; 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);
+ // 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 << "; 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);
+
+ //update collison msg data.
+ this->collision_msg_.front_icp_err = front_icp_srv_.response.error/front_icp_srv_.response.nvalid;
+ this->collision_msg_.front_laser_disp = front_icp_srv_.response.new_laser_pose;
+ for (unsigned int i=0; i<9; i++)
+ this->collision_msg_.front_covariance[i] = front_icp_srv_.response.covariance[i];
+
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;
@@ -167,13 +187,19 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->tf2_broadcaster.sendTransform(this->transform_msg);
this->front_icp_succedded_ = get_base_link_new_pose_from_icp(new_laser_pose, covariances, front_blink_pose, front_cov, true);
+
+ this->collision_msg_.front_icp_points = front_icp_srv_.response.nvalid;
}
else
+ {
+ this->collision_msg_.front_icp_points = 0;
this->front_icp_succedded_ = false;
+ }
}
else
{
this->front_icp_succedded_ = false;
+ this->collision_msg_.front_icp_points = 0;
ROS_ERROR("CollisionManagerAlgNode::mainNodeThread -> Failed to call service on topic %s",this->icp_client_.getService().c_str());
}
if (this->config_.debug)
@@ -193,13 +219,19 @@ void CollisionManagerAlgNode::mainNodeThread(void)
{
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 << "; 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);
+ // 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 << "; 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);
+
+ //update collison msg data.
+ this->collision_msg_.rear_icp_err = rear_icp_srv_.response.error/rear_icp_srv_.response.nvalid;
+ this->collision_msg_.rear_laser_disp = rear_icp_srv_.response.new_laser_pose;
+ for (unsigned int i=0; i<9; i++)
+ this->collision_msg_.rear_covariance[i] = rear_icp_srv_.response.covariance[i];
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)
{
@@ -225,13 +257,19 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->tf2_broadcaster.sendTransform(this->transform_msg);
this->rear_icp_succedded_ = get_base_link_new_pose_from_icp(new_laser_pose, covariances, rear_blink_pose, rear_cov, false);
+
+ this->collision_msg_.rear_icp_points = rear_icp_srv_.response.nvalid;
}
else
+ {
+ this->collision_msg_.rear_icp_points = 0;
this->rear_icp_succedded_ = false;
+ }
}
else
{
this->rear_icp_succedded_ = false;
+ this->collision_msg_.rear_icp_points = 0;
ROS_ERROR("CollisionManagerAlgNode::mainNodeThread -> Failed to call service on topic %s",this->icp_client_.getService().c_str());
}
if (this->config_.debug)
@@ -258,6 +296,9 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->low_ang_vel_count_ = 0;
this->is_front_icp_input_data_ok_ = false;
this->is_rear_icp_input_data_ok_ = false;
+
+ //publish collision.
+ this->collisions_publisher_.publish(this->collision_msg_);
}
else
{
@@ -291,6 +332,11 @@ void CollisionManagerAlgNode::mainNodeThread(void)
{
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);
+
+ if (!this->is_front_icp_input_data_ok_)
+ this->collision_msg_.front_icp_points = 0;
+ if (!this->is_rear_icp_input_data_ok_)
+ this->collision_msg_.rear_icp_points = 0;
}
//init variables
@@ -299,12 +345,19 @@ void CollisionManagerAlgNode::mainNodeThread(void)
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;
+
+ //update collision msg data
+ this->collision_msg_.header.stamp = this->imu_stamp_;
+ this->collision_msg_.ang_vel_diff = std::fabs(blink_ang_vel(2));
}
}
this->new_imu_input_ = false;
}
// [fill msg structures]
+ // Initialize the topic message structure
+ //this->collisions_collision_msg_.data = my_var;
+
// Initialize the topic message structure
//this->debug_rear_after_icp_scan_LaserScan_msg_.data = my_var;
@@ -392,6 +445,9 @@ 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->collisions_publisher_.publish(this->collisions_collision_msg_);
+
// Uncomment the following line to publish the topic message
//this->debug_rear_after_icp_scan_publisher_.publish(this->debug_rear_after_icp_scan_LaserScan_msg_);
@@ -435,6 +491,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
//this->debug_front_icp_ref_scan_publisher_.publish(this->debug_front_icp_ref_scan_LaserScan_msg_);
+ this->odom_mutex_exit();
this->front_laser_scan_mutex_exit();
this->rear_laser_scan_mutex_exit();
this->imu_mutex_exit();
@@ -481,16 +538,17 @@ bool CollisionManagerAlgNode::check_acc_collision_transition(double _acc_norm_2d
bool CollisionManagerAlgNode::check_ang_vel_collision_transition(double _ang_vel, bool _start)
{
+ double diff = std::fabs(_ang_vel - this->angular_vel_);
if (!this->config_.collision_ang_vel_transition_counter_en)
return true;
- if ((_start && (_ang_vel > this->config_.collision_ang_vel_th)) || (!_start && (_ang_vel < this->config_.collision_ang_vel_th)))
+ if ((_start && (diff > this->config_.collision_ang_vel_th)) || (!_start && (diff < this->config_.collision_ang_vel_th)))
this->low_ang_vel_count_++;
else
{
this->low_ang_vel_count_ = 0;
return false;
}
- return ((_start && (_ang_vel > this->config_.collision_ang_vel_th)) || (!_start && (_ang_vel < this->config_.collision_ang_vel_th)))
+ return ((_start && (diff > this->config_.collision_ang_vel_th)) || (!_start && (diff < this->config_.collision_ang_vel_th)))
&& (_start || (!_start && (!this->config_.collision_ang_vel_transition_counter_en || (this->low_ang_vel_count_ >= this->config_.collision_ang_vel_counter_limit))));
}
@@ -498,10 +556,10 @@ bool CollisionManagerAlgNode::check_end_of_collision(double _acc_norm_2d, double
{
bool acc_true = check_acc_collision_transition(_acc_norm_2d, false);
bool ang_vel_true = check_ang_vel_collision_transition(_ang_vel, false);
- if (acc_true)
- ROS_INFO_STREAM("CollisionManagerAlgNode::check_end_of_collision -> (END) Acc true. acc: " << _acc_norm_2d << "; vel: " << _ang_vel);
- if (ang_vel_true)
- ROS_INFO_STREAM("CollisionManagerAlgNode::check_end_of_collision -> (END) Angular vel true. acc: " << _acc_norm_2d << "; vel: " << _ang_vel);
+ // if (acc_true)
+ // ROS_INFO_STREAM("CollisionManagerAlgNode::check_end_of_collision -> (END) Acc true. acc: " << _acc_norm_2d << "; vel: " << _ang_vel);
+ // if (ang_vel_true)
+ // ROS_INFO_STREAM("CollisionManagerAlgNode::check_end_of_collision -> (END) Angular vel true. acc: " << _acc_norm_2d << "; vel: " << _ang_vel);
return acc_true && ang_vel_true;
}
@@ -509,10 +567,10 @@ bool CollisionManagerAlgNode::check_start_of_collision(double _acc_norm_2d, doub
{
bool acc_true = check_acc_collision_transition(_acc_norm_2d, true);
bool ang_vel_true = check_ang_vel_collision_transition(_ang_vel, true);
- if (acc_true)
- ROS_INFO_STREAM("CollisionManagerAlgNode::check_start_of_collision -> (START) Acc true. acc: " << _acc_norm_2d << "; vel: " << _ang_vel);
- if (ang_vel_true)
- ROS_INFO_STREAM("CollisionManagerAlgNode::check_start_of_collision -> (START) Angular vel true. acc: " << _acc_norm_2d << "; vel: " << _ang_vel);
+ // if (acc_true)
+ // ROS_INFO_STREAM("CollisionManagerAlgNode::check_start_of_collision -> (START) Acc true. acc: " << _acc_norm_2d << "; vel: " << _ang_vel);
+ // if (ang_vel_true)
+ // ROS_INFO_STREAM("CollisionManagerAlgNode::check_start_of_collision -> (START) Angular vel true. acc: " << _acc_norm_2d << "; vel: " << _ang_vel);
return acc_true || ang_vel_true;
}
@@ -657,6 +715,31 @@ bool CollisionManagerAlgNode::load_tf_blink_laser(bool _front)
}
/* [subscriber callbacks] */
+void CollisionManagerAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
+{
+ // ROS_INFO("CollisionManagerAlgNode::odom_callback: New Message Received");
+
+ //use appropiate mutex to shared variables if necessary
+ //this->alg_.lock();
+ this->odom_mutex_enter();
+ this->angular_vel_ = msg->twist.twist.angular.z;
+
+ //std::cout << msg->data << std::endl;
+ //unlock previously blocked shared variables
+ //this->alg_.unlock();
+ this->odom_mutex_exit();
+}
+
+void CollisionManagerAlgNode::odom_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->odom_mutex_);
+}
+
+void CollisionManagerAlgNode::odom_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->odom_mutex_);
+}
+
void CollisionManagerAlgNode::rear_laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
// ROS_INFO("CollisionManagerAlgNode::rear_laser_scan_callback: New Message Received");
--
GitLab