Moves sensor data transformation into SensorBridge. (#88)

* Pulls out TfBridge.
* SensorBridge has its own options.
* SensorBridge is now responsible for transforming the sensor data into the tracking frame.
* Constant odometry covariance is no longer optional.
master
Damon Kohler 2016-10-10 16:15:17 +02:00 committed by GitHub
parent 2a040bc755
commit d4825af3d4
15 changed files with 352 additions and 280 deletions

View File

@ -94,6 +94,8 @@ add_executable(cartographer_node
src/sensor_bridge.cc src/sensor_bridge.cc
src/sensor_bridge.h src/sensor_bridge.h
src/sensor_data.h src/sensor_data.h
src/tf_bridge.cc
src/tf_bridge.h
src/time_conversion.cc src/time_conversion.cc
src/time_conversion.h src/time_conversion.h
) )

View File

@ -16,20 +16,21 @@ include "map_builder.lua"
options = { options = {
map_builder = MAP_BUILDER, map_builder = MAP_BUILDER,
sensor_bridge = {
horizontal_laser_min_range = 0.,
horizontal_laser_max_range = 30.,
horizontal_laser_missing_echo_ray_length = 5.,
constant_odometry_translational_variance = 0.,
constant_odometry_rotational_variance = 0.,
},
map_frame = "map", map_frame = "map",
tracking_frame = "base_link", tracking_frame = "base_link",
published_frame = "base_link", published_frame = "base_link",
odom_frame = "odom", odom_frame = "odom",
provide_odom_frame = true, provide_odom_frame = true,
use_odometry_data = false, use_odometry_data = false,
use_constant_odometry_variance = false,
constant_odometry_translational_variance = 0.,
constant_odometry_rotational_variance = 0.,
use_horizontal_laser = false, use_horizontal_laser = false,
use_horizontal_multi_echo_laser = true, 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, num_lasers_3d = 0,
lookup_transform_timeout_sec = 0.2, lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3, submap_publish_period_sec = 0.3,

View File

@ -16,20 +16,21 @@ include "map_builder.lua"
options = { options = {
map_builder = MAP_BUILDER, map_builder = MAP_BUILDER,
sensor_bridge = {
horizontal_laser_min_range = 0.,
horizontal_laser_max_range = 30.,
horizontal_laser_missing_echo_ray_length = 5.,
constant_odometry_translational_variance = 0.,
constant_odometry_rotational_variance = 0.,
},
map_frame = "map", map_frame = "map",
tracking_frame = "base_link", tracking_frame = "base_link",
published_frame = "base_link", published_frame = "base_link",
odom_frame = "odom", odom_frame = "odom",
provide_odom_frame = true, provide_odom_frame = true,
use_odometry_data = false, use_odometry_data = false,
use_constant_odometry_variance = false,
constant_odometry_translational_variance = 0.,
constant_odometry_rotational_variance = 0.,
use_horizontal_laser = false, use_horizontal_laser = false,
use_horizontal_multi_echo_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, num_lasers_3d = 2,
lookup_transform_timeout_sec = 0.2, lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3, submap_publish_period_sec = 0.3,

View File

@ -16,20 +16,21 @@ include "map_builder.lua"
options = { options = {
map_builder = MAP_BUILDER, map_builder = MAP_BUILDER,
sensor_bridge = {
horizontal_laser_min_range = 0.,
horizontal_laser_max_range = 30.,
horizontal_laser_missing_echo_ray_length = 5.,
constant_odometry_translational_variance = 0.,
constant_odometry_rotational_variance = 0.,
},
map_frame = "map", map_frame = "map",
tracking_frame = "base_footprint", tracking_frame = "base_footprint",
published_frame = "base_footprint", published_frame = "base_footprint",
odom_frame = "odom", odom_frame = "odom",
provide_odom_frame = true, provide_odom_frame = true,
use_odometry_data = false, use_odometry_data = false,
use_constant_odometry_variance = false,
constant_odometry_translational_variance = 0.,
constant_odometry_rotational_variance = 0.,
use_horizontal_laser = true, use_horizontal_laser = true,
use_horizontal_multi_echo_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 = 0, num_lasers_3d = 0,
lookup_transform_timeout_sec = 0.2, lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3, submap_publish_period_sec = 0.3,

View File

@ -16,20 +16,21 @@ include "map_builder.lua"
options = { options = {
map_builder = MAP_BUILDER, map_builder = MAP_BUILDER,
sensor_bridge = {
horizontal_laser_min_range = 0.3,
horizontal_laser_max_range = 8.,
horizontal_laser_missing_echo_ray_length = 1.,
constant_odometry_translational_variance = 0.,
constant_odometry_rotational_variance = 0.,
},
map_frame = "map", map_frame = "map",
tracking_frame = "horizontal_laser_link", tracking_frame = "horizontal_laser_link",
published_frame = "horizontal_laser_link", published_frame = "horizontal_laser_link",
odom_frame = "odom", odom_frame = "odom",
provide_odom_frame = true, provide_odom_frame = true,
use_odometry_data = false, use_odometry_data = false,
use_constant_odometry_variance = false,
constant_odometry_translational_variance = 0.,
constant_odometry_rotational_variance = 0.,
use_horizontal_laser = true, use_horizontal_laser = true,
use_horizontal_multi_echo_laser = false, use_horizontal_multi_echo_laser = false,
horizontal_laser_min_range = 0.3,
horizontal_laser_max_range = 8.,
horizontal_laser_missing_echo_ray_length = 1.,
num_lasers_3d = 0, num_lasers_3d = 0,
lookup_transform_timeout_sec = 0.2, lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3, submap_publish_period_sec = 0.3,

View File

@ -69,6 +69,7 @@
#include "ros_log_sink.h" #include "ros_log_sink.h"
#include "sensor_bridge.h" #include "sensor_bridge.h"
#include "sensor_data.h" #include "sensor_data.h"
#include "tf_bridge.h"
#include "time_conversion.h" #include "time_conversion.h"
DEFINE_string(configuration_directory, "", DEFINE_string(configuration_directory, "",
@ -122,20 +123,6 @@ class Node {
private: private:
void HandleSensorData(int64 timestamp, void HandleSensorData(int64 timestamp,
std::unique_ptr<SensorData> sensor_data); std::unique_ptr<SensorData> sensor_data);
void AddOdometry(int64 timestamp, const string& frame_id,
const SensorData::Odometry& odometry);
void AddImu(int64 timestamp, const string& frame_id, const SensorData::Imu& imu);
void AddHorizontalLaserFan(int64 timestamp, const string& frame_id,
const proto::LaserScan& laser_scan);
void AddLaserFan3D(int64 timestamp, const string& frame_id,
const proto::LaserFan3D& laser_fan_3d);
// 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);
bool HandleSubmapQuery( bool HandleSubmapQuery(
::cartographer_ros_msgs::SubmapQuery::Request& request, ::cartographer_ros_msgs::SubmapQuery::Request& request,
::cartographer_ros_msgs::SubmapQuery::Response& response); ::cartographer_ros_msgs::SubmapQuery::Response& response);
@ -150,6 +137,11 @@ class Node {
const NodeOptions options_; const NodeOptions options_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_;
tf2_ros::TransformBroadcaster tf_broadcaster_;
TfBridge tf_bridge_;
carto::common::Mutex mutex_; carto::common::Mutex mutex_;
std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_data_ std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_data_
GUARDED_BY(mutex_); GUARDED_BY(mutex_);
@ -170,10 +162,6 @@ class Node {
carto::common::Time::min(); carto::common::Time::min();
::ros::ServiceServer finish_trajectory_server_; ::ros::ServiceServer finish_trajectory_server_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_;
tf2_ros::TransformBroadcaster tf_broadcaster_;
::ros::Publisher occupancy_grid_publisher_; ::ros::Publisher occupancy_grid_publisher_;
std::thread occupancy_grid_thread_; std::thread occupancy_grid_thread_;
bool terminating_ = false GUARDED_BY(mutex_); bool terminating_ = false GUARDED_BY(mutex_);
@ -189,10 +177,13 @@ class Node {
Node::Node(const NodeOptions& options) Node::Node(const NodeOptions& options)
: options_(options), : options_(options),
map_builder_(options.map_builder_options, &constant_data_),
sensor_bridge_(kTrajectoryBuilderId, &sensor_collator_),
tf_buffer_(::ros::Duration(1000)), tf_buffer_(::ros::Duration(1000)),
tf_(tf_buffer_) {} 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_) {}
Node::~Node() { Node::~Node() {
{ {
@ -204,107 +195,6 @@ Node::~Node() {
} }
} }
Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time,
const string& frame_id) {
::ros::Duration timeout(options_.lookup_transform_timeout_sec);
const ::ros::Time latest_tf_time =
tf_buffer_
.lookupTransform(options_.tracking_frame, frame_id, ::ros::Time(0.),
timeout)
.header.stamp;
const ::ros::Time requested_time = ToRos(time);
if (latest_tf_time >= requested_time) {
// We already have newer data, so we do not wait. Otherwise, we would wait
// for the full 'timeout' even if we ask for data that is too old.
timeout = ::ros::Duration(0.);
}
return ToRigid3d(tf_buffer_.lookupTransform(options_.tracking_frame, frame_id,
requested_time, timeout));
}
void Node::AddOdometry(const int64 timestamp, const string& frame_id,
const SensorData::Odometry& odometry) {
const carto::common::Time time = carto::common::FromUniversal(timestamp);
PoseCovariance applied_covariance = odometry.covariance;
if (options_.use_constant_odometry_variance) {
const Eigen::Matrix3d translational =
Eigen::Matrix3d::Identity() *
options_.constant_odometry_translational_variance;
const Eigen::Matrix3d rotational =
Eigen::Matrix3d::Identity() *
options_.constant_odometry_rotational_variance;
// clang-format off
applied_covariance <<
translational, Eigen::Matrix3d::Zero(),
Eigen::Matrix3d::Zero(), rotational;
// clang-format on
}
try {
const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrThrow(time, frame_id);
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
->AddOdometerPose(time, odometry.pose * sensor_to_tracking.inverse(),
applied_covariance);
} catch (const tf2::TransformException& ex) {
LOG(WARNING) << ex.what();
}
}
void Node::AddImu(const int64 timestamp, const string& frame_id,
const SensorData::Imu& imu) {
const carto::common::Time time = carto::common::FromUniversal(timestamp);
try {
const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrThrow(time, frame_id);
CHECK(sensor_to_tracking.translation().norm() < 1e-5)
<< "The IMU frame must be colocated with the tracking frame. "
"Transforming linear acceleration into the tracking frame will "
"otherwise be imprecise.";
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
->AddImuData(time,
sensor_to_tracking.rotation() * imu.linear_acceleration,
sensor_to_tracking.rotation() * imu.angular_velocity);
} catch (const tf2::TransformException& ex) {
LOG(WARNING) << ex.what();
}
}
void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id,
const proto::LaserScan& laser_scan) {
const carto::common::Time time = carto::common::FromUniversal(timestamp);
try {
const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrThrow(time, frame_id);
const carto::sensor::LaserFan laser_fan = carto::sensor::ToLaserFan(
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>());
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
->AddHorizontalLaserFan(time, laser_fan_3d);
} catch (const tf2::TransformException& ex) {
LOG(WARNING) << ex.what();
}
}
void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id,
const proto::LaserFan3D& laser_fan_3d) {
const carto::common::Time time = carto::common::FromUniversal(timestamp);
try {
const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrThrow(time, frame_id);
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) << ex.what();
}
}
void Node::Initialize() { void Node::Initialize() {
// Set of all topics we subscribe to. We use the non-remapped default names // Set of all topics we subscribe to. We use the non-remapped default names
// which are unique. // which are unique.
@ -316,7 +206,7 @@ void Node::Initialize() {
kLaserScanTopic, kInfiniteSubscriberQueueSize, kLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>( boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
[this](const sensor_msgs::LaserScan::ConstPtr& msg) { [this](const sensor_msgs::LaserScan::ConstPtr& msg) {
sensor_bridge_.AddLaserScanMessage(kLaserScanTopic, msg); sensor_bridge_.HandleLaserScanMessage(kLaserScanTopic, msg);
})); }));
expected_sensor_identifiers.insert(kLaserScanTopic); expected_sensor_identifiers.insert(kLaserScanTopic);
} }
@ -325,7 +215,7 @@ void Node::Initialize() {
kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize, kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>( boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
[this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { [this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
sensor_bridge_.AddMultiEchoLaserScanMessage( sensor_bridge_.HandleMultiEchoLaserScanMessage(
kMultiEchoLaserScanTopic, msg); kMultiEchoLaserScanTopic, msg);
})); }));
expected_sensor_identifiers.insert(kMultiEchoLaserScanTopic); expected_sensor_identifiers.insert(kMultiEchoLaserScanTopic);
@ -342,7 +232,7 @@ void Node::Initialize() {
topic, kInfiniteSubscriberQueueSize, topic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>( boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
[this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { [this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
sensor_bridge_.AddPointCloud2Message(topic, msg); sensor_bridge_.HandlePointCloud2Message(topic, msg);
}))); })));
expected_sensor_identifiers.insert(topic); expected_sensor_identifiers.insert(topic);
} }
@ -358,7 +248,7 @@ void Node::Initialize() {
kImuTopic, kInfiniteSubscriberQueueSize, kImuTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>( boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>(
[this](const sensor_msgs::Imu::ConstPtr& msg) { [this](const sensor_msgs::Imu::ConstPtr& msg) {
sensor_bridge_.AddImuMessage(kImuTopic, msg); sensor_bridge_.HandleImuMessage(kImuTopic, msg);
})); }));
expected_sensor_identifiers.insert(kImuTopic); expected_sensor_identifiers.insert(kImuTopic);
} }
@ -368,7 +258,7 @@ void Node::Initialize() {
kOdometryTopic, kInfiniteSubscriberQueueSize, kOdometryTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const nav_msgs::Odometry::ConstPtr&)>( boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
[this](const nav_msgs::Odometry::ConstPtr& msg) { [this](const nav_msgs::Odometry::ConstPtr& msg) {
sensor_bridge_.AddOdometryMessage(kOdometryTopic, msg); sensor_bridge_.HandleOdometryMessage(kOdometryTopic, msg);
})); }));
expected_sensor_identifiers.insert(kOdometryTopic); expected_sensor_identifiers.insert(kOdometryTopic);
} }
@ -533,9 +423,9 @@ void Node::PublishPoseAndScanMatchedPointCloud(
geometry_msgs::TransformStamped stamped_transform; geometry_msgs::TransformStamped stamped_transform;
stamped_transform.header.stamp = ToRos(last_pose_estimate.time); stamped_transform.header.stamp = ToRos(last_pose_estimate.time);
try { const auto published_to_tracking = tf_bridge_.LookupToTracking(
const Rigid3d published_to_tracking = LookupToTrackingTransformOrThrow( last_pose_estimate.time, options_.published_frame);
last_pose_estimate.time, options_.published_frame); if (published_to_tracking != nullptr) {
if (options_.provide_odom_frame) { if (options_.provide_odom_frame) {
stamped_transform.header.frame_id = options_.map_frame; stamped_transform.header.frame_id = options_.map_frame;
stamped_transform.child_frame_id = options_.odom_frame; stamped_transform.child_frame_id = options_.odom_frame;
@ -545,17 +435,15 @@ void Node::PublishPoseAndScanMatchedPointCloud(
stamped_transform.header.frame_id = options_.odom_frame; stamped_transform.header.frame_id = options_.odom_frame;
stamped_transform.child_frame_id = options_.published_frame; stamped_transform.child_frame_id = options_.published_frame;
stamped_transform.transform = stamped_transform.transform =
ToGeometryMsgTransform(tracking_to_local * published_to_tracking); ToGeometryMsgTransform(tracking_to_local * (*published_to_tracking));
tf_broadcaster_.sendTransform(stamped_transform); tf_broadcaster_.sendTransform(stamped_transform);
} else { } else {
stamped_transform.header.frame_id = options_.map_frame; stamped_transform.header.frame_id = options_.map_frame;
stamped_transform.child_frame_id = options_.published_frame; stamped_transform.child_frame_id = options_.published_frame;
stamped_transform.transform = stamped_transform.transform =
ToGeometryMsgTransform(tracking_to_map * published_to_tracking); ToGeometryMsgTransform(tracking_to_map * (*published_to_tracking));
tf_broadcaster_.sendTransform(stamped_transform); tf_broadcaster_.sendTransform(stamped_transform);
} }
} catch (const tf2::TransformException& ex) {
LOG(WARNING) << ex.what();
} }
// We only publish a point cloud if it has changed. It is not needed at high // We only publish a point cloud if it has changed. It is not needed at high
@ -615,23 +503,28 @@ void Node::HandleSensorData(const int64 timestamp,
last_sensor_data_rates_logging_time_ = std::chrono::steady_clock::now(); last_sensor_data_rates_logging_time_ = std::chrono::steady_clock::now();
} }
const auto time = carto::common::FromUniversal(timestamp);
auto* const trajectory_builder =
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId);
switch (sensor_data->type) { switch (sensor_data->type) {
case SensorType::kImu: case SensorType::kImu:
AddImu(timestamp, sensor_data->frame_id, sensor_data->imu); trajectory_builder->AddImuData(time, sensor_data->imu.linear_acceleration,
return; sensor_data->imu.angular_velocity);
case SensorType::kLaserScan:
AddHorizontalLaserFan(timestamp, sensor_data->frame_id,
sensor_data->laser_scan);
return; return;
case SensorType::kLaserFan3D: case SensorType::kLaserFan3D:
AddLaserFan3D(timestamp, sensor_data->frame_id, if (options_.map_builder_options.use_trajectory_builder_2d()) {
sensor_data->laser_fan_3d); trajectory_builder->AddHorizontalLaserFan(time,
sensor_data->laser_fan_3d);
} else {
CHECK(options_.map_builder_options.use_trajectory_builder_3d());
trajectory_builder->AddLaserFan3D(time, sensor_data->laser_fan_3d);
}
return; return;
case SensorType::kOdometry: case SensorType::kOdometry:
AddOdometry(timestamp, sensor_data->frame_id, sensor_data->odometry); trajectory_builder->AddOdometerPose(time, sensor_data->odometry.pose,
sensor_data->odometry.covariance);
return; return;
} }
LOG(FATAL); LOG(FATAL);

View File

@ -73,6 +73,7 @@ geometry_msgs::Pose ToGeometryMsgPose(
::cartographer::transform::Rigid3d ToRigid3d(const geometry_msgs::Pose& pose); ::cartographer::transform::Rigid3d ToRigid3d(const geometry_msgs::Pose& pose);
Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3); Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3);
Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion); Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion);
::cartographer::kalman_filter::PoseCovariance ToPoseCovariance( ::cartographer::kalman_filter::PoseCovariance ToPoseCovariance(

View File

@ -27,6 +27,8 @@ NodeOptions CreateNodeOptions(
options.map_builder_options = options.map_builder_options =
::cartographer::mapping::CreateMapBuilderOptions( ::cartographer::mapping::CreateMapBuilderOptions(
lua_parameter_dictionary->GetDictionary("map_builder").get()); lua_parameter_dictionary->GetDictionary("map_builder").get());
options.sensor_bridge_options = CreateSensorBridgeOptions(
lua_parameter_dictionary->GetDictionary("sensor_bridge").get());
options.map_frame = lua_parameter_dictionary->GetString("map_frame"); options.map_frame = lua_parameter_dictionary->GetString("map_frame");
options.tracking_frame = options.tracking_frame =
lua_parameter_dictionary->GetString("tracking_frame"); lua_parameter_dictionary->GetString("tracking_frame");
@ -37,25 +39,10 @@ NodeOptions CreateNodeOptions(
lua_parameter_dictionary->GetBool("provide_odom_frame"); lua_parameter_dictionary->GetBool("provide_odom_frame");
options.use_odometry_data = options.use_odometry_data =
lua_parameter_dictionary->GetBool("use_odometry_data"); lua_parameter_dictionary->GetBool("use_odometry_data");
options.use_constant_odometry_variance =
lua_parameter_dictionary->GetBool("use_constant_odometry_variance");
options.constant_odometry_translational_variance =
lua_parameter_dictionary->GetDouble(
"constant_odometry_translational_variance");
options.constant_odometry_rotational_variance =
lua_parameter_dictionary->GetDouble(
"constant_odometry_rotational_variance");
options.use_horizontal_laser = options.use_horizontal_laser =
lua_parameter_dictionary->GetBool("use_horizontal_laser"); lua_parameter_dictionary->GetBool("use_horizontal_laser");
options.use_horizontal_multi_echo_laser = options.use_horizontal_multi_echo_laser =
lua_parameter_dictionary->GetBool("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 = options.num_lasers_3d =
lua_parameter_dictionary->GetNonNegativeInt("num_lasers_3d"); lua_parameter_dictionary->GetNonNegativeInt("num_lasers_3d");
options.lookup_transform_timeout_sec = options.lookup_transform_timeout_sec =

View File

@ -23,25 +23,22 @@
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/mapping/map_builder.h" #include "cartographer/mapping/map_builder.h"
#include "sensor_bridge.h"
namespace cartographer_ros { 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;
SensorBridgeOptions sensor_bridge_options;
string map_frame; string map_frame;
string tracking_frame; string tracking_frame;
string published_frame; string published_frame;
string odom_frame; string odom_frame;
bool provide_odom_frame; bool provide_odom_frame;
bool use_odometry_data; bool use_odometry_data;
bool use_constant_odometry_variance;
double constant_odometry_translational_variance;
double constant_odometry_rotational_variance;
bool use_horizontal_laser; bool use_horizontal_laser;
bool use_horizontal_multi_echo_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; int num_lasers_3d;
double lookup_transform_timeout_sec; double lookup_transform_timeout_sec;
double submap_publish_period_sec; double submap_publish_period_sec;

View File

@ -16,75 +16,144 @@
#include "sensor_bridge.h" #include "sensor_bridge.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "msg_conversion.h" #include "msg_conversion.h"
#include "time_conversion.h" #include "time_conversion.h"
namespace cartographer_ros { namespace cartographer_ros {
namespace carto = ::cartographer;
using carto::transform::Rigid3d;
SensorBridgeOptions CreateSensorBridgeOptions(
carto::common::LuaParameterDictionary* const lua_parameter_dictionary) {
SensorBridgeOptions options;
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.constant_odometry_translational_variance =
lua_parameter_dictionary->GetDouble(
"constant_odometry_translational_variance");
options.constant_odometry_rotational_variance =
lua_parameter_dictionary->GetDouble(
"constant_odometry_rotational_variance");
return options;
}
SensorBridge::SensorBridge( SensorBridge::SensorBridge(
const SensorBridgeOptions& options, const TfBridge* const tf_bridge,
const int trajectory_id, const int trajectory_id,
::cartographer::mapping::SensorCollator<SensorData>* sensor_collator) carto::mapping::SensorCollator<SensorData>* const sensor_collator)
: trajectory_id_(trajectory_id), sensor_collator_(sensor_collator) {} : options_(options),
tf_bridge_(tf_bridge),
trajectory_id_(trajectory_id),
sensor_collator_(sensor_collator) {}
void SensorBridge::AddOdometryMessage(const string& topic, void SensorBridge::HandleOdometryMessage(
const nav_msgs::Odometry::ConstPtr& msg) { const string& topic, const nav_msgs::Odometry::ConstPtr& msg) {
auto sensor_data = ::cartographer::common::make_unique<SensorData>( const carto::common::Time time = FromRos(msg->header.stamp);
msg->child_frame_id, const Eigen::Matrix3d translational =
SensorData::Odometry{ToRigid3d(msg->pose.pose), Eigen::Matrix3d::Identity() *
ToPoseCovariance(msg->pose.covariance)}); options_.constant_odometry_translational_variance;
sensor_collator_->AddSensorData( const Eigen::Matrix3d rotational =
trajectory_id_, Eigen::Matrix3d::Identity() *
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic, options_.constant_odometry_rotational_variance;
std::move(sensor_data)); // clang-format off
carto::kalman_filter::PoseCovariance covariance;
covariance <<
translational, Eigen::Matrix3d::Zero(),
Eigen::Matrix3d::Zero(), rotational;
// clang-format on
const auto sensor_to_tracking =
tf_bridge_->LookupToTracking(time, msg->child_frame_id);
if (sensor_to_tracking != nullptr) {
sensor_collator_->AddSensorData(
trajectory_id_, carto::common::ToUniversal(time), topic,
carto::common::make_unique<SensorData>(
msg->child_frame_id,
SensorData::Odometry{
ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(),
covariance}));
}
} }
void SensorBridge::AddImuMessage(const string& topic, void SensorBridge::HandleImuMessage(const string& topic,
const sensor_msgs::Imu::ConstPtr& msg) { const sensor_msgs::Imu::ConstPtr& msg) {
CHECK_NE(msg->angular_velocity_covariance[0], -1);
CHECK_NE(msg->linear_acceleration_covariance[0], -1); CHECK_NE(msg->linear_acceleration_covariance[0], -1);
auto sensor_data = ::cartographer::common::make_unique<SensorData>( CHECK_NE(msg->angular_velocity_covariance[0], -1);
msg->header.frame_id, const carto::common::Time time = FromRos(msg->header.stamp);
SensorData::Imu{{msg->angular_velocity.x, msg->angular_velocity.y, const auto sensor_to_tracking =
msg->angular_velocity.z}, tf_bridge_->LookupToTracking(time, msg->header.frame_id);
{msg->linear_acceleration.x, msg->linear_acceleration.y, if (sensor_to_tracking != nullptr) {
msg->linear_acceleration.z}}); CHECK(sensor_to_tracking->translation().norm() < 1e-5)
sensor_collator_->AddSensorData( << "The IMU frame must be colocated with the tracking frame. "
trajectory_id_, "Transforming linear acceleration into the tracking frame will "
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic, "otherwise be imprecise.";
std::move(sensor_data)); sensor_collator_->AddSensorData(
trajectory_id_, carto::common::ToUniversal(time), topic,
carto::common::make_unique<SensorData>(
msg->header.frame_id,
SensorData::Imu{
sensor_to_tracking->rotation() *
ToEigen(msg->linear_acceleration),
sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity),
}));
}
} }
void SensorBridge::AddLaserScanMessage( void SensorBridge::HandleLaserScanMessage(
const string& topic, const sensor_msgs::LaserScan::ConstPtr& msg) { const string& topic, const sensor_msgs::LaserScan::ConstPtr& msg) {
auto sensor_data = ::cartographer::common::make_unique<SensorData>( HandleLaserScanProto(topic, FromRos(msg->header.stamp), msg->header.frame_id,
msg->header.frame_id, ToCartographer(*msg)); ToCartographer(*msg));
sensor_collator_->AddSensorData(
trajectory_id_,
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic,
std::move(sensor_data));
} }
void SensorBridge::AddMultiEchoLaserScanMessage( void SensorBridge::HandleMultiEchoLaserScanMessage(
const string& topic, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { const string& topic, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
auto sensor_data = ::cartographer::common::make_unique<SensorData>( HandleLaserScanProto(topic, FromRos(msg->header.stamp), msg->header.frame_id,
msg->header.frame_id, ToCartographer(*msg)); ToCartographer(*msg));
sensor_collator_->AddSensorData(
trajectory_id_,
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic,
std::move(sensor_data));
} }
void SensorBridge::AddPointCloud2Message( void SensorBridge::HandlePointCloud2Message(
const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg) { const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg) {
pcl::PointCloud<pcl::PointXYZ> pcl_points; pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
pcl::fromROSMsg(*msg, pcl_points); pcl::fromROSMsg(*msg, pcl_point_cloud);
const carto::common::Time time = FromRos(msg->header.stamp);
const auto sensor_to_tracking =
tf_bridge_->LookupToTracking(time, msg->header.frame_id);
if (sensor_to_tracking != nullptr) {
sensor_collator_->AddSensorData(
trajectory_id_, carto::common::ToUniversal(time), topic,
carto::common::make_unique<SensorData>(
msg->header.frame_id,
carto::sensor::TransformLaserFan3D(
carto::sensor::FromProto(ToCartographer(pcl_point_cloud)),
sensor_to_tracking->cast<float>())));
}
}
auto sensor_data = ::cartographer::common::make_unique<SensorData>( void SensorBridge::HandleLaserScanProto(
msg->header.frame_id, ToCartographer(pcl_points)); const string& topic, const ::cartographer::common::Time time,
sensor_collator_->AddSensorData( const string& frame_id,
trajectory_id_, const ::cartographer::sensor::proto::LaserScan& laser_scan) {
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic, const auto laser_fan_2d = carto::sensor::ToLaserFan(
std::move(sensor_data)); laser_scan, options_.horizontal_laser_min_range,
options_.horizontal_laser_max_range,
options_.horizontal_laser_missing_echo_ray_length);
const auto sensor_to_tracking = tf_bridge_->LookupToTracking(time, frame_id);
if (sensor_to_tracking != nullptr) {
sensor_collator_->AddSensorData(
trajectory_id_, carto::common::ToUniversal(time), topic,
carto::common::make_unique<SensorData>(
frame_id, carto::sensor::TransformLaserFan3D(
carto::sensor::ToLaserFan3D(laser_fan_2d),
sensor_to_tracking->cast<float>())));
}
} }
} // namespace cartographer_ros } // namespace cartographer_ros

View File

@ -18,6 +18,8 @@
#define CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_ #define CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_
#include "cartographer/mapping/sensor_collator.h" #include "cartographer/mapping/sensor_collator.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "geometry_msgs/Transform.h" #include "geometry_msgs/Transform.h"
#include "geometry_msgs/TransformStamped.h" #include "geometry_msgs/TransformStamped.h"
#include "nav_msgs/OccupancyGrid.h" #include "nav_msgs/OccupancyGrid.h"
@ -28,34 +30,54 @@
#include "sensor_msgs/MultiEchoLaserScan.h" #include "sensor_msgs/MultiEchoLaserScan.h"
#include "sensor_msgs/PointCloud2.h" #include "sensor_msgs/PointCloud2.h"
#include "tf_bridge.h"
namespace cartographer_ros { namespace cartographer_ros {
// A wrapper around SensorCollator that converts ROS messages into our internal struct SensorBridgeOptions {
// representation and passes them on to the 'sensor_collator'. double horizontal_laser_min_range;
double horizontal_laser_max_range;
double horizontal_laser_missing_echo_ray_length;
double constant_odometry_translational_variance;
double constant_odometry_rotational_variance;
};
SensorBridgeOptions CreateSensorBridgeOptions(
::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
// Converts ROS messages into SensorData in tracking frame for the MapBuilder.
class SensorBridge { class SensorBridge {
public: public:
explicit SensorBridge( explicit SensorBridge(
const SensorBridgeOptions& options, const TfBridge* tf_bridge,
int trajectory_id, int trajectory_id,
::cartographer::mapping::SensorCollator<SensorData>* sensor_collator); ::cartographer::mapping::SensorCollator<SensorData>* sensor_collator);
SensorBridge(const SensorBridge&) = delete; SensorBridge(const SensorBridge&) = delete;
SensorBridge& operator=(const SensorBridge&) = delete; SensorBridge& operator=(const SensorBridge&) = delete;
void AddOdometryMessage(const string& topic, void HandleOdometryMessage(const string& topic,
const nav_msgs::Odometry::ConstPtr& msg); const nav_msgs::Odometry::ConstPtr& msg);
void AddImuMessage(const string& topic, void HandleImuMessage(const string& topic,
const sensor_msgs::Imu::ConstPtr& msg); const sensor_msgs::Imu::ConstPtr& msg);
void AddLaserScanMessage(const string& topic, void HandleLaserScanMessage(const string& topic,
const sensor_msgs::LaserScan::ConstPtr& msg); const sensor_msgs::LaserScan::ConstPtr& msg);
void AddMultiEchoLaserScanMessage( void HandleMultiEchoLaserScanMessage(
const string& topic, const string& topic,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
void AddPointCloud2Message(const string& topic, void HandlePointCloud2Message(const string& topic,
const sensor_msgs::PointCloud2::ConstPtr& msg); const sensor_msgs::PointCloud2::ConstPtr& msg);
private: private:
void HandleLaserScanProto(
const string& topic, const ::cartographer::common::Time time,
const string& frame_id,
const ::cartographer::sensor::proto::LaserScan& laser_scan);
const SensorBridgeOptions options_;
const TfBridge* const tf_bridge_;
const int trajectory_id_; const int trajectory_id_;
::cartographer::mapping::SensorCollator<SensorData>* sensor_collator_; ::cartographer::mapping::SensorCollator<SensorData>* const sensor_collator_;
}; };
} // namespace cartographer_ros } // namespace cartographer_ros

View File

@ -20,16 +20,15 @@
#include <string> #include <string>
#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/sensor/proto/sensor.pb.h" #include "cartographer/sensor/laser.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "glog/logging.h"
namespace cartographer_ros { namespace cartographer_ros {
// This type is a logical union, i.e. only one type of sensor data is actually // This type is a logical union, i.e. only one type of sensor data is actually
// filled in. It is only used for time ordering sensor data before passing it // filled in. It is only used for time ordering sensor data before passing it
// on. // on.
enum class SensorType { kImu, kLaserScan, kLaserFan3D, kOdometry }; enum class SensorType { kImu, kLaserFan3D, kOdometry };
struct SensorData { struct SensorData {
struct Odometry { struct Odometry {
::cartographer::transform::Rigid3d pose; ::cartographer::transform::Rigid3d pose;
@ -37,8 +36,8 @@ struct SensorData {
}; };
struct Imu { struct Imu {
Eigen::Vector3d angular_velocity;
Eigen::Vector3d linear_acceleration; Eigen::Vector3d linear_acceleration;
Eigen::Vector3d angular_velocity;
}; };
SensorData(const string& frame_id, const Imu& imu) SensorData(const string& frame_id, const Imu& imu)
@ -46,33 +45,30 @@ struct SensorData {
frame_id(CheckNoLeadingSlash(frame_id)), frame_id(CheckNoLeadingSlash(frame_id)),
imu(imu) {} imu(imu) {}
SensorData(const string& frame_id, const ::cartographer::sensor::proto::LaserScan& laser_scan) SensorData(const string& frame_id,
: type(SensorType::kLaserScan), const ::cartographer::sensor::LaserFan3D& laser_fan_3d)
frame_id(CheckNoLeadingSlash(frame_id)),
laser_scan(laser_scan) {}
SensorData(const string& frame_id, const ::cartographer::sensor::proto::LaserFan3D& laser_fan_3d)
: type(SensorType::kLaserFan3D), : type(SensorType::kLaserFan3D),
frame_id(CheckNoLeadingSlash(frame_id)), frame_id(CheckNoLeadingSlash(frame_id)),
laser_fan_3d(laser_fan_3d) {} laser_fan_3d(laser_fan_3d) {}
SensorData(const string& frame_id, const Odometry& odometry) SensorData(const string& frame_id, const Odometry& odometry)
: type(SensorType::kOdometry), frame_id(frame_id), odometry(odometry) {} : type(SensorType::kOdometry),
frame_id(CheckNoLeadingSlash(frame_id)),
odometry(odometry) {}
SensorType type; SensorType type;
string frame_id; string frame_id;
Imu imu; Imu imu;
::cartographer::sensor::proto::LaserScan laser_scan; ::cartographer::sensor::LaserFan3D laser_fan_3d;
::cartographer::sensor::proto::LaserFan3D laser_fan_3d;
Odometry odometry; Odometry odometry;
private: private:
static const string& CheckNoLeadingSlash(const string& frame_id) { static const string& CheckNoLeadingSlash(const string& frame_id) {
if (frame_id.size() > 0) { if (frame_id.size() > 0) {
CHECK_NE(frame_id[0], '/'); CHECK_NE(frame_id[0], '/');
}
return frame_id;
} }
return frame_id;
}
}; };
} // namespace cartographer_ros } // namespace cartographer_ros

View File

@ -0,0 +1,56 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/common/make_unique.h"
#include "msg_conversion.h"
#include "tf_bridge.h"
namespace cartographer_ros {
TfBridge::TfBridge(const string& tracking_frame,
const double lookup_transform_timeout_sec,
const tf2_ros::Buffer* buffer)
: tracking_frame_(tracking_frame),
lookup_transform_timeout_sec_(lookup_transform_timeout_sec),
buffer_(buffer) {}
std::unique_ptr<::cartographer::transform::Rigid3d> TfBridge::LookupToTracking(
const ::cartographer::common::Time time, const string& frame_id) const {
::ros::Duration timeout(lookup_transform_timeout_sec_);
std::unique_ptr<::cartographer::transform::Rigid3d> frame_id_to_tracking;
try {
const ::ros::Time latest_tf_time =
buffer_
->lookupTransform(tracking_frame_, frame_id, ::ros::Time(0.),
timeout)
.header.stamp;
const ::ros::Time requested_time = ToRos(time);
if (latest_tf_time >= requested_time) {
// We already have newer data, so we do not wait. Otherwise, we would wait
// for the full 'timeout' even if we ask for data that is too old.
timeout = ::ros::Duration(0.);
}
return ::cartographer::common::make_unique<
::cartographer::transform::Rigid3d>(ToRigid3d(buffer_->lookupTransform(
tracking_frame_, frame_id, requested_time, timeout)));
} catch (const tf2::TransformException& ex) {
LOG(WARNING) << ex.what();
}
return nullptr;
}
} // namespace cartographer_ros

View File

@ -0,0 +1,51 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_TF_BRIDGE_H_
#define CARTOGRAPHER_ROS_TF_BRIDGE_H_
#include <memory>
#include "cartographer/transform/rigid_transform.h"
#include "tf2_ros/buffer.h"
#include "time_conversion.h"
namespace cartographer_ros {
class TfBridge {
public:
TfBridge(const string& tracking_frame, double lookup_transform_timeout_sec,
const tf2_ros::Buffer* buffer);
~TfBridge() {}
TfBridge(const TfBridge&) = delete;
TfBridge& operator=(const TfBridge&) = delete;
// Returns the transform for 'frame_id' to 'tracking_frame_' if it exists at
// 'time'.
std::unique_ptr<::cartographer::transform::Rigid3d> LookupToTracking(
::cartographer::common::Time time, const string& frame_id) const;
private:
const string tracking_frame_;
const double lookup_transform_timeout_sec_;
const tf2_ros::Buffer* const buffer_;
};
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_TF_BRIDGE_H_

View File

@ -55,17 +55,11 @@ use_odometry_data
If enabled, subscribes to `nav_msgs/Odometry`_ on the topic "odom". Odometry If enabled, subscribes to `nav_msgs/Odometry`_ on the topic "odom". Odometry
must be provided in this case, and the information will be included in SLAM. must be provided in this case, and the information will be included in SLAM.
use_constant_odometry_variance sensor_bridge.constant_odometry_translational_variance
If enabled, the configured variances will be used instead of the ones provided The variance to use for the translational component of odometry.
in the Odometry messages.
constant_odometry_translational_variance sensor_bridge.constant_odometry_rotational_variance
The variance to use for the translational component of odometry to use if The variance to use for the rotational component of odometry.
*use_constant_odometry_variance* is enabled.
constant_odometry_rotational_variance
The variance to use for the rotational component of odometry to use if
*use_constant_odometry_variance* is enabled.
use_horizontal_laser use_horizontal_laser
If enabled, the node subscribes to `sensor_msgs/LaserScan`_ on the "scan" If enabled, the node subscribes to `sensor_msgs/LaserScan`_ on the "scan"
@ -77,13 +71,13 @@ use_horizontal_multi_echo_laser
"echoes" topic. If 2D SLAM is used, either this or *use_horizontal_laser* "echoes" topic. If 2D SLAM is used, either this or *use_horizontal_laser*
must be enabled. must be enabled.
horizontal_laser_min_range sensor_bridge.horizontal_laser_min_range
Range in meters below which laser returns are ignored for the purpose of SLAM. Range in meters below which laser returns are ignored for the purpose of SLAM.
horizontal_laser_max_range sensor_bridge.horizontal_laser_max_range
Range in meters above which laser returns are ignored for the purpose of SLAM. Range in meters above which laser returns are ignored for the purpose of SLAM.
horizontal_laser_missing_echo_ray_length sensor_bridge.horizontal_laser_missing_echo_ray_length
Range in meters to use for inserting free space when no laser return was Range in meters to use for inserting free space when no laser return was
detected. detected.