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

Add support for publishing nav_msgs/OccupanyGrid messages. (#31) 

Adds an option publish_occupancy_grid (disabled by default) which continuously
computes and publishes OccupancyGrid in a background thread. Only the 2D case is
currently supported.

And small code style and typo fixes.
master
Wolfgang Hess 2016-08-30 13:54:31 +02:00 committed by GitHub
parent 058958e38c
commit 32be55f42c
7 changed files with 134 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer_ros/CMakeLists.txt
View File

@ -28,7 +28,7 @@ set(PACKAGE_DEPENDENCIES
tf2_eigen
)
set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS "-std=c++11 -Wreorder")
find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})
include_directories(${catkin_INCLUDE_DIRS})

1
cartographer_ros/configuration_files/backpack_2d.lua
View File

@ -23,6 +23,7 @@ options = {
tracking_frame = "base_link",
provide_odom_frame = true,
expect_odometry_data = false,
publish_occupancy_grid = false,
laser_min_range = 0.,
laser_max_range = 30.,
laser_missing_echo_ray_length = 5.,

1
cartographer_ros/configuration_files/backpack_3d.lua
View File

@ -23,6 +23,7 @@ options = {
tracking_frame = "base_link",
provide_odom_frame = true,
expect_odometry_data = false,
publish_occupancy_grid = false,
laser_min_range = 0.,
laser_max_range = 30.,
laser_missing_echo_ray_length = 5.,

1
cartographer_ros/configuration_files/turtlebot.lua
View File

@ -23,6 +23,7 @@ options = {
tracking_frame = "base_link",
provide_odom_frame = false,
expect_odometry_data = false,
publish_occupancy_grid = false,
laser_min_range = 0.,
laser_max_range = 30.,
laser_missing_echo_ray_length = 5.,

139
cartographer_ros/src/cartographer_node_main.cc
View File

@ -54,6 +54,7 @@
#include "gflags/gflags.h"
#include "glog/log_severity.h"
#include "glog/logging.h"
#include "nav_msgs/OccupancyGrid.h"
#include "nav_msgs/Odometry.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
@ -102,6 +103,7 @@ constexpr char kMultiEchoLaserScanTopic[] = "/echoes";
constexpr char kPointCloud2Topic[] = "/points2";
constexpr char kImuTopic[] = "/imu";
constexpr char kOdometryTopic[] = "/odom";
constexpr char kOccupancyGridTopic[] = "/map";
const string& CheckNoLeadingSlash(const string& frame_id) {
if (frame_id.size() > 0) {
@ -194,6 +196,7 @@ struct SensorData {
class Node {
public:
Node();
~Node();
Node(const Node&) = delete;
Node& operator=(const Node&) = delete;
@ -230,6 +233,7 @@ class Node {
void PublishSubmapList(int64 timestamp);
void PublishPose(int64 timestamp);
void SpinOccupancyGridThreadForever();
// TODO(hrapp): Pull out the common functionality between this and MapWriter
// into an open sourcable MapWriter.
@ -251,9 +255,7 @@ class Node {
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_;
tf2_ros::TransformBroadcaster tf_broadcaster_;
carto::common::ThreadPool thread_pool_;
int64 last_pose_publish_timestamp_;
carto::common::Mutex mutex_;
std::unique_ptr<carto::mapping::GlobalTrajectoryBuilderInterface>
trajectory_builder_ GUARDED_BY(mutex_);
@ -262,9 +264,17 @@ class Node {
std::unique_ptr<carto::mapping::SparsePoseGraph> sparse_pose_graph_;
::ros::Publisher submap_list_publisher_;
int64 last_submap_list_publish_timestamp_;
int64 last_submap_list_publish_timestamp_ = 0;
::ros::ServiceServer submap_query_server_;
tf2_ros::TransformBroadcaster tf_broadcaster_;
int64 last_pose_publish_timestamp_ = 0;
::ros::Publisher occupancy_grid_publisher_;
carto::mapping_2d::proto::SubmapsOptions submaps_options_;
std::thread occupancy_grid_thread_;
bool terminating_ = false GUARDED_BY(mutex_);
// Time at which we last logged the rates of incoming sensor data.
std::chrono::steady_clock::time_point last_sensor_data_rates_logging_time_;
std::map<string, carto::common::RateTimer<>> rate_timers_;
@ -274,9 +284,17 @@ Node::Node()
: node_handle_("~"),
tf_buffer_(ros::Duration(1000)),
tf_(tf_buffer_),
thread_pool_(10),
last_submap_list_publish_timestamp_(0),
last_pose_publish_timestamp_(0) {}
thread_pool_(10) {}
Node::~Node() {
{
carto::common::MutexLocker lock(&mutex_);
terminating_ = true;
}
if (occupancy_grid_thread_.joinable()) {
occupancy_grid_thread_.join();
}
}
Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time,
const string& frame_id) {
@ -290,15 +308,13 @@ void Node::OdometryMessageCallback(const nav_msgs::Odometry::ConstPtr& msg) {
msg->header.frame_id, ToRigid3d(msg->pose.pose),
ToPoseCovariance(msg->pose.covariance));
sensor_collator_.AddSensorData(
kTrajectoryId,
carto::common::ToUniversal(FromRos(msg->header.stamp)),
kTrajectoryId, carto::common::ToUniversal(FromRos(msg->header.stamp)),
kOdometryTopic, std::move(sensor_data));
}
void Node::AddOdometry(int64 timestamp, const string& frame_id,
const Rigid3d& pose, const PoseCovariance& covariance) {
const carto::common::Time time =
carto::common::FromUniversal(timestamp);
const carto::common::Time time = carto::common::FromUniversal(timestamp);
trajectory_builder_->AddOdometerPose(time, pose, covariance);
}
@ -464,6 +480,7 @@ void Node::Initialize() {
&thread_pool_, &constant_node_data_);
auto options = carto::mapping_2d::CreateLocalTrajectoryBuilderOptions(
lua_parameter_dictionary.GetDictionary("trajectory_builder").get());
submaps_options_ = options.submaps_options();
expect_imu_data = options.expect_imu_data();
trajectory_builder_ =
carto::common::make_unique<carto::mapping_2d::GlobalTrajectoryBuilder>(
@ -485,15 +502,17 @@ void Node::Initialize() {
})));
expected_sensor_identifiers.insert(topic);
}
auto sparse_pose_graph_3d = carto::common::make_unique<
carto::mapping_3d::SparsePoseGraph>(
auto sparse_pose_graph_3d =
carto::common::make_unique<carto::mapping_3d::SparsePoseGraph>(
carto::mapping::CreateSparsePoseGraphOptions(
lua_parameter_dictionary.GetDictionary("sparse_pose_graph").get()),
lua_parameter_dictionary.GetDictionary("sparse_pose_graph")
.get()),
&thread_pool_, &constant_node_data_);
trajectory_builder_ = carto::common::make_unique<
carto::mapping_3d::GlobalTrajectoryBuilder>(
trajectory_builder_ =
carto::common::make_unique<carto::mapping_3d::GlobalTrajectoryBuilder>(
carto::mapping_3d::CreateLocalTrajectoryBuilderOptions(
lua_parameter_dictionary.GetDictionary("trajectory_builder").get()),
lua_parameter_dictionary.GetDictionary("trajectory_builder")
.get()),
sparse_pose_graph_3d.get());
sparse_pose_graph_ = std::move(sparse_pose_graph_3d);
}
@ -524,6 +543,16 @@ void Node::Initialize() {
kSubmapListTopic, 10);
submap_query_server_ = node_handle_.advertiseService(
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this);
if (lua_parameter_dictionary.GetBool("publish_occupancy_grid")) {
CHECK_EQ(num_lasers_3d, 0)
<< "Publishing OccupancyGrids for 3D data is not yet supported";
occupancy_grid_publisher_ =
node_handle_.advertise<::nav_msgs::OccupancyGrid>(kOccupancyGridTopic,
1, true);
occupancy_grid_thread_ =
std::thread(&Node::SpinOccupancyGridThreadForever, this);
}
}
bool Node::HandleSubmapQuery(
@ -601,6 +630,7 @@ void Node::PublishSubmapList(const int64 timestamp) {
void Node::PublishPose(const int64 timestamp) {
const carto::common::Time time = carto::common::FromUniversal(timestamp);
carto::common::MutexLocker lock(&mutex_);
const Rigid3d tracking_to_local = trajectory_builder_->pose_estimate().pose;
const carto::mapping::Submaps* submaps = trajectory_builder_->submaps();
const Rigid3d local_to_map =
@ -613,7 +643,6 @@ void Node::PublishPose(const int64 timestamp) {
stamped_transform.child_frame_id = odom_frame_;
if (provide_odom_frame_) {
carto::common::MutexLocker lock(&mutex_);
stamped_transform.transform = ToGeometryMsgTransform(local_to_map);
tf_broadcaster_.sendTransform(stamped_transform);
@ -636,6 +665,82 @@ void Node::PublishPose(const int64 timestamp) {
last_pose_publish_timestamp_ = timestamp;
}
void Node::SpinOccupancyGridThreadForever() {
for (;;) {
{
carto::common::MutexLocker lock(&mutex_);
if (terminating_) {
return;
}
}
const auto trajectory_nodes = sparse_pose_graph_->GetTrajectoryNodes();
if (trajectory_nodes.empty()) {
std::this_thread::sleep_for(carto::common::FromMilliseconds(1000));
continue;
}
const carto::mapping_2d::MapLimits map_limits =
carto::mapping_2d::MapLimits::ComputeMapLimits(
submaps_options_.resolution(), trajectory_nodes);
carto::mapping_2d::ProbabilityGrid probability_grid(map_limits);
carto::mapping_2d::LaserFanInserter laser_fan_inserter(
submaps_options_.laser_fan_inserter_options());
for (const auto& node : trajectory_nodes) {
CHECK(node.constant_data->laser_fan_3d.returns.empty()); // No 3D yet.
laser_fan_inserter.Insert(
carto::sensor::TransformLaserFan(
node.constant_data->laser_fan,
carto::transform::Project2D(node.pose).cast<float>()),
&probability_grid);
}
::nav_msgs::OccupancyGrid occupancy_grid;
occupancy_grid.header.stamp = ToRos(trajectory_nodes.back().time());
occupancy_grid.header.frame_id = map_frame_;
occupancy_grid.info.map_load_time = occupancy_grid.header.stamp;
Eigen::Array2i offset;
carto::mapping_2d::CellLimits cell_limits;
probability_grid.ComputeCroppedLimits(&offset, &cell_limits);
const double resolution = probability_grid.limits().resolution();
occupancy_grid.info.resolution = resolution;
occupancy_grid.info.width = cell_limits.num_y_cells;
occupancy_grid.info.height = cell_limits.num_x_cells;
occupancy_grid.info.origin.position.x =
probability_grid.limits().max().x() -
(offset.y() + cell_limits.num_y_cells) * resolution;
occupancy_grid.info.origin.position.y =
probability_grid.limits().max().y() -
(offset.x() + cell_limits.num_x_cells) * resolution;
occupancy_grid.info.origin.position.z = 0.;
occupancy_grid.info.origin.orientation.w = 1.;
occupancy_grid.info.origin.orientation.x = 0.;
occupancy_grid.info.origin.orientation.y = 0.;
occupancy_grid.info.origin.orientation.z = 0.;
occupancy_grid.data.resize(
cell_limits.num_x_cells * cell_limits.num_y_cells, -1);
for (const Eigen::Array2i& xy_index :
carto::mapping_2d::XYIndexRangeIterator(cell_limits)) {
if (probability_grid.IsKnown(xy_index + offset)) {
const int value = carto::common::RoundToInt(
(probability_grid.GetProbability(xy_index + offset) -
carto::mapping::kMinProbability) *
100. / (carto::mapping::kMaxProbability -
carto::mapping::kMinProbability));
CHECK_LE(0, value);
CHECK_GE(100, value);
occupancy_grid.data[(cell_limits.num_x_cells - xy_index.x()) *
cell_limits.num_y_cells -
xy_index.y() - 1] = value;
}
}
occupancy_grid_publisher_.publish(occupancy_grid);
}
}
void Node::HandleSensorData(const int64 timestamp,
std::unique_ptr<SensorData> sensor_data) {
if (last_submap_list_publish_timestamp_ + kSubmapPublishPeriodInUts <

6
cartographer_ros/src/drawable_submap.cc
View File

@ -55,12 +55,12 @@ std::string GetSubmapIdentifier(const int trajectory_id, const int submap_id) {
DrawableSubmap::DrawableSubmap(const int submap_id, const int trajectory_id,
Ogre::SceneManager* const scene_manager)
: scene_manager_(scene_manager),
: submap_id_(submap_id),
trajectory_id_(trajectory_id),
scene_manager_(scene_manager),
scene_node_(scene_manager->getRootSceneNode()->createChildSceneNode()),
manual_object_(scene_manager->createManualObject(
kManualObjectPrefix + GetSubmapIdentifier(trajectory_id, submap_id))),
submap_id_(submap_id),
trajectory_id_(trajectory_id),
last_query_timestamp_(0) {
material_ = Ogre::MaterialManager::getSingleton().getByName(
kSubmapSourceMaterialName);

2
cartographer_ros/src/node_constants.h
View File

@ -19,7 +19,7 @@
namespace cartographer_ros {
// The topic that the node will subscrbe under.
// The topic that the node will subscribe to.
constexpr char kSubmapListTopic[] = "submap_list";
// The service we serve in the Node and query in the RViz plugin for submap