Follow googlecartographer/cartographer#637. (#583)
Use c++ standard types instead of the values from port.h.master
parent
e63126f329
commit
ff92e9e4bd
|
@ -77,7 +77,7 @@ namespace carto = ::cartographer;
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
std::unique_ptr<carto::io::PointsBatch> HandleMessage(
|
std::unique_ptr<carto::io::PointsBatch> HandleMessage(
|
||||||
const T& message, const string& tracking_frame,
|
const T& message, const std::string& tracking_frame,
|
||||||
const tf2_ros::Buffer& tf_buffer,
|
const tf2_ros::Buffer& tf_buffer,
|
||||||
const carto::transform::TransformInterpolationBuffer&
|
const carto::transform::TransformInterpolationBuffer&
|
||||||
transform_interpolation_buffer) {
|
transform_interpolation_buffer) {
|
||||||
|
@ -116,15 +116,16 @@ std::unique_ptr<carto::io::PointsBatch> HandleMessage(
|
||||||
return points_batch;
|
return points_batch;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Run(const string& pose_graph_filename,
|
void Run(const std::string& pose_graph_filename,
|
||||||
const std::vector<string>& bag_filenames,
|
const std::vector<std::string>& bag_filenames,
|
||||||
const string& configuration_directory,
|
const std::string& configuration_directory,
|
||||||
const string& configuration_basename, const string& urdf_filename,
|
const std::string& configuration_basename,
|
||||||
const string& output_file_prefix) {
|
const std::string& urdf_filename,
|
||||||
|
const std::string& output_file_prefix) {
|
||||||
auto file_resolver =
|
auto file_resolver =
|
||||||
carto::common::make_unique<carto::common::ConfigurationFileResolver>(
|
carto::common::make_unique<carto::common::ConfigurationFileResolver>(
|
||||||
std::vector<string>{configuration_directory});
|
std::vector<std::string>{configuration_directory});
|
||||||
const string code =
|
const std::string code =
|
||||||
file_resolver->GetFileContentOrDie(configuration_basename);
|
file_resolver->GetFileContentOrDie(configuration_basename);
|
||||||
carto::common::LuaParameterDictionary lua_parameter_dictionary(
|
carto::common::LuaParameterDictionary lua_parameter_dictionary(
|
||||||
code, std::move(file_resolver));
|
code, std::move(file_resolver));
|
||||||
|
@ -139,10 +140,10 @@ void Run(const string& pose_graph_filename,
|
||||||
"trajectory in the same order as the correponding trajectories in the "
|
"trajectory in the same order as the correponding trajectories in the "
|
||||||
"pose graph proto.";
|
"pose graph proto.";
|
||||||
|
|
||||||
const string file_prefix = !output_file_prefix.empty()
|
const std::string file_prefix = !output_file_prefix.empty()
|
||||||
? output_file_prefix
|
? output_file_prefix
|
||||||
: bag_filenames.front() + "_";
|
: bag_filenames.front() + "_";
|
||||||
const auto file_writer_factory = [file_prefix](const string& filename) {
|
const auto file_writer_factory = [file_prefix](const std::string& filename) {
|
||||||
return carto::common::make_unique<carto::io::StreamFileWriter>(file_prefix +
|
return carto::common::make_unique<carto::io::StreamFileWriter>(file_prefix +
|
||||||
filename);
|
filename);
|
||||||
};
|
};
|
||||||
|
@ -169,14 +170,14 @@ void Run(const string& pose_graph_filename,
|
||||||
builder.CreatePipeline(
|
builder.CreatePipeline(
|
||||||
lua_parameter_dictionary.GetDictionary("pipeline").get());
|
lua_parameter_dictionary.GetDictionary("pipeline").get());
|
||||||
|
|
||||||
const string tracking_frame =
|
const std::string tracking_frame =
|
||||||
lua_parameter_dictionary.GetString("tracking_frame");
|
lua_parameter_dictionary.GetString("tracking_frame");
|
||||||
do {
|
do {
|
||||||
for (size_t trajectory_id = 0; trajectory_id < bag_filenames.size();
|
for (size_t trajectory_id = 0; trajectory_id < bag_filenames.size();
|
||||||
++trajectory_id) {
|
++trajectory_id) {
|
||||||
const carto::mapping::proto::Trajectory& trajectory_proto =
|
const carto::mapping::proto::Trajectory& trajectory_proto =
|
||||||
pose_graph_proto.trajectory(trajectory_id);
|
pose_graph_proto.trajectory(trajectory_id);
|
||||||
const string& bag_filename = bag_filenames[trajectory_id];
|
const std::string& bag_filename = bag_filenames[trajectory_id];
|
||||||
LOG(INFO) << "Processing " << bag_filename << "...";
|
LOG(INFO) << "Processing " << bag_filename << "...";
|
||||||
if (trajectory_proto.node_size() == 0) {
|
if (trajectory_proto.node_size() == 0) {
|
||||||
continue;
|
continue;
|
||||||
|
|
|
@ -75,7 +75,7 @@ void MapBuilderBridge::LoadMap(const std::string& map_filename) {
|
||||||
}
|
}
|
||||||
|
|
||||||
int MapBuilderBridge::AddTrajectory(
|
int MapBuilderBridge::AddTrajectory(
|
||||||
const std::unordered_set<string>& expected_sensor_ids,
|
const std::unordered_set<std::string>& expected_sensor_ids,
|
||||||
const TrajectoryOptions& trajectory_options) {
|
const TrajectoryOptions& trajectory_options) {
|
||||||
const int trajectory_id = map_builder_.AddTrajectoryBuilder(
|
const int trajectory_id = map_builder_.AddTrajectoryBuilder(
|
||||||
expected_sensor_ids, trajectory_options.trajectory_builder_options);
|
expected_sensor_ids, trajectory_options.trajectory_builder_options);
|
||||||
|
@ -150,7 +150,7 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
|
||||||
cartographer_ros_msgs::SubmapList submap_list;
|
cartographer_ros_msgs::SubmapList submap_list;
|
||||||
submap_list.header.stamp = ::ros::Time::now();
|
submap_list.header.stamp = ::ros::Time::now();
|
||||||
submap_list.header.frame_id = node_options_.map_frame;
|
submap_list.header.frame_id = node_options_.map_frame;
|
||||||
for (const auto &submap_id_data :
|
for (const auto& submap_id_data :
|
||||||
map_builder_.sparse_pose_graph()->GetAllSubmapData()) {
|
map_builder_.sparse_pose_graph()->GetAllSubmapData()) {
|
||||||
cartographer_ros_msgs::SubmapEntry submap_entry;
|
cartographer_ros_msgs::SubmapEntry submap_entry;
|
||||||
submap_entry.trajectory_id = submap_id_data.id.trajectory_id;
|
submap_entry.trajectory_id = submap_id_data.id.trajectory_id;
|
||||||
|
|
|
@ -51,11 +51,11 @@ class MapBuilderBridge {
|
||||||
MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
|
MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
|
||||||
|
|
||||||
void LoadMap(const std::string& map_filename);
|
void LoadMap(const std::string& map_filename);
|
||||||
int AddTrajectory(const std::unordered_set<string>& expected_sensor_ids,
|
int AddTrajectory(const std::unordered_set<std::string>& expected_sensor_ids,
|
||||||
const TrajectoryOptions& trajectory_options);
|
const TrajectoryOptions& trajectory_options);
|
||||||
void FinishTrajectory(int trajectory_id);
|
void FinishTrajectory(int trajectory_id);
|
||||||
void RunFinalOptimization();
|
void RunFinalOptimization();
|
||||||
void SerializeState(const string& filename);
|
void SerializeState(const std::string& filename);
|
||||||
|
|
||||||
bool HandleSubmapQuery(
|
bool HandleSubmapQuery(
|
||||||
cartographer_ros_msgs::SubmapQuery::Request& request,
|
cartographer_ros_msgs::SubmapQuery::Request& request,
|
||||||
|
|
|
@ -46,8 +46,8 @@ constexpr float kPointCloudComponentFourMagic = 1.;
|
||||||
using ::cartographer::sensor::PointCloudWithIntensities;
|
using ::cartographer::sensor::PointCloudWithIntensities;
|
||||||
using ::cartographer::transform::Rigid3d;
|
using ::cartographer::transform::Rigid3d;
|
||||||
|
|
||||||
sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64 timestamp,
|
sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64_t timestamp,
|
||||||
const string& frame_id,
|
const std::string& frame_id,
|
||||||
const int num_points) {
|
const int num_points) {
|
||||||
sensor_msgs::PointCloud2 msg;
|
sensor_msgs::PointCloud2 msg;
|
||||||
msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp));
|
msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp));
|
||||||
|
@ -140,7 +140,7 @@ bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2,
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
sensor_msgs::PointCloud2 ToPointCloud2Message(
|
sensor_msgs::PointCloud2 ToPointCloud2Message(
|
||||||
const int64 timestamp, const string& frame_id,
|
const int64_t timestamp, const std::string& frame_id,
|
||||||
const ::cartographer::sensor::TimedPointCloud& point_cloud) {
|
const ::cartographer::sensor::TimedPointCloud& point_cloud) {
|
||||||
auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size());
|
auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size());
|
||||||
::ros::serialization::OStream stream(msg.data.data(), msg.data.size());
|
::ros::serialization::OStream stream(msg.data.data(), msg.data.size());
|
||||||
|
|
|
@ -34,7 +34,7 @@
|
||||||
namespace cartographer_ros {
|
namespace cartographer_ros {
|
||||||
|
|
||||||
sensor_msgs::PointCloud2 ToPointCloud2Message(
|
sensor_msgs::PointCloud2 ToPointCloud2Message(
|
||||||
int64 timestamp, const string& frame_id,
|
int64_t timestamp, const std::string& frame_id,
|
||||||
const ::cartographer::sensor::TimedPointCloud& point_cloud);
|
const ::cartographer::sensor::TimedPointCloud& point_cloud);
|
||||||
|
|
||||||
geometry_msgs::Transform ToGeometryMsgTransform(
|
geometry_msgs::Transform ToGeometryMsgTransform(
|
||||||
|
|
|
@ -60,9 +60,9 @@ cartographer_ros_msgs::SensorTopics DefaultSensorTopics() {
|
||||||
// calls 'handler' on the 'node' to handle messages. Returns the subscriber.
|
// calls 'handler' on the 'node' to handle messages. Returns the subscriber.
|
||||||
template <typename MessageType>
|
template <typename MessageType>
|
||||||
::ros::Subscriber SubscribeWithHandler(
|
::ros::Subscriber SubscribeWithHandler(
|
||||||
void (Node::*handler)(int, const string&,
|
void (Node::*handler)(int, const std::string&,
|
||||||
const typename MessageType::ConstPtr&),
|
const typename MessageType::ConstPtr&),
|
||||||
const int trajectory_id, const string& topic,
|
const int trajectory_id, const std::string& topic,
|
||||||
::ros::NodeHandle* const node_handle, Node* const node) {
|
::ros::NodeHandle* const node_handle, Node* const node) {
|
||||||
return node_handle->subscribe<MessageType>(
|
return node_handle->subscribe<MessageType>(
|
||||||
topic, kInfiniteSubscriberQueueSize,
|
topic, kInfiniteSubscriberQueueSize,
|
||||||
|
@ -250,21 +250,21 @@ void Node::PublishConstraintList(
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
std::unordered_set<string> Node::ComputeExpectedTopics(
|
std::unordered_set<std::string> Node::ComputeExpectedTopics(
|
||||||
const TrajectoryOptions& options,
|
const TrajectoryOptions& options,
|
||||||
const cartographer_ros_msgs::SensorTopics& topics) {
|
const cartographer_ros_msgs::SensorTopics& topics) {
|
||||||
std::unordered_set<string> expected_topics;
|
std::unordered_set<std::string> expected_topics;
|
||||||
// Subscribe to all laser scan, multi echo laser scan, and point cloud topics.
|
// Subscribe to all laser scan, multi echo laser scan, and point cloud topics.
|
||||||
for (const string& topic : ComputeRepeatedTopicNames(
|
for (const std::string& topic : ComputeRepeatedTopicNames(
|
||||||
topics.laser_scan_topic, options.num_laser_scans)) {
|
topics.laser_scan_topic, options.num_laser_scans)) {
|
||||||
expected_topics.insert(topic);
|
expected_topics.insert(topic);
|
||||||
}
|
}
|
||||||
for (const string& topic :
|
for (const std::string& topic :
|
||||||
ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
|
ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
|
||||||
options.num_multi_echo_laser_scans)) {
|
options.num_multi_echo_laser_scans)) {
|
||||||
expected_topics.insert(topic);
|
expected_topics.insert(topic);
|
||||||
}
|
}
|
||||||
for (const string& topic : ComputeRepeatedTopicNames(
|
for (const std::string& topic : ComputeRepeatedTopicNames(
|
||||||
topics.point_cloud2_topic, options.num_point_clouds)) {
|
topics.point_cloud2_topic, options.num_point_clouds)) {
|
||||||
expected_topics.insert(topic);
|
expected_topics.insert(topic);
|
||||||
}
|
}
|
||||||
|
@ -285,7 +285,7 @@ std::unordered_set<string> Node::ComputeExpectedTopics(
|
||||||
|
|
||||||
int Node::AddTrajectory(const TrajectoryOptions& options,
|
int Node::AddTrajectory(const TrajectoryOptions& options,
|
||||||
const cartographer_ros_msgs::SensorTopics& topics) {
|
const cartographer_ros_msgs::SensorTopics& topics) {
|
||||||
const std::unordered_set<string> expected_sensor_ids =
|
const std::unordered_set<std::string> expected_sensor_ids =
|
||||||
ComputeExpectedTopics(options, topics);
|
ComputeExpectedTopics(options, topics);
|
||||||
const int trajectory_id =
|
const int trajectory_id =
|
||||||
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
|
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
|
||||||
|
@ -301,7 +301,7 @@ int Node::AddTrajectory(const TrajectoryOptions& options,
|
||||||
void Node::LaunchSubscribers(const TrajectoryOptions& options,
|
void Node::LaunchSubscribers(const TrajectoryOptions& options,
|
||||||
const cartographer_ros_msgs::SensorTopics& topics,
|
const cartographer_ros_msgs::SensorTopics& topics,
|
||||||
const int trajectory_id) {
|
const int trajectory_id) {
|
||||||
for (const string& topic : ComputeRepeatedTopicNames(
|
for (const std::string& topic : ComputeRepeatedTopicNames(
|
||||||
topics.laser_scan_topic, options.num_laser_scans)) {
|
topics.laser_scan_topic, options.num_laser_scans)) {
|
||||||
subscribers_[trajectory_id].push_back(
|
subscribers_[trajectory_id].push_back(
|
||||||
{SubscribeWithHandler<sensor_msgs::LaserScan>(
|
{SubscribeWithHandler<sensor_msgs::LaserScan>(
|
||||||
|
@ -309,7 +309,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
|
||||||
this),
|
this),
|
||||||
topic});
|
topic});
|
||||||
}
|
}
|
||||||
for (const string& topic :
|
for (const std::string& topic :
|
||||||
ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
|
ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
|
||||||
options.num_multi_echo_laser_scans)) {
|
options.num_multi_echo_laser_scans)) {
|
||||||
subscribers_[trajectory_id].push_back(
|
subscribers_[trajectory_id].push_back(
|
||||||
|
@ -318,7 +318,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
|
||||||
&node_handle_, this),
|
&node_handle_, this),
|
||||||
topic});
|
topic});
|
||||||
}
|
}
|
||||||
for (const string& topic : ComputeRepeatedTopicNames(
|
for (const std::string& topic : ComputeRepeatedTopicNames(
|
||||||
topics.point_cloud2_topic, options.num_point_clouds)) {
|
topics.point_cloud2_topic, options.num_point_clouds)) {
|
||||||
subscribers_[trajectory_id].push_back(
|
subscribers_[trajectory_id].push_back(
|
||||||
{SubscribeWithHandler<sensor_msgs::PointCloud2>(
|
{SubscribeWithHandler<sensor_msgs::PointCloud2>(
|
||||||
|
@ -333,7 +333,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
|
||||||
(node_options_.map_builder_options.use_trajectory_builder_2d() &&
|
(node_options_.map_builder_options.use_trajectory_builder_2d() &&
|
||||||
options.trajectory_builder_options.trajectory_builder_2d_options()
|
options.trajectory_builder_options.trajectory_builder_2d_options()
|
||||||
.use_imu_data())) {
|
.use_imu_data())) {
|
||||||
string topic = topics.imu_topic;
|
std::string topic = topics.imu_topic;
|
||||||
subscribers_[trajectory_id].push_back(
|
subscribers_[trajectory_id].push_back(
|
||||||
{SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
|
{SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
|
||||||
trajectory_id, topic,
|
trajectory_id, topic,
|
||||||
|
@ -342,7 +342,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
|
||||||
}
|
}
|
||||||
|
|
||||||
if (options.use_odometry) {
|
if (options.use_odometry) {
|
||||||
string topic = topics.odometry_topic;
|
std::string topic = topics.odometry_topic;
|
||||||
subscribers_[trajectory_id].push_back(
|
subscribers_[trajectory_id].push_back(
|
||||||
{SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
|
{SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
|
||||||
trajectory_id, topic,
|
trajectory_id, topic,
|
||||||
|
@ -399,14 +399,14 @@ void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
|
||||||
AddTrajectory(options, DefaultSensorTopics());
|
AddTrajectory(options, DefaultSensorTopics());
|
||||||
}
|
}
|
||||||
|
|
||||||
std::unordered_set<string> Node::ComputeDefaultTopics(
|
std::unordered_set<std::string> Node::ComputeDefaultTopics(
|
||||||
const TrajectoryOptions& options) {
|
const TrajectoryOptions& options) {
|
||||||
carto::common::MutexLocker lock(&mutex_);
|
carto::common::MutexLocker lock(&mutex_);
|
||||||
return ComputeExpectedTopics(options, DefaultSensorTopics());
|
return ComputeExpectedTopics(options, DefaultSensorTopics());
|
||||||
}
|
}
|
||||||
|
|
||||||
int Node::AddOfflineTrajectory(
|
int Node::AddOfflineTrajectory(
|
||||||
const std::unordered_set<string>& expected_sensor_ids,
|
const std::unordered_set<std::string>& expected_sensor_ids,
|
||||||
const TrajectoryOptions& options) {
|
const TrajectoryOptions& options) {
|
||||||
carto::common::MutexLocker lock(&mutex_);
|
carto::common::MutexLocker lock(&mutex_);
|
||||||
const int trajectory_id =
|
const int trajectory_id =
|
||||||
|
@ -483,7 +483,7 @@ void Node::RunFinalOptimization() {
|
||||||
}
|
}
|
||||||
|
|
||||||
void Node::HandleOdometryMessage(const int trajectory_id,
|
void Node::HandleOdometryMessage(const int trajectory_id,
|
||||||
const string& sensor_id,
|
const std::string& sensor_id,
|
||||||
const nav_msgs::Odometry::ConstPtr& msg) {
|
const nav_msgs::Odometry::ConstPtr& msg) {
|
||||||
carto::common::MutexLocker lock(&mutex_);
|
carto::common::MutexLocker lock(&mutex_);
|
||||||
if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) {
|
if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) {
|
||||||
|
@ -497,7 +497,8 @@ void Node::HandleOdometryMessage(const int trajectory_id,
|
||||||
sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
|
sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Node::HandleImuMessage(const int trajectory_id, const string& sensor_id,
|
void Node::HandleImuMessage(const int trajectory_id,
|
||||||
|
const std::string& sensor_id,
|
||||||
const sensor_msgs::Imu::ConstPtr& msg) {
|
const sensor_msgs::Imu::ConstPtr& msg) {
|
||||||
carto::common::MutexLocker lock(&mutex_);
|
carto::common::MutexLocker lock(&mutex_);
|
||||||
if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
|
if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
|
||||||
|
@ -512,7 +513,7 @@ void Node::HandleImuMessage(const int trajectory_id, const string& sensor_id,
|
||||||
}
|
}
|
||||||
|
|
||||||
void Node::HandleLaserScanMessage(const int trajectory_id,
|
void Node::HandleLaserScanMessage(const int trajectory_id,
|
||||||
const string& sensor_id,
|
const std::string& sensor_id,
|
||||||
const sensor_msgs::LaserScan::ConstPtr& msg) {
|
const sensor_msgs::LaserScan::ConstPtr& msg) {
|
||||||
carto::common::MutexLocker lock(&mutex_);
|
carto::common::MutexLocker lock(&mutex_);
|
||||||
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
|
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
|
||||||
|
@ -523,7 +524,7 @@ void Node::HandleLaserScanMessage(const int trajectory_id,
|
||||||
}
|
}
|
||||||
|
|
||||||
void Node::HandleMultiEchoLaserScanMessage(
|
void Node::HandleMultiEchoLaserScanMessage(
|
||||||
int trajectory_id, const string& sensor_id,
|
int trajectory_id, const std::string& sensor_id,
|
||||||
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
|
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
|
||||||
carto::common::MutexLocker lock(&mutex_);
|
carto::common::MutexLocker lock(&mutex_);
|
||||||
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
|
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
|
||||||
|
@ -534,7 +535,7 @@ void Node::HandleMultiEchoLaserScanMessage(
|
||||||
}
|
}
|
||||||
|
|
||||||
void Node::HandlePointCloud2Message(
|
void Node::HandlePointCloud2Message(
|
||||||
const int trajectory_id, const string& sensor_id,
|
const int trajectory_id, const std::string& sensor_id,
|
||||||
const sensor_msgs::PointCloud2::ConstPtr& msg) {
|
const sensor_msgs::PointCloud2::ConstPtr& msg) {
|
||||||
carto::common::MutexLocker lock(&mutex_);
|
carto::common::MutexLocker lock(&mutex_);
|
||||||
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
|
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
|
||||||
|
@ -544,7 +545,7 @@ void Node::HandlePointCloud2Message(
|
||||||
->HandlePointCloud2Message(sensor_id, msg);
|
->HandlePointCloud2Message(sensor_id, msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Node::SerializeState(const string& filename) {
|
void Node::SerializeState(const std::string& filename) {
|
||||||
carto::common::MutexLocker lock(&mutex_);
|
carto::common::MutexLocker lock(&mutex_);
|
||||||
map_builder_bridge_.SerializeState(filename);
|
map_builder_bridge_.SerializeState(filename);
|
||||||
}
|
}
|
||||||
|
|
|
@ -64,29 +64,29 @@ class Node {
|
||||||
void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
|
void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
|
||||||
|
|
||||||
// Compute the default topics for the given 'options'.
|
// Compute the default topics for the given 'options'.
|
||||||
std::unordered_set<string> ComputeDefaultTopics(
|
std::unordered_set<std::string> ComputeDefaultTopics(
|
||||||
const TrajectoryOptions& options);
|
const TrajectoryOptions& options);
|
||||||
|
|
||||||
// Adds a trajectory for offline processing, i.e. not listening to topics.
|
// Adds a trajectory for offline processing, i.e. not listening to topics.
|
||||||
int AddOfflineTrajectory(
|
int AddOfflineTrajectory(
|
||||||
const std::unordered_set<string>& expected_sensor_ids,
|
const std::unordered_set<std::string>& expected_sensor_ids,
|
||||||
const TrajectoryOptions& options);
|
const TrajectoryOptions& options);
|
||||||
|
|
||||||
// The following functions handle adding sensor data to a trajectory.
|
// The following functions handle adding sensor data to a trajectory.
|
||||||
void HandleOdometryMessage(int trajectory_id, const string& sensor_id,
|
void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id,
|
||||||
const nav_msgs::Odometry::ConstPtr& msg);
|
const nav_msgs::Odometry::ConstPtr& msg);
|
||||||
void HandleImuMessage(int trajectory_id, const string& sensor_id,
|
void HandleImuMessage(int trajectory_id, const std::string& sensor_id,
|
||||||
const sensor_msgs::Imu::ConstPtr& msg);
|
const sensor_msgs::Imu::ConstPtr& msg);
|
||||||
void HandleLaserScanMessage(int trajectory_id, const string& sensor_id,
|
void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id,
|
||||||
const sensor_msgs::LaserScan::ConstPtr& msg);
|
const sensor_msgs::LaserScan::ConstPtr& msg);
|
||||||
void HandleMultiEchoLaserScanMessage(
|
void HandleMultiEchoLaserScanMessage(
|
||||||
int trajectory_id, const string& sensor_id,
|
int trajectory_id, const std::string& sensor_id,
|
||||||
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
|
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
|
||||||
void HandlePointCloud2Message(int trajectory_id, const string& sensor_id,
|
void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id,
|
||||||
const sensor_msgs::PointCloud2::ConstPtr& msg);
|
const sensor_msgs::PointCloud2::ConstPtr& msg);
|
||||||
|
|
||||||
// Serializes the complete Node state.
|
// Serializes the complete Node state.
|
||||||
void SerializeState(const string& filename);
|
void SerializeState(const std::string& filename);
|
||||||
|
|
||||||
// Loads a persisted state to use as a map.
|
// Loads a persisted state to use as a map.
|
||||||
void LoadMap(const std::string& map_filename);
|
void LoadMap(const std::string& map_filename);
|
||||||
|
@ -97,10 +97,11 @@ class Node {
|
||||||
struct Subscriber {
|
struct Subscriber {
|
||||||
::ros::Subscriber subscriber;
|
::ros::Subscriber subscriber;
|
||||||
|
|
||||||
// ::ros::Subscriber::getTopic() does not necessarily return the same string
|
// ::ros::Subscriber::getTopic() does not necessarily return the same
|
||||||
|
// std::string
|
||||||
// it was given in its constructor. Since we rely on the topic name as the
|
// it was given in its constructor. Since we rely on the topic name as the
|
||||||
// unique identifier of a subscriber, we remember it ourselves.
|
// unique identifier of a subscriber, we remember it ourselves.
|
||||||
string topic;
|
std::string topic;
|
||||||
};
|
};
|
||||||
|
|
||||||
bool HandleSubmapQuery(
|
bool HandleSubmapQuery(
|
||||||
|
@ -115,7 +116,7 @@ class Node {
|
||||||
bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request,
|
bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request,
|
||||||
cartographer_ros_msgs::WriteState::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<std::string> ComputeExpectedTopics(
|
||||||
const TrajectoryOptions& options,
|
const TrajectoryOptions& options,
|
||||||
const cartographer_ros_msgs::SensorTopics& topics);
|
const cartographer_ros_msgs::SensorTopics& topics);
|
||||||
int AddTrajectory(const TrajectoryOptions& options,
|
int AddTrajectory(const TrajectoryOptions& options,
|
||||||
|
|
|
@ -44,12 +44,12 @@ NodeOptions CreateNodeOptions(
|
||||||
}
|
}
|
||||||
|
|
||||||
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
|
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
|
||||||
const string& configuration_directory,
|
const std::string& configuration_directory,
|
||||||
const string& configuration_basename) {
|
const std::string& configuration_basename) {
|
||||||
auto file_resolver = cartographer::common::make_unique<
|
auto file_resolver = cartographer::common::make_unique<
|
||||||
cartographer::common::ConfigurationFileResolver>(
|
cartographer::common::ConfigurationFileResolver>(
|
||||||
std::vector<string>{configuration_directory});
|
std::vector<std::string>{configuration_directory});
|
||||||
const string code =
|
const std::string code =
|
||||||
file_resolver->GetFileContentOrDie(configuration_basename);
|
file_resolver->GetFileContentOrDie(configuration_basename);
|
||||||
cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
|
cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
|
||||||
code, std::move(file_resolver));
|
code, std::move(file_resolver));
|
||||||
|
|
|
@ -30,7 +30,7 @@ namespace cartographer_ros {
|
||||||
// Top-level options of Cartographer's ROS integration.
|
// Top-level options of Cartographer's ROS integration.
|
||||||
struct NodeOptions {
|
struct NodeOptions {
|
||||||
::cartographer::mapping::proto::MapBuilderOptions map_builder_options;
|
::cartographer::mapping::proto::MapBuilderOptions map_builder_options;
|
||||||
string map_frame;
|
std::string map_frame;
|
||||||
double lookup_transform_timeout_sec;
|
double lookup_transform_timeout_sec;
|
||||||
double submap_publish_period_sec;
|
double submap_publish_period_sec;
|
||||||
double pose_publish_period_sec;
|
double pose_publish_period_sec;
|
||||||
|
@ -41,8 +41,8 @@ NodeOptions CreateNodeOptions(
|
||||||
::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
|
::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
|
||||||
|
|
||||||
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
|
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
|
||||||
const string& configuration_directory,
|
const std::string& configuration_directory,
|
||||||
const string& configuration_basename);
|
const std::string& configuration_basename);
|
||||||
|
|
||||||
} // namespace cartographer_ros
|
} // namespace cartographer_ros
|
||||||
|
|
||||||
|
|
|
@ -107,7 +107,7 @@ class Node {
|
||||||
private:
|
private:
|
||||||
void HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg);
|
void HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg);
|
||||||
void DrawAndPublish(const ::ros::WallTimerEvent& timer_event);
|
void DrawAndPublish(const ::ros::WallTimerEvent& timer_event);
|
||||||
void PublishOccupancyGrid(const string& frame_id, const ros::Time& time,
|
void PublishOccupancyGrid(const std::string& frame_id, const ros::Time& time,
|
||||||
const Eigen::Array2f& origin,
|
const Eigen::Array2f& origin,
|
||||||
const Eigen::Array2i& size,
|
const Eigen::Array2i& size,
|
||||||
cairo_surface_t* surface);
|
cairo_surface_t* surface);
|
||||||
|
@ -261,7 +261,8 @@ void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void Node::PublishOccupancyGrid(const string& frame_id, const ros::Time& time,
|
void Node::PublishOccupancyGrid(const std::string& frame_id,
|
||||||
|
const ros::Time& time,
|
||||||
const Eigen::Array2f& origin,
|
const Eigen::Array2f& origin,
|
||||||
const Eigen::Array2i& size,
|
const Eigen::Array2i& size,
|
||||||
cairo_surface_t* surface) {
|
cairo_surface_t* surface) {
|
||||||
|
@ -281,12 +282,12 @@ void Node::PublishOccupancyGrid(const string& frame_id, const ros::Time& time,
|
||||||
occupancy_grid.info.origin.orientation.y = 0.;
|
occupancy_grid.info.origin.orientation.y = 0.;
|
||||||
occupancy_grid.info.origin.orientation.z = 0.;
|
occupancy_grid.info.origin.orientation.z = 0.;
|
||||||
|
|
||||||
const uint32* pixel_data =
|
const uint32_t* pixel_data =
|
||||||
reinterpret_cast<uint32*>(cairo_image_surface_get_data(surface));
|
reinterpret_cast<uint32_t*>(cairo_image_surface_get_data(surface));
|
||||||
occupancy_grid.data.reserve(size.x() * size.y());
|
occupancy_grid.data.reserve(size.x() * size.y());
|
||||||
for (int y = size.y() - 1; y >= 0; --y) {
|
for (int y = size.y() - 1; y >= 0; --y) {
|
||||||
for (int x = 0; x < size.x(); ++x) {
|
for (int x = 0; x < size.x(); ++x) {
|
||||||
const uint32 packed = pixel_data[y * size.x() + x];
|
const uint32_t packed = pixel_data[y * size.x() + x];
|
||||||
const unsigned char color = packed >> 16;
|
const unsigned char color = packed >> 16;
|
||||||
const unsigned char observed = packed >> 8;
|
const unsigned char observed = packed >> 8;
|
||||||
const int value =
|
const int value =
|
||||||
|
|
|
@ -16,8 +16,8 @@
|
||||||
|
|
||||||
#include <errno.h>
|
#include <errno.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include <sys/time.h>
|
|
||||||
#include <sys/resource.h>
|
#include <sys/resource.h>
|
||||||
|
#include <sys/time.h>
|
||||||
#include <time.h>
|
#include <time.h>
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
#include <sstream>
|
#include <sstream>
|
||||||
|
@ -68,7 +68,7 @@ constexpr char kTfTopic[] = "tf";
|
||||||
constexpr double kClockPublishFrequencySec = 1. / 30.;
|
constexpr double kClockPublishFrequencySec = 1. / 30.;
|
||||||
constexpr int kSingleThreaded = 1;
|
constexpr int kSingleThreaded = 1;
|
||||||
|
|
||||||
void Run(const std::vector<string>& bag_filenames) {
|
void Run(const std::vector<std::string>& bag_filenames) {
|
||||||
const std::chrono::time_point<std::chrono::steady_clock> start_time =
|
const std::chrono::time_point<std::chrono::steady_clock> start_time =
|
||||||
std::chrono::steady_clock::now();
|
std::chrono::steady_clock::now();
|
||||||
NodeOptions node_options;
|
NodeOptions node_options;
|
||||||
|
@ -97,8 +97,9 @@ void Run(const std::vector<string>& bag_filenames) {
|
||||||
node.LoadMap(FLAGS_pbstream_filename);
|
node.LoadMap(FLAGS_pbstream_filename);
|
||||||
}
|
}
|
||||||
|
|
||||||
std::unordered_set<string> expected_sensor_ids;
|
std::unordered_set<std::string> expected_sensor_ids;
|
||||||
for (const string& topic : node.ComputeDefaultTopics(trajectory_options)) {
|
for (const std::string& topic :
|
||||||
|
node.ComputeDefaultTopics(trajectory_options)) {
|
||||||
CHECK(expected_sensor_ids.insert(node.node_handle()->resolveName(topic))
|
CHECK(expected_sensor_ids.insert(node.node_handle()->resolveName(topic))
|
||||||
.second);
|
.second);
|
||||||
}
|
}
|
||||||
|
@ -127,7 +128,7 @@ void Run(const std::vector<string>& bag_filenames) {
|
||||||
},
|
},
|
||||||
false /* oneshot */, false /* autostart */);
|
false /* oneshot */, false /* autostart */);
|
||||||
|
|
||||||
for (const string& bag_filename : bag_filenames) {
|
for (const std::string& bag_filename : bag_filenames) {
|
||||||
if (!::ros::ok()) {
|
if (!::ros::ok()) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -169,10 +170,9 @@ void Run(const std::vector<string>& bag_filenames) {
|
||||||
}
|
}
|
||||||
|
|
||||||
while (!delayed_messages.empty() &&
|
while (!delayed_messages.empty() &&
|
||||||
delayed_messages.front().getTime() <
|
delayed_messages.front().getTime() < msg.getTime() - kDelay) {
|
||||||
msg.getTime() - kDelay) {
|
|
||||||
const rosbag::MessageInstance& delayed_msg = delayed_messages.front();
|
const rosbag::MessageInstance& delayed_msg = delayed_messages.front();
|
||||||
const string topic = node.node_handle()->resolveName(
|
const std::string topic = node.node_handle()->resolveName(
|
||||||
delayed_msg.getTopic(), false /* resolve */);
|
delayed_msg.getTopic(), false /* resolve */);
|
||||||
if (delayed_msg.isType<sensor_msgs::LaserScan>()) {
|
if (delayed_msg.isType<sensor_msgs::LaserScan>()) {
|
||||||
node.HandleLaserScanMessage(
|
node.HandleLaserScanMessage(
|
||||||
|
@ -208,7 +208,7 @@ void Run(const std::vector<string>& bag_filenames) {
|
||||||
delayed_messages.pop_front();
|
delayed_messages.pop_front();
|
||||||
}
|
}
|
||||||
|
|
||||||
const string topic =
|
const std::string topic =
|
||||||
node.node_handle()->resolveName(msg.getTopic(), false /* resolve */);
|
node.node_handle()->resolveName(msg.getTopic(), false /* resolve */);
|
||||||
if (expected_sensor_ids.count(topic) == 0) {
|
if (expected_sensor_ids.count(topic) == 0) {
|
||||||
continue;
|
continue;
|
||||||
|
@ -245,7 +245,7 @@ void Run(const std::vector<string>& bag_filenames) {
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (::ros::ok()) {
|
if (::ros::ok()) {
|
||||||
const string output_filename = bag_filenames.front() + ".pbstream";
|
const std::string output_filename = bag_filenames.front() + ".pbstream";
|
||||||
LOG(INFO) << "Writing state to '" << output_filename << "'...";
|
LOG(INFO) << "Writing state to '" << output_filename << "'...";
|
||||||
node.SerializeState(output_filename);
|
node.SerializeState(output_filename);
|
||||||
}
|
}
|
||||||
|
|
|
@ -42,7 +42,7 @@ void WritePgm(const ::cartographer::io::Image& image, const double resolution,
|
||||||
|
|
||||||
void WriteYaml(const ::cartographer::io::Image& image,
|
void WriteYaml(const ::cartographer::io::Image& image,
|
||||||
const ::cartographer::mapping_2d::MapLimits& limits,
|
const ::cartographer::mapping_2d::MapLimits& limits,
|
||||||
const string& pgm_filename, const Eigen::Array2i& offset,
|
const std::string& pgm_filename, const Eigen::Array2i& offset,
|
||||||
::cartographer::io::FileWriter* file_writer) {
|
::cartographer::io::FileWriter* file_writer) {
|
||||||
const double resolution = limits.resolution();
|
const double resolution = limits.resolution();
|
||||||
const double x_offset =
|
const double x_offset =
|
||||||
|
@ -66,7 +66,8 @@ RosMapWritingPointsProcessor::RosMapWritingPointsProcessor(
|
||||||
const ::cartographer::mapping_2d::proto::RangeDataInserterOptions&
|
const ::cartographer::mapping_2d::proto::RangeDataInserterOptions&
|
||||||
range_data_inserter_options,
|
range_data_inserter_options,
|
||||||
::cartographer::io::FileWriterFactory file_writer_factory,
|
::cartographer::io::FileWriterFactory file_writer_factory,
|
||||||
const string& filestem, ::cartographer::io::PointsProcessor* const next)
|
const std::string& filestem,
|
||||||
|
::cartographer::io::PointsProcessor* const next)
|
||||||
: filestem_(filestem),
|
: filestem_(filestem),
|
||||||
next_(next),
|
next_(next),
|
||||||
file_writer_factory_(file_writer_factory),
|
file_writer_factory_(file_writer_factory),
|
||||||
|
|
|
@ -36,7 +36,7 @@ class RosMapWritingPointsProcessor
|
||||||
const ::cartographer::mapping_2d::proto::RangeDataInserterOptions&
|
const ::cartographer::mapping_2d::proto::RangeDataInserterOptions&
|
||||||
range_data_inserter_options,
|
range_data_inserter_options,
|
||||||
::cartographer::io::FileWriterFactory file_writer_factory,
|
::cartographer::io::FileWriterFactory file_writer_factory,
|
||||||
const string& filestem, PointsProcessor* next);
|
const std::string& filestem, PointsProcessor* next);
|
||||||
RosMapWritingPointsProcessor(const RosMapWritingPointsProcessor&) = delete;
|
RosMapWritingPointsProcessor(const RosMapWritingPointsProcessor&) = delete;
|
||||||
RosMapWritingPointsProcessor& operator=(const RosMapWritingPointsProcessor&) =
|
RosMapWritingPointsProcessor& operator=(const RosMapWritingPointsProcessor&) =
|
||||||
delete;
|
delete;
|
||||||
|
@ -52,7 +52,7 @@ class RosMapWritingPointsProcessor
|
||||||
FlushResult Flush() override;
|
FlushResult Flush() override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const string filestem_;
|
const std::string filestem_;
|
||||||
PointsProcessor* const next_;
|
PointsProcessor* const next_;
|
||||||
::cartographer::io::FileWriterFactory file_writer_factory_;
|
::cartographer::io::FileWriterFactory file_writer_factory_;
|
||||||
::cartographer::mapping_2d::RangeDataInserter range_data_inserter_;
|
::cartographer::mapping_2d::RangeDataInserter range_data_inserter_;
|
||||||
|
|
|
@ -47,17 +47,18 @@ namespace {
|
||||||
|
|
||||||
struct PerFrameId {
|
struct PerFrameId {
|
||||||
ros::Time last_timestamp;
|
ros::Time last_timestamp;
|
||||||
string topic;
|
std::string topic;
|
||||||
::cartographer::common::Histogram histogram;
|
::cartographer::common::Histogram histogram;
|
||||||
std::unique_ptr<std::ofstream> timing_file;
|
std::unique_ptr<std::ofstream> timing_file;
|
||||||
};
|
};
|
||||||
|
|
||||||
std::unique_ptr<std::ofstream> CreateTimingFile(const string& frame_id) {
|
std::unique_ptr<std::ofstream> CreateTimingFile(const std::string& frame_id) {
|
||||||
auto timing_file = ::cartographer::common::make_unique<std::ofstream>(
|
auto timing_file = ::cartographer::common::make_unique<std::ofstream>(
|
||||||
std::string("timing_") + frame_id + ".csv", std::ios_base::out);
|
std::string("timing_") + frame_id + ".csv", std::ios_base::out);
|
||||||
|
|
||||||
(*timing_file) << "# Timing information for sensor with frame id: "
|
(*timing_file)
|
||||||
<< frame_id << std::endl
|
<< "# Timing information for sensor with frame id: " << frame_id
|
||||||
|
<< std::endl
|
||||||
<< "# Columns are in order" << std::endl
|
<< "# Columns are in order" << std::endl
|
||||||
<< "# - packet index of the packet in the bag, first packet is 1"
|
<< "# - packet index of the packet in the bag, first packet is 1"
|
||||||
<< std::endl
|
<< std::endl
|
||||||
|
@ -75,16 +76,16 @@ std::unique_ptr<std::ofstream> CreateTimingFile(const string& frame_id) {
|
||||||
return timing_file;
|
return timing_file;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Run(const string& bag_filename, const bool dump_timing) {
|
void Run(const std::string& bag_filename, const bool dump_timing) {
|
||||||
rosbag::Bag bag;
|
rosbag::Bag bag;
|
||||||
bag.open(bag_filename, rosbag::bagmode::Read);
|
bag.open(bag_filename, rosbag::bagmode::Read);
|
||||||
rosbag::View view(bag);
|
rosbag::View view(bag);
|
||||||
|
|
||||||
std::map<string, PerFrameId> per_frame_id;
|
std::map<std::string, PerFrameId> per_frame_id;
|
||||||
size_t message_index = 0;
|
size_t message_index = 0;
|
||||||
for (const rosbag::MessageInstance& message : view) {
|
for (const rosbag::MessageInstance& message : view) {
|
||||||
++message_index;
|
++message_index;
|
||||||
string frame_id;
|
std::string frame_id;
|
||||||
ros::Time time;
|
ros::Time time;
|
||||||
if (message.isType<sensor_msgs::PointCloud2>()) {
|
if (message.isType<sensor_msgs::PointCloud2>()) {
|
||||||
auto msg = message.instantiate<sensor_msgs::PointCloud2>();
|
auto msg = message.instantiate<sensor_msgs::PointCloud2>();
|
||||||
|
|
|
@ -28,7 +28,7 @@ using carto::transform::Rigid3d;
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
const string& CheckNoLeadingSlash(const string& frame_id) {
|
const std::string& CheckNoLeadingSlash(const std::string& frame_id) {
|
||||||
if (frame_id.size() > 0) {
|
if (frame_id.size() > 0) {
|
||||||
CHECK_NE(frame_id[0], '/') << "The frame_id " << frame_id
|
CHECK_NE(frame_id[0], '/') << "The frame_id " << frame_id
|
||||||
<< " should not start with a /. See 1.7 in "
|
<< " should not start with a /. See 1.7 in "
|
||||||
|
@ -40,7 +40,8 @@ const string& CheckNoLeadingSlash(const string& frame_id) {
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
SensorBridge::SensorBridge(
|
SensorBridge::SensorBridge(
|
||||||
const int num_subdivisions_per_laser_scan, const string& tracking_frame,
|
const int num_subdivisions_per_laser_scan,
|
||||||
|
const std::string& tracking_frame,
|
||||||
const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
|
const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
|
||||||
carto::mapping::TrajectoryBuilder* const trajectory_builder)
|
carto::mapping::TrajectoryBuilder* const trajectory_builder)
|
||||||
: num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
|
: num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
|
||||||
|
@ -62,7 +63,7 @@ SensorBridge::ToOdometryData(const nav_msgs::Odometry::ConstPtr& msg) {
|
||||||
}
|
}
|
||||||
|
|
||||||
void SensorBridge::HandleOdometryMessage(
|
void SensorBridge::HandleOdometryMessage(
|
||||||
const string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
|
const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
|
||||||
std::unique_ptr<::cartographer::sensor::OdometryData> odometry_data =
|
std::unique_ptr<::cartographer::sensor::OdometryData> odometry_data =
|
||||||
ToOdometryData(msg);
|
ToOdometryData(msg);
|
||||||
if (odometry_data != nullptr) {
|
if (odometry_data != nullptr) {
|
||||||
|
@ -101,7 +102,7 @@ std::unique_ptr<::cartographer::sensor::ImuData> SensorBridge::ToImuData(
|
||||||
sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)});
|
sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)});
|
||||||
}
|
}
|
||||||
|
|
||||||
void SensorBridge::HandleImuMessage(const string& sensor_id,
|
void SensorBridge::HandleImuMessage(const std::string& sensor_id,
|
||||||
const sensor_msgs::Imu::ConstPtr& msg) {
|
const sensor_msgs::Imu::ConstPtr& msg) {
|
||||||
std::unique_ptr<::cartographer::sensor::ImuData> imu_data = ToImuData(msg);
|
std::unique_ptr<::cartographer::sensor::ImuData> imu_data = ToImuData(msg);
|
||||||
if (imu_data != nullptr) {
|
if (imu_data != nullptr) {
|
||||||
|
@ -112,20 +113,21 @@ void SensorBridge::HandleImuMessage(const string& sensor_id,
|
||||||
}
|
}
|
||||||
|
|
||||||
void SensorBridge::HandleLaserScanMessage(
|
void SensorBridge::HandleLaserScanMessage(
|
||||||
const string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
|
const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
|
||||||
HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
|
HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
|
||||||
ToPointCloudWithIntensities(*msg));
|
ToPointCloudWithIntensities(*msg));
|
||||||
}
|
}
|
||||||
|
|
||||||
void SensorBridge::HandleMultiEchoLaserScanMessage(
|
void SensorBridge::HandleMultiEchoLaserScanMessage(
|
||||||
const string& sensor_id,
|
const std::string& sensor_id,
|
||||||
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
|
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
|
||||||
HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
|
HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
|
||||||
ToPointCloudWithIntensities(*msg));
|
ToPointCloudWithIntensities(*msg));
|
||||||
}
|
}
|
||||||
|
|
||||||
void SensorBridge::HandlePointCloud2Message(
|
void SensorBridge::HandlePointCloud2Message(
|
||||||
const string& sensor_id, const sensor_msgs::PointCloud2::ConstPtr& msg) {
|
const std::string& sensor_id,
|
||||||
|
const sensor_msgs::PointCloud2::ConstPtr& msg) {
|
||||||
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
|
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
|
||||||
pcl::fromROSMsg(*msg, pcl_point_cloud);
|
pcl::fromROSMsg(*msg, pcl_point_cloud);
|
||||||
carto::sensor::TimedPointCloud point_cloud;
|
carto::sensor::TimedPointCloud point_cloud;
|
||||||
|
@ -139,8 +141,8 @@ void SensorBridge::HandlePointCloud2Message(
|
||||||
const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
|
const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
|
||||||
|
|
||||||
void SensorBridge::HandleLaserScan(
|
void SensorBridge::HandleLaserScan(
|
||||||
const string& sensor_id, const carto::common::Time start_time,
|
const std::string& sensor_id, const carto::common::Time start_time,
|
||||||
const string& frame_id,
|
const std::string& frame_id,
|
||||||
const carto::sensor::PointCloudWithIntensities& points) {
|
const carto::sensor::PointCloudWithIntensities& points) {
|
||||||
// TODO(gaschler): Use per-point time instead of subdivisions.
|
// TODO(gaschler): Use per-point time instead of subdivisions.
|
||||||
for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
|
for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
|
||||||
|
@ -162,8 +164,8 @@ void SensorBridge::HandleLaserScan(
|
||||||
}
|
}
|
||||||
|
|
||||||
void SensorBridge::HandleRangefinder(
|
void SensorBridge::HandleRangefinder(
|
||||||
const string& sensor_id, const carto::common::Time time,
|
const std::string& sensor_id, const carto::common::Time time,
|
||||||
const string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
|
const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
|
||||||
const auto sensor_to_tracking =
|
const auto sensor_to_tracking =
|
||||||
tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
|
tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
|
||||||
if (sensor_to_tracking != nullptr) {
|
if (sensor_to_tracking != nullptr) {
|
||||||
|
|
|
@ -40,7 +40,7 @@ namespace cartographer_ros {
|
||||||
class SensorBridge {
|
class SensorBridge {
|
||||||
public:
|
public:
|
||||||
explicit SensorBridge(
|
explicit SensorBridge(
|
||||||
int num_subdivisions_per_laser_scan, const string& tracking_frame,
|
int num_subdivisions_per_laser_scan, const std::string& tracking_frame,
|
||||||
double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer,
|
double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer,
|
||||||
::cartographer::mapping::TrajectoryBuilder* trajectory_builder);
|
::cartographer::mapping::TrajectoryBuilder* trajectory_builder);
|
||||||
|
|
||||||
|
@ -49,30 +49,30 @@ class SensorBridge {
|
||||||
|
|
||||||
std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData(
|
std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData(
|
||||||
const nav_msgs::Odometry::ConstPtr& msg);
|
const nav_msgs::Odometry::ConstPtr& msg);
|
||||||
void HandleOdometryMessage(const string& sensor_id,
|
void HandleOdometryMessage(const std::string& sensor_id,
|
||||||
const nav_msgs::Odometry::ConstPtr& msg);
|
const nav_msgs::Odometry::ConstPtr& msg);
|
||||||
std::unique_ptr<::cartographer::sensor::ImuData> ToImuData(
|
std::unique_ptr<::cartographer::sensor::ImuData> ToImuData(
|
||||||
const sensor_msgs::Imu::ConstPtr& msg);
|
const sensor_msgs::Imu::ConstPtr& msg);
|
||||||
void HandleImuMessage(const string& sensor_id,
|
void HandleImuMessage(const std::string& sensor_id,
|
||||||
const sensor_msgs::Imu::ConstPtr& msg);
|
const sensor_msgs::Imu::ConstPtr& msg);
|
||||||
void HandleLaserScanMessage(const string& sensor_id,
|
void HandleLaserScanMessage(const std::string& sensor_id,
|
||||||
const sensor_msgs::LaserScan::ConstPtr& msg);
|
const sensor_msgs::LaserScan::ConstPtr& msg);
|
||||||
void HandleMultiEchoLaserScanMessage(
|
void HandleMultiEchoLaserScanMessage(
|
||||||
const string& sensor_id,
|
const std::string& sensor_id,
|
||||||
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
|
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
|
||||||
void HandlePointCloud2Message(const string& sensor_id,
|
void HandlePointCloud2Message(const std::string& sensor_id,
|
||||||
const sensor_msgs::PointCloud2::ConstPtr& msg);
|
const sensor_msgs::PointCloud2::ConstPtr& msg);
|
||||||
|
|
||||||
const TfBridge& tf_bridge() const;
|
const TfBridge& tf_bridge() const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void HandleLaserScan(
|
void HandleLaserScan(
|
||||||
const string& sensor_id, ::cartographer::common::Time start_time,
|
const std::string& sensor_id, ::cartographer::common::Time start_time,
|
||||||
const string& frame_id,
|
const std::string& frame_id,
|
||||||
const ::cartographer::sensor::PointCloudWithIntensities& points);
|
const ::cartographer::sensor::PointCloudWithIntensities& points);
|
||||||
void HandleRangefinder(const string& sensor_id,
|
void HandleRangefinder(const std::string& sensor_id,
|
||||||
::cartographer::common::Time time,
|
::cartographer::common::Time time,
|
||||||
const string& frame_id,
|
const std::string& frame_id,
|
||||||
const ::cartographer::sensor::TimedPointCloud& ranges);
|
const ::cartographer::sensor::TimedPointCloud& ranges);
|
||||||
|
|
||||||
const int num_subdivisions_per_laser_scan_;
|
const int num_subdivisions_per_laser_scan_;
|
||||||
|
|
|
@ -43,8 +43,8 @@ namespace {
|
||||||
TrajectoryOptions LoadOptions() {
|
TrajectoryOptions LoadOptions() {
|
||||||
auto file_resolver = cartographer::common::make_unique<
|
auto file_resolver = cartographer::common::make_unique<
|
||||||
cartographer::common::ConfigurationFileResolver>(
|
cartographer::common::ConfigurationFileResolver>(
|
||||||
std::vector<string>{FLAGS_configuration_directory});
|
std::vector<std::string>{FLAGS_configuration_directory});
|
||||||
const string code =
|
const std::string code =
|
||||||
file_resolver->GetFileContentOrDie(FLAGS_configuration_basename);
|
file_resolver->GetFileContentOrDie(FLAGS_configuration_basename);
|
||||||
auto lua_parameter_dictionary =
|
auto lua_parameter_dictionary =
|
||||||
cartographer::common::LuaParameterDictionary::NonReferenceCounted(
|
cartographer::common::LuaParameterDictionary::NonReferenceCounted(
|
||||||
|
|
|
@ -21,7 +21,7 @@
|
||||||
|
|
||||||
namespace cartographer_ros {
|
namespace cartographer_ros {
|
||||||
|
|
||||||
TfBridge::TfBridge(const string& tracking_frame,
|
TfBridge::TfBridge(const std::string& tracking_frame,
|
||||||
const double lookup_transform_timeout_sec,
|
const double lookup_transform_timeout_sec,
|
||||||
const tf2_ros::Buffer* buffer)
|
const tf2_ros::Buffer* buffer)
|
||||||
: tracking_frame_(tracking_frame),
|
: tracking_frame_(tracking_frame),
|
||||||
|
@ -29,7 +29,8 @@ TfBridge::TfBridge(const string& tracking_frame,
|
||||||
buffer_(buffer) {}
|
buffer_(buffer) {}
|
||||||
|
|
||||||
std::unique_ptr<::cartographer::transform::Rigid3d> TfBridge::LookupToTracking(
|
std::unique_ptr<::cartographer::transform::Rigid3d> TfBridge::LookupToTracking(
|
||||||
const ::cartographer::common::Time time, const string& frame_id) const {
|
const ::cartographer::common::Time time,
|
||||||
|
const std::string& frame_id) const {
|
||||||
::ros::Duration timeout(lookup_transform_timeout_sec_);
|
::ros::Duration timeout(lookup_transform_timeout_sec_);
|
||||||
std::unique_ptr<::cartographer::transform::Rigid3d> frame_id_to_tracking;
|
std::unique_ptr<::cartographer::transform::Rigid3d> frame_id_to_tracking;
|
||||||
try {
|
try {
|
||||||
|
|
|
@ -28,8 +28,8 @@ namespace cartographer_ros {
|
||||||
|
|
||||||
class TfBridge {
|
class TfBridge {
|
||||||
public:
|
public:
|
||||||
TfBridge(const string& tracking_frame, double lookup_transform_timeout_sec,
|
TfBridge(const std::string& tracking_frame,
|
||||||
const tf2_ros::Buffer* buffer);
|
double lookup_transform_timeout_sec, const tf2_ros::Buffer* buffer);
|
||||||
~TfBridge() {}
|
~TfBridge() {}
|
||||||
|
|
||||||
TfBridge(const TfBridge&) = delete;
|
TfBridge(const TfBridge&) = delete;
|
||||||
|
@ -38,10 +38,10 @@ class TfBridge {
|
||||||
// Returns the transform for 'frame_id' to 'tracking_frame_' if it exists at
|
// Returns the transform for 'frame_id' to 'tracking_frame_' if it exists at
|
||||||
// 'time'.
|
// 'time'.
|
||||||
std::unique_ptr<::cartographer::transform::Rigid3d> LookupToTracking(
|
std::unique_ptr<::cartographer::transform::Rigid3d> LookupToTracking(
|
||||||
::cartographer::common::Time time, const string& frame_id) const;
|
::cartographer::common::Time time, const std::string& frame_id) const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const string tracking_frame_;
|
const std::string tracking_frame_;
|
||||||
const double lookup_transform_timeout_sec_;
|
const double lookup_transform_timeout_sec_;
|
||||||
const tf2_ros::Buffer* const buffer_;
|
const tf2_ros::Buffer* const buffer_;
|
||||||
};
|
};
|
||||||
|
|
|
@ -22,8 +22,8 @@
|
||||||
namespace cartographer_ros {
|
namespace cartographer_ros {
|
||||||
|
|
||||||
::ros::Time ToRos(::cartographer::common::Time time) {
|
::ros::Time ToRos(::cartographer::common::Time time) {
|
||||||
int64 uts_timestamp = ::cartographer::common::ToUniversal(time);
|
int64_t uts_timestamp = ::cartographer::common::ToUniversal(time);
|
||||||
int64 ns_since_unix_epoch =
|
int64_t ns_since_unix_epoch =
|
||||||
(uts_timestamp -
|
(uts_timestamp -
|
||||||
::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds *
|
::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds *
|
||||||
10000000ll) *
|
10000000ll) *
|
||||||
|
|
|
@ -26,9 +26,9 @@ namespace cartographer_ros {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
TEST(TimeConversion, testToRos) {
|
TEST(TimeConversion, testToRos) {
|
||||||
std::vector<int64> values = {0, 1469091375, 1466481821, 1462101382,
|
std::vector<int64_t> values = {0, 1469091375, 1466481821, 1462101382,
|
||||||
1468238899};
|
1468238899};
|
||||||
for (int64 seconds_since_epoch : values) {
|
for (int64_t seconds_since_epoch : values) {
|
||||||
::ros::Time ros_now;
|
::ros::Time ros_now;
|
||||||
ros_now.fromSec(seconds_since_epoch);
|
ros_now.fromSec(seconds_since_epoch);
|
||||||
::cartographer::common::Time cartographer_now(
|
::cartographer::common::Time cartographer_now(
|
||||||
|
|
|
@ -29,9 +29,9 @@ namespace cartographer_ros {
|
||||||
struct TrajectoryOptions {
|
struct TrajectoryOptions {
|
||||||
::cartographer::mapping::proto::TrajectoryBuilderOptions
|
::cartographer::mapping::proto::TrajectoryBuilderOptions
|
||||||
trajectory_builder_options;
|
trajectory_builder_options;
|
||||||
string tracking_frame;
|
std::string tracking_frame;
|
||||||
string published_frame;
|
std::string published_frame;
|
||||||
string odom_frame;
|
std::string odom_frame;
|
||||||
bool provide_odom_frame;
|
bool provide_odom_frame;
|
||||||
bool use_odometry;
|
bool use_odometry;
|
||||||
int num_laser_scans;
|
int num_laser_scans;
|
||||||
|
|
|
@ -25,7 +25,7 @@
|
||||||
namespace cartographer_ros {
|
namespace cartographer_ros {
|
||||||
|
|
||||||
std::vector<geometry_msgs::TransformStamped> ReadStaticTransformsFromUrdf(
|
std::vector<geometry_msgs::TransformStamped> ReadStaticTransformsFromUrdf(
|
||||||
const string& urdf_filename, tf2_ros::Buffer* const tf_buffer) {
|
const std::string& urdf_filename, tf2_ros::Buffer* const tf_buffer) {
|
||||||
urdf::Model model;
|
urdf::Model model;
|
||||||
CHECK(model.initFile(urdf_filename));
|
CHECK(model.initFile(urdf_filename));
|
||||||
#if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS
|
#if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS
|
||||||
|
|
|
@ -25,7 +25,7 @@
|
||||||
namespace cartographer_ros {
|
namespace cartographer_ros {
|
||||||
|
|
||||||
std::vector<geometry_msgs::TransformStamped> ReadStaticTransformsFromUrdf(
|
std::vector<geometry_msgs::TransformStamped> ReadStaticTransformsFromUrdf(
|
||||||
const string& urdf_filename, tf2_ros::Buffer* tf_buffer);
|
const std::string& urdf_filename, tf2_ros::Buffer* tf_buffer);
|
||||||
|
|
||||||
} // namespace cartographer_ros
|
} // namespace cartographer_ros
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue