Also extrapolate based on IMU if available. (#458)

master
Wolfgang Hess 2017-07-28 17:52:39 +02:00 committed by GitHub
parent a40357b61d
commit b6b1c572f6
4 changed files with 50 additions and 23 deletions

View File

@ -106,21 +106,24 @@ void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList()); submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
} }
::cartographer::mapping::PoseExtrapolator* Node::GetExtrapolator(
int trajectory_id) {
constexpr double kExtrapolationEstimationTimeSec = 0.001; // 1 ms
if (extrapolators_.count(trajectory_id) == 0) {
extrapolators_.emplace(
std::piecewise_construct, std::forward_as_tuple(trajectory_id),
std::forward_as_tuple(::cartographer::common::FromSeconds(
kExtrapolationEstimationTimeSec)));
}
return &extrapolators_.at(trajectory_id);
}
void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
const auto& trajectory_state = entry.second; const auto& trajectory_state = entry.second;
// TODO(whess): Use improved models, e.g. using odometry and IMU when auto& extrapolator = *GetExtrapolator(entry.first);
// available.
constexpr double extrapolation_estimation_time_sec = 0.001; // 1 ms
if (extrapolators_.count(entry.first) == 0) {
extrapolators_.emplace(
std::piecewise_construct, std::forward_as_tuple(entry.first),
std::forward_as_tuple(::cartographer::common::FromSeconds(
extrapolation_estimation_time_sec)));
}
auto& extrapolator = extrapolators_.at(entry.first);
// 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
// frequency, and republishing it would be computationally wasteful. // frequency, and republishing it would be computationally wasteful.
if (trajectory_state.pose_estimate.time != extrapolator.GetLastPoseTime()) { if (trajectory_state.pose_estimate.time != extrapolator.GetLastPoseTime()) {
@ -292,8 +295,13 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
boost::function<void(const sensor_msgs::Imu::ConstPtr&)>( boost::function<void(const sensor_msgs::Imu::ConstPtr&)>(
[this, trajectory_id, [this, trajectory_id,
topic](const sensor_msgs::Imu::ConstPtr& msg) { topic](const sensor_msgs::Imu::ConstPtr& msg) {
map_builder_bridge_.sensor_bridge(trajectory_id) auto sensor_bridge_ptr =
->HandleImuMessage(topic, msg); map_builder_bridge_.sensor_bridge(trajectory_id);
sensor_bridge_ptr->HandleImuMessage(topic, msg);
auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
if (imu_data_ptr != nullptr) {
GetExtrapolator(trajectory_id)->AddImuData(*imu_data_ptr);
}
}))); })));
} }

View File

@ -85,6 +85,7 @@ class Node {
const cartographer_ros_msgs::SensorTopics& topics, const cartographer_ros_msgs::SensorTopics& topics,
int trajectory_id); int trajectory_id);
void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
::cartographer::mapping::PoseExtrapolator* GetExtrapolator(int trajectory_id);
void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event); void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event);
void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event); void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event);
void PublishConstraintList(const ::ros::WallTimerEvent& timer_event); void PublishConstraintList(const ::ros::WallTimerEvent& timer_event);

View File

@ -16,6 +16,7 @@
#include "cartographer_ros/sensor_bridge.h" #include "cartographer_ros/sensor_bridge.h"
#include "cartographer/common/make_unique.h"
#include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/time_conversion.h" #include "cartographer_ros/time_conversion.h"
@ -58,7 +59,7 @@ void SensorBridge::HandleOdometryMessage(
} }
} }
void SensorBridge::HandleImuMessage(const string& sensor_id, std::unique_ptr<::cartographer::sensor::ImuData> SensorBridge::ToImuData(
const sensor_msgs::Imu::ConstPtr& msg) { const sensor_msgs::Imu::ConstPtr& msg) {
CHECK_NE(msg->linear_acceleration_covariance[0], -1) CHECK_NE(msg->linear_acceleration_covariance[0], -1)
<< "Your IMU data claims to not contain linear acceleration measurements " << "Your IMU data claims to not contain linear acceleration measurements "
@ -74,15 +75,27 @@ void SensorBridge::HandleImuMessage(const string& sensor_id,
const carto::common::Time time = FromRos(msg->header.stamp); const carto::common::Time time = FromRos(msg->header.stamp);
const auto sensor_to_tracking = tf_bridge_.LookupToTracking( const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
time, CheckNoLeadingSlash(msg->header.frame_id)); time, CheckNoLeadingSlash(msg->header.frame_id));
if (sensor_to_tracking != nullptr) { if (sensor_to_tracking == nullptr) {
return nullptr;
}
CHECK(sensor_to_tracking->translation().norm() < 1e-5) CHECK(sensor_to_tracking->translation().norm() < 1e-5)
<< "The IMU frame must be colocated with the tracking frame. " << "The IMU frame must be colocated with the tracking frame. "
"Transforming linear acceleration into the tracking frame will " "Transforming linear acceleration into the tracking frame will "
"otherwise be imprecise."; "otherwise be imprecise.";
trajectory_builder_->AddImuData( return ::cartographer::common::make_unique<::cartographer::sensor::ImuData>(
sensor_id, time, ::cartographer::sensor::ImuData{
time,
sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration), sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)); sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)});
}
void SensorBridge::HandleImuMessage(const string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg) {
std::unique_ptr<::cartographer::sensor::ImuData> imu_data = ToImuData(msg);
if (imu_data != nullptr) {
trajectory_builder_->AddImuData(sensor_id, imu_data->time,
imu_data->linear_acceleration,
imu_data->angular_velocity);
} }
} }

View File

@ -17,7 +17,10 @@
#ifndef CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_ #ifndef CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_
#define CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_ #define CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_
#include <memory>
#include "cartographer/mapping/trajectory_builder.h" #include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "cartographer_ros/tf_bridge.h" #include "cartographer_ros/tf_bridge.h"
@ -45,6 +48,8 @@ class SensorBridge {
void HandleOdometryMessage(const string& sensor_id, void HandleOdometryMessage(const string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg); const nav_msgs::Odometry::ConstPtr& msg);
std::unique_ptr<::cartographer::sensor::ImuData> ToImuData(
const sensor_msgs::Imu::ConstPtr& msg);
void HandleImuMessage(const string& sensor_id, void HandleImuMessage(const string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg); const sensor_msgs::Imu::ConstPtr& msg);
void HandleLaserScanMessage(const string& sensor_id, void HandleLaserScanMessage(const string& sensor_id,