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

Uses MapBuilder and cleans up some configurations. (#51)

master
Damon Kohler 2016-09-08 17:01:21 +02:00 committed by GitHub
parent 0cd8f047ed
commit 9003b19343
4 changed files with 233 additions and 212 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
cartographer_ros/configuration_files/backpack_2d.lua
View File

@ -12,25 +12,27 @@
-- See the License for the specific language governing permissions and
-- limitations under the License.
include "trajectory_builder.lua"
include "sparse_pose_graph.lua"
include "map_builder.lua"
options = {
sparse_pose_graph = SPARSE_POSE_GRAPH,
trajectory_builder = TRAJECTORY_BUILDER,
map_builder = MAP_BUILDER,
map_frame = "map",
odom_frame = "odom",
tracking_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
expect_odometry_data = false,
use_odometry_data = false,
publish_occupancy_grid = false,
laser_min_range = 0.,
laser_max_range = 30.,
laser_missing_echo_ray_length = 5.,
lookup_transform_timeout = 0.01,
use_horizontal_laser = false,
use_horizontal_multi_echo_laser = true,
horizontal_laser_min_range = 0.,
horizontal_laser_max_range = 30.,
horizontal_laser_missing_echo_ray_length = 5.,
num_lasers_3d = 0,
lookup_transform_timeout_sec = 0.01,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
use_multi_echo_laser_scan_2d = true
}
options.map_builder.use_trajectory_builder_2d = true
return options

23
cartographer_ros/configuration_files/backpack_3d.lua
View File

@ -12,27 +12,28 @@
-- See the License for the specific language governing permissions and
-- limitations under the License.
include "trajectory_builder_3d.lua"
include "sparse_pose_graph.lua"
include "map_builder.lua"
options = {
sparse_pose_graph = SPARSE_POSE_GRAPH,
trajectory_builder = TRAJECTORY_BUILDER_3D,
map_builder = MAP_BUILDER,
map_frame = "map",
odom_frame = "odom",
tracking_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
expect_odometry_data = false,
use_odometry_data = false,
publish_occupancy_grid = false,
laser_min_range = 0.,
laser_max_range = 30.,
laser_missing_echo_ray_length = 5.,
lookup_transform_timeout = 0.01,
use_horizontal_laser = false,
use_horizontal_multi_echo_laser = false,
horizontal_laser_min_range = 0.,
horizontal_laser_max_range = 30.,
horizontal_laser_missing_echo_ray_length = 5.,
num_lasers_3d = 2,
lookup_transform_timeout_sec = 0.01,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
num_lasers_3d = 2
}
options.map_builder.use_trajectory_builder_3d = true
options.sparse_pose_graph.optimize_every_n_scans = 320
options.sparse_pose_graph.constraint_builder.sampling_ratio = 0.03
options.sparse_pose_graph.optimization_problem.ceres_solver_options.max_num_iterations = 10

23
cartographer_ros/configuration_files/turtlebot.lua
View File

@ -12,27 +12,28 @@
-- See the License for the specific language governing permissions and
-- limitations under the License.
include "trajectory_builder.lua"
include "sparse_pose_graph.lua"
include "map_builder.lua"
options = {
sparse_pose_graph = SPARSE_POSE_GRAPH,
trajectory_builder = TRAJECTORY_BUILDER,
map_builder = MAP_BUILDER,
map_frame = "map",
odom_frame = "odom",
tracking_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = false,
expect_odometry_data = false,
use_odometry_data = false,
publish_occupancy_grid = false,
laser_min_range = 0.,
laser_max_range = 30.,
laser_missing_echo_ray_length = 5.,
lookup_transform_timeout = 0.01,
use_horizontal_laser = true,
use_horizontal_multi_echo_laser = false,
horizontal_laser_min_range = 0.,
horizontal_laser_max_range = 30.,
horizontal_laser_missing_echo_ray_length = 5.,
num_lasers_3d = 0,
lookup_transform_timeout_sec = 0.01,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
use_laser_scan_2d = true
}
options.map_builder.use_trajectory_builder_2d = true
options.trajectory_builder.expect_imu_data = false
options.trajectory_builder.use_online_correlative_scan_matching = true

365
cartographer_ros/src/cartographer_node_main.cc
View File

@ -32,6 +32,7 @@
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/map_builder.h"
#include "cartographer/mapping/proto/submaps.pb.h"
#include "cartographer/mapping/sensor_collator.h"
#include "cartographer/mapping_2d/global_trajectory_builder.h"
@ -85,7 +86,7 @@ using carto::transform::Rigid3d;
using carto::kalman_filter::PoseCovariance;
// TODO(hrapp): Support multi trajectory mapping.
constexpr int64 kTrajectoryId = 0;
constexpr int64 kTrajectoryBuilderId = 0;
constexpr int kSubscriberQueueSize = 150;
constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
@ -97,12 +98,86 @@ constexpr char kImuTopic[] = "/imu";
constexpr char kOdometryTopic[] = "/odom";
constexpr char kOccupancyGridTopic[] = "/map";
struct NodeOptions {
carto::mapping::proto::MapBuilderOptions map_builder_options;
string map_frame;
string tracking_frame;
string odom_frame;
bool publish_occupancy_grid;
bool provide_odom_frame;
bool use_odometry_data;
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.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_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 {
public:
Node();
Node(const NodeOptions& options);
~Node();
Node(const Node&) = delete;
Node& operator=(const Node&) = delete;
@ -121,8 +196,8 @@ class Node {
void AddLaserFan3D(int64 timestamp, const string& frame_id,
const proto::LaserFan3D& laser_fan_3d);
// Returns a transform for 'frame_id' to 'tracking_frame_' if it exists at
// 'time' or throws tf2::TransformException if it does not exist.
// Returns a transform for 'frame_id' to 'options_.tracking_frame' if it
// exists at 'time' or throws tf2::TransformException if it does not exist.
Rigid3d LookupToTrackingTransformOrThrow(carto::common::Time time,
const string& frame_id);
@ -130,42 +205,30 @@ class Node {
::cartographer_ros_msgs::SubmapQuery::Request& request,
::cartographer_ros_msgs::SubmapQuery::Response& response);
void PublishSubmapList(const ros::WallTimerEvent& timer_event);
void PublishPose(const ros::WallTimerEvent& timer_event);
void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
void PublishPose(const ::ros::WallTimerEvent& timer_event);
void SpinOccupancyGridThreadForever();
// TODO(hrapp): Pull out the common functionality between this and MapWriter
// into an open sourcable MapWriter.
carto::mapping::SensorCollator<SensorData> sensor_collator_;
SensorDataProducer sensor_data_producer_;
ros::NodeHandle node_handle_;
ros::Subscriber imu_subscriber_;
ros::Subscriber laser_subscriber_2d_;
std::vector<ros::Subscriber> laser_subscribers_3d_;
ros::Subscriber odometry_subscriber_;
string tracking_frame_;
string odom_frame_;
string map_frame_;
bool provide_odom_frame_;
bool expect_odometry_data_;
double laser_min_range_;
double laser_max_range_;
double laser_missing_echo_ray_length_;
double lookup_transform_timeout_;
const NodeOptions options_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_;
carto::common::ThreadPool thread_pool_;
carto::common::Mutex mutex_;
std::unique_ptr<carto::mapping::GlobalTrajectoryBuilderInterface>
trajectory_builder_ GUARDED_BY(mutex_);
std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_node_data_
std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_data_
GUARDED_BY(mutex_);
std::unique_ptr<carto::mapping::SparsePoseGraph> sparse_pose_graph_;
carto::mapping::MapBuilder map_builder_ GUARDED_BY(mutex_);
carto::mapping::SensorCollator<SensorData> sensor_collator_
GUARDED_BY(mutex_);
SensorDataProducer sensor_data_producer_ GUARDED_BY(mutex_);
::ros::NodeHandle node_handle_;
::ros::Subscriber imu_subscriber_;
::ros::Subscriber horizontal_laser_scan_subscriber_;
std::vector<::ros::Subscriber> laser_subscribers_3d_;
::ros::Subscriber odometry_subscriber_;
::ros::Publisher submap_list_publisher_;
::ros::ServiceServer submap_query_server_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_;
tf2_ros::TransformBroadcaster tf_broadcaster_;
::ros::Publisher occupancy_grid_publisher_;
@ -177,17 +240,18 @@ class Node {
std::chrono::steady_clock::time_point last_sensor_data_rates_logging_time_;
std::map<string, carto::common::RateTimer<>> rate_timers_;
// We have to keep the timer handles of ros::WallTimers around, otherwise they
// do not fire.
std::vector<ros::WallTimer> wall_timers_;
// We have to keep the timer handles of ::ros::WallTimers around, otherwise
// they do not fire.
std::vector<::ros::WallTimer> wall_timers_;
};
Node::Node()
: sensor_data_producer_(kTrajectoryId, &sensor_collator_),
Node::Node(const NodeOptions& options)
: options_(options),
map_builder_(options.map_builder_options, &constant_data_),
sensor_data_producer_(kTrajectoryBuilderId, &sensor_collator_),
node_handle_("~"),
tf_buffer_(ros::Duration(1000)),
tf_(tf_buffer_),
thread_pool_(10) {}
tf_buffer_(::ros::Duration(1000)),
tf_(tf_buffer_) {}
Node::~Node() {
{
@ -201,15 +265,16 @@ Node::~Node() {
Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time,
const string& frame_id) {
return ToRigid3d(
tf_buffer_.lookupTransform(tracking_frame_, frame_id, ToRos(time),
ros::Duration(lookup_transform_timeout_)));
return ToRigid3d(tf_buffer_.lookupTransform(
options_.tracking_frame, frame_id, ToRos(time),
::ros::Duration(options_.lookup_transform_timeout_sec)));
}
void Node::AddOdometry(int64 timestamp, const string& frame_id,
const Rigid3d& pose, const PoseCovariance& covariance) {
const carto::common::Time time = carto::common::FromUniversal(timestamp);
trajectory_builder_->AddOdometerPose(time, pose, covariance);
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
->AddOdometerPose(time, pose, covariance);
}
void Node::AddImu(const int64 timestamp, const string& frame_id,
@ -222,14 +287,15 @@ void Node::AddImu(const int64 timestamp, const string& frame_id,
<< "The IMU frame must be colocated with the tracking frame. "
"Transforming linear accelaration into the tracking frame will "
"otherwise be imprecise.";
trajectory_builder_->AddImuData(
time, sensor_to_tracking.rotation() *
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
->AddImuData(time,
sensor_to_tracking.rotation() *
carto::transform::ToEigen(imu.linear_acceleration()),
sensor_to_tracking.rotation() *
carto::transform::ToEigen(imu.angular_velocity()));
} catch (const tf2::TransformException& ex) {
LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
<< ": " << ex.what();
LOG(WARNING) << "Cannot transform " << frame_id << " -> "
<< options_.tracking_frame << ": " << ex.what();
}
}
void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id,
@ -239,16 +305,18 @@ void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id,
const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrThrow(time, frame_id);
const carto::sensor::LaserFan laser_fan = carto::sensor::ToLaserFan(
laser_scan, laser_min_range_, laser_max_range_,
laser_missing_echo_ray_length_);
laser_scan, options_.horizontal_laser_min_range,
options_.horizontal_laser_max_range,
options_.horizontal_laser_missing_echo_ray_length);
const auto laser_fan_3d = carto::sensor::TransformLaserFan3D(
carto::sensor::ToLaserFan3D(laser_fan),
sensor_to_tracking.cast<float>());
trajectory_builder_->AddHorizontalLaserFan(time, laser_fan_3d);
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
->AddHorizontalLaserFan(time, laser_fan_3d);
} catch (const tf2::TransformException& ex) {
LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
<< ": " << ex.what();
LOG(WARNING) << "Cannot transform " << frame_id << " -> "
<< options_.tracking_frame << ": " << ex.what();
}
}
@ -258,62 +326,24 @@ void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id,
try {
const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrThrow(time, frame_id);
trajectory_builder_->AddLaserFan3D(
time, carto::sensor::TransformLaserFan3D(
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
->AddLaserFan3D(time, carto::sensor::TransformLaserFan3D(
carto::sensor::FromProto(laser_fan_3d),
sensor_to_tracking.cast<float>()));
} catch (const tf2::TransformException& ex) {
LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
<< ": " << ex.what();
LOG(WARNING) << "Cannot transform " << frame_id << " -> "
<< options_.tracking_frame << ": " << ex.what();
}
}
void Node::Initialize() {
auto file_resolver =
carto::common::make_unique<carto::common::ConfigurationFileResolver>(
std::vector<string>{FLAGS_configuration_directory});
const string code =
file_resolver->GetFileContentOrDie(FLAGS_configuration_basename);
carto::common::LuaParameterDictionary lua_parameter_dictionary(
code, std::move(file_resolver), nullptr);
tracking_frame_ = lua_parameter_dictionary.GetString("tracking_frame");
odom_frame_ = lua_parameter_dictionary.GetString("odom_frame");
map_frame_ = lua_parameter_dictionary.GetString("map_frame");
provide_odom_frame_ = lua_parameter_dictionary.GetBool("provide_odom_frame");
expect_odometry_data_ =
lua_parameter_dictionary.GetBool("expect_odometry_data");
laser_min_range_ = lua_parameter_dictionary.GetDouble("laser_min_range");
laser_max_range_ = lua_parameter_dictionary.GetDouble("laser_max_range");
laser_missing_echo_ray_length_ =
lua_parameter_dictionary.GetDouble("laser_missing_echo_ray_length");
lookup_transform_timeout_ =
lua_parameter_dictionary.GetDouble("lookup_transform_timeout");
// Set of all topics we subscribe to. We use the non-remapped default names
// which are unique.
std::unordered_set<string> expected_sensor_identifiers;
// Subscribe to exactly one laser.
const bool has_laser_scan_2d =
lua_parameter_dictionary.HasKey("use_laser_scan_2d") &&
lua_parameter_dictionary.GetBool("use_laser_scan_2d");
const bool has_multi_echo_laser_scan_2d =
lua_parameter_dictionary.HasKey("use_multi_echo_laser_scan_2d") &&
lua_parameter_dictionary.GetBool("use_multi_echo_laser_scan_2d");
const int num_lasers_3d =
lua_parameter_dictionary.HasKey("num_lasers_3d")
? lua_parameter_dictionary.GetNonNegativeInt("num_lasers_3d")
: 0;
CHECK(has_laser_scan_2d + has_multi_echo_laser_scan_2d +
(num_lasers_3d > 0) ==
1)
<< "Parameters 'use_laser_scan_2d', 'use_multi_echo_laser_scan_2d' and "
"'num_lasers_3d' are mutually exclusive, but one is required.";
if (has_laser_scan_2d) {
laser_subscriber_2d_ = node_handle_.subscribe(
// For 2D SLAM, subscribe to exactly one horizontal laser.
if (options_.use_horizontal_laser) {
horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
kLaserScanTopic, kSubscriberQueueSize,
boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
[this](const sensor_msgs::LaserScan::ConstPtr& msg) {
@ -321,8 +351,8 @@ void Node::Initialize() {
}));
expected_sensor_identifiers.insert(kLaserScanTopic);
}
if (has_multi_echo_laser_scan_2d) {
laser_subscriber_2d_ = node_handle_.subscribe(
if (options_.use_horizontal_multi_echo_laser) {
horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
kMultiEchoLaserScanTopic, kSubscriberQueueSize,
boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
[this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
@ -332,28 +362,11 @@ void Node::Initialize() {
expected_sensor_identifiers.insert(kMultiEchoLaserScanTopic);
}
bool expect_imu_data = true;
if (has_laser_scan_2d || has_multi_echo_laser_scan_2d) {
auto sparse_pose_graph_2d =
carto::common::make_unique<carto::mapping_2d::SparsePoseGraph>(
carto::mapping::CreateSparsePoseGraphOptions(
lua_parameter_dictionary.GetDictionary("sparse_pose_graph")
.get()),
&thread_pool_, &constant_node_data_);
auto options = carto::mapping_2d::CreateLocalTrajectoryBuilderOptions(
lua_parameter_dictionary.GetDictionary("trajectory_builder").get());
submaps_options_ = options.submaps_options();
expect_imu_data = options.expect_imu_data();
trajectory_builder_ =
carto::common::make_unique<carto::mapping_2d::GlobalTrajectoryBuilder>(
options, sparse_pose_graph_2d.get());
sparse_pose_graph_ = std::move(sparse_pose_graph_2d);
}
if (num_lasers_3d > 0) {
for (int i = 0; i < num_lasers_3d; ++i) {
// For 3D SLAM, subscribe to all 3D lasers.
if (options_.num_lasers_3d > 0) {
for (int i = 0; i < options_.num_lasers_3d; ++i) {
string topic = kPointCloud2Topic;
if (num_lasers_3d > 1) {
if (options_.num_lasers_3d > 1) {
topic += "_" + std::to_string(i + 1);
}
laser_subscribers_3d_.push_back(node_handle_.subscribe(
@ -364,24 +377,14 @@ void Node::Initialize() {
})));
expected_sensor_identifiers.insert(topic);
}
auto sparse_pose_graph_3d =
carto::common::make_unique<carto::mapping_3d::SparsePoseGraph>(
carto::mapping::CreateSparsePoseGraphOptions(
lua_parameter_dictionary.GetDictionary("sparse_pose_graph")
.get()),
&thread_pool_, &constant_node_data_);
trajectory_builder_ =
carto::common::make_unique<carto::mapping_3d::GlobalTrajectoryBuilder>(
carto::mapping_3d::CreateLocalTrajectoryBuilderOptions(
lua_parameter_dictionary.GetDictionary("trajectory_builder")
.get()),
sparse_pose_graph_3d.get());
sparse_pose_graph_ = std::move(sparse_pose_graph_3d);
}
CHECK(sparse_pose_graph_ != nullptr);
// Maybe subscribe to the IMU.
if (expect_imu_data) {
// For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
// required.
if (options_.map_builder_options.use_trajectory_builder_3d() ||
(options_.map_builder_options.use_trajectory_builder_2d() &&
options_.map_builder_options.trajectory_builder_2d_options()
.use_imu_data())) {
imu_subscriber_ = node_handle_.subscribe(
kImuTopic, kSubscriberQueueSize,
boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>(
@ -391,7 +394,7 @@ void Node::Initialize() {
expected_sensor_identifiers.insert(kImuTopic);
}
if (expect_odometry_data_) {
if (options_.use_odometry_data) {
odometry_subscriber_ = node_handle_.subscribe(
kOdometryTopic, kSubscriberQueueSize,
boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
@ -401,8 +404,10 @@ void Node::Initialize() {
expected_sensor_identifiers.insert(kOdometryTopic);
}
// TODO(damonkohler): Add multi-trajectory support.
CHECK_EQ(kTrajectoryBuilderId, map_builder_.AddTrajectoryBuilder());
sensor_collator_.AddTrajectory(
kTrajectoryId, expected_sensor_identifiers,
kTrajectoryBuilderId, expected_sensor_identifiers,
[this](const int64 timestamp, std::unique_ptr<SensorData> sensor_data) {
HandleSensorData(timestamp, std::move(sensor_data));
});
@ -413,9 +418,7 @@ void Node::Initialize() {
submap_query_server_ = node_handle_.advertiseService(
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this);
if (lua_parameter_dictionary.GetBool("publish_occupancy_grid")) {
CHECK_EQ(num_lasers_3d, 0)
<< "Publishing OccupancyGrids for 3D data is not yet supported";
if (options_.publish_occupancy_grid) {
occupancy_grid_publisher_ =
node_handle_.advertise<::nav_msgs::OccupancyGrid>(kOccupancyGridTopic,
1, true);
@ -424,13 +427,11 @@ void Node::Initialize() {
}
wall_timers_.push_back(node_handle_.createWallTimer(
ros::WallDuration(
lua_parameter_dictionary.GetDouble("submap_publish_period_sec")),
::ros::WallDuration(options_.submap_publish_period_sec),
&Node::PublishSubmapList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
ros::WallDuration(
lua_parameter_dictionary.GetDouble("pose_publish_period_sec")),
&Node::PublishPose, this));
::ros::WallDuration(options_.pose_publish_period_sec), &Node::PublishPose,
this));
}
bool Node::HandleSubmapQuery(
@ -442,7 +443,8 @@ bool Node::HandleSubmapQuery(
carto::common::MutexLocker lock(&mutex_);
// TODO(hrapp): return error messages and extract common code from MapBuilder.
carto::mapping::Submaps* submaps = trajectory_builder_->submaps();
carto::mapping::Submaps* submaps =
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->submaps();
if (request.submap_id < 0 || request.submap_id >= submaps->size()) {
return false;
}
@ -452,10 +454,10 @@ bool Node::HandleSubmapQuery(
response_proto.set_submap_version(
submaps->Get(request.submap_id)->end_laser_fan_index);
const std::vector<carto::transform::Rigid3d> submap_transforms =
sparse_pose_graph_->GetSubmapTransforms(*submaps);
map_builder_.sparse_pose_graph()->GetSubmapTransforms(*submaps);
submaps->SubmapToProto(request.submap_id,
sparse_pose_graph_->GetTrajectoryNodes(),
map_builder_.sparse_pose_graph()->GetTrajectoryNodes(),
submap_transforms[request.submap_id], &response_proto);
response.submap_version = response_proto.submap_version();
@ -483,11 +485,12 @@ bool Node::HandleSubmapQuery(
return true;
}
void Node::PublishSubmapList(const ros::WallTimerEvent& timer_event) {
void Node::PublishSubmapList(const ::ros::WallTimerEvent& timer_event) {
carto::common::MutexLocker lock(&mutex_);
const carto::mapping::Submaps* submaps = trajectory_builder_->submaps();
const carto::mapping::Submaps* submaps =
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->submaps();
const std::vector<carto::transform::Rigid3d> submap_transforms =
sparse_pose_graph_->GetSubmapTransforms(*submaps);
map_builder_.sparse_pose_graph()->GetSubmapTransforms(*submaps);
CHECK_EQ(submap_transforms.size(), submaps->size());
::cartographer_ros_msgs::TrajectorySubmapList ros_trajectory;
@ -499,44 +502,49 @@ void Node::PublishSubmapList(const ros::WallTimerEvent& timer_event) {
}
::cartographer_ros_msgs::SubmapList ros_submap_list;
ros_submap_list.header.stamp = ros::Time::now();
ros_submap_list.header.frame_id = map_frame_;
ros_submap_list.header.stamp = ::ros::Time::now();
ros_submap_list.header.frame_id = options_.map_frame;
ros_submap_list.trajectory.push_back(ros_trajectory);
submap_list_publisher_.publish(ros_submap_list);
}
void Node::PublishPose(const ros::WallTimerEvent& timer_event) {
void Node::PublishPose(const ::ros::WallTimerEvent& timer_event) {
carto::common::MutexLocker lock(&mutex_);
const Rigid3d tracking_to_local = trajectory_builder_->pose_estimate().pose;
const carto::mapping::Submaps* submaps = trajectory_builder_->submaps();
const Rigid3d tracking_to_local =
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
->pose_estimate()
.pose;
const carto::mapping::Submaps* submaps =
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->submaps();
const Rigid3d local_to_map =
sparse_pose_graph_->GetLocalToGlobalTransform(*submaps);
map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform(*submaps);
const Rigid3d tracking_to_map = local_to_map * tracking_to_local;
const ros::Time now = ros::Time::now();
const ::ros::Time now = ::ros::Time::now();
geometry_msgs::TransformStamped stamped_transform;
stamped_transform.header.stamp = now;
stamped_transform.header.frame_id = map_frame_;
stamped_transform.child_frame_id = odom_frame_;
stamped_transform.header.frame_id = options_.map_frame;
stamped_transform.child_frame_id = options_.odom_frame;
if (provide_odom_frame_) {
if (options_.provide_odom_frame) {
stamped_transform.transform = ToGeometryMsgTransform(local_to_map);
tf_broadcaster_.sendTransform(stamped_transform);
stamped_transform.header.frame_id = odom_frame_;
stamped_transform.child_frame_id = tracking_frame_;
stamped_transform.header.frame_id = options_.odom_frame;
stamped_transform.child_frame_id = options_.tracking_frame;
stamped_transform.transform = ToGeometryMsgTransform(tracking_to_local);
tf_broadcaster_.sendTransform(stamped_transform);
} else {
try {
const Rigid3d tracking_to_odom =
LookupToTrackingTransformOrThrow(FromRos(now), odom_frame_).inverse();
LookupToTrackingTransformOrThrow(FromRos(now), options_.odom_frame)
.inverse();
const Rigid3d odom_to_map = tracking_to_map * tracking_to_odom.inverse();
stamped_transform.transform = ToGeometryMsgTransform(odom_to_map);
tf_broadcaster_.sendTransform(stamped_transform);
} catch (const tf2::TransformException& ex) {
LOG(WARNING) << "Cannot transform " << tracking_frame_ << " -> "
<< odom_frame_ << ": " << ex.what();
LOG(WARNING) << "Cannot transform " << options_.tracking_frame << " -> "
<< options_.odom_frame << ": " << ex.what();
}
}
}
@ -549,7 +557,8 @@ void Node::SpinOccupancyGridThreadForever() {
return;
}
}
const auto trajectory_nodes = sparse_pose_graph_->GetTrajectoryNodes();
const auto trajectory_nodes =
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
if (trajectory_nodes.empty()) {
std::this_thread::sleep_for(carto::common::FromMilliseconds(1000));
continue;
@ -571,7 +580,7 @@ void Node::SpinOccupancyGridThreadForever() {
::nav_msgs::OccupancyGrid occupancy_grid;
occupancy_grid.header.stamp = ToRos(trajectory_nodes.back().time());
occupancy_grid.header.frame_id = map_frame_;
occupancy_grid.header.frame_id = options_.map_frame;
occupancy_grid.info.map_load_time = occupancy_grid.header.stamp;
Eigen::Array2i offset;
@ -661,10 +670,18 @@ void Node::HandleSensorData(const int64 timestamp,
LOG(FATAL);
}
void Node::SpinForever() { ros::spin(); }
void Node::SpinForever() { ::ros::spin(); }
void Run() {
Node node;
auto file_resolver =
carto::common::make_unique<carto::common::ConfigurationFileResolver>(
std::vector<string>{FLAGS_configuration_directory});
const string code =
file_resolver->GetFileContentOrDie(FLAGS_configuration_basename);
carto::common::LuaParameterDictionary lua_parameter_dictionary(
code, std::move(file_resolver), nullptr);
Node node(CreateNodeOptions(&lua_parameter_dictionary));
node.Initialize();
node.SpinForever();
}
@ -729,11 +746,11 @@ int main(int argc, char** argv) {
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
ros::init(argc, argv, "cartographer_node");
ros::start();
::ros::init(argc, argv, "cartographer_node");
::ros::start();
::cartographer_ros::ScopedRosLogSink ros_log_sink;
::cartographer_ros::Run();
ros::shutdown();
::ros::shutdown();
return 0;
}