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

Allow to ignore (un-)frozen submaps in the occupancy grid node. (#899)

master
Michael Grupp 2018-06-29 13:28:40 +02:00 committed by Wally B. Feed
parent 0e65aa55b9
commit f5b583fde8
3 changed files with 17 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -206,6 +206,8 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
for (const auto& submap_id_pose : for (const auto& submap_id_pose :
map_builder_->pose_graph()->GetAllSubmapPoses()) { map_builder_->pose_graph()->GetAllSubmapPoses()) {
cartographer_ros_msgs::SubmapEntry submap_entry; cartographer_ros_msgs::SubmapEntry submap_entry;
submap_entry.is_frozen = map_builder_->pose_graph()->IsTrajectoryFrozen(
submap_id_pose.id.trajectory_id);
submap_entry.trajectory_id = submap_id_pose.id.trajectory_id; submap_entry.trajectory_id = submap_id_pose.id.trajectory_id;
submap_entry.submap_index = submap_id_pose.id.submap_index; submap_entry.submap_index = submap_id_pose.id.submap_index;
submap_entry.submap_version = submap_id_pose.data.version; submap_entry.submap_version = submap_id_pose.data.version;

15
cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
View File

@ -40,6 +40,12 @@
DEFINE_double(resolution, 0.05, DEFINE_double(resolution, 0.05,
"Resolution of a grid cell in the published occupancy grid."); "Resolution of a grid cell in the published occupancy grid.");
DEFINE_double(publish_period_sec, 1.0, "OccupancyGrid publishing period."); DEFINE_double(publish_period_sec, 1.0, "OccupancyGrid publishing period.");
DEFINE_bool(include_frozen_submaps, true,
"Include frozen submaps in the occupancy grid.");
DEFINE_bool(include_unfrozen_submaps, true,
"Include unfrozen submaps in the occupancy grid.");
DEFINE_string(occupancy_grid_topic, cartographer_ros::kOccupancyGridTopic,
"Name of the topic on which the occupancy grid is published.");
namespace cartographer_ros { namespace cartographer_ros {
namespace { namespace {
@ -89,7 +95,7 @@ Node::Node(const double resolution, const double publish_period_sec)
}))), }))),
occupancy_grid_publisher_( occupancy_grid_publisher_(
node_handle_.advertise<::nav_msgs::OccupancyGrid>( node_handle_.advertise<::nav_msgs::OccupancyGrid>(
kOccupancyGridTopic, kLatestOnlyPublisherQueueSize, FLAGS_occupancy_grid_topic, kLatestOnlyPublisherQueueSize,
true /* latched */)), true /* latched */)),
occupancy_grid_publisher_timer_( occupancy_grid_publisher_timer_(
node_handle_.createWallTimer(::ros::WallDuration(publish_period_sec), node_handle_.createWallTimer(::ros::WallDuration(publish_period_sec),
@ -113,6 +119,10 @@ void Node::HandleSubmapList(
for (const auto& submap_msg : msg->submap) { for (const auto& submap_msg : msg->submap) {
const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index}; const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index};
submap_ids_to_delete.erase(id); submap_ids_to_delete.erase(id);
if ((submap_msg.is_frozen && !FLAGS_include_frozen_submaps) ||
(!submap_msg.is_frozen && !FLAGS_include_unfrozen_submaps)) {
continue;
}
SubmapSlice& submap_slice = submap_slices_[id]; SubmapSlice& submap_slice = submap_slices_[id];
submap_slice.pose = ToRigid3d(submap_msg.pose); submap_slice.pose = ToRigid3d(submap_msg.pose);
submap_slice.metadata_version = submap_msg.submap_version; submap_slice.metadata_version = submap_msg.submap_version;
@ -172,6 +182,9 @@ int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]); google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true); google::ParseCommandLineFlags(&argc, &argv, true);
CHECK(FLAGS_include_frozen_submaps || FLAGS_include_unfrozen_submaps)
<< "Ignoring both frozen and unfrozen submaps makes no sense.";
::ros::init(argc, argv, "cartographer_occupancy_grid_node"); ::ros::init(argc, argv, "cartographer_occupancy_grid_node");
::ros::start(); ::ros::start();

1
cartographer_ros_msgs/msg/SubmapEntry.msg
View File

@ -16,3 +16,4 @@ int32 trajectory_id
int32 submap_index int32 submap_index
int32 submap_version int32 submap_version
geometry_msgs/Pose pose geometry_msgs/Pose pose
bool is_frozen