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

Pulls out GetOccupancyGrid to MapBuilderBridge. (#187)

master
Damon Kohler 2016-11-25 14:13:04 +01:00 committed by GitHub
parent 500b83ff0d
commit 012beb5c0c
4 changed files with 20 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer_ros/cartographer_ros/CMakeLists.txt
View File

@ -20,6 +20,7 @@ google_library(map_builder_bridge
assets_writer assets_writer
msg_conversion msg_conversion
node_options node_options
occupancy_grid
sensor_bridge sensor_bridge
tf_bridge tf_bridge
) )
@ -145,7 +146,6 @@ google_binary(cartographer_node
map_builder_bridge map_builder_bridge
msg_conversion msg_conversion
node_options node_options
occupancy_grid
ros_log_sink ros_log_sink
sensor_bridge sensor_bridge
tf_bridge tf_bridge

14
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -18,6 +18,7 @@
#include "cartographer_ros/assets_writer.h" #include "cartographer_ros/assets_writer.h"
#include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/occupancy_grid.h"
#include "cartographer_ros_msgs/TrajectorySubmapList.h" #include "cartographer_ros_msgs/TrajectorySubmapList.h"
namespace cartographer_ros { namespace cartographer_ros {
@ -115,4 +116,17 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
return submap_list; return submap_list;
} }
std::unique_ptr<nav_msgs::OccupancyGrid> MapBuilderBridge::BuildOccupancyGrid() {
const auto trajectory_nodes =
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
std::unique_ptr<nav_msgs::OccupancyGrid> occupancy_grid;
if (!trajectory_nodes.empty()) {
occupancy_grid =
cartographer::common::make_unique<nav_msgs::OccupancyGrid>();
cartographer_ros::BuildOccupancyGrid(trajectory_nodes, options_,
occupancy_grid.get());
}
return occupancy_grid;
}
} // namespace cartographer_ros } // namespace cartographer_ros

2
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -25,6 +25,7 @@
#include "cartographer_ros_msgs/SubmapEntry.h" #include "cartographer_ros_msgs/SubmapEntry.h"
#include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapList.h"
#include "cartographer_ros_msgs/SubmapQuery.h" #include "cartographer_ros_msgs/SubmapQuery.h"
#include "nav_msgs/OccupancyGrid.h"
namespace cartographer_ros { namespace cartographer_ros {
@ -46,6 +47,7 @@ class MapBuilderBridge {
cartographer_ros_msgs::FinishTrajectory::Response& response); cartographer_ros_msgs::FinishTrajectory::Response& response);
cartographer_ros_msgs::SubmapList GetSubmapList(); cartographer_ros_msgs::SubmapList GetSubmapList();
std::unique_ptr<nav_msgs::OccupancyGrid> BuildOccupancyGrid();
SensorBridge* sensor_bridge() { return sensor_bridge_.get(); } SensorBridge* sensor_bridge() { return sensor_bridge_.get(); }

13
cartographer_ros/cartographer_ros/node_main.cc
View File

@ -36,7 +36,6 @@
#include "cartographer_ros/map_builder_bridge.h" #include "cartographer_ros/map_builder_bridge.h"
#include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/node_options.h" #include "cartographer_ros/node_options.h"
#include "cartographer_ros/occupancy_grid.h"
#include "cartographer_ros/ros_log_sink.h" #include "cartographer_ros/ros_log_sink.h"
#include "cartographer_ros/sensor_bridge.h" #include "cartographer_ros/sensor_bridge.h"
#include "cartographer_ros/tf_bridge.h" #include "cartographer_ros/tf_bridge.h"
@ -48,7 +47,6 @@
#include "cartographer_ros_msgs/TrajectorySubmapList.h" #include "cartographer_ros_msgs/TrajectorySubmapList.h"
#include "gflags/gflags.h" #include "gflags/gflags.h"
#include "glog/logging.h" #include "glog/logging.h"
#include "nav_msgs/OccupancyGrid.h"
#include "nav_msgs/Odometry.h" #include "nav_msgs/Odometry.h"
#include "ros/ros.h" #include "ros/ros.h"
#include "ros/serialization.h" #include "ros/serialization.h"
@ -362,15 +360,10 @@ void Node::SpinOccupancyGridThreadForever() {
if (occupancy_grid_publisher_.getNumSubscribers() == 0) { if (occupancy_grid_publisher_.getNumSubscribers() == 0) {
continue; continue;
} }
const auto trajectory_nodes = map_builder_bridge_->map_builder() const auto occupancy_grid = map_builder_bridge_->BuildOccupancyGrid();
->sparse_pose_graph() if (occupancy_grid != nullptr) {
->GetTrajectoryNodes(); occupancy_grid_publisher_.publish(*occupancy_grid);
if (trajectory_nodes.empty()) {
continue;
} }
::nav_msgs::OccupancyGrid occupancy_grid;
BuildOccupancyGrid(trajectory_nodes, options_, &occupancy_grid);
occupancy_grid_publisher_.publish(occupancy_grid);
} }
} }