Moves SensorData struct into Cartographer. (#110)
parent
bc3a4978f2
commit
0a76d10546
|
@ -63,17 +63,10 @@ google_library(sensor_bridge
|
||||||
sensor_bridge.h
|
sensor_bridge.h
|
||||||
DEPENDS
|
DEPENDS
|
||||||
msg_conversion
|
msg_conversion
|
||||||
sensor_data
|
|
||||||
tf_bridge
|
tf_bridge
|
||||||
time_conversion
|
time_conversion
|
||||||
)
|
)
|
||||||
|
|
||||||
google_library(sensor_data
|
|
||||||
USES_CARTOGRAPHER
|
|
||||||
HDRS
|
|
||||||
sensor_data.h
|
|
||||||
)
|
|
||||||
|
|
||||||
google_library(tf_bridge
|
google_library(tf_bridge
|
||||||
USES_CARTOGRAPHER
|
USES_CARTOGRAPHER
|
||||||
SRCS
|
SRCS
|
||||||
|
@ -118,7 +111,6 @@ google_binary(cartographer_node
|
||||||
occupancy_grid
|
occupancy_grid
|
||||||
ros_log_sink
|
ros_log_sink
|
||||||
sensor_bridge
|
sensor_bridge
|
||||||
sensor_data
|
|
||||||
tf_bridge
|
tf_bridge
|
||||||
time_conversion
|
time_conversion
|
||||||
)
|
)
|
||||||
|
|
|
@ -41,6 +41,7 @@
|
||||||
#include "cartographer/mapping_3d/local_trajectory_builder.h"
|
#include "cartographer/mapping_3d/local_trajectory_builder.h"
|
||||||
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"
|
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"
|
||||||
#include "cartographer/mapping_3d/sparse_pose_graph.h"
|
#include "cartographer/mapping_3d/sparse_pose_graph.h"
|
||||||
|
#include "cartographer/sensor/data.h"
|
||||||
#include "cartographer/sensor/laser.h"
|
#include "cartographer/sensor/laser.h"
|
||||||
#include "cartographer/sensor/point_cloud.h"
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
#include "cartographer/sensor/proto/sensor.pb.h"
|
#include "cartographer/sensor/proto/sensor.pb.h"
|
||||||
|
@ -52,7 +53,6 @@
|
||||||
#include "cartographer_ros/occupancy_grid.h"
|
#include "cartographer_ros/occupancy_grid.h"
|
||||||
#include "cartographer_ros/ros_log_sink.h"
|
#include "cartographer_ros/ros_log_sink.h"
|
||||||
#include "cartographer_ros/sensor_bridge.h"
|
#include "cartographer_ros/sensor_bridge.h"
|
||||||
#include "cartographer_ros/sensor_data.h"
|
|
||||||
#include "cartographer_ros/tf_bridge.h"
|
#include "cartographer_ros/tf_bridge.h"
|
||||||
#include "cartographer_ros/time_conversion.h"
|
#include "cartographer_ros/time_conversion.h"
|
||||||
#include "cartographer_ros_msgs/FinishTrajectory.h"
|
#include "cartographer_ros_msgs/FinishTrajectory.h"
|
||||||
|
@ -121,7 +121,7 @@ class Node {
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void HandleSensorData(int64 timestamp,
|
void HandleSensorData(int64 timestamp,
|
||||||
std::unique_ptr<SensorData> sensor_data);
|
std::unique_ptr<carto::sensor::Data> sensor_data);
|
||||||
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);
|
||||||
|
@ -145,7 +145,7 @@ class Node {
|
||||||
std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_data_
|
std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_data_
|
||||||
GUARDED_BY(mutex_);
|
GUARDED_BY(mutex_);
|
||||||
carto::mapping::MapBuilder map_builder_ GUARDED_BY(mutex_);
|
carto::mapping::MapBuilder map_builder_ GUARDED_BY(mutex_);
|
||||||
carto::mapping::SensorCollator<SensorData> sensor_collator_
|
carto::mapping::SensorCollator<carto::sensor::Data> sensor_collator_
|
||||||
GUARDED_BY(mutex_);
|
GUARDED_BY(mutex_);
|
||||||
SensorBridge sensor_bridge_ GUARDED_BY(mutex_);
|
SensorBridge sensor_bridge_ GUARDED_BY(mutex_);
|
||||||
|
|
||||||
|
@ -266,7 +266,8 @@ void Node::Initialize() {
|
||||||
CHECK_EQ(kTrajectoryBuilderId, map_builder_.AddTrajectoryBuilder());
|
CHECK_EQ(kTrajectoryBuilderId, map_builder_.AddTrajectoryBuilder());
|
||||||
sensor_collator_.AddTrajectory(
|
sensor_collator_.AddTrajectory(
|
||||||
kTrajectoryBuilderId, expected_sensor_identifiers,
|
kTrajectoryBuilderId, expected_sensor_identifiers,
|
||||||
[this](const int64 timestamp, std::unique_ptr<SensorData> sensor_data) {
|
[this](const int64 timestamp,
|
||||||
|
std::unique_ptr<carto::sensor::Data> sensor_data) {
|
||||||
HandleSensorData(timestamp, std::move(sensor_data));
|
HandleSensorData(timestamp, std::move(sensor_data));
|
||||||
});
|
});
|
||||||
|
|
||||||
|
@ -481,7 +482,7 @@ void Node::SpinOccupancyGridThreadForever() {
|
||||||
}
|
}
|
||||||
|
|
||||||
void Node::HandleSensorData(const int64 timestamp,
|
void Node::HandleSensorData(const int64 timestamp,
|
||||||
std::unique_ptr<SensorData> sensor_data) {
|
std::unique_ptr<carto::sensor::Data> sensor_data) {
|
||||||
auto it = rate_timers_.find(sensor_data->frame_id);
|
auto it = rate_timers_.find(sensor_data->frame_id);
|
||||||
if (it == rate_timers_.end()) {
|
if (it == rate_timers_.end()) {
|
||||||
it = rate_timers_
|
it = rate_timers_
|
||||||
|
@ -505,12 +506,12 @@ void Node::HandleSensorData(const int64 timestamp,
|
||||||
auto* const trajectory_builder =
|
auto* const trajectory_builder =
|
||||||
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId);
|
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId);
|
||||||
switch (sensor_data->type) {
|
switch (sensor_data->type) {
|
||||||
case SensorType::kImu:
|
case carto::sensor::Data::Type::kImu:
|
||||||
trajectory_builder->AddImuData(time, sensor_data->imu.linear_acceleration,
|
trajectory_builder->AddImuData(time, sensor_data->imu.linear_acceleration,
|
||||||
sensor_data->imu.angular_velocity);
|
sensor_data->imu.angular_velocity);
|
||||||
return;
|
return;
|
||||||
|
|
||||||
case SensorType::kLaserFan3D:
|
case carto::sensor::Data::Type::kLaserFan3D:
|
||||||
if (options_.map_builder_options.use_trajectory_builder_2d()) {
|
if (options_.map_builder_options.use_trajectory_builder_2d()) {
|
||||||
trajectory_builder->AddHorizontalLaserFan(time,
|
trajectory_builder->AddHorizontalLaserFan(time,
|
||||||
sensor_data->laser_fan_3d);
|
sensor_data->laser_fan_3d);
|
||||||
|
@ -520,7 +521,7 @@ void Node::HandleSensorData(const int64 timestamp,
|
||||||
}
|
}
|
||||||
return;
|
return;
|
||||||
|
|
||||||
case SensorType::kOdometry:
|
case carto::sensor::Data::Type::kOdometry:
|
||||||
trajectory_builder->AddOdometerPose(time, sensor_data->odometry.pose,
|
trajectory_builder->AddOdometerPose(time, sensor_data->odometry.pose,
|
||||||
sensor_data->odometry.covariance);
|
sensor_data->odometry.covariance);
|
||||||
return;
|
return;
|
||||||
|
|
|
@ -26,6 +26,17 @@ namespace carto = ::cartographer;
|
||||||
|
|
||||||
using carto::transform::Rigid3d;
|
using carto::transform::Rigid3d;
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
const string& CheckNoLeadingSlash(const string& frame_id) {
|
||||||
|
if (frame_id.size() > 0) {
|
||||||
|
CHECK_NE(frame_id[0], '/');
|
||||||
|
}
|
||||||
|
return frame_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
SensorBridgeOptions CreateSensorBridgeOptions(
|
SensorBridgeOptions CreateSensorBridgeOptions(
|
||||||
carto::common::LuaParameterDictionary* const lua_parameter_dictionary) {
|
carto::common::LuaParameterDictionary* const lua_parameter_dictionary) {
|
||||||
SensorBridgeOptions options;
|
SensorBridgeOptions options;
|
||||||
|
@ -48,7 +59,7 @@ SensorBridgeOptions CreateSensorBridgeOptions(
|
||||||
SensorBridge::SensorBridge(
|
SensorBridge::SensorBridge(
|
||||||
const SensorBridgeOptions& options, const TfBridge* const tf_bridge,
|
const SensorBridgeOptions& options, const TfBridge* const tf_bridge,
|
||||||
const int trajectory_id,
|
const int trajectory_id,
|
||||||
carto::mapping::SensorCollator<SensorData>* const sensor_collator)
|
carto::mapping::SensorCollator<carto::sensor::Data>* const sensor_collator)
|
||||||
: options_(options),
|
: options_(options),
|
||||||
tf_bridge_(tf_bridge),
|
tf_bridge_(tf_bridge),
|
||||||
trajectory_id_(trajectory_id),
|
trajectory_id_(trajectory_id),
|
||||||
|
@ -74,9 +85,9 @@ void SensorBridge::HandleOdometryMessage(
|
||||||
if (sensor_to_tracking != nullptr) {
|
if (sensor_to_tracking != nullptr) {
|
||||||
sensor_collator_->AddSensorData(
|
sensor_collator_->AddSensorData(
|
||||||
trajectory_id_, carto::common::ToUniversal(time), topic,
|
trajectory_id_, carto::common::ToUniversal(time), topic,
|
||||||
carto::common::make_unique<SensorData>(
|
carto::common::make_unique<::cartographer::sensor::Data>(
|
||||||
msg->child_frame_id,
|
CheckNoLeadingSlash(msg->child_frame_id),
|
||||||
SensorData::Odometry{
|
::cartographer::sensor::Data::Odometry{
|
||||||
ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(),
|
ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(),
|
||||||
covariance}));
|
covariance}));
|
||||||
}
|
}
|
||||||
|
@ -96,9 +107,9 @@ void SensorBridge::HandleImuMessage(const string& topic,
|
||||||
"otherwise be imprecise.";
|
"otherwise be imprecise.";
|
||||||
sensor_collator_->AddSensorData(
|
sensor_collator_->AddSensorData(
|
||||||
trajectory_id_, carto::common::ToUniversal(time), topic,
|
trajectory_id_, carto::common::ToUniversal(time), topic,
|
||||||
carto::common::make_unique<SensorData>(
|
carto::common::make_unique<::cartographer::sensor::Data>(
|
||||||
msg->header.frame_id,
|
CheckNoLeadingSlash(msg->header.frame_id),
|
||||||
SensorData::Imu{
|
::cartographer::sensor::Data::Imu{
|
||||||
sensor_to_tracking->rotation() *
|
sensor_to_tracking->rotation() *
|
||||||
ToEigen(msg->linear_acceleration),
|
ToEigen(msg->linear_acceleration),
|
||||||
sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity),
|
sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity),
|
||||||
|
@ -128,8 +139,8 @@ void SensorBridge::HandlePointCloud2Message(
|
||||||
if (sensor_to_tracking != nullptr) {
|
if (sensor_to_tracking != nullptr) {
|
||||||
sensor_collator_->AddSensorData(
|
sensor_collator_->AddSensorData(
|
||||||
trajectory_id_, carto::common::ToUniversal(time), topic,
|
trajectory_id_, carto::common::ToUniversal(time), topic,
|
||||||
carto::common::make_unique<SensorData>(
|
carto::common::make_unique<::cartographer::sensor::Data>(
|
||||||
msg->header.frame_id,
|
CheckNoLeadingSlash(msg->header.frame_id),
|
||||||
carto::sensor::TransformLaserFan3D(
|
carto::sensor::TransformLaserFan3D(
|
||||||
carto::sensor::FromProto(ToCartographer(pcl_point_cloud)),
|
carto::sensor::FromProto(ToCartographer(pcl_point_cloud)),
|
||||||
sensor_to_tracking->cast<float>())));
|
sensor_to_tracking->cast<float>())));
|
||||||
|
@ -148,8 +159,9 @@ void SensorBridge::HandleLaserScanProto(
|
||||||
if (sensor_to_tracking != nullptr) {
|
if (sensor_to_tracking != nullptr) {
|
||||||
sensor_collator_->AddSensorData(
|
sensor_collator_->AddSensorData(
|
||||||
trajectory_id_, carto::common::ToUniversal(time), topic,
|
trajectory_id_, carto::common::ToUniversal(time), topic,
|
||||||
carto::common::make_unique<SensorData>(
|
carto::common::make_unique<::cartographer::sensor::Data>(
|
||||||
frame_id, carto::sensor::TransformLaserFan3D(
|
CheckNoLeadingSlash(frame_id),
|
||||||
|
carto::sensor::TransformLaserFan3D(
|
||||||
carto::sensor::ToLaserFan3D(laser_fan_2d),
|
carto::sensor::ToLaserFan3D(laser_fan_2d),
|
||||||
sensor_to_tracking->cast<float>())));
|
sensor_to_tracking->cast<float>())));
|
||||||
}
|
}
|
||||||
|
|
|
@ -18,9 +18,9 @@
|
||||||
#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/sensor/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/sensor_data.h"
|
|
||||||
#include "cartographer_ros/tf_bridge.h"
|
#include "cartographer_ros/tf_bridge.h"
|
||||||
#include "geometry_msgs/Transform.h"
|
#include "geometry_msgs/Transform.h"
|
||||||
#include "geometry_msgs/TransformStamped.h"
|
#include "geometry_msgs/TransformStamped.h"
|
||||||
|
@ -50,7 +50,8 @@ class SensorBridge {
|
||||||
explicit SensorBridge(
|
explicit SensorBridge(
|
||||||
const SensorBridgeOptions& options, const TfBridge* tf_bridge,
|
const SensorBridgeOptions& options, const TfBridge* tf_bridge,
|
||||||
int trajectory_id,
|
int trajectory_id,
|
||||||
::cartographer::mapping::SensorCollator<SensorData>* sensor_collator);
|
::cartographer::mapping::SensorCollator<::cartographer::sensor::Data>*
|
||||||
|
sensor_collator);
|
||||||
|
|
||||||
SensorBridge(const SensorBridge&) = delete;
|
SensorBridge(const SensorBridge&) = delete;
|
||||||
SensorBridge& operator=(const SensorBridge&) = delete;
|
SensorBridge& operator=(const SensorBridge&) = delete;
|
||||||
|
@ -76,7 +77,8 @@ class SensorBridge {
|
||||||
const SensorBridgeOptions options_;
|
const SensorBridgeOptions options_;
|
||||||
const TfBridge* const tf_bridge_;
|
const TfBridge* const tf_bridge_;
|
||||||
const int trajectory_id_;
|
const int trajectory_id_;
|
||||||
::cartographer::mapping::SensorCollator<SensorData>* const sensor_collator_;
|
::cartographer::mapping::SensorCollator<::cartographer::sensor::Data>* const
|
||||||
|
sensor_collator_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace cartographer_ros
|
} // namespace cartographer_ros
|
||||||
|
|
|
@ -1,76 +0,0 @@
|
||||||
/*
|
|
||||||
* 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_SENSOR_DATA_H_
|
|
||||||
#define CARTOGRAPHER_ROS_SENSOR_DATA_H_
|
|
||||||
|
|
||||||
#include <string>
|
|
||||||
|
|
||||||
#include "cartographer/kalman_filter/pose_tracker.h"
|
|
||||||
#include "cartographer/sensor/laser.h"
|
|
||||||
#include "cartographer/transform/rigid_transform.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, kLaserFan3D, kOdometry };
|
|
||||||
struct SensorData {
|
|
||||||
struct Odometry {
|
|
||||||
::cartographer::transform::Rigid3d pose;
|
|
||||||
::cartographer::kalman_filter::PoseCovariance covariance;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct Imu {
|
|
||||||
Eigen::Vector3d linear_acceleration;
|
|
||||||
Eigen::Vector3d angular_velocity;
|
|
||||||
};
|
|
||||||
|
|
||||||
SensorData(const string& frame_id, const Imu& imu)
|
|
||||||
: type(SensorType::kImu),
|
|
||||||
frame_id(CheckNoLeadingSlash(frame_id)),
|
|
||||||
imu(imu) {}
|
|
||||||
|
|
||||||
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(CheckNoLeadingSlash(frame_id)),
|
|
||||||
odometry(odometry) {}
|
|
||||||
|
|
||||||
SensorType type;
|
|
||||||
string frame_id;
|
|
||||||
Imu imu;
|
|
||||||
::cartographer::sensor::LaserFan3D laser_fan_3d;
|
|
||||||
Odometry odometry;
|
|
||||||
|
|
||||||
private:
|
|
||||||
static const string& CheckNoLeadingSlash(const string& frame_id) {
|
|
||||||
if (frame_id.size() > 0) {
|
|
||||||
CHECK_NE(frame_id[0], '/');
|
|
||||||
}
|
|
||||||
return frame_id;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace cartographer_ros
|
|
||||||
|
|
||||||
#endif // CARTOGRAPHER_ROS_SENSOR_DATA_H_
|
|
Loading…
Reference in New Issue