Remove direct use of sensor::Collator. (#132)
Collating the sensor data is now handled by the MapBuilder.master
Wolfgang Hess
GitHub
parent
e4abb77835
commit
b8d9ac585e
|
|
@ -26,7 +26,6 @@
|
|||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/common/mutex.h"
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/common/rate_timer.h"
|
||||
#include "cartographer/common/thread_pool.h"
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/kalman_filter/pose_tracker.h"
|
||||
|
|
@ -93,7 +92,6 @@ using carto::kalman_filter::PoseCovariance;
|
|||
constexpr int64 kTrajectoryBuilderId = 0;
|
||||
constexpr int kInfiniteSubscriberQueueSize = 0;
|
||||
constexpr int kLatestOnlyPublisherQueueSize = 1;
|
||||
constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
|
||||
|
||||
// Unique default topic names. Expected to be remapped as needed.
|
||||
constexpr char kLaserScanTopic[] = "scan";
|
||||
|
|
@ -121,8 +119,6 @@ class Node {
|
|||
void Initialize();
|
||||
|
||||
private:
|
||||
void HandleSensorData(const string& sensor_id,
|
||||
std::unique_ptr<carto::sensor::Data> sensor_data);
|
||||
bool HandleSubmapQuery(
|
||||
::cartographer_ros_msgs::SubmapQuery::Request& request,
|
||||
::cartographer_ros_msgs::SubmapQuery::Response& response);
|
||||
|
|
@ -146,8 +142,7 @@ class Node {
|
|||
std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_data_
|
||||
GUARDED_BY(mutex_);
|
||||
carto::mapping::MapBuilder map_builder_ GUARDED_BY(mutex_);
|
||||
carto::sensor::Collator sensor_collator_ GUARDED_BY(mutex_);
|
||||
SensorBridge sensor_bridge_ GUARDED_BY(mutex_);
|
||||
std::unique_ptr<SensorBridge> sensor_bridge_ GUARDED_BY(mutex_);
|
||||
|
||||
::ros::NodeHandle node_handle_;
|
||||
::ros::Subscriber imu_subscriber_;
|
||||
|
|
@ -165,10 +160,6 @@ class Node {
|
|||
std::thread occupancy_grid_thread_;
|
||||
bool terminating_ = false GUARDED_BY(mutex_);
|
||||
|
||||
// Time at which we last logged the rates of incoming sensor data.
|
||||
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_;
|
||||
|
|
@ -180,9 +171,7 @@ Node::Node(const NodeOptions& options)
|
|||
tf_(tf_buffer_),
|
||||
tf_bridge_(options_.tracking_frame, options_.lookup_transform_timeout_sec,
|
||||
&tf_buffer_),
|
||||
map_builder_(options.map_builder_options, &constant_data_),
|
||||
sensor_bridge_(options.sensor_bridge_options, &tf_bridge_,
|
||||
kTrajectoryBuilderId, &sensor_collator_) {}
|
||||
map_builder_(options.map_builder_options, &constant_data_) {}
|
||||
|
||||
Node::~Node() {
|
||||
{
|
||||
|
|
@ -197,7 +186,7 @@ Node::~Node() {
|
|||
void Node::Initialize() {
|
||||
// Set of all topics we subscribe to. We use the non-remapped default names
|
||||
// which are unique.
|
||||
std::unordered_set<string> expected_sensor_identifiers;
|
||||
std::unordered_set<string> expected_sensor_ids;
|
||||
|
||||
// For 2D SLAM, subscribe to exactly one horizontal laser.
|
||||
if (options_.use_horizontal_laser) {
|
||||
|
|
@ -205,19 +194,19 @@ void Node::Initialize() {
|
|||
kLaserScanTopic, kInfiniteSubscriberQueueSize,
|
||||
boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
|
||||
[this](const sensor_msgs::LaserScan::ConstPtr& msg) {
|
||||
sensor_bridge_.HandleLaserScanMessage(kLaserScanTopic, msg);
|
||||
sensor_bridge_->HandleLaserScanMessage(kLaserScanTopic, msg);
|
||||
}));
|
||||
expected_sensor_identifiers.insert(kLaserScanTopic);
|
||||
expected_sensor_ids.insert(kLaserScanTopic);
|
||||
}
|
||||
if (options_.use_horizontal_multi_echo_laser) {
|
||||
horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
|
||||
kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize,
|
||||
boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
|
||||
[this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
|
||||
sensor_bridge_.HandleMultiEchoLaserScanMessage(
|
||||
sensor_bridge_->HandleMultiEchoLaserScanMessage(
|
||||
kMultiEchoLaserScanTopic, msg);
|
||||
}));
|
||||
expected_sensor_identifiers.insert(kMultiEchoLaserScanTopic);
|
||||
expected_sensor_ids.insert(kMultiEchoLaserScanTopic);
|
||||
}
|
||||
|
||||
// For 3D SLAM, subscribe to all 3D lasers.
|
||||
|
|
@ -231,9 +220,9 @@ void Node::Initialize() {
|
|||
topic, kInfiniteSubscriberQueueSize,
|
||||
boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
|
||||
[this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
|
||||
sensor_bridge_.HandlePointCloud2Message(topic, msg);
|
||||
sensor_bridge_->HandlePointCloud2Message(topic, msg);
|
||||
})));
|
||||
expected_sensor_identifiers.insert(topic);
|
||||
expected_sensor_ids.insert(topic);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -247,9 +236,9 @@ void Node::Initialize() {
|
|||
kImuTopic, kInfiniteSubscriberQueueSize,
|
||||
boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>(
|
||||
[this](const sensor_msgs::Imu::ConstPtr& msg) {
|
||||
sensor_bridge_.HandleImuMessage(kImuTopic, msg);
|
||||
sensor_bridge_->HandleImuMessage(kImuTopic, msg);
|
||||
}));
|
||||
expected_sensor_identifiers.insert(kImuTopic);
|
||||
expected_sensor_ids.insert(kImuTopic);
|
||||
}
|
||||
|
||||
if (options_.use_odometry_data) {
|
||||
|
|
@ -257,19 +246,17 @@ void Node::Initialize() {
|
|||
kOdometryTopic, kInfiniteSubscriberQueueSize,
|
||||
boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
|
||||
[this](const nav_msgs::Odometry::ConstPtr& msg) {
|
||||
sensor_bridge_.HandleOdometryMessage(kOdometryTopic, msg);
|
||||
sensor_bridge_->HandleOdometryMessage(kOdometryTopic, msg);
|
||||
}));
|
||||
expected_sensor_identifiers.insert(kOdometryTopic);
|
||||
expected_sensor_ids.insert(kOdometryTopic);
|
||||
}
|
||||
|
||||
// TODO(damonkohler): Add multi-trajectory support.
|
||||
CHECK_EQ(kTrajectoryBuilderId, map_builder_.AddTrajectoryBuilder());
|
||||
sensor_collator_.AddTrajectory(
|
||||
kTrajectoryBuilderId, expected_sensor_identifiers,
|
||||
[this](const string& sensor_id,
|
||||
std::unique_ptr<carto::sensor::Data> sensor_data) {
|
||||
HandleSensorData(sensor_id, std::move(sensor_data));
|
||||
});
|
||||
CHECK_EQ(kTrajectoryBuilderId,
|
||||
map_builder_.AddTrajectoryBuilder(expected_sensor_ids));
|
||||
sensor_bridge_ = carto::common::make_unique<SensorBridge>(
|
||||
options_.sensor_bridge_options, &tf_bridge_,
|
||||
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId));
|
||||
|
||||
submap_list_publisher_ =
|
||||
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
|
||||
|
|
@ -310,7 +297,7 @@ bool Node::HandleSubmapQuery(
|
|||
|
||||
carto::common::MutexLocker lock(&mutex_);
|
||||
// TODO(hrapp): return error messages and extract common code from MapBuilder.
|
||||
carto::mapping::Submaps* submaps =
|
||||
carto::mapping::Submaps* const submaps =
|
||||
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->submaps();
|
||||
if (request.submap_id < 0 || request.submap_id >= submaps->size()) {
|
||||
return false;
|
||||
|
|
@ -361,7 +348,7 @@ bool Node::HandleFinishTrajectory(
|
|||
::ros::shutdown();
|
||||
carto::common::MutexLocker lock(&mutex_);
|
||||
// TODO(whess): Add multi-trajectory support.
|
||||
sensor_collator_.FinishTrajectory(kTrajectoryBuilderId);
|
||||
map_builder_.FinishTrajectory(kTrajectoryBuilderId);
|
||||
map_builder_.sparse_pose_graph()->RunFinalOptimization();
|
||||
|
||||
const auto trajectory_nodes =
|
||||
|
|
@ -490,50 +477,6 @@ void Node::SpinOccupancyGridThreadForever() {
|
|||
}
|
||||
}
|
||||
|
||||
void Node::HandleSensorData(const string& sensor_id,
|
||||
std::unique_ptr<carto::sensor::Data> sensor_data) {
|
||||
auto it = rate_timers_.find(sensor_id);
|
||||
if (it == rate_timers_.end()) {
|
||||
it =
|
||||
rate_timers_
|
||||
.emplace(std::piecewise_construct, std::forward_as_tuple(sensor_id),
|
||||
std::forward_as_tuple(carto::common::FromSeconds(
|
||||
kSensorDataRatesLoggingPeriodSeconds)))
|
||||
.first;
|
||||
}
|
||||
it->second.Pulse(sensor_data->time);
|
||||
|
||||
if (std::chrono::steady_clock::now() - last_sensor_data_rates_logging_time_ >
|
||||
carto::common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
|
||||
for (const auto& pair : rate_timers_) {
|
||||
LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
|
||||
}
|
||||
last_sensor_data_rates_logging_time_ = std::chrono::steady_clock::now();
|
||||
}
|
||||
|
||||
auto* const trajectory_builder =
|
||||
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId);
|
||||
switch (sensor_data->type) {
|
||||
case carto::sensor::Data::Type::kImu:
|
||||
trajectory_builder->AddImuData(sensor_data->time,
|
||||
sensor_data->imu.linear_acceleration,
|
||||
sensor_data->imu.angular_velocity);
|
||||
return;
|
||||
|
||||
case carto::sensor::Data::Type::kLaserFan:
|
||||
trajectory_builder->AddLaserFan(sensor_data->time,
|
||||
sensor_data->laser_fan);
|
||||
return;
|
||||
|
||||
case carto::sensor::Data::Type::kOdometry:
|
||||
trajectory_builder->AddOdometerPose(sensor_data->time,
|
||||
sensor_data->odometry.pose,
|
||||
sensor_data->odometry.covariance);
|
||||
return;
|
||||
}
|
||||
LOG(FATAL);
|
||||
}
|
||||
|
||||
void Node::SpinForever() { ::ros::spin(); }
|
||||
|
||||
void Run() {
|
||||
|
|
|
|||
|
|
@ -56,14 +56,12 @@ SensorBridgeOptions CreateSensorBridgeOptions(
|
|||
return options;
|
||||
}
|
||||
|
||||
SensorBridge::SensorBridge(const SensorBridgeOptions& options,
|
||||
const TfBridge* const tf_bridge,
|
||||
const int trajectory_id,
|
||||
carto::sensor::Collator* const sensor_collator)
|
||||
SensorBridge::SensorBridge(
|
||||
const SensorBridgeOptions& options, const TfBridge* const tf_bridge,
|
||||
carto::mapping::TrajectoryBuilder* const trajectory_builder)
|
||||
: options_(options),
|
||||
tf_bridge_(tf_bridge),
|
||||
trajectory_id_(trajectory_id),
|
||||
sensor_collator_(sensor_collator) {}
|
||||
trajectory_builder_(trajectory_builder) {}
|
||||
|
||||
void SensorBridge::HandleOdometryMessage(
|
||||
const string& topic, const nav_msgs::Odometry::ConstPtr& msg) {
|
||||
|
|
@ -83,12 +81,9 @@ void SensorBridge::HandleOdometryMessage(
|
|||
const auto sensor_to_tracking = tf_bridge_->LookupToTracking(
|
||||
time, CheckNoLeadingSlash(msg->child_frame_id));
|
||||
if (sensor_to_tracking != nullptr) {
|
||||
sensor_collator_->AddSensorData(
|
||||
trajectory_id_, topic,
|
||||
carto::common::make_unique<::cartographer::sensor::Data>(
|
||||
time, ::cartographer::sensor::Data::Odometry{
|
||||
ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(),
|
||||
covariance}));
|
||||
trajectory_builder_->AddOdometerPose(
|
||||
topic, time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(),
|
||||
covariance);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -104,15 +99,10 @@ void SensorBridge::HandleImuMessage(const string& topic,
|
|||
<< "The IMU frame must be colocated with the tracking frame. "
|
||||
"Transforming linear acceleration into the tracking frame will "
|
||||
"otherwise be imprecise.";
|
||||
sensor_collator_->AddSensorData(
|
||||
trajectory_id_, topic,
|
||||
carto::common::make_unique<::cartographer::sensor::Data>(
|
||||
time,
|
||||
::cartographer::sensor::Data::Imu{
|
||||
sensor_to_tracking->rotation() *
|
||||
ToEigen(msg->linear_acceleration),
|
||||
sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity),
|
||||
}));
|
||||
trajectory_builder_->AddImuData(
|
||||
topic, time,
|
||||
sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
|
||||
sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -136,19 +126,17 @@ void SensorBridge::HandlePointCloud2Message(
|
|||
const auto sensor_to_tracking = tf_bridge_->LookupToTracking(
|
||||
time, CheckNoLeadingSlash(msg->header.frame_id));
|
||||
if (sensor_to_tracking != nullptr) {
|
||||
sensor_collator_->AddSensorData(
|
||||
trajectory_id_, topic,
|
||||
carto::common::make_unique<::cartographer::sensor::Data>(
|
||||
time, carto::sensor::TransformLaserFan(
|
||||
trajectory_builder_->AddLaserFan(
|
||||
topic, time,
|
||||
carto::sensor::TransformLaserFan(
|
||||
carto::sensor::FromProto(ToCartographer(pcl_point_cloud)),
|
||||
sensor_to_tracking->cast<float>())));
|
||||
sensor_to_tracking->cast<float>()));
|
||||
}
|
||||
}
|
||||
|
||||
void SensorBridge::HandleLaserScanProto(
|
||||
const string& topic, const ::cartographer::common::Time time,
|
||||
const string& frame_id,
|
||||
const ::cartographer::sensor::proto::LaserScan& laser_scan) {
|
||||
const string& topic, const carto::common::Time time, const string& frame_id,
|
||||
const carto::sensor::proto::LaserScan& laser_scan) {
|
||||
const auto laser_fan = carto::sensor::ToLaserFan(
|
||||
laser_scan, options_.horizontal_laser_min_range,
|
||||
options_.horizontal_laser_max_range,
|
||||
|
|
@ -156,11 +144,9 @@ void SensorBridge::HandleLaserScanProto(
|
|||
const auto sensor_to_tracking =
|
||||
tf_bridge_->LookupToTracking(time, CheckNoLeadingSlash(frame_id));
|
||||
if (sensor_to_tracking != nullptr) {
|
||||
sensor_collator_->AddSensorData(
|
||||
trajectory_id_, topic,
|
||||
carto::common::make_unique<::cartographer::sensor::Data>(
|
||||
time, carto::sensor::TransformLaserFan(
|
||||
laser_fan, sensor_to_tracking->cast<float>())));
|
||||
trajectory_builder_->AddLaserFan(
|
||||
topic, time, carto::sensor::TransformLaserFan(
|
||||
laser_fan, sensor_to_tracking->cast<float>()));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -17,8 +17,7 @@
|
|||
#ifndef CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_
|
||||
#define CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_
|
||||
|
||||
#include "cartographer/sensor/collator.h"
|
||||
#include "cartographer/sensor/data.h"
|
||||
#include "cartographer/mapping/trajectory_builder.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
#include "cartographer_ros/tf_bridge.h"
|
||||
|
|
@ -47,9 +46,9 @@ SensorBridgeOptions CreateSensorBridgeOptions(
|
|||
// Converts ROS messages into SensorData in tracking frame for the MapBuilder.
|
||||
class SensorBridge {
|
||||
public:
|
||||
explicit SensorBridge(const SensorBridgeOptions& options,
|
||||
const TfBridge* tf_bridge, int trajectory_id,
|
||||
::cartographer::sensor::Collator* sensor_collator);
|
||||
explicit SensorBridge(
|
||||
const SensorBridgeOptions& options, const TfBridge* tf_bridge,
|
||||
::cartographer::mapping::TrajectoryBuilder* trajectory_builder);
|
||||
|
||||
SensorBridge(const SensorBridge&) = delete;
|
||||
SensorBridge& operator=(const SensorBridge&) = delete;
|
||||
|
|
@ -74,8 +73,7 @@ class SensorBridge {
|
|||
|
||||
const SensorBridgeOptions options_;
|
||||
const TfBridge* const tf_bridge_;
|
||||
const int trajectory_id_;
|
||||
::cartographer::sensor::Collator* const sensor_collator_;
|
||||
::cartographer::mapping::TrajectoryBuilder* const trajectory_builder_;
|
||||
};
|
||||
|
||||
} // namespace cartographer_ros
|
||||
|
|
|
|||
Loading…
Reference in New Issue