diff --git a/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp index 89dcb8142..0d271fc7c 100644 --- a/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp @@ -418,6 +418,12 @@ radar_msgs::msg::RadarTracks ContinentalSRR520DecoderWrapper::convert_to_radar_t visualization_msgs::msg::MarkerArray ContinentalSRR520DecoderWrapper::convert_to_markers( const continental_msgs::msg::ContinentalSrr520ObjectList & msg) { + std::string base_frame; + { + std::lock_guard lock(mtx_driver_ptr_); + base_frame = sensor_cfg_->base_frame; + } + visualization_msgs::msg::MarkerArray marker_array; marker_array.markers.reserve(4 * msg.objects.size()); @@ -481,7 +487,7 @@ visualization_msgs::msg::MarkerArray ContinentalSRR520DecoderWrapper::convert_to current_ids.emplace(object.object_id); visualization_msgs::msg::Marker box_marker; - box_marker.header.frame_id = sensor_cfg_->base_frame; + box_marker.header.frame_id = base_frame; box_marker.header.stamp = msg.header.stamp; box_marker.ns = "boxes"; box_marker.id = object.object_id; @@ -556,7 +562,7 @@ visualization_msgs::msg::MarkerArray ContinentalSRR520DecoderWrapper::convert_to } visualization_msgs::msg::Marker delete_marker; - delete_marker.header.frame_id = sensor_cfg_->base_frame; + delete_marker.header.frame_id = base_frame; delete_marker.header.stamp = msg.header.stamp; delete_marker.ns = "boxes"; delete_marker.id = previous_id; diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp index 01f7ba2e3..406c4cb1d 100644 --- a/nebula_ros/src/hesai/decoder_wrapper.cpp +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -113,6 +113,7 @@ void HesaiDecoderWrapper::on_calibration_change( drivers::PacketDecodeResult HesaiDecoderWrapper::process_cloud_packet( std::unique_ptr packet_msg, uint64_t receive_time_ns) { + std::lock_guard lock(mtx_driver_ptr_); current_scan_perf_counters_.receive_time_current_scan_ns += receive_time_ns; // Ideally, we would only accumulate packets if someone is subscribed to the packets topic. @@ -131,7 +132,6 @@ drivers::PacketDecodeResult HesaiDecoderWrapper::process_cloud_packet( current_scan_msg_->packets.emplace_back(pandar_packet_msg); } - std::lock_guard lock(mtx_driver_ptr_); auto decode_result = driver_ptr_->parse_cloud_packet(packet_msg->data); current_scan_perf_counters_.decode_time_current_scan_ns += @@ -216,7 +216,11 @@ void HesaiDecoderWrapper::publish_cloud( if (pointcloud->header.stamp.sec < 0) { RCLCPP_WARN_STREAM(logger_, "Timestamp error, verify clock source."); } - pointcloud->header.frame_id = sensor_cfg_->frame_id; + + { + std::lock_guard lock(mtx_driver_ptr_); + pointcloud->header.frame_id = sensor_cfg_->frame_id; + } publisher->publish(std::move(pointcloud)); } diff --git a/nebula_ros/src/robosense/decoder_wrapper.cpp b/nebula_ros/src/robosense/decoder_wrapper.cpp index 0dcee387c..80922f275 100644 --- a/nebula_ros/src/robosense/decoder_wrapper.cpp +++ b/nebula_ros/src/robosense/decoder_wrapper.cpp @@ -157,7 +157,11 @@ void RobosenseDecoderWrapper::publish_cloud( RCLCPP_WARN_STREAM(logger_, "Timestamp error, verify clock source."); return; } - pointcloud->header.frame_id = sensor_cfg_->frame_id; + + { + std::lock_guard lock(mtx_driver_ptr_); + pointcloud->header.frame_id = sensor_cfg_->frame_id; + } publisher->publish(std::move(pointcloud)); } diff --git a/nebula_ros/src/velodyne/decoder_wrapper.cpp b/nebula_ros/src/velodyne/decoder_wrapper.cpp index 3a86fbe61..8704da88a 100644 --- a/nebula_ros/src/velodyne/decoder_wrapper.cpp +++ b/nebula_ros/src/velodyne/decoder_wrapper.cpp @@ -130,8 +130,11 @@ rcl_interfaces::msg::SetParametersResult VelodyneDecoderWrapper::on_parameter_ch } on_calibration_change(get_calibration_result.value()); - RCLCPP_INFO_STREAM( - logger_, "Changed calibration to '" << calibration_cfg_ptr_->calibration_file << "'"); + { + std::lock_guard lock(mtx_driver_ptr_); + RCLCPP_INFO_STREAM( + logger_, "Changed calibration to '" << calibration_cfg_ptr_->calibration_file << "'"); + } return rcl_interfaces::build().successful(true).reason(""); } @@ -238,7 +241,11 @@ void VelodyneDecoderWrapper::publish_cloud( if (pointcloud->header.stamp.sec < 0) { RCLCPP_WARN_STREAM(logger_, "Timestamp error, verify clock source."); } - pointcloud->header.frame_id = sensor_cfg_->frame_id; + + { + std::lock_guard lock(mtx_driver_ptr_); + pointcloud->header.frame_id = sensor_cfg_->frame_id; + } publisher->publish(std::move(pointcloud)); }