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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer_ros/CMakeLists.txt
View File

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

13
cartographer_ros/configuration_files/backpack_2d.lua
View File

@ -16,20 +16,21 @@ include "map_builder.lua"
options = {
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",
tracking_frame = "base_link",
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
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_multi_echo_laser = true,
horizontal_laser_min_range = 0.,
horizontal_laser_max_range = 30.,
horizontal_laser_missing_echo_ray_length = 5.,
num_lasers_3d = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,

13
cartographer_ros/configuration_files/backpack_3d.lua
View File

@ -16,20 +16,21 @@ include "map_builder.lua"
options = {
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",
tracking_frame = "base_link",
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
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_multi_echo_laser = false,
horizontal_laser_min_range = 0.,
horizontal_laser_max_range = 30.,
horizontal_laser_missing_echo_ray_length = 5.,
num_lasers_3d = 2,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,

13
cartographer_ros/configuration_files/pr2.lua
View File

@ -16,20 +16,21 @@ include "map_builder.lua"
options = {
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",
tracking_frame = "base_footprint",
published_frame = "base_footprint",
odom_frame = "odom",
provide_odom_frame = true,
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_multi_echo_laser = false,
horizontal_laser_min_range = 0.,
horizontal_laser_max_range = 30.,
horizontal_laser_missing_echo_ray_length = 5.,
num_lasers_3d = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,

13
cartographer_ros/configuration_files/revo_lds.lua
View File

@ -16,20 +16,21 @@ include "map_builder.lua"
options = {
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",
tracking_frame = "horizontal_laser_link",
published_frame = "horizontal_laser_link",
odom_frame = "odom",
provide_odom_frame = true,
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_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,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,

175
cartographer_ros/src/cartographer_node_main.cc
View File

@ -69,6 +69,7 @@
#include "ros_log_sink.h"
#include "sensor_bridge.h"
#include "sensor_data.h"
#include "tf_bridge.h"
#include "time_conversion.h"
DEFINE_string(configuration_directory, "",
@ -122,20 +123,6 @@ class Node {
private:
void HandleSensorData(int64 timestamp,
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(
::cartographer_ros_msgs::SubmapQuery::Request& request,
::cartographer_ros_msgs::SubmapQuery::Response& response);
@ -150,6 +137,11 @@ class Node {
const NodeOptions options_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_;
tf2_ros::TransformBroadcaster tf_broadcaster_;
TfBridge tf_bridge_;
carto::common::Mutex mutex_;
std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_data_
GUARDED_BY(mutex_);
@ -170,10 +162,6 @@ class Node {
carto::common::Time::min();
::ros::ServiceServer finish_trajectory_server_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_;
tf2_ros::TransformBroadcaster tf_broadcaster_;
::ros::Publisher occupancy_grid_publisher_;
std::thread occupancy_grid_thread_;
bool terminating_ = false GUARDED_BY(mutex_);
@ -189,10 +177,13 @@ class Node {
Node::Node(const NodeOptions& options)
: options_(options),
map_builder_(options.map_builder_options, &constant_data_),
sensor_bridge_(kTrajectoryBuilderId, &sensor_collator_),
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() {
{
@ -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() {
// Set of all topics we subscribe to. We use the non-remapped default names
// which are unique.
@ -316,7 +206,7 @@ void Node::Initialize() {
kLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
[this](const sensor_msgs::LaserScan::ConstPtr& msg) {
sensor_bridge_.AddLaserScanMessage(kLaserScanTopic, msg);
sensor_bridge_.HandleLaserScanMessage(kLaserScanTopic, msg);
}));
expected_sensor_identifiers.insert(kLaserScanTopic);
}
@ -325,7 +215,7 @@ void Node::Initialize() {
kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
[this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
sensor_bridge_.AddMultiEchoLaserScanMessage(
sensor_bridge_.HandleMultiEchoLaserScanMessage(
kMultiEchoLaserScanTopic, msg);
}));
expected_sensor_identifiers.insert(kMultiEchoLaserScanTopic);
@ -342,7 +232,7 @@ void Node::Initialize() {
topic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
[this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
sensor_bridge_.AddPointCloud2Message(topic, msg);
sensor_bridge_.HandlePointCloud2Message(topic, msg);
})));
expected_sensor_identifiers.insert(topic);
}
@ -358,7 +248,7 @@ void Node::Initialize() {
kImuTopic, kInfiniteSubscriberQueueSize,
boost::function<void(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);
}
@ -368,7 +258,7 @@ void Node::Initialize() {
kOdometryTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
[this](const nav_msgs::Odometry::ConstPtr& msg) {
sensor_bridge_.AddOdometryMessage(kOdometryTopic, msg);
sensor_bridge_.HandleOdometryMessage(kOdometryTopic, msg);
}));
expected_sensor_identifiers.insert(kOdometryTopic);
}
@ -533,9 +423,9 @@ void Node::PublishPoseAndScanMatchedPointCloud(
geometry_msgs::TransformStamped stamped_transform;
stamped_transform.header.stamp = ToRos(last_pose_estimate.time);
try {
const Rigid3d published_to_tracking = LookupToTrackingTransformOrThrow(
const auto published_to_tracking = tf_bridge_.LookupToTracking(
last_pose_estimate.time, options_.published_frame);
if (published_to_tracking != nullptr) {
if (options_.provide_odom_frame) {
stamped_transform.header.frame_id = options_.map_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.child_frame_id = options_.published_frame;
stamped_transform.transform =
ToGeometryMsgTransform(tracking_to_local * published_to_tracking);
ToGeometryMsgTransform(tracking_to_local * (*published_to_tracking));
tf_broadcaster_.sendTransform(stamped_transform);
} else {
stamped_transform.header.frame_id = options_.map_frame;
stamped_transform.child_frame_id = options_.published_frame;
stamped_transform.transform =
ToGeometryMsgTransform(tracking_to_map * published_to_tracking);
ToGeometryMsgTransform(tracking_to_map * (*published_to_tracking));
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
@ -615,23 +503,28 @@ void Node::HandleSensorData(const int64 timestamp,
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) {
case SensorType::kImu:
AddImu(timestamp, sensor_data->frame_id, sensor_data->imu);
return;
case SensorType::kLaserScan:
AddHorizontalLaserFan(timestamp, sensor_data->frame_id,
sensor_data->laser_scan);
trajectory_builder->AddImuData(time, sensor_data->imu.linear_acceleration,
sensor_data->imu.angular_velocity);
return;
case SensorType::kLaserFan3D:
AddLaserFan3D(timestamp, sensor_data->frame_id,
if (options_.map_builder_options.use_trajectory_builder_2d()) {
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;
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;
}
LOG(FATAL);

1
cartographer_ros/src/msg_conversion.h
View File

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

17
cartographer_ros/src/node_options.cc
View File

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

9
cartographer_ros/src/node_options.h
View File

@ -23,25 +23,22 @@
#include "cartographer/common/port.h"
#include "cartographer/mapping/map_builder.h"
#include "sensor_bridge.h"
namespace cartographer_ros {
// Top-level options of Cartographer's ROS integration.
struct NodeOptions {
::cartographer::mapping::proto::MapBuilderOptions map_builder_options;
SensorBridgeOptions sensor_bridge_options;
string map_frame;
string tracking_frame;
string published_frame;
string odom_frame;
bool provide_odom_frame;
bool use_odometry_data;
bool use_constant_odometry_variance;
double constant_odometry_translational_variance;
double constant_odometry_rotational_variance;
bool use_horizontal_laser;
bool use_horizontal_multi_echo_laser;
double horizontal_laser_min_range;
double horizontal_laser_max_range;
double horizontal_laser_missing_echo_ray_length;
int num_lasers_3d;
double lookup_transform_timeout_sec;
double submap_publish_period_sec;

159
cartographer_ros/src/sensor_bridge.cc
View File

@ -16,75 +16,144 @@
#include "sensor_bridge.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "msg_conversion.h"
#include "time_conversion.h"
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(
const SensorBridgeOptions& options, const TfBridge* const tf_bridge,
const int trajectory_id,
::cartographer::mapping::SensorCollator<SensorData>* sensor_collator)
: trajectory_id_(trajectory_id), sensor_collator_(sensor_collator) {}
carto::mapping::SensorCollator<SensorData>* const sensor_collator)
: options_(options),
tf_bridge_(tf_bridge),
trajectory_id_(trajectory_id),
sensor_collator_(sensor_collator) {}
void SensorBridge::AddOdometryMessage(const string& topic,
const nav_msgs::Odometry::ConstPtr& msg) {
auto sensor_data = ::cartographer::common::make_unique<SensorData>(
void SensorBridge::HandleOdometryMessage(
const string& topic, const nav_msgs::Odometry::ConstPtr& msg) {
const carto::common::Time time = FromRos(msg->header.stamp);
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
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),
ToPoseCovariance(msg->pose.covariance)});
sensor_collator_->AddSensorData(
trajectory_id_,
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic,
std::move(sensor_data));
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) {
CHECK_NE(msg->angular_velocity_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);
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) {
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.";
sensor_collator_->AddSensorData(
trajectory_id_, carto::common::ToUniversal(time), topic,
carto::common::make_unique<SensorData>(
msg->header.frame_id,
SensorData::Imu{{msg->angular_velocity.x, msg->angular_velocity.y,
msg->angular_velocity.z},
{msg->linear_acceleration.x, msg->linear_acceleration.y,
msg->linear_acceleration.z}});
sensor_collator_->AddSensorData(
trajectory_id_,
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic,
std::move(sensor_data));
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) {
auto sensor_data = ::cartographer::common::make_unique<SensorData>(
msg->header.frame_id, ToCartographer(*msg));
sensor_collator_->AddSensorData(
trajectory_id_,
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic,
std::move(sensor_data));
HandleLaserScanProto(topic, FromRos(msg->header.stamp), msg->header.frame_id,
ToCartographer(*msg));
}
void SensorBridge::AddMultiEchoLaserScanMessage(
void SensorBridge::HandleMultiEchoLaserScanMessage(
const string& topic, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
auto sensor_data = ::cartographer::common::make_unique<SensorData>(
msg->header.frame_id, ToCartographer(*msg));
sensor_collator_->AddSensorData(
trajectory_id_,
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic,
std::move(sensor_data));
HandleLaserScanProto(topic, FromRos(msg->header.stamp), msg->header.frame_id,
ToCartographer(*msg));
}
void SensorBridge::AddPointCloud2Message(
void SensorBridge::HandlePointCloud2Message(
const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg) {
pcl::PointCloud<pcl::PointXYZ> pcl_points;
pcl::fromROSMsg(*msg, pcl_points);
auto sensor_data = ::cartographer::common::make_unique<SensorData>(
msg->header.frame_id, ToCartographer(pcl_points));
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
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_,
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic,
std::move(sensor_data));
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>())));
}
}
void SensorBridge::HandleLaserScanProto(
const string& topic, const ::cartographer::common::Time time,
const string& frame_id,
const ::cartographer::sensor::proto::LaserScan& laser_scan) {
const auto laser_fan_2d = carto::sensor::ToLaserFan(
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

38
cartographer_ros/src/sensor_bridge.h
View File

@ -18,6 +18,8 @@
#define CARTOGRAPHER_ROS_SENSOR_BRIDGE_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/TransformStamped.h"
#include "nav_msgs/OccupancyGrid.h"
@ -28,34 +30,54 @@
#include "sensor_msgs/MultiEchoLaserScan.h"
#include "sensor_msgs/PointCloud2.h"
#include "tf_bridge.h"
namespace cartographer_ros {
// A wrapper around SensorCollator that converts ROS messages into our internal
// representation and passes them on to the 'sensor_collator'.
struct SensorBridgeOptions {
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 {
public:
explicit SensorBridge(
const SensorBridgeOptions& options, const TfBridge* tf_bridge,
int trajectory_id,
::cartographer::mapping::SensorCollator<SensorData>* sensor_collator);
SensorBridge(const SensorBridge&) = delete;
SensorBridge& operator=(const SensorBridge&) = delete;
void AddOdometryMessage(const string& topic,
void HandleOdometryMessage(const string& topic,
const nav_msgs::Odometry::ConstPtr& msg);
void AddImuMessage(const string& topic,
void HandleImuMessage(const string& topic,
const sensor_msgs::Imu::ConstPtr& msg);
void AddLaserScanMessage(const string& topic,
void HandleLaserScanMessage(const string& topic,
const sensor_msgs::LaserScan::ConstPtr& msg);
void AddMultiEchoLaserScanMessage(
void HandleMultiEchoLaserScanMessage(
const string& topic,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
void AddPointCloud2Message(const string& topic,
void HandlePointCloud2Message(const string& topic,
const sensor_msgs::PointCloud2::ConstPtr& msg);
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_;
::cartographer::mapping::SensorCollator<SensorData>* sensor_collator_;
::cartographer::mapping::SensorCollator<SensorData>* const sensor_collator_;
};
} // namespace cartographer_ros

22
cartographer_ros/src/sensor_data.h
View File

@ -20,16 +20,15 @@
#include <string>
#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 "glog/logging.h"
namespace cartographer_ros {
// 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
// on.
enum class SensorType { kImu, kLaserScan, kLaserFan3D, kOdometry };
enum class SensorType { kImu, kLaserFan3D, kOdometry };
struct SensorData {
struct Odometry {
::cartographer::transform::Rigid3d pose;
@ -37,8 +36,8 @@ struct SensorData {
};
struct Imu {
Eigen::Vector3d angular_velocity;
Eigen::Vector3d linear_acceleration;
Eigen::Vector3d angular_velocity;
};
SensorData(const string& frame_id, const Imu& imu)
@ -46,24 +45,21 @@ struct SensorData {
frame_id(CheckNoLeadingSlash(frame_id)),
imu(imu) {}
SensorData(const string& frame_id, const ::cartographer::sensor::proto::LaserScan& laser_scan)
: type(SensorType::kLaserScan),
frame_id(CheckNoLeadingSlash(frame_id)),
laser_scan(laser_scan) {}
SensorData(const string& frame_id, const ::cartographer::sensor::proto::LaserFan3D& laser_fan_3d)
SensorData(const string& frame_id,
const ::cartographer::sensor::LaserFan3D& laser_fan_3d)
: type(SensorType::kLaserFan3D),
frame_id(CheckNoLeadingSlash(frame_id)),
laser_fan_3d(laser_fan_3d) {}
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;
string frame_id;
Imu imu;
::cartographer::sensor::proto::LaserScan laser_scan;
::cartographer::sensor::proto::LaserFan3D laser_fan_3d;
::cartographer::sensor::LaserFan3D laser_fan_3d;
Odometry odometry;
private:

56
cartographer_ros/src/tf_bridge.cc Normal file
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

51
cartographer_ros/src/tf_bridge.h Normal file
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_

20
docs/source/configuration.rst
View File

@ -55,17 +55,11 @@ use_odometry_data
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.
use_constant_odometry_variance
If enabled, the configured variances will be used instead of the ones provided
in the Odometry messages.
sensor_bridge.constant_odometry_translational_variance
The variance to use for the translational component of odometry.
constant_odometry_translational_variance
The variance to use for the translational component of odometry to use if
*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.
sensor_bridge.constant_odometry_rotational_variance
The variance to use for the rotational component of odometry.
use_horizontal_laser
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*
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.
horizontal_laser_max_range
sensor_bridge.horizontal_laser_max_range
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
detected.