diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index 1e82443..2b1cec4 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -44,7 +44,6 @@ google_enable_testing() find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) include(FindPkgConfig) -pkg_search_module(YAMLCPP REQUIRED yaml-cpp>=0.5.1) find_package(LuaGoogle REQUIRED) 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 # protobuf specifies a library '-lpthread' instead of just 'pthread'. # CARTOGRAPHER - YAMLCPP PCL EIGEN3 Boost @@ -99,10 +97,6 @@ add_subdirectory("cartographer_ros") 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 target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc deleted file mode 100644 index 0d4d9ad..0000000 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ /dev/null @@ -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>& - 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>& - 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>& - 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(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()); - auto points_batch = carto::common::make_unique(); - 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 diff --git a/cartographer_ros/cartographer_ros/assets_writer.h b/cartographer_ros/cartographer_ros/assets_writer.h deleted file mode 100644 index 51625dc..0000000 --- a/cartographer_ros/cartographer_ros/assets_writer.h +++ /dev/null @@ -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 -#include - -#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>& - all_trajectory_nodes); - -// Writes a trajectory proto and an occupancy grid. -void Write2DAssets( - const std::vector>& - 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>& - all_trajectory_nodes, - const double voxel_size, const std::string& stem); - -} // namespace cartographer_ros - -#endif // CARTOGRAPHER_ROS_ASSETS_WRITER_H_ diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index e4e54e6..0fe326a 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -17,10 +17,8 @@ #include "cartographer_ros/map_builder_bridge.h" #include "cartographer/io/proto_stream.h" -#include "cartographer_ros/assets_writer.h" #include "cartographer_ros/color.h" #include "cartographer_ros/msg_conversion.h" -#include "cartographer_ros/occupancy_grid.h" namespace cartographer_ros { @@ -70,44 +68,12 @@ void MapBuilderBridge::FinishTrajectory(const int trajectory_id) { sensor_bridges_.erase(trajectory_id); } -void MapBuilderBridge::SerializeState(const std::string& stem) { - cartographer::io::ProtoStreamWriter writer(stem + ".pbstream"); +void MapBuilderBridge::SerializeState(const std::string& filename) { + cartographer::io::ProtoStreamWriter writer(filename); map_builder_.SerializeState(&writer); 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( cartographer_ros_msgs::SubmapQuery::Request& request, cartographer_ros_msgs::SubmapQuery::Response& response) { @@ -157,28 +123,6 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() { return submap_list; } -std::unique_ptr -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 occupancy_grid; - if (HasNonTrimmedNode(all_trajectory_nodes)) { - occupancy_grid = - cartographer::common::make_unique(); - // 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 MapBuilderBridge::GetTrajectoryStates() { std::unordered_map trajectory_states; diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index f99f890..5a974aa 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -54,15 +54,13 @@ class MapBuilderBridge { int AddTrajectory(const std::unordered_set& expected_sensor_ids, const TrajectoryOptions& trajectory_options); void FinishTrajectory(int trajectory_id); - void SerializeState(const string& stem); - void WriteAssets(const string& stem); + void SerializeState(const string& filename); bool HandleSubmapQuery( cartographer_ros_msgs::SubmapQuery::Request& request, cartographer_ros_msgs::SubmapQuery::Response& response); cartographer_ros_msgs::SubmapList GetSubmapList(); - std::unique_ptr BuildOccupancyGrid(); std::unordered_map GetTrajectoryStates(); visualization_msgs::MarkerArray GetTrajectoryNodeList(); visualization_msgs::MarkerArray GetConstraintList(); diff --git a/cartographer_ros/cartographer_ros/map_writer.cc b/cartographer_ros/cartographer_ros/map_writer.cc deleted file mode 100644 index 4309a8a..0000000 --- a/cartographer_ros/cartographer_ros/map_writer.cc +++ /dev/null @@ -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 - -#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 diff --git a/cartographer_ros/cartographer_ros/map_writer.h b/cartographer_ros/cartographer_ros/map_writer.h deleted file mode 100644 index f0159a1..0000000 --- a/cartographer_ros/cartographer_ros/map_writer.h +++ /dev/null @@ -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 - -#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_ diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 888061c..c010d6a 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -68,16 +68,7 @@ Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer) service_servers_.push_back(node_handle_.advertiseService( kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this)); service_servers_.push_back(node_handle_.advertiseService( - kWriteAssetsServiceName, &Node::HandleWriteAssets, 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); - } + kWriteStateServiceName, &Node::HandleWriteState, this)); scan_matched_point_cloud_publisher_ = node_handle_.advertise( @@ -97,15 +88,7 @@ Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer) &Node::PublishConstraintList, this)); } -Node::~Node() { - { - carto::common::MutexLocker lock(&mutex_); - terminating_ = true; - } - if (occupancy_grid_thread_.joinable()) { - occupancy_grid_thread_.join(); - } -} +Node::~Node() {} ::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 Node::ComputeExpectedTopics( const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics) { @@ -429,12 +393,11 @@ bool Node::HandleFinishTrajectory( return true; } -bool Node::HandleWriteAssets( - ::cartographer_ros_msgs::WriteAssets::Request& request, - ::cartographer_ros_msgs::WriteAssets::Response& response) { +bool Node::HandleWriteState( + ::cartographer_ros_msgs::WriteState::Request& request, + ::cartographer_ros_msgs::WriteState::Response& response) { carto::common::MutexLocker lock(&mutex_); - map_builder_bridge_.SerializeState(request.stem); - map_builder_bridge_.WriteAssets(request.stem); + map_builder_bridge_.SerializeState(request.filename); return true; } diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index eaa9e68..9c15896 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -34,7 +34,7 @@ #include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapQuery.h" #include "cartographer_ros_msgs/TrajectoryOptions.h" -#include "cartographer_ros_msgs/WriteAssets.h" +#include "cartographer_ros_msgs/WriteState.h" #include "ros/ros.h" #include "tf2_ros/transform_broadcaster.h" @@ -71,9 +71,9 @@ class Node { bool HandleFinishTrajectory( cartographer_ros_msgs::FinishTrajectory::Request& request, cartographer_ros_msgs::FinishTrajectory::Response& response); - bool HandleWriteAssets( - cartographer_ros_msgs::WriteAssets::Request& request, - cartographer_ros_msgs::WriteAssets::Response& response); + bool HandleWriteState( + cartographer_ros_msgs::WriteState::Request& request, + cartographer_ros_msgs::WriteState::Response& response); // Returns the set of topic names we want to subscribe to. std::unordered_set ComputeExpectedTopics( const TrajectoryOptions& options, @@ -113,9 +113,6 @@ class Node { std::unordered_map> subscribers_; std::unordered_set subscribed_topics_; std::unordered_map 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 // they do not fire. diff --git a/cartographer_ros/cartographer_ros/node_constants.h b/cartographer_ros/cartographer_ros/node_constants.h index c861c96..1aeb6a8 100644 --- a/cartographer_ros/cartographer_ros/node_constants.h +++ b/cartographer_ros/cartographer_ros/node_constants.h @@ -34,7 +34,7 @@ constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2"; constexpr char kSubmapListTopic[] = "submap_list"; constexpr char kSubmapQueryServiceName[] = "submap_query"; constexpr char kStartTrajectoryServiceName[] = "start_trajectory"; -constexpr char kWriteAssetsServiceName[] = "write_assets"; +constexpr char kWriteStateServiceName[] = "write_state"; constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list"; constexpr char kConstraintListTopic[] = "constraint_list"; constexpr double kConstraintPublishPeriodSec = 0.5; diff --git a/cartographer_ros/cartographer_ros/occupancy_grid.cc b/cartographer_ros/cartographer_ros/occupancy_grid.cc deleted file mode 100644 index 00d8513..0000000 --- a/cartographer_ros/cartographer_ros/occupancy_grid.cc +++ /dev/null @@ -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>& - 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()); - 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>& - 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()), - &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>& - 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() / resolution; - return ::cartographer::mapping_2d::MapLimits( - resolution, bounding_box.max().cast(), - ::cartographer::mapping_2d::CellLimits( - ::cartographer::common::RoundToInt(pixel_sizes.y()), - ::cartographer::common::RoundToInt(pixel_sizes.x()))); -} - -} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/occupancy_grid.h b/cartographer_ros/cartographer_ros/occupancy_grid.h deleted file mode 100644 index 3a2cd4e..0000000 --- a/cartographer_ros/cartographer_ros/occupancy_grid.h +++ /dev/null @@ -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 - -#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>& - 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>& - all_trajectory_nodes); - -} // namespace cartographer_ros - -#endif // CARTOGRAPHER_ROS_OCCUPANCY_GRID_H_ diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_test.cc b/cartographer_ros/cartographer_ros/occupancy_grid_test.cc deleted file mode 100644 index 70bc104..0000000 --- a/cartographer_ros/cartographer_ros/occupancy_grid_test.cc +++ /dev/null @@ -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 - -#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{ - ::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 diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index 7137650..ce2b958 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -273,8 +273,7 @@ void Run(const std::vector& bag_filenames) { << (cpu_timespec.tv_sec + 1e-9 * cpu_timespec.tv_nsec) << " s"; #endif - node.map_builder_bridge()->SerializeState(bag_filenames.front()); - node.map_builder_bridge()->WriteAssets(bag_filenames.front()); + node.map_builder_bridge()->SerializeState(bag_filenames.front() + ".pbstream"); } } // namespace diff --git a/cartographer_ros/launch/backpack_2d.launch b/cartographer_ros/launch/backpack_2d.launch index e50da78..e016df4 100644 --- a/cartographer_ros/launch/backpack_2d.launch +++ b/cartographer_ros/launch/backpack_2d.launch @@ -28,4 +28,7 @@ output="screen"> + + diff --git a/cartographer_ros/launch/backpack_3d.launch b/cartographer_ros/launch/backpack_3d.launch index f29c1f9..52dea59 100644 --- a/cartographer_ros/launch/backpack_3d.launch +++ b/cartographer_ros/launch/backpack_3d.launch @@ -29,4 +29,7 @@ + + diff --git a/cartographer_ros/launch/demo_backpack_2d_localization.launch b/cartographer_ros/launch/demo_backpack_2d_localization.launch index 2c9c55e..b1e4658 100644 --- a/cartographer_ros/launch/demo_backpack_2d_localization.launch +++ b/cartographer_ros/launch/demo_backpack_2d_localization.launch @@ -32,6 +32,9 @@ + + + + diff --git a/cartographer_ros/launch/demo_revo_lds.launch b/cartographer_ros/launch/demo_revo_lds.launch index 555e119..3876644 100644 --- a/cartographer_ros/launch/demo_revo_lds.launch +++ b/cartographer_ros/launch/demo_revo_lds.launch @@ -25,6 +25,9 @@ + + + + diff --git a/cartographer_ros/launch/offline_backpack_3d.launch b/cartographer_ros/launch/offline_backpack_3d.launch index e7a4d26..e9d7ba8 100644 --- a/cartographer_ros/launch/offline_backpack_3d.launch +++ b/cartographer_ros/launch/offline_backpack_3d.launch @@ -30,4 +30,7 @@ + + diff --git a/cartographer_ros/launch/taurob_tracker.launch b/cartographer_ros/launch/taurob_tracker.launch index bc5b51a..4111fa0 100644 --- a/cartographer_ros/launch/taurob_tracker.launch +++ b/cartographer_ros/launch/taurob_tracker.launch @@ -23,4 +23,7 @@ + + diff --git a/cartographer_ros/package.xml b/cartographer_ros/package.xml index 3bc4514..398efd9 100644 --- a/cartographer_ros/package.xml +++ b/cartographer_ros/package.xml @@ -59,7 +59,6 @@ tf2_ros urdf visualization_msgs - yaml-cpp rosunit diff --git a/cartographer_ros_msgs/CMakeLists.txt b/cartographer_ros_msgs/CMakeLists.txt index e24ae77..2f36e2f 100644 --- a/cartographer_ros_msgs/CMakeLists.txt +++ b/cartographer_ros_msgs/CMakeLists.txt @@ -35,7 +35,7 @@ add_service_files( SubmapQuery.srv FinishTrajectory.srv StartTrajectory.srv - WriteAssets.srv + WriteState.srv ) generate_messages( diff --git a/cartographer_ros_msgs/srv/WriteAssets.srv b/cartographer_ros_msgs/srv/WriteState.srv similarity index 97% rename from cartographer_ros_msgs/srv/WriteAssets.srv rename to cartographer_ros_msgs/srv/WriteState.srv index 9ef69f4..fdbac78 100644 --- a/cartographer_ros_msgs/srv/WriteAssets.srv +++ b/cartographer_ros_msgs/srv/WriteState.srv @@ -12,5 +12,5 @@ # See the License for the specific language governing permissions and # limitations under the License. -string stem +string filename --- diff --git a/docs/source/ros_api.rst b/docs/source/ros_api.rst index 5351ae8..5e2c5ca 100644 --- a/docs/source/ros_api.rst +++ b/docs/source/ros_api.rst @@ -99,10 +99,11 @@ start_trajectory (`cartographer_ros_msgs/StartTrajectory`_) finish_trajectory (`cartographer_ros_msgs/FinishTrajectory`_) Finishes the given `trajectory_id`'s trajectory by running a final optimization. -write_assets (`cartographer_ros_msgs/WriteAssets`_) - Writes artifacts (e.g. the map) to disk. The `stem` argument is used as a prefix - for the various files which are written. Files will usually end up in `~/.ros` or - `ROS_HOME` if it is set. +write_state (`cartographer_ros_msgs/WriteState`_) + Writes the current internal state to disk into `filename`. The file will + usually end up in `~/.ros` or `ROS_HOME` if it is set. This file can be used + as input to the `assets_writer_main` to generate assets like probability + grids, X-Rays or PLY files. 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/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/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/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