Skip to content

Commit

Permalink
在topic接收功能中添加线程锁
Browse files Browse the repository at this point in the history
  • Loading branch information
Little-Potato-1990 committed Mar 7, 2020
1 parent bd27c2b commit 5b727bc
Show file tree
Hide file tree
Showing 16 changed files with 75 additions and 8 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,8 @@
#define LIDAR_LOCALIZATION_SUBSCRIBER_CLOUD_SUBSCRIBER_HPP_

#include <deque>
#include <mutex>
#include <thread>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
Expand All @@ -30,8 +32,9 @@ class CloudSubscriber {
private:
ros::NodeHandle nh_;
ros::Subscriber subscriber_;

std::deque<CloudData> new_cloud_data_;

std::mutex buff_mutex_;
};
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,12 @@
#define LIDAR_LOCALIZATION_SUBSCRIBER_GNSS_SUBSCRIBER_HPP_

#include <deque>
#include <mutex>
#include <thread>

#include <ros/ros.h>
#include "sensor_msgs/NavSatFix.h"

#include "lidar_localization/sensor_data/gnss_data.hpp"

namespace lidar_localization {
Expand All @@ -24,8 +28,9 @@ class GNSSSubscriber {
private:
ros::NodeHandle nh_;
ros::Subscriber subscriber_;

std::deque<GNSSData> new_gnss_data_;

std::mutex buff_mutex_;
};
}
#endif
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,12 @@
#define LIDAR_LOCALIZATION_SUBSCRIBER_IMU_SUBSCRIBER_HPP_

#include <deque>
#include <mutex>
#include <thread>

#include <ros/ros.h>
#include "sensor_msgs/Imu.h"

#include "lidar_localization/sensor_data/imu_data.hpp"

namespace lidar_localization {
Expand All @@ -24,8 +28,9 @@ class IMUSubscriber {
private:
ros::NodeHandle nh_;
ros::Subscriber subscriber_;
std::deque<IMUData> new_imu_data_;

std::deque<IMUData> new_imu_data_;
std::mutex buff_mutex_;
};
}
#endif
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,9 @@
#define LIDAR_LOCALIZATION_SUBSCRIBER_KEY_FRAME_SUBSCRIBER_HPP_

#include <deque>
#include <mutex>
#include <thread>

#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

Expand All @@ -25,8 +28,9 @@ class KeyFrameSubscriber {
private:
ros::NodeHandle nh_;
ros::Subscriber subscriber_;
std::deque<KeyFrame> new_key_frame_;

std::deque<KeyFrame> new_key_frame_;
std::mutex buff_mutex_;
};
}
#endif
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,9 @@
#define LIDAR_LOCALIZATION_SUBSCRIBER_KEY_FRAMES_SUBSCRIBER_HPP_

#include <deque>
#include <mutex>
#include <thread>

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
Expand All @@ -26,8 +29,9 @@ class KeyFramesSubscriber {
private:
ros::NodeHandle nh_;
ros::Subscriber subscriber_;
std::deque<KeyFrame> new_key_frames_;

std::deque<KeyFrame> new_key_frames_;
std::mutex buff_mutex_;
};
}
#endif
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,9 @@
#define LIDAR_LOCALIZATION_SUBSCRIBER_LOOP_POSE_SUBSCRIBER_HPP_

#include <deque>
#include <mutex>
#include <thread>

#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

Expand All @@ -25,8 +28,9 @@ class LoopPoseSubscriber {
private:
ros::NodeHandle nh_;
ros::Subscriber subscriber_;
std::deque<LoopPose> new_loop_pose_;

std::deque<LoopPose> new_loop_pose_;
std::mutex buff_mutex_;
};
}
#endif
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,12 @@
#define LIDAR_LOCALIZATION_SUBSCRIBER_ODOMETRY_SUBSCRIBER_HPP_

#include <deque>
#include <mutex>
#include <thread>

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>

#include "lidar_localization/sensor_data/pose_data.hpp"

namespace lidar_localization {
Expand All @@ -24,8 +28,9 @@ class OdometrySubscriber {
private:
ros::NodeHandle nh_;
ros::Subscriber subscriber_;
std::deque<PoseData> new_pose_data_;

std::deque<PoseData> new_pose_data_;
std::mutex buff_mutex_;
};
}
#endif
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,12 @@
#define LIDAR_LOCALIZATION_SUBSCRIBER_VELOCITY_SUBSCRIBER_HPP_

#include <deque>
#include <mutex>
#include <thread>

#include <ros/ros.h>
#include "geometry_msgs/TwistStamped.h"

#include "lidar_localization/sensor_data/velocity_data.hpp"

namespace lidar_localization {
Expand All @@ -24,8 +28,9 @@ class VelocitySubscriber {
private:
ros::NodeHandle nh_;
ros::Subscriber subscriber_;
std::deque<VelocityData> new_velocity_data_;

std::deque<VelocityData> new_velocity_data_;
std::mutex buff_mutex_;
};
}
#endif
4 changes: 4 additions & 0 deletions lidar_localization/src/subscriber/cloud_subscriber.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,17 +15,21 @@ CloudSubscriber::CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, si
}

void CloudSubscriber::msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr) {
buff_mutex_.lock();
CloudData cloud_data;
cloud_data.time = cloud_msg_ptr->header.stamp.toSec();
pcl::fromROSMsg(*cloud_msg_ptr, *(cloud_data.cloud_ptr));

new_cloud_data_.push_back(cloud_data);
buff_mutex_.unlock();
}

void CloudSubscriber::ParseData(std::deque<CloudData>& cloud_data_buff) {
buff_mutex_.lock();
if (new_cloud_data_.size() > 0) {
cloud_data_buff.insert(cloud_data_buff.end(), new_cloud_data_.begin(), new_cloud_data_.end());
new_cloud_data_.clear();
}
buff_mutex_.unlock();
}
} // namespace data_input
4 changes: 4 additions & 0 deletions lidar_localization/src/subscriber/gnss_subscriber.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@ GNSSSubscriber::GNSSSubscriber(ros::NodeHandle& nh, std::string topic_name, size
}

void GNSSSubscriber::msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_fix_ptr) {
buff_mutex_.lock();
GNSSData gnss_data;
gnss_data.time = nav_sat_fix_ptr->header.stamp.toSec();
gnss_data.latitude = nav_sat_fix_ptr->latitude;
Expand All @@ -23,12 +24,15 @@ void GNSSSubscriber::msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_
gnss_data.service = nav_sat_fix_ptr->status.service;

new_gnss_data_.push_back(gnss_data);
buff_mutex_.unlock();
}

void GNSSSubscriber::ParseData(std::deque<GNSSData>& gnss_data_buff) {
buff_mutex_.lock();
if (new_gnss_data_.size() > 0) {
gnss_data_buff.insert(gnss_data_buff.end(), new_gnss_data_.begin(), new_gnss_data_.end());
new_gnss_data_.clear();
}
buff_mutex_.unlock();
}
}
4 changes: 4 additions & 0 deletions lidar_localization/src/subscriber/imu_subscriber.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ IMUSubscriber::IMUSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t
}

void IMUSubscriber::msg_callback(const sensor_msgs::ImuConstPtr& imu_msg_ptr) {
buff_mutex_.lock();
IMUData imu_data;
imu_data.time = imu_msg_ptr->header.stamp.toSec();

Expand All @@ -30,12 +31,15 @@ void IMUSubscriber::msg_callback(const sensor_msgs::ImuConstPtr& imu_msg_ptr) {
imu_data.orientation.w = imu_msg_ptr->orientation.w;

new_imu_data_.push_back(imu_data);
buff_mutex_.unlock();
}

void IMUSubscriber::ParseData(std::deque<IMUData>& imu_data_buff) {
buff_mutex_.lock();
if (new_imu_data_.size() > 0) {
imu_data_buff.insert(imu_data_buff.end(), new_imu_data_.begin(), new_imu_data_.end());
new_imu_data_.clear();
}
buff_mutex_.unlock();
}
}
4 changes: 4 additions & 0 deletions lidar_localization/src/subscriber/key_frame_subscriber.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ KeyFrameSubscriber::KeyFrameSubscriber(ros::NodeHandle& nh, std::string topic_na
}

void KeyFrameSubscriber::msg_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& key_frame_msg_ptr) {
buff_mutex_.lock();
KeyFrame key_frame;
key_frame.time = key_frame_msg_ptr->header.stamp.toSec();
key_frame.index = (unsigned int)key_frame_msg_ptr->pose.covariance[0];
Expand All @@ -29,12 +30,15 @@ void KeyFrameSubscriber::msg_callback(const geometry_msgs::PoseWithCovarianceSta
key_frame.pose.block<3,3>(0,0) = q.matrix();

new_key_frame_.push_back(key_frame);
buff_mutex_.unlock();
}

void KeyFrameSubscriber::ParseData(std::deque<KeyFrame>& key_frame_buff) {
buff_mutex_.lock();
if (new_key_frame_.size() > 0) {
key_frame_buff.insert(key_frame_buff.end(), new_key_frame_.begin(), new_key_frame_.end());
new_key_frame_.clear();
}
buff_mutex_.unlock();
}
}
4 changes: 4 additions & 0 deletions lidar_localization/src/subscriber/key_frames_subscriber.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ KeyFramesSubscriber::KeyFramesSubscriber(ros::NodeHandle& nh, std::string topic_
}

void KeyFramesSubscriber::msg_callback(const nav_msgs::Path::ConstPtr& key_frames_msg_ptr) {
buff_mutex_.lock();
new_key_frames_.clear();

for (size_t i = 0; i < key_frames_msg_ptr->poses.size(); i++) {
Expand All @@ -33,12 +34,15 @@ void KeyFramesSubscriber::msg_callback(const nav_msgs::Path::ConstPtr& key_frame

new_key_frames_.push_back(key_frame);
}
buff_mutex_.unlock();
}

void KeyFramesSubscriber::ParseData(std::deque<KeyFrame>& key_frames_buff) {
buff_mutex_.lock();
if (new_key_frames_.size() > 0) {
key_frames_buff = new_key_frames_;
new_key_frames_.clear();
}
buff_mutex_.unlock();
}
}
4 changes: 4 additions & 0 deletions lidar_localization/src/subscriber/loop_pose_subscriber.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ LoopPoseSubscriber::LoopPoseSubscriber(ros::NodeHandle& nh, std::string topic_na
}

void LoopPoseSubscriber::msg_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& loop_pose_msg_ptr) {
buff_mutex_.lock();
LoopPose loop_pose;
loop_pose.time = loop_pose_msg_ptr->header.stamp.toSec();
loop_pose.index0 = (unsigned int)loop_pose_msg_ptr->pose.covariance[0];
Expand All @@ -30,12 +31,15 @@ void LoopPoseSubscriber::msg_callback(const geometry_msgs::PoseWithCovarianceSta
loop_pose.pose.block<3,3>(0,0) = q.matrix();

new_loop_pose_.push_back(loop_pose);
buff_mutex_.unlock();
}

void LoopPoseSubscriber::ParseData(std::deque<LoopPose>& loop_pose_buff) {
buff_mutex_.lock();
if (new_loop_pose_.size() > 0) {
loop_pose_buff.insert(loop_pose_buff.end(), new_loop_pose_.begin(), new_loop_pose_.end());
new_loop_pose_.clear();
}
buff_mutex_.unlock();
}
}
4 changes: 4 additions & 0 deletions lidar_localization/src/subscriber/odometry_subscriber.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ OdometrySubscriber::OdometrySubscriber(ros::NodeHandle& nh, std::string topic_na
}

void OdometrySubscriber::msg_callback(const nav_msgs::OdometryConstPtr& odom_msg_ptr) {
buff_mutex_.lock();
PoseData pose_data;
pose_data.time = odom_msg_ptr->header.stamp.toSec();

Expand All @@ -29,12 +30,15 @@ void OdometrySubscriber::msg_callback(const nav_msgs::OdometryConstPtr& odom_msg
pose_data.pose.block<3,3>(0,0) = q.matrix();

new_pose_data_.push_back(pose_data);
buff_mutex_.unlock();
}

void OdometrySubscriber::ParseData(std::deque<PoseData>& pose_data_buff) {
buff_mutex_.lock();
if (new_pose_data_.size() > 0) {
pose_data_buff.insert(pose_data_buff.end(), new_pose_data_.begin(), new_pose_data_.end());
new_pose_data_.clear();
}
buff_mutex_.unlock();
}
}
4 changes: 4 additions & 0 deletions lidar_localization/src/subscriber/velocity_subscriber.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@ VelocitySubscriber::VelocitySubscriber(ros::NodeHandle& nh, std::string topic_na
}

void VelocitySubscriber::msg_callback(const geometry_msgs::TwistStampedConstPtr& twist_msg_ptr) {
buff_mutex_.lock();
VelocityData velocity_data;
velocity_data.time = twist_msg_ptr->header.stamp.toSec();

Expand All @@ -26,12 +27,15 @@ void VelocitySubscriber::msg_callback(const geometry_msgs::TwistStampedConstPtr&
velocity_data.angular_velocity.z = twist_msg_ptr->twist.angular.z;

new_velocity_data_.push_back(velocity_data);
buff_mutex_.unlock();
}

void VelocitySubscriber::ParseData(std::deque<VelocityData>& velocity_data_buff) {
buff_mutex_.lock();
if (new_velocity_data_.size() > 0) {
velocity_data_buff.insert(velocity_data_buff.end(), new_velocity_data_.begin(), new_velocity_data_.end());
new_velocity_data_.clear();
}
buff_mutex_.unlock();
}
}

0 comments on commit 5b727bc

Please sign in to comment.