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

Follow googlecartographer/cartographer#1353 (#959) 

FIXES=#944
master
gaschler 2018-07-31 16:22:29 +02:00 committed by Wally B. Feed
parent b353e1ed64
commit 7fbb628a21
4 changed files with 6 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc
View File

@ -25,16 +25,10 @@
#include "cartographer/mapping/2d/probability_grid.h"
#include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/3d/submap_3d.h"
#include "cartographer/mapping/proto/pose_graph.pb.h"
#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"
#include "cartographer_ros/ros_map.h"
#include "cartographer_ros/submap.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "nav_msgs/OccupancyGrid.h"
@ -57,24 +51,9 @@ std::unique_ptr<nav_msgs::OccupancyGrid> LoadOccupancyGridMsg(
LOG(INFO) << "Loading submap slices from serialized data.";
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() &&
(Has2DGrid(proto.submap()) || Has3DGrids(proto.submap()))) {
const auto& submap = proto.submap();
const ::cartographer::mapping::SubmapId id{
submap.submap_id().trajectory_id(),
submap.submap_id().submap_index()};
const ::cartographer::transform::Rigid3d global_submap_pose =
::cartographer::transform::ToRigid3(deserializer.pose_graph()
.trajectory(id.trajectory_id)
.submap(id.submap_index)
.pose());
FillSubmapSlice(global_submap_pose, submap, &submap_slices[id],
&conversion_lookup_tables);
}
}
::cartographer::mapping::ValueConversionTables conversion_tables;
::cartographer::io::DeserializeAndFillSubmapSlices(
&deserializer, &submap_slices, &conversion_tables);
CHECK(reader.eof());
LOG(INFO) << "Generating combined map image from submap slices.";

28
cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc
View File

@ -23,12 +23,7 @@
#include "cartographer/mapping/2d/probability_grid.h"
#include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/3d/submap_3d.h"
#include "cartographer/mapping/proto/pose_graph.pb.h"
#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_ros/ros_map.h"
#include "cartographer_ros/submap.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
@ -45,29 +40,12 @@ void Run(const std::string& pbstream_filename, const std::string& map_filestem,
::cartographer::io::ProtoStreamReader reader(pbstream_filename);
::cartographer::io::ProtoStreamDeserializer deserializer(&reader);
const auto& pose_graph = deserializer.pose_graph();
LOG(INFO) << "Loading submap slices from serialized data.";
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() &&
(Has2DGrid(proto.submap()) || Has3DGrids(proto.submap()))) {
const auto& submap = proto.submap();
const ::cartographer::mapping::SubmapId id{
submap.submap_id().trajectory_id(),
submap.submap_id().submap_index()};
const ::cartographer::transform::Rigid3d global_submap_pose =
::cartographer::transform::ToRigid3(
pose_graph.trajectory(id.trajectory_id)
.submap(id.submap_index)
.pose());
FillSubmapSlice(global_submap_pose, submap, &submap_slices[id],
&conversion_lookup_tables);
}
}
::cartographer::mapping::ValueConversionTables conversion_tables;
::cartographer::io::DeserializeAndFillSubmapSlices(
&deserializer, &submap_slices, &conversion_tables);
CHECK(reader.eof());
LOG(INFO) << "Generating combined map image from submap slices.";

10
cartographer_ros/cartographer_ros/submap.cc
View File

@ -52,14 +52,4 @@ std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures(
return response;
}
bool Has2DGrid(const ::cartographer::mapping::proto::Submap& submap) {
return submap.has_submap_2d() && submap.submap_2d().has_grid();
}
bool Has3DGrids(const ::cartographer::mapping::proto::Submap& submap) {
return submap.has_submap_3d() &&
submap.submap_3d().has_low_resolution_hybrid_grid() &&
submap.submap_3d().has_high_resolution_hybrid_grid();
}
} // namespace cartographer_ros

3
cartographer_ros/cartographer_ros/submap.h
View File

@ -35,9 +35,6 @@ std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures(
const ::cartographer::mapping::SubmapId& submap_id,
ros::ServiceClient* client);
bool Has2DGrid(const ::cartographer::mapping::proto::Submap& submap);
bool Has3DGrids(const ::cartographer::mapping::proto::Submap& submap);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H