diff --git a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc b/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc index 640ce76..34b3f20 100644 --- a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc +++ b/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc @@ -29,6 +29,7 @@ #include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/mapping/proto/submap.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" +#include "cartographer/mapping/value_conversion_tables.h" #include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/node_constants.h" #include "cartographer_ros/ros_log_sink.h" @@ -49,7 +50,8 @@ namespace cartographer_ros { namespace { std::unique_ptr LoadOccupancyGridMsg( - const std::string& pbstream_filename, const double resolution) { + const std::string& pbstream_filename, const double resolution, + ::cartographer::mapping::ValueConversionTables* conversion_lookup_tables) { ::cartographer::io::ProtoStreamReader reader(pbstream_filename); ::cartographer::io::ProtoStreamDeserializer deserializer(&reader); @@ -68,7 +70,8 @@ std::unique_ptr LoadOccupancyGridMsg( .trajectory(id.trajectory_id) .submap(id.submap_index) .pose()); - FillSubmapSlice(global_submap_pose, submap, &submap_slices[id]); + FillSubmapSlice(global_submap_pose, submap, &submap_slices[id], + conversion_lookup_tables); } } CHECK(reader.eof()); @@ -82,8 +85,9 @@ std::unique_ptr LoadOccupancyGridMsg( void Run(const std::string& pbstream_filename, const std::string& map_topic, const std::string& map_frame_id, const double resolution) { - std::unique_ptr msg_ptr = - LoadOccupancyGridMsg(pbstream_filename, resolution); + ::cartographer::mapping::ValueConversionTables conversion_lookup_tables; + std::unique_ptr msg_ptr = LoadOccupancyGridMsg( + pbstream_filename, resolution, &conversion_lookup_tables); ::ros::NodeHandle node_handle(""); ::ros::Publisher pub = node_handle.advertise( diff --git a/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc b/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc index b75fcc0..d3a29e4 100644 --- a/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc +++ b/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc @@ -51,6 +51,7 @@ void Run(const std::string& pbstream_filename, const std::string& map_filestem, std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice> submap_slices; ::cartographer::mapping::proto::SerializedData proto; + ::cartographer::mapping::ValueConversionTables conversion_lookup_tables; while (deserializer.ReadNextSerializedData(&proto)) { if (proto.has_submap()) { const auto& submap = proto.submap(); @@ -62,7 +63,8 @@ void Run(const std::string& pbstream_filename, const std::string& map_filestem, pose_graph.trajectory(id.trajectory_id) .submap(id.submap_index) .pose()); - FillSubmapSlice(global_submap_pose, submap, &submap_slices[id]); + FillSubmapSlice(global_submap_pose, submap, &submap_slices[id], + &conversion_lookup_tables); } } CHECK(reader.eof()); diff --git a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc index c25cb3b..be16ca2 100644 --- a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc +++ b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc @@ -34,8 +34,8 @@ RosMapWritingPointsProcessor::RosMapWritingPointsProcessor( next_(next), file_writer_factory_(file_writer_factory), range_data_inserter_(range_data_inserter_options), - probability_grid_(::cartographer::io::CreateProbabilityGrid(resolution)) { -} + probability_grid_(::cartographer::io::CreateProbabilityGrid( + resolution, &conversion_tables_)) {} std::unique_ptr RosMapWritingPointsProcessor::FromDictionary( diff --git a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h index 04dca75..71c165b 100644 --- a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h +++ b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h @@ -23,6 +23,7 @@ #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" #include "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping/value_conversion_tables.h" namespace cartographer_ros { @@ -59,6 +60,7 @@ class RosMapWritingPointsProcessor ::cartographer::io::FileWriterFactory file_writer_factory_; ::cartographer::mapping::ProbabilityGridRangeDataInserter2D range_data_inserter_; + ::cartographer::mapping::ValueConversionTables conversion_tables_; ::cartographer::mapping::ProbabilityGrid probability_grid_; };