12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Introduce value converter tables. (#937)

master
Kevin Daun 2018-07-13 00:52:49 +02:00 committed by Wally B. Feed
parent 54e8aa4f6d
commit 683129243e
4 changed files with 15 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc
View File

@ -29,6 +29,7 @@
#include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/mapping/proto/serialization.pb.h"
#include "cartographer/mapping/proto/submap.pb.h" #include "cartographer/mapping/proto/submap.pb.h"
#include "cartographer/mapping/proto/trajectory_builder_options.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/msg_conversion.h"
#include "cartographer_ros/node_constants.h" #include "cartographer_ros/node_constants.h"
#include "cartographer_ros/ros_log_sink.h" #include "cartographer_ros/ros_log_sink.h"
@ -49,7 +50,8 @@ namespace cartographer_ros {
namespace { namespace {
std::unique_ptr<nav_msgs::OccupancyGrid> LoadOccupancyGridMsg( std::unique_ptr<nav_msgs::OccupancyGrid> 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::ProtoStreamReader reader(pbstream_filename);
::cartographer::io::ProtoStreamDeserializer deserializer(&reader); ::cartographer::io::ProtoStreamDeserializer deserializer(&reader);
@ -68,7 +70,8 @@ std::unique_ptr<nav_msgs::OccupancyGrid> LoadOccupancyGridMsg(
.trajectory(id.trajectory_id) .trajectory(id.trajectory_id)
.submap(id.submap_index) .submap(id.submap_index)
.pose()); .pose());
FillSubmapSlice(global_submap_pose, submap, &submap_slices[id]); FillSubmapSlice(global_submap_pose, submap, &submap_slices[id],
conversion_lookup_tables);
} }
} }
CHECK(reader.eof()); CHECK(reader.eof());
@ -82,8 +85,9 @@ std::unique_ptr<nav_msgs::OccupancyGrid> LoadOccupancyGridMsg(
void Run(const std::string& pbstream_filename, const std::string& map_topic, void Run(const std::string& pbstream_filename, const std::string& map_topic,
const std::string& map_frame_id, const double resolution) { const std::string& map_frame_id, const double resolution) {
std::unique_ptr<nav_msgs::OccupancyGrid> msg_ptr = ::cartographer::mapping::ValueConversionTables conversion_lookup_tables;
LoadOccupancyGridMsg(pbstream_filename, resolution); std::unique_ptr<nav_msgs::OccupancyGrid> msg_ptr = LoadOccupancyGridMsg(
pbstream_filename, resolution, &conversion_lookup_tables);
::ros::NodeHandle node_handle(""); ::ros::NodeHandle node_handle("");
::ros::Publisher pub = node_handle.advertise<nav_msgs::OccupancyGrid>( ::ros::Publisher pub = node_handle.advertise<nav_msgs::OccupancyGrid>(

4
cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc
View File

@ -51,6 +51,7 @@ void Run(const std::string& pbstream_filename, const std::string& map_filestem,
std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice> std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice>
submap_slices; submap_slices;
::cartographer::mapping::proto::SerializedData proto; ::cartographer::mapping::proto::SerializedData proto;
::cartographer::mapping::ValueConversionTables conversion_lookup_tables;
while (deserializer.ReadNextSerializedData(&proto)) { while (deserializer.ReadNextSerializedData(&proto)) {
if (proto.has_submap()) { if (proto.has_submap()) {
const auto& submap = proto.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) pose_graph.trajectory(id.trajectory_id)
.submap(id.submap_index) .submap(id.submap_index)
.pose()); .pose());
FillSubmapSlice(global_submap_pose, submap, &submap_slices[id]); FillSubmapSlice(global_submap_pose, submap, &submap_slices[id],
&conversion_lookup_tables);
} }
} }
CHECK(reader.eof()); CHECK(reader.eof());

4
cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc
View File

@ -34,8 +34,8 @@ RosMapWritingPointsProcessor::RosMapWritingPointsProcessor(
next_(next), next_(next),
file_writer_factory_(file_writer_factory), file_writer_factory_(file_writer_factory),
range_data_inserter_(range_data_inserter_options), 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> std::unique_ptr<RosMapWritingPointsProcessor>
RosMapWritingPointsProcessor::FromDictionary( RosMapWritingPointsProcessor::FromDictionary(

2
cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h
View File

@ -23,6 +23,7 @@
#include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid.h"
#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.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/proto/2d/probability_grid_range_data_inserter_options_2d.pb.h"
#include "cartographer/mapping/value_conversion_tables.h"
namespace cartographer_ros { namespace cartographer_ros {
@ -59,6 +60,7 @@ class RosMapWritingPointsProcessor
::cartographer::io::FileWriterFactory file_writer_factory_; ::cartographer::io::FileWriterFactory file_writer_factory_;
::cartographer::mapping::ProbabilityGridRangeDataInserter2D ::cartographer::mapping::ProbabilityGridRangeDataInserter2D
range_data_inserter_; range_data_inserter_;
::cartographer::mapping::ValueConversionTables conversion_tables_;
::cartographer::mapping::ProbabilityGrid probability_grid_; ::cartographer::mapping::ProbabilityGrid probability_grid_;
}; };