diff --git a/patch/ros-jazzy-cartographer-ros.patch b/patch/ros-jazzy-cartographer-ros.patch new file mode 100644 index 00000000..b5e82e42 --- /dev/null +++ b/patch/ros-jazzy-cartographer-ros.patch @@ -0,0 +1,152 @@ +diff --git a/CMakeLists.txt b/CMakeLists.txt +index f7f476296..0725a05d5 100644 +--- a/CMakeLists.txt ++++ b/CMakeLists.txt +@@ -50,6 +50,16 @@ find_package(urdf REQUIRED) + find_package(urdfdom_headers REQUIRED) + find_package(visualization_msgs REQUIRED) + ++set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) ++# glog is not linked, however we look for it to detect the glog version ++# and use a different code path if glog >= 0.7.0 is detected ++find_package(glog CONFIG QUIET) ++if(DEFINED glog_VERSION) ++ if(NOT glog_VERSION VERSION_LESS 0.7.0) ++ add_definitions(-DROS_CARTOGRAPHER_GLOG_GE_070) ++ endif() ++endif() ++ + include_directories( + include + ${PCL_INCLUDE_DIRS} +diff --git a/src/ros_log_sink.cpp b/src/ros_log_sink.cpp +index 1396381d4..9e189c690 100644 +--- a/src/ros_log_sink.cpp ++++ b/src/ros_log_sink.cpp +@@ -33,6 +33,11 @@ const char* GetBasename(const char* filepath) { + return base ? (base + 1) : filepath; + } + ++std::chrono::system_clock::time_point ConvertTmToTimePoint(const std::tm& tm) { ++ std::time_t timeT = std::mktime(const_cast(&tm)); // Convert std::tm to time_t ++ return std::chrono::system_clock::from_time_t(timeT); // Convert time_t to time_point ++} ++ + } // namespace + + ScopedRosLogSink::ScopedRosLogSink() : will_die_(false) { AddLogSink(this); } +@@ -46,10 +51,13 @@ void ScopedRosLogSink::send(const ::google::LogSeverity severity, + const size_t message_len) { + (void) base_filename; // TODO: remove unused arg ? + ++#if defined(ROS_CARTOGRAPHER_GLOG_GE_070) ++ const std::string message_string = ::google::LogSink::ToString( ++ severity, GetBasename(filename), line, ::google::LogMessageTime(ConvertTmToTimePoint(*tm_time)), message, message_len); + // Google glog broke the ToString API, but has no way to tell what version it is using. + // To support both newer and older glog versions, use a nasty hack were we infer the + // version based on whether GOOGLE_GLOG_DLL_DECL is defined +-#if defined(GOOGLE_GLOG_DLL_DECL) ++#elif defined(GOOGLE_GLOG_DLL_DECL) + const std::string message_string = ::google::LogSink::ToString( + severity, GetBasename(filename), line, tm_time, message, message_len); + #else + +diff --git a/include/cartographer_ros/map_builder_bridge.h b/include/cartographer_ros/map_builder_bridge.h +index b2c00b7..9c1befd 100644 +--- a/include/cartographer_ros/map_builder_bridge.h ++++ b/include/cartographer_ros/map_builder_bridge.h +@@ -95,7 +95,7 @@ class MapBuilderBridge { + GetTrajectoryStates(); + cartographer_ros_msgs::msg::SubmapList GetSubmapList(rclcpp::Time node_time); + std::unordered_map GetLocalTrajectoryData() +- LOCKS_EXCLUDED(mutex_); ++ ABSL_LOCKS_EXCLUDED(mutex_); + visualization_msgs::msg::MarkerArray GetTrajectoryNodeList(rclcpp::Time node_time); + visualization_msgs::msg::MarkerArray GetLandmarkPosesList(rclcpp::Time node_time); + visualization_msgs::msg::MarkerArray GetConstraintList(rclcpp::Time node_time); +@@ -107,13 +107,13 @@ class MapBuilderBridge { + const ::cartographer::common::Time time, + const ::cartographer::transform::Rigid3d local_pose, + ::cartographer::sensor::RangeData range_data_in_local) +- LOCKS_EXCLUDED(mutex_); ++ ABSL_LOCKS_EXCLUDED(mutex_); + + absl::Mutex mutex_; + const NodeOptions node_options_; + std::unordered_map> +- local_slam_data_ GUARDED_BY(mutex_); ++ local_slam_data_ ABSL_GUARDED_BY(mutex_); + std::unique_ptr map_builder_; + tf2_ros::Buffer* const tf_buffer_; + +diff --git a/include/cartographer_ros/metrics/internal/gauge.h b/include/cartographer_ros/metrics/internal/gauge.h +index f311ab1..26d0caf 100644 +--- a/include/cartographer_ros/metrics/internal/gauge.h ++++ b/include/cartographer_ros/metrics/internal/gauge.h +@@ -71,7 +71,7 @@ class Gauge : public ::cartographer::metrics::Gauge { + + absl::Mutex mutex_; + const std::map labels_; +- double value_ GUARDED_BY(mutex_); ++ double value_ ABSL_GUARDED_BY(mutex_); + }; + + } // namespace metrics +diff --git a/include/cartographer_ros/metrics/internal/histogram.h b/include/cartographer_ros/metrics/internal/histogram.h +index b5d8404..e47f99b 100644 +--- a/include/cartographer_ros/metrics/internal/histogram.h ++++ b/include/cartographer_ros/metrics/internal/histogram.h +@@ -50,8 +50,8 @@ class Histogram : public ::cartographer::metrics::Histogram { + absl::Mutex mutex_; + const std::map labels_; + const BucketBoundaries bucket_boundaries_; +- std::vector bucket_counts_ GUARDED_BY(mutex_); +- double sum_ GUARDED_BY(mutex_); ++ std::vector bucket_counts_ ABSL_GUARDED_BY(mutex_); ++ double sum_ ABSL_GUARDED_BY(mutex_); + }; + + } // namespace metrics +diff --git a/include/cartographer_ros/node.h b/include/cartographer_ros/node.h +index f3527ca..f9fb9fb 100644 +--- a/include/cartographer_ros/node.h ++++ b/include/cartographer_ros/node.h +@@ -168,7 +168,7 @@ class Node { + bool ValidateTrajectoryOptions(const TrajectoryOptions& options); + bool ValidateTopicNames(const TrajectoryOptions& options); + cartographer_ros_msgs::msg::StatusResponse FinishTrajectoryUnderLock( +- int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_); ++ int trajectory_id) ABSL_EXCLUSIVE_LOCKS_REQUIRED(mutex_); + void MaybeWarnAboutTopicMismatch(); + + // Helper function for service handlers that need to check trajectory states. +@@ -183,7 +183,7 @@ class Node { + + absl::Mutex mutex_; + std::unique_ptr metrics_registry_; +- std::shared_ptr map_builder_bridge_ GUARDED_BY(mutex_); ++ std::shared_ptr map_builder_bridge_ ABSL_GUARDED_BY(mutex_); + + rclcpp::Node::SharedPtr node_; + ::rclcpp::Publisher<::cartographer_ros_msgs::msg::SubmapList>::SharedPtr submap_list_publisher_; +diff --git a/src/occupancy_grid_node_main.cpp b/src/occupancy_grid_node_main.cpp +index 324426b..443dac2 100644 +--- a/src/occupancy_grid_node_main.cpp ++++ b/src/occupancy_grid_node_main.cpp +@@ -73,10 +73,10 @@ class Node : public rclcpp::Node + absl::Mutex mutex_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor_; +- ::rclcpp::Client::SharedPtr client_ GUARDED_BY(mutex_); +- ::rclcpp::Subscription::SharedPtr submap_list_subscriber_ GUARDED_BY(mutex_); +- ::rclcpp::Publisher<::nav_msgs::msg::OccupancyGrid>::SharedPtr occupancy_grid_publisher_ GUARDED_BY(mutex_); +- std::map submap_slices_ GUARDED_BY(mutex_); ++ ::rclcpp::Client::SharedPtr client_ ABSL_GUARDED_BY(mutex_); ++ ::rclcpp::Subscription::SharedPtr submap_list_subscriber_ ABSL_GUARDED_BY(mutex_); ++ ::rclcpp::Publisher<::nav_msgs::msg::OccupancyGrid>::SharedPtr occupancy_grid_publisher_ ABSL_GUARDED_BY(mutex_); ++ std::map submap_slices_ ABSL_GUARDED_BY(mutex_); + rclcpp::TimerBase::SharedPtr occupancy_grid_publisher_timer_; + std::string last_frame_id_; + rclcpp::Time last_timestamp_; +