diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index bf8edb2..750ca82 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -83,6 +83,10 @@ add_executable(cartographer_node src/cartographer_node_main.cc src/msg_conversion.cc src/msg_conversion.h + src/node_options.cc + src/node_options.h + src/occupancy_grid.cc + src/occupancy_grid.h src/sensor_data.cc src/sensor_data.h src/sensor_data_producer.cc diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index be95611..364e125 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/cartographer_ros/src/cartographer_node_main.cc @@ -23,7 +23,6 @@ #include "Eigen/Core" #include "cartographer/common/configuration_file_resolver.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/mutex.h" #include "cartographer/common/port.h" @@ -64,6 +63,8 @@ #include "tf2_ros/transform_listener.h" #include "msg_conversion.h" +#include "node_options.h" +#include "occupancy_grid.h" #include "sensor_data.h" #include "sensor_data_producer.h" #include "time_conversion.h" @@ -102,93 +103,6 @@ constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2"; constexpr char kSubmapListTopic[] = "submap_list"; constexpr char kSubmapQueryServiceName[] = "submap_query"; -struct NodeOptions { - carto::mapping::proto::MapBuilderOptions map_builder_options; - string map_frame; - string tracking_frame; - string published_frame; - string odom_frame; - bool publish_occupancy_grid; - bool provide_odom_frame; - bool use_odometry_data; - bool use_constant_odometry_variance; - double constant_odometry_translational_variance; - double constant_odometry_rotational_variance; - bool use_horizontal_laser; - bool use_horizontal_multi_echo_laser; - double horizontal_laser_min_range; - double horizontal_laser_max_range; - double horizontal_laser_missing_echo_ray_length; - int num_lasers_3d; - double lookup_transform_timeout_sec; - double submap_publish_period_sec; - double pose_publish_period_sec; -}; - -NodeOptions CreateNodeOptions( - carto::common::LuaParameterDictionary* const lua_parameter_dictionary) { - NodeOptions options; - options.map_builder_options = carto::mapping::CreateMapBuilderOptions( - lua_parameter_dictionary->GetDictionary("map_builder").get()); - options.map_frame = lua_parameter_dictionary->GetString("map_frame"); - options.tracking_frame = - lua_parameter_dictionary->GetString("tracking_frame"); - options.published_frame = - lua_parameter_dictionary->GetString("published_frame"); - options.odom_frame = lua_parameter_dictionary->GetString("odom_frame"); - options.publish_occupancy_grid = - lua_parameter_dictionary->GetBool("publish_occupancy_grid"); - options.provide_odom_frame = - lua_parameter_dictionary->GetBool("provide_odom_frame"); - options.use_odometry_data = - lua_parameter_dictionary->GetBool("use_odometry_data"); - options.use_constant_odometry_variance = - lua_parameter_dictionary->GetBool("use_constant_odometry_variance"); - options.constant_odometry_translational_variance = - lua_parameter_dictionary->GetDouble( - "constant_odometry_translational_variance"); - options.constant_odometry_rotational_variance = - lua_parameter_dictionary->GetDouble( - "constant_odometry_rotational_variance"); - options.use_horizontal_laser = - lua_parameter_dictionary->GetBool("use_horizontal_laser"); - options.use_horizontal_multi_echo_laser = - lua_parameter_dictionary->GetBool("use_horizontal_multi_echo_laser"); - options.horizontal_laser_min_range = - lua_parameter_dictionary->GetDouble("horizontal_laser_min_range"); - options.horizontal_laser_max_range = - lua_parameter_dictionary->GetDouble("horizontal_laser_max_range"); - options.horizontal_laser_missing_echo_ray_length = - lua_parameter_dictionary->GetDouble( - "horizontal_laser_missing_echo_ray_length"); - options.num_lasers_3d = - lua_parameter_dictionary->GetNonNegativeInt("num_lasers_3d"); - options.lookup_transform_timeout_sec = - lua_parameter_dictionary->GetDouble("lookup_transform_timeout_sec"); - options.submap_publish_period_sec = - lua_parameter_dictionary->GetDouble("submap_publish_period_sec"); - options.pose_publish_period_sec = - lua_parameter_dictionary->GetDouble("pose_publish_period_sec"); - - CHECK_EQ(options.use_horizontal_laser + - options.use_horizontal_multi_echo_laser + - (options.num_lasers_3d > 0), - 1) - << "Configuration error: 'use_horizontal_laser', " - "'use_horizontal_multi_echo_laser' and 'num_lasers_3d' are " - "mutually exclusive, but one is required."; - CHECK_EQ( - options.map_builder_options.use_trajectory_builder_2d(), - options.use_horizontal_laser || options.use_horizontal_multi_echo_laser); - CHECK_EQ(options.map_builder_options.use_trajectory_builder_3d(), - options.num_lasers_3d > 0); - if (options.publish_occupancy_grid) { - CHECK(options.map_builder_options.use_trajectory_builder_2d()) - << "Publishing OccupancyGrids for 3D data is not yet supported"; - } - return options; -} - // Node that listens to all the sensor data that we are interested in and wires // it up to the SLAM. class Node { @@ -633,68 +547,8 @@ void Node::SpinOccupancyGridThreadForever() { std::this_thread::sleep_for(carto::common::FromMilliseconds(1000)); continue; } - const auto& submaps_options = - options_.map_builder_options.trajectory_builder_2d_options() - .submaps_options(); - 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()), - &probability_grid); - } - ::nav_msgs::OccupancyGrid occupancy_grid; - occupancy_grid.header.stamp = ToRos(trajectory_nodes.back().time()); - occupancy_grid.header.frame_id = options_.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; - } - } - + BuildOccupancyGrid(trajectory_nodes, options_, &occupancy_grid); occupancy_grid_publisher_.publish(occupancy_grid); } } diff --git a/cartographer_ros/src/node_options.cc b/cartographer_ros/src/node_options.cc new file mode 100644 index 0000000..c562c07 --- /dev/null +++ b/cartographer_ros/src/node_options.cc @@ -0,0 +1,89 @@ +/* + * 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 "node_options.h" + +#include "glog/logging.h" + +namespace cartographer_ros { + +NodeOptions CreateNodeOptions( + ::cartographer::common::LuaParameterDictionary* const + lua_parameter_dictionary) { + NodeOptions options; + options.map_builder_options = + ::cartographer::mapping::CreateMapBuilderOptions( + lua_parameter_dictionary->GetDictionary("map_builder").get()); + options.map_frame = lua_parameter_dictionary->GetString("map_frame"); + options.tracking_frame = + lua_parameter_dictionary->GetString("tracking_frame"); + options.published_frame = + lua_parameter_dictionary->GetString("published_frame"); + options.odom_frame = lua_parameter_dictionary->GetString("odom_frame"); + options.publish_occupancy_grid = + lua_parameter_dictionary->GetBool("publish_occupancy_grid"); + options.provide_odom_frame = + lua_parameter_dictionary->GetBool("provide_odom_frame"); + options.use_odometry_data = + lua_parameter_dictionary->GetBool("use_odometry_data"); + options.use_constant_odometry_variance = + lua_parameter_dictionary->GetBool("use_constant_odometry_variance"); + options.constant_odometry_translational_variance = + lua_parameter_dictionary->GetDouble( + "constant_odometry_translational_variance"); + options.constant_odometry_rotational_variance = + lua_parameter_dictionary->GetDouble( + "constant_odometry_rotational_variance"); + options.use_horizontal_laser = + lua_parameter_dictionary->GetBool("use_horizontal_laser"); + options.use_horizontal_multi_echo_laser = + lua_parameter_dictionary->GetBool("use_horizontal_multi_echo_laser"); + options.horizontal_laser_min_range = + lua_parameter_dictionary->GetDouble("horizontal_laser_min_range"); + options.horizontal_laser_max_range = + lua_parameter_dictionary->GetDouble("horizontal_laser_max_range"); + options.horizontal_laser_missing_echo_ray_length = + lua_parameter_dictionary->GetDouble( + "horizontal_laser_missing_echo_ray_length"); + options.num_lasers_3d = + lua_parameter_dictionary->GetNonNegativeInt("num_lasers_3d"); + options.lookup_transform_timeout_sec = + lua_parameter_dictionary->GetDouble("lookup_transform_timeout_sec"); + options.submap_publish_period_sec = + lua_parameter_dictionary->GetDouble("submap_publish_period_sec"); + options.pose_publish_period_sec = + lua_parameter_dictionary->GetDouble("pose_publish_period_sec"); + + CHECK_EQ(options.use_horizontal_laser + + options.use_horizontal_multi_echo_laser + + (options.num_lasers_3d > 0), + 1) + << "Configuration error: 'use_horizontal_laser', " + "'use_horizontal_multi_echo_laser' and 'num_lasers_3d' are " + "mutually exclusive, but one is required."; + CHECK_EQ( + options.map_builder_options.use_trajectory_builder_2d(), + options.use_horizontal_laser || options.use_horizontal_multi_echo_laser); + CHECK_EQ(options.map_builder_options.use_trajectory_builder_3d(), + options.num_lasers_3d > 0); + if (options.publish_occupancy_grid) { + CHECK(options.map_builder_options.use_trajectory_builder_2d()) + << "Publishing OccupancyGrids for 3D data is not yet supported"; + } + return options; +} + +} // namespace cartographer_ros diff --git a/cartographer_ros/src/node_options.h b/cartographer_ros/src/node_options.h new file mode 100644 index 0000000..189d023 --- /dev/null +++ b/cartographer_ros/src/node_options.h @@ -0,0 +1,57 @@ +/* + * 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 CARTOGRAHPER_ROS_NODE_OPTIONS_H_ +#define CARTOGRAHPER_ROS_NODE_OPTIONS_H_ + +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/port.h" +#include "cartographer/mapping/map_builder.h" + +namespace cartographer_ros { + +// Top-level options of Cartographer's ROS integration. +struct NodeOptions { + ::cartographer::mapping::proto::MapBuilderOptions map_builder_options; + string map_frame; + string tracking_frame; + string published_frame; + string odom_frame; + bool publish_occupancy_grid; + bool provide_odom_frame; + bool use_odometry_data; + bool use_constant_odometry_variance; + double constant_odometry_translational_variance; + double constant_odometry_rotational_variance; + bool use_horizontal_laser; + bool use_horizontal_multi_echo_laser; + double horizontal_laser_min_range; + double horizontal_laser_max_range; + double horizontal_laser_missing_echo_ray_length; + int num_lasers_3d; + double lookup_transform_timeout_sec; + double submap_publish_period_sec; + double pose_publish_period_sec; +}; + +NodeOptions CreateNodeOptions( + ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary); + +} // namespace cartographer_ros + +#endif // CARTOGRAHPER_ROS_NODE_OPTIONS_H_ diff --git a/cartographer_ros/src/occupancy_grid.cc b/cartographer_ros/src/occupancy_grid.cc new file mode 100644 index 0000000..0fd445c --- /dev/null +++ b/cartographer_ros/src/occupancy_grid.cc @@ -0,0 +1,93 @@ +/* + * 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 "occupancy_grid.h" + +#include "time_conversion.h" + +#include "glog/logging.h" + +namespace cartographer_ros { + +void BuildOccupancyGrid( + const std::vector<::cartographer::mapping::TrajectoryNode>& + trajectory_nodes, + const NodeOptions& options, + ::nav_msgs::OccupancyGrid* const occupancy_grid) { + namespace carto = ::cartographer; + const auto& submaps_options = + options.map_builder_options.trajectory_builder_2d_options() + .submaps_options(); + 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()), + &probability_grid); + } + + occupancy_grid->header.stamp = ToRos(trajectory_nodes.back().time()); + occupancy_grid->header.frame_id = options.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; + } + } +} + +} // namespace cartographer_ros diff --git a/cartographer_ros/src/occupancy_grid.h b/cartographer_ros/src/occupancy_grid.h new file mode 100644 index 0000000..66482ed --- /dev/null +++ b/cartographer_ros/src/occupancy_grid.h @@ -0,0 +1,35 @@ +/* + * 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 CARTOGRAHPER_ROS_OCCUPANCY_GRID_H_ +#define CARTOGRAHPER_ROS_OCCUPANCY_GRID_H_ + +#include + +#include "cartographer/mapping/trajectory_node.h" +#include "nav_msgs/OccupancyGrid.h" +#include "node_options.h" + +namespace cartographer_ros { + +void BuildOccupancyGrid( + const std::vector<::cartographer::mapping::TrajectoryNode>& + trajectory_nodes, + const NodeOptions& options, ::nav_msgs::OccupancyGrid* occupancy_grid); + +} // namespace cartographer_ros + +#endif // CARTOGRAHPER_ROS_OCCUPANCY_GRID_H_ diff --git a/cartographer_ros/src/sensor_data.cc b/cartographer_ros/src/sensor_data.cc index dd5e765..0f38a22 100644 --- a/cartographer_ros/src/sensor_data.cc +++ b/cartographer_ros/src/sensor_data.cc @@ -56,4 +56,4 @@ SensorData::SensorData(const string& frame_id, proto::LaserFan3D laser_fan_3d) SensorData::SensorData(const string& frame_id, const Odometry& odometry) : type(SensorType::kOdometry), frame_id(frame_id), odometry(odometry) {} -} // namespace cartorapher_ros +} // namespace cartographer_ros