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

Enable OccupancyGridNode and remove generating of assets in the node. (#436) 

This will allow us to keep way less information in memory for SLAMing and fixes crashes related to multi trajectory (e.g. this fixes #413).

It also means that to get to an X-Ray or a map, users need to run the asset writer now after SLAMing, which is inconvenient.
Remove dependency on YAML and delete more dead code.
master
Holger Rapp 2017-07-21 17:19:47 +02:00 committed by GitHub
parent 1ea5f90b8b
commit 99da2f20d8
26 changed files with 47 additions and 652 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer_ros/CMakeLists.txt
View File

@ -44,7 +44,6 @@ google_enable_testing()
find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})
include(FindPkgConfig) include(FindPkgConfig)
pkg_search_module(YAMLCPP REQUIRED yaml-cpp>=0.5.1)
find_package(LuaGoogle REQUIRED) find_package(LuaGoogle REQUIRED)
find_package(PCL REQUIRED COMPONENTS common io) find_package(PCL REQUIRED COMPONENTS common io)
@ -80,7 +79,6 @@ catkin_package(
# TODO(damonkohler): This should be here but causes Catkin to abort because # TODO(damonkohler): This should be here but causes Catkin to abort because
# protobuf specifies a library '-lpthread' instead of just 'pthread'. # protobuf specifies a library '-lpthread' instead of just 'pthread'.
# CARTOGRAPHER # CARTOGRAPHER
YAMLCPP
PCL PCL
EIGEN3 EIGEN3
Boost Boost
@ -99,10 +97,6 @@ add_subdirectory("cartographer_ros")
target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) target_link_libraries(${PROJECT_NAME} PUBLIC cartographer)
# YAML
target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${YAMLCPP_INCLUDE_DIRS})
target_link_libraries(${PROJECT_NAME} PUBLIC ${YAMLCPP_LIBRARIES})
# Lua # Lua
target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR})

105
cartographer_ros/cartographer_ros/assets_writer.cc
View File

@ -1,105 +0,0 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer_ros/assets_writer.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/time.h"
#include "cartographer/io/file_writer.h"
#include "cartographer/io/null_points_processor.h"
#include "cartographer/io/ply_writing_points_processor.h"
#include "cartographer/io/points_processor.h"
#include "cartographer/io/xray_points_processor.h"
#include "cartographer/mapping/proto/trajectory.pb.h"
#include "cartographer/mapping_2d/proto/range_data_inserter_options.pb.h"
#include "cartographer_ros/map_writer.h"
#include "cartographer_ros/occupancy_grid.h"
#include "nav_msgs/OccupancyGrid.h"
namespace cartographer_ros {
bool HasNonTrimmedNode(
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes) {
for (const auto& trajectory_nodes : all_trajectory_nodes) {
for (const auto& node : trajectory_nodes) {
if (!node.trimmed()) {
return true;
}
}
}
return false;
}
void Write2DAssets(
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes,
const string& map_frame,
const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
const std::string& stem) {
::nav_msgs::OccupancyGrid occupancy_grid;
BuildOccupancyGrid2D(all_trajectory_nodes, map_frame, submaps_options,
&occupancy_grid);
WriteOccupancyGridToPgmAndYaml(occupancy_grid, stem);
}
void Write3DAssets(
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes,
const double voxel_size, const std::string& stem) {
namespace carto = ::cartographer;
const auto file_writer_factory = [](const string& filename) {
return carto::common::make_unique<carto::io::StreamFileWriter>(filename);
};
carto::io::NullPointsProcessor null_points_processor;
carto::io::XRayPointsProcessor xy_xray_points_processor(
voxel_size,
carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())),
{}, stem + "_xray_xy", file_writer_factory, &null_points_processor);
carto::io::XRayPointsProcessor yz_xray_points_processor(
voxel_size,
carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())),
{}, stem + "_xray_yz", file_writer_factory, &xy_xray_points_processor);
carto::io::XRayPointsProcessor xz_xray_points_processor(
voxel_size,
carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
{}, stem + "_xray_xz", file_writer_factory, &yz_xray_points_processor);
carto::io::PlyWritingPointsProcessor ply_writing_points_processor(
file_writer_factory(stem + ".ply"), &xz_xray_points_processor);
for (size_t trajectory_id = 0; trajectory_id < all_trajectory_nodes.size();
++trajectory_id) {
for (const auto& node : all_trajectory_nodes[trajectory_id]) {
const carto::sensor::RangeData range_data =
carto::sensor::TransformRangeData(
carto::sensor::Decompress(node.constant_data->range_data),
node.pose.cast<float>());
auto points_batch = carto::common::make_unique<carto::io::PointsBatch>();
points_batch->time = node.time();
points_batch->origin = range_data.origin;
points_batch->trajectory_id = trajectory_id;
points_batch->points = range_data.returns;
ply_writing_points_processor.Process(std::move(points_batch));
}
}
ply_writing_points_processor.Flush();
}
} // namespace cartographer_ros

51
cartographer_ros/cartographer_ros/assets_writer.h
View File

@ -1,51 +0,0 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_ASSETS_WRITER_H_
#define CARTOGRAPHER_ROS_ASSETS_WRITER_H_
#include <string>
#include <vector>
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer_ros/node_options.h"
namespace cartographer_ros {
// Returns 'true' if there is at least one untrimmed node for any trajectory.
// The Write?DAssets functions expects this to be 'true'.
bool HasNonTrimmedNode(
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes);
// Writes a trajectory proto and an occupancy grid.
void Write2DAssets(
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes,
const string& map_frame,
const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
const std::string& stem);
// Writes X-ray images, trajectory proto, and PLY files from the
// 'all_trajectory_nodes'. The filenames will all start with 'stem'.
void Write3DAssets(
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes,
const double voxel_size, const std::string& stem);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_ASSETS_WRITER_H_

60
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -17,10 +17,8 @@
#include "cartographer_ros/map_builder_bridge.h" #include "cartographer_ros/map_builder_bridge.h"
#include "cartographer/io/proto_stream.h" #include "cartographer/io/proto_stream.h"
#include "cartographer_ros/assets_writer.h"
#include "cartographer_ros/color.h" #include "cartographer_ros/color.h"
#include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/occupancy_grid.h"
namespace cartographer_ros { namespace cartographer_ros {
@ -70,44 +68,12 @@ void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
sensor_bridges_.erase(trajectory_id); sensor_bridges_.erase(trajectory_id);
} }
void MapBuilderBridge::SerializeState(const std::string& stem) { void MapBuilderBridge::SerializeState(const std::string& filename) {
cartographer::io::ProtoStreamWriter writer(stem + ".pbstream"); cartographer::io::ProtoStreamWriter writer(filename);
map_builder_.SerializeState(&writer); map_builder_.SerializeState(&writer);
CHECK(writer.Close()) << "Could not write state."; CHECK(writer.Close()) << "Could not write state.";
} }
void MapBuilderBridge::WriteAssets(const string& stem) {
const auto all_trajectory_nodes =
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
if (!HasNonTrimmedNode(all_trajectory_nodes)) {
LOG(WARNING) << "No data was collected and no assets will be written.";
return;
}
// Make sure there is a trajectory with id = 0.
CHECK_EQ(trajectory_options_.count(0), 1);
LOG(INFO) << "Writing assets with stem '" << stem << "'...";
if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
// We arbitrarily use the submap_options() from the first trajectory to
// write the 2D assets.
Write2DAssets(
all_trajectory_nodes, node_options_.map_frame,
trajectory_options_[0]
.trajectory_builder_options.trajectory_builder_2d_options()
.submaps_options(),
stem);
}
if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
Write3DAssets(
all_trajectory_nodes,
trajectory_options_[0]
.trajectory_builder_options.trajectory_builder_3d_options()
.submaps_options()
.high_resolution(),
stem);
}
}
bool MapBuilderBridge::HandleSubmapQuery( bool MapBuilderBridge::HandleSubmapQuery(
cartographer_ros_msgs::SubmapQuery::Request& request, cartographer_ros_msgs::SubmapQuery::Request& request,
cartographer_ros_msgs::SubmapQuery::Response& response) { cartographer_ros_msgs::SubmapQuery::Response& response) {
@ -157,28 +123,6 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
return submap_list; return submap_list;
} }
std::unique_ptr<nav_msgs::OccupancyGrid>
MapBuilderBridge::BuildOccupancyGrid() {
CHECK(node_options_.map_builder_options.use_trajectory_builder_2d())
<< "Publishing OccupancyGrids for 3D data is not yet supported.";
const auto all_trajectory_nodes =
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
std::unique_ptr<nav_msgs::OccupancyGrid> occupancy_grid;
if (HasNonTrimmedNode(all_trajectory_nodes)) {
occupancy_grid =
cartographer::common::make_unique<nav_msgs::OccupancyGrid>();
// Make sure there is a trajectory with id = 0.
CHECK_EQ(trajectory_options_.count(0), 1);
BuildOccupancyGrid2D(
all_trajectory_nodes, node_options_.map_frame,
trajectory_options_[0]
.trajectory_builder_options.trajectory_builder_2d_options()
.submaps_options(),
occupancy_grid.get());
}
return occupancy_grid;
}
std::unordered_map<int, MapBuilderBridge::TrajectoryState> std::unordered_map<int, MapBuilderBridge::TrajectoryState>
MapBuilderBridge::GetTrajectoryStates() { MapBuilderBridge::GetTrajectoryStates() {
std::unordered_map<int, TrajectoryState> trajectory_states; std::unordered_map<int, TrajectoryState> trajectory_states;

4
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -54,15 +54,13 @@ class MapBuilderBridge {
int AddTrajectory(const std::unordered_set<string>& expected_sensor_ids, int AddTrajectory(const std::unordered_set<string>& expected_sensor_ids,
const TrajectoryOptions& trajectory_options); const TrajectoryOptions& trajectory_options);
void FinishTrajectory(int trajectory_id); void FinishTrajectory(int trajectory_id);
void SerializeState(const string& stem); void SerializeState(const string& filename);
void WriteAssets(const string& stem);
bool HandleSubmapQuery( bool HandleSubmapQuery(
cartographer_ros_msgs::SubmapQuery::Request& request, cartographer_ros_msgs::SubmapQuery::Request& request,
cartographer_ros_msgs::SubmapQuery::Response& response); cartographer_ros_msgs::SubmapQuery::Response& response);
cartographer_ros_msgs::SubmapList GetSubmapList(); cartographer_ros_msgs::SubmapList GetSubmapList();
std::unique_ptr<nav_msgs::OccupancyGrid> BuildOccupancyGrid();
std::unordered_map<int, TrajectoryState> GetTrajectoryStates(); std::unordered_map<int, TrajectoryState> GetTrajectoryStates();
visualization_msgs::MarkerArray GetTrajectoryNodeList(); visualization_msgs::MarkerArray GetTrajectoryNodeList();
visualization_msgs::MarkerArray GetConstraintList(); visualization_msgs::MarkerArray GetConstraintList();

89
cartographer_ros/cartographer_ros/map_writer.cc
View File

@ -1,89 +0,0 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer_ros/map_writer.h"
#include <fstream>
#include "glog/logging.h"
#include "yaml-cpp/yaml.h"
namespace cartographer_ros {
namespace {
void WriteOccupancyGridToPgm(const ::nav_msgs::OccupancyGrid& grid,
const std::string& filename) {
LOG(INFO) << "Saving map to '" << filename << "'...";
std::ofstream pgm_file(filename, std::ios::out | std::ios::binary);
const std::string header = "P5\n# Cartographer map; " +
std::to_string(grid.info.resolution) +
" m/pixel\n" + std::to_string(grid.info.width) +
" " + std::to_string(grid.info.height) + "\n255\n";
pgm_file.write(header.data(), header.size());
for (size_t y = 0; y < grid.info.height; ++y) {
for (size_t x = 0; x < grid.info.width; ++x) {
const size_t i = x + (grid.info.height - y - 1) * grid.info.width;
if (grid.data[i] >= 0 && grid.data[i] <= 100) {
pgm_file.put((100 - grid.data[i]) * 255 / 100);
} else {
// We choose a value between the free and occupied threshold.
constexpr uint8_t kUnknownValue = 128;
pgm_file.put(kUnknownValue);
}
}
}
pgm_file.close();
CHECK(pgm_file) << "Writing '" << filename << "' failed.";
}
void WriteOccupancyGridInfoToYaml(const ::nav_msgs::OccupancyGrid& grid,
const std::string& map_filename,
const std::string& yaml_filename) {
LOG(INFO) << "Saving map info to '" << yaml_filename << "'...";
std::ofstream yaml_file(yaml_filename, std::ios::out | std::ios::binary);
{
YAML::Emitter out(yaml_file);
out << YAML::BeginMap;
// TODO(whess): Use basename only?
out << YAML::Key << "image" << YAML::Value << map_filename;
out << YAML::Key << "resolution" << YAML::Value << grid.info.resolution;
// According to map_server documentation "many parts of the system currently
// ignore yaw" so it is good we use a zero value.
constexpr double kYawButMaybeIgnored = 0.;
out << YAML::Key << "origin" << YAML::Value << YAML::Flow << YAML::BeginSeq
<< grid.info.origin.position.x << grid.info.origin.position.y
<< kYawButMaybeIgnored << YAML::EndSeq;
out << YAML::Key << "occupied_thresh" << YAML::Value << 0.51;
out << YAML::Key << "free_thresh" << YAML::Value << 0.49;
out << YAML::Key << "negate" << YAML::Value << 0;
out << YAML::EndMap;
CHECK(out.good()) << out.GetLastError();
}
yaml_file.close();
CHECK(yaml_file) << "Writing '" << yaml_filename << "' failed.";
}
} // namespace
void WriteOccupancyGridToPgmAndYaml(
const ::nav_msgs::OccupancyGrid& occupancy_grid, const std::string& stem) {
const std::string pgm_filename = stem + ".pgm";
WriteOccupancyGridToPgm(occupancy_grid, pgm_filename);
WriteOccupancyGridInfoToYaml(occupancy_grid, pgm_filename, stem + ".yaml");
}
} // namespace cartographer_ros

32
cartographer_ros/cartographer_ros/map_writer.h
View File

@ -1,32 +0,0 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_MAP_WRITER_H_
#define CARTOGRAPHER_ROS_MAP_WRITER_H_
#include <string>
#include "nav_msgs/OccupancyGrid.h"
namespace cartographer_ros {
// Writes the given 'occupancy_grid' as 'stem'.pgm and 'stem'.yaml.
void WriteOccupancyGridToPgmAndYaml(
const ::nav_msgs::OccupancyGrid& occupancy_grid, const std::string& stem);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_MAP_WRITER_H_

49
cartographer_ros/cartographer_ros/node.cc
View File

@ -68,16 +68,7 @@ Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer)
service_servers_.push_back(node_handle_.advertiseService( service_servers_.push_back(node_handle_.advertiseService(
kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this)); kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService( service_servers_.push_back(node_handle_.advertiseService(
kWriteAssetsServiceName, &Node::HandleWriteAssets, this)); kWriteStateServiceName, &Node::HandleWriteState, this));
if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
occupancy_grid_publisher_ =
node_handle_.advertise<::nav_msgs::OccupancyGrid>(
kOccupancyGridTopic, kLatestOnlyPublisherQueueSize,
true /* latched */);
occupancy_grid_thread_ =
std::thread(&Node::SpinOccupancyGridThreadForever, this);
}
scan_matched_point_cloud_publisher_ = scan_matched_point_cloud_publisher_ =
node_handle_.advertise<sensor_msgs::PointCloud2>( node_handle_.advertise<sensor_msgs::PointCloud2>(
@ -97,15 +88,7 @@ Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer)
&Node::PublishConstraintList, this)); &Node::PublishConstraintList, this));
} }
Node::~Node() { Node::~Node() {}
{
carto::common::MutexLocker lock(&mutex_);
terminating_ = true;
}
if (occupancy_grid_thread_.joinable()) {
occupancy_grid_thread_.join();
}
}
::ros::NodeHandle* Node::node_handle() { return &node_handle_; } ::ros::NodeHandle* Node::node_handle() { return &node_handle_; }
@ -201,25 +184,6 @@ void Node::PublishConstraintList(
} }
} }
void Node::SpinOccupancyGridThreadForever() {
for (;;) {
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
{
carto::common::MutexLocker lock(&mutex_);
if (terminating_) {
return;
}
}
if (occupancy_grid_publisher_.getNumSubscribers() == 0) {
continue;
}
const auto occupancy_grid = map_builder_bridge_.BuildOccupancyGrid();
if (occupancy_grid != nullptr) {
occupancy_grid_publisher_.publish(*occupancy_grid);
}
}
}
std::unordered_set<string> Node::ComputeExpectedTopics( std::unordered_set<string> Node::ComputeExpectedTopics(
const TrajectoryOptions& options, const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics) { const cartographer_ros_msgs::SensorTopics& topics) {
@ -429,12 +393,11 @@ bool Node::HandleFinishTrajectory(
return true; return true;
} }
bool Node::HandleWriteAssets( bool Node::HandleWriteState(
::cartographer_ros_msgs::WriteAssets::Request& request, ::cartographer_ros_msgs::WriteState::Request& request,
::cartographer_ros_msgs::WriteAssets::Response& response) { ::cartographer_ros_msgs::WriteState::Response& response) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
map_builder_bridge_.SerializeState(request.stem); map_builder_bridge_.SerializeState(request.filename);
map_builder_bridge_.WriteAssets(request.stem);
return true; return true;
} }

11
cartographer_ros/cartographer_ros/node.h
View File

@ -34,7 +34,7 @@
#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 "cartographer_ros_msgs/TrajectoryOptions.h" #include "cartographer_ros_msgs/TrajectoryOptions.h"
#include "cartographer_ros_msgs/WriteAssets.h" #include "cartographer_ros_msgs/WriteState.h"
#include "ros/ros.h" #include "ros/ros.h"
#include "tf2_ros/transform_broadcaster.h" #include "tf2_ros/transform_broadcaster.h"
@ -71,9 +71,9 @@ class Node {
bool HandleFinishTrajectory( bool HandleFinishTrajectory(
cartographer_ros_msgs::FinishTrajectory::Request& request, cartographer_ros_msgs::FinishTrajectory::Request& request,
cartographer_ros_msgs::FinishTrajectory::Response& response); cartographer_ros_msgs::FinishTrajectory::Response& response);
bool HandleWriteAssets( bool HandleWriteState(
cartographer_ros_msgs::WriteAssets::Request& request, cartographer_ros_msgs::WriteState::Request& request,
cartographer_ros_msgs::WriteAssets::Response& response); cartographer_ros_msgs::WriteState::Response& response);
// Returns the set of topic names we want to subscribe to. // Returns the set of topic names we want to subscribe to.
std::unordered_set<string> ComputeExpectedTopics( std::unordered_set<string> ComputeExpectedTopics(
const TrajectoryOptions& options, const TrajectoryOptions& options,
@ -113,9 +113,6 @@ class Node {
std::unordered_map<int, std::vector<::ros::Subscriber>> subscribers_; std::unordered_map<int, std::vector<::ros::Subscriber>> subscribers_;
std::unordered_set<std::string> subscribed_topics_; std::unordered_set<std::string> subscribed_topics_;
std::unordered_map<int, bool> is_active_trajectory_ GUARDED_BY(mutex_); std::unordered_map<int, bool> is_active_trajectory_ GUARDED_BY(mutex_);
::ros::Publisher occupancy_grid_publisher_;
std::thread occupancy_grid_thread_;
bool terminating_ = false GUARDED_BY(mutex_);
// We have to keep the timer handles of ::ros::WallTimers around, otherwise // We have to keep the timer handles of ::ros::WallTimers around, otherwise
// they do not fire. // they do not fire.

2
cartographer_ros/cartographer_ros/node_constants.h
View File

@ -34,7 +34,7 @@ constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2";
constexpr char kSubmapListTopic[] = "submap_list"; constexpr char kSubmapListTopic[] = "submap_list";
constexpr char kSubmapQueryServiceName[] = "submap_query"; constexpr char kSubmapQueryServiceName[] = "submap_query";
constexpr char kStartTrajectoryServiceName[] = "start_trajectory"; constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
constexpr char kWriteAssetsServiceName[] = "write_assets"; constexpr char kWriteStateServiceName[] = "write_state";
constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list"; constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list";
constexpr char kConstraintListTopic[] = "constraint_list"; constexpr char kConstraintListTopic[] = "constraint_list";
constexpr double kConstraintPublishPeriodSec = 0.5; constexpr double kConstraintPublishPeriodSec = 0.5;

146
cartographer_ros/cartographer_ros/occupancy_grid.cc
View File

@ -1,146 +0,0 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer_ros/occupancy_grid.h"
#include "cartographer/common/port.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/range_data_inserter.h"
#include "cartographer_ros/time_conversion.h"
#include "glog/logging.h"
namespace {
Eigen::AlignedBox2f ComputeMapBoundingBox2D(
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes) {
Eigen::AlignedBox2f bounding_box(Eigen::Vector2f::Zero());
for (const auto& trajectory_nodes : all_trajectory_nodes) {
for (const auto& node : trajectory_nodes) {
if (node.trimmed()) {
continue;
}
const auto& data = *node.constant_data;
::cartographer::sensor::RangeData range_data;
range_data = ::cartographer::sensor::TransformRangeData(
Decompress(data.range_data), node.pose.cast<float>());
bounding_box.extend(range_data.origin.head<2>());
for (const Eigen::Vector3f& hit : range_data.returns) {
bounding_box.extend(hit.head<2>());
}
for (const Eigen::Vector3f& miss : range_data.misses) {
bounding_box.extend(miss.head<2>());
}
}
}
return bounding_box;
}
} // namespace
namespace cartographer_ros {
void BuildOccupancyGrid2D(
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes,
const string& map_frame,
const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
::nav_msgs::OccupancyGrid* const occupancy_grid) {
namespace carto = ::cartographer;
const carto::mapping_2d::MapLimits map_limits =
ComputeMapLimits(submaps_options.resolution(), all_trajectory_nodes);
carto::mapping_2d::ProbabilityGrid probability_grid(map_limits);
carto::mapping_2d::RangeDataInserter range_data_inserter(
submaps_options.range_data_inserter_options());
carto::common::Time latest_time = carto::common::Time::min();
for (const auto& trajectory_nodes : all_trajectory_nodes) {
for (const auto& node : trajectory_nodes) {
if (node.trimmed()) {
continue;
}
latest_time = std::max(latest_time, node.time());
range_data_inserter.Insert(carto::sensor::TransformRangeData(
Decompress(node.constant_data->range_data),
node.pose.cast<float>()),
&probability_grid);
}
}
CHECK(latest_time != carto::common::Time::min());
occupancy_grid->header.stamp = ToRos(latest_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;
}
}
}
::cartographer::mapping_2d::MapLimits ComputeMapLimits(
const double resolution,
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes) {
Eigen::AlignedBox2f bounding_box =
ComputeMapBoundingBox2D(all_trajectory_nodes);
// Add some padding to ensure all rays are still contained in the map after
// discretization.
const float kPadding = 3.f * resolution;
bounding_box.min() -= kPadding * Eigen::Vector2f::Ones();
bounding_box.max() += kPadding * Eigen::Vector2f::Ones();
const Eigen::Vector2d pixel_sizes =
bounding_box.sizes().cast<double>() / resolution;
return ::cartographer::mapping_2d::MapLimits(
resolution, bounding_box.max().cast<double>(),
::cartographer::mapping_2d::CellLimits(
::cartographer::common::RoundToInt(pixel_sizes.y()),
::cartographer::common::RoundToInt(pixel_sizes.x())));
}
} // namespace cartographer_ros

47
cartographer_ros/cartographer_ros/occupancy_grid.h
View File

@ -1,47 +0,0 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_OCCUPANCY_GRID_H_
#define CARTOGRAPHER_ROS_OCCUPANCY_GRID_H_
#include <vector>
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/mapping_2d/map_limits.h"
#include "cartographer/mapping_2d/proto/submaps_options.pb.h"
#include "nav_msgs/OccupancyGrid.h"
namespace cartographer_ros {
void BuildOccupancyGrid2D(
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes,
const string& map_frame,
const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
::nav_msgs::OccupancyGrid* const occupancy_grid);
// Computes MapLimits that contain the origin, and all rays (both returns and
// misses) in the 'trajectory_nodes'.
::cartographer::mapping_2d::MapLimits ComputeMapLimits(
double resolution,
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_OCCUPANCY_GRID_H_

54
cartographer_ros/cartographer_ros/occupancy_grid_test.cc
View File

@ -1,54 +0,0 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer_ros/occupancy_grid.h"
#include <memory>
#include "cartographer/common/time.h"
#include "cartographer/sensor/range_data.h"
#include "gtest/gtest.h"
#include "ros/ros.h"
namespace cartographer_ros {
namespace {
TEST(OccupancyGridTest, ComputeMapLimits) {
using ::cartographer::mapping::TrajectoryNode;
const TrajectoryNode trajectory_node{
std::make_shared<TrajectoryNode::Data>(TrajectoryNode::Data{
::cartographer::common::FromUniversal(52),
::cartographer::sensor::Compress(::cartographer::sensor::RangeData{
Eigen::Vector3f::Zero(),
{Eigen::Vector3f(-30.f, 1.f, 0.f),
Eigen::Vector3f(50.f, -10.f, 0.f)},
{}}),
::cartographer::transform::Rigid3d::Identity()}),
::cartographer::transform::Rigid3d::Identity()};
constexpr double kResolution = 0.05;
const ::cartographer::mapping_2d::MapLimits limits =
ComputeMapLimits(kResolution, {{trajectory_node}});
constexpr float kPaddingAwareTolerance = 5 * kResolution;
EXPECT_NEAR(50.f, limits.max().x(), kPaddingAwareTolerance);
EXPECT_NEAR(1.f, limits.max().y(), kPaddingAwareTolerance);
EXPECT_LT(200, limits.cell_limits().num_x_cells);
EXPECT_LT(1600, limits.cell_limits().num_y_cells);
EXPECT_GT(400, limits.cell_limits().num_x_cells);
EXPECT_GT(2000, limits.cell_limits().num_y_cells);
}
} // namespace
} // namespace cartographer_ros

3
cartographer_ros/cartographer_ros/offline_node_main.cc
View File

@ -273,8 +273,7 @@ void Run(const std::vector<string>& bag_filenames) {
<< (cpu_timespec.tv_sec + 1e-9 * cpu_timespec.tv_nsec) << " s"; << (cpu_timespec.tv_sec + 1e-9 * cpu_timespec.tv_nsec) << " s";
#endif #endif
node.map_builder_bridge()->SerializeState(bag_filenames.front()); node.map_builder_bridge()->SerializeState(bag_filenames.front() + ".pbstream");
node.map_builder_bridge()->WriteAssets(bag_filenames.front());
} }
} // namespace } // namespace

3
cartographer_ros/launch/backpack_2d.launch
View File

@ -28,4 +28,7 @@
output="screen"> output="screen">
<remap from="echoes" to="horizontal_laser_2d" /> <remap from="echoes" to="horizontal_laser_2d" />
</node> </node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch> </launch>

3
cartographer_ros/launch/backpack_3d.launch
View File

@ -29,4 +29,7 @@
<remap from="points2_1" to="horizontal_laser_3d" /> <remap from="points2_1" to="horizontal_laser_3d" />
<remap from="points2_2" to="vertical_laser_3d" /> <remap from="points2_2" to="vertical_laser_3d" />
</node> </node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch> </launch>

3
cartographer_ros/launch/demo_backpack_2d_localization.launch
View File

@ -32,6 +32,9 @@
<remap from="echoes" to="horizontal_laser_2d" /> <remap from="echoes" to="horizontal_laser_2d" />
</node> </node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
<node name="rviz" pkg="rviz" type="rviz" required="true" <node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" /> args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
<node name="playbag" pkg="rosbag" type="play" <node name="playbag" pkg="rosbag" type="play"

3
cartographer_ros/launch/demo_pr2.launch
View File

@ -26,6 +26,9 @@
<remap from="scan" to="/base_scan" /> <remap from="scan" to="/base_scan" />
</node> </node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
<node name="tf_remove_frames" pkg="cartographer_ros" <node name="tf_remove_frames" pkg="cartographer_ros"
type="tf_remove_frames.py"> type="tf_remove_frames.py">
<remap from="tf_out" to="/tf" /> <remap from="tf_out" to="/tf" />

3
cartographer_ros/launch/demo_revo_lds.launch
View File

@ -25,6 +25,9 @@
<remap from="scan" to="horizontal_laser_2d" /> <remap from="scan" to="horizontal_laser_2d" />
</node> </node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
<node name="rviz" pkg="rviz" type="rviz" required="true" <node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" /> args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
<node name="playbag" pkg="rosbag" type="play" <node name="playbag" pkg="rosbag" type="play"

3
cartographer_ros/launch/offline_backpack_2d.launch
View File

@ -29,4 +29,7 @@
output="screen"> output="screen">
<remap from="echoes" to="horizontal_laser_2d" /> <remap from="echoes" to="horizontal_laser_2d" />
</node> </node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch> </launch>

3
cartographer_ros/launch/offline_backpack_3d.launch
View File

@ -30,4 +30,7 @@
<remap from="points2_1" to="horizontal_laser_3d" /> <remap from="points2_1" to="horizontal_laser_3d" />
<remap from="points2_2" to="vertical_laser_3d" /> <remap from="points2_2" to="vertical_laser_3d" />
</node> </node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch> </launch>

3
cartographer_ros/launch/taurob_tracker.launch
View File

@ -23,4 +23,7 @@
<remap from="scan" to="/spin_laser/scan" /> <remap from="scan" to="/spin_laser/scan" />
<remap from="imu" to="/imu/data" /> <remap from="imu" to="/imu/data" />
</node> </node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch> </launch>

1
cartographer_ros/package.xml
View File

@ -59,7 +59,6 @@
<depend>tf2_ros</depend> <depend>tf2_ros</depend>
<depend>urdf</depend> <depend>urdf</depend>
<depend>visualization_msgs</depend> <depend>visualization_msgs</depend>
<depend>yaml-cpp</depend>
<test_depend>rosunit</test_depend> <test_depend>rosunit</test_depend>

2
cartographer_ros_msgs/CMakeLists.txt
View File

@ -35,7 +35,7 @@ add_service_files(
SubmapQuery.srv SubmapQuery.srv
FinishTrajectory.srv FinishTrajectory.srv
StartTrajectory.srv StartTrajectory.srv
WriteAssets.srv WriteState.srv
) )
generate_messages( generate_messages(

2
cartographer_ros_msgs/srv/WriteAssets.srv → cartographer_ros_msgs/srv/WriteState.srv
View File

@ -12,5 +12,5 @@
# See the License for the specific language governing permissions and # See the License for the specific language governing permissions and
# limitations under the License. # limitations under the License.
string stem string filename
--- ---

11
docs/source/ros_api.rst
View File

@ -99,10 +99,11 @@ start_trajectory (`cartographer_ros_msgs/StartTrajectory`_)
finish_trajectory (`cartographer_ros_msgs/FinishTrajectory`_) finish_trajectory (`cartographer_ros_msgs/FinishTrajectory`_)
Finishes the given `trajectory_id`'s trajectory by running a final optimization. Finishes the given `trajectory_id`'s trajectory by running a final optimization.
write_assets (`cartographer_ros_msgs/WriteAssets`_) write_state (`cartographer_ros_msgs/WriteState`_)
Writes artifacts (e.g. the map) to disk. The `stem` argument is used as a prefix Writes the current internal state to disk into `filename`. The file will
for the various files which are written. Files will usually end up in `~/.ros` or usually end up in `~/.ros` or `ROS_HOME` if it is set. This file can be used
`ROS_HOME` if it is set. as input to the `assets_writer_main` to generate assets like probability
grids, X-Rays or PLY files.
Required tf Transforms Required tf Transforms
====================== ======================
@ -129,7 +130,7 @@ If *provide_odom_frame* is enabled in the :doc:`configuration`, a continuous
.. _cartographer_ros_msgs/SubmapList: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/msg/SubmapList.msg .. _cartographer_ros_msgs/SubmapList: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/msg/SubmapList.msg
.. _cartographer_ros_msgs/SubmapQuery: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/SubmapQuery.srv .. _cartographer_ros_msgs/SubmapQuery: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/SubmapQuery.srv
.. _cartographer_ros_msgs/StartTrajectory: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/StartTrajectory.srv .. _cartographer_ros_msgs/StartTrajectory: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/StartTrajectory.srv
.. _cartographer_ros_msgs/WriteAssets: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/WriteAssets.srv .. _cartographer_ros_msgs/WriteState: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/WriteState.srv
.. _nav_msgs/OccupancyGrid: http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html .. _nav_msgs/OccupancyGrid: http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html
.. _nav_msgs/Odometry: http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html .. _nav_msgs/Odometry: http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html
.. _sensor_msgs/Imu: http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html .. _sensor_msgs/Imu: http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html