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
parent
2a040bc755
commit
d4825af3d4
|
@ -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
|
||||||
)
|
)
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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(
|
||||||
|
|
|
@ -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 =
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
|
@ -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_
|
|
@ -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.
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue