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

Moves SensorData struct into Cartographer. (#110)

master
Damon Kohler 2016-10-13 14:38:00 +02:00 committed by GitHub
parent bc3a4978f2
commit 0a76d10546
5 changed files with 39 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cartographer_ros/cartographer_ros/CMakeLists.txt
View File

@ -63,17 +63,10 @@ google_library(sensor_bridge
sensor_bridge.h
DEPENDS
msg_conversion
sensor_data
tf_bridge
time_conversion
)
google_library(sensor_data
USES_CARTOGRAPHER
HDRS
sensor_data.h
)
google_library(tf_bridge
USES_CARTOGRAPHER
SRCS
@ -118,7 +111,6 @@ google_binary(cartographer_node
occupancy_grid
ros_log_sink
sensor_bridge
sensor_data
tf_bridge
time_conversion
)

17
cartographer_ros/cartographer_ros/node_main.cc
View File

@ -41,6 +41,7 @@
#include "cartographer/mapping_3d/local_trajectory_builder.h"
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"
#include "cartographer/mapping_3d/sparse_pose_graph.h"
#include "cartographer/sensor/data.h"
#include "cartographer/sensor/laser.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/proto/sensor.pb.h"
@ -52,7 +53,6 @@
#include "cartographer_ros/occupancy_grid.h"
#include "cartographer_ros/ros_log_sink.h"
#include "cartographer_ros/sensor_bridge.h"
#include "cartographer_ros/sensor_data.h"
#include "cartographer_ros/tf_bridge.h"
#include "cartographer_ros/time_conversion.h"
#include "cartographer_ros_msgs/FinishTrajectory.h"
@ -121,7 +121,7 @@ class Node {
private:
void HandleSensorData(int64 timestamp,
std::unique_ptr<SensorData> sensor_data);
std::unique_ptr<carto::sensor::Data> sensor_data);
bool HandleSubmapQuery(
::cartographer_ros_msgs::SubmapQuery::Request& request,
::cartographer_ros_msgs::SubmapQuery::Response& response);
@ -145,7 +145,7 @@ class Node {
std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_data_
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_);
SensorBridge sensor_bridge_ GUARDED_BY(mutex_);
@ -266,7 +266,8 @@ void Node::Initialize() {
CHECK_EQ(kTrajectoryBuilderId, map_builder_.AddTrajectoryBuilder());
sensor_collator_.AddTrajectory(
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));
});
@ -481,7 +482,7 @@ void Node::SpinOccupancyGridThreadForever() {
}
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);
if (it == rate_timers_.end()) {
it = rate_timers_
@ -505,12 +506,12 @@ void Node::HandleSensorData(const int64 timestamp,
auto* const trajectory_builder =
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId);
switch (sensor_data->type) {
case SensorType::kImu:
case carto::sensor::Data::Type::kImu:
trajectory_builder->AddImuData(time, sensor_data->imu.linear_acceleration,
sensor_data->imu.angular_velocity);
return;
case SensorType::kLaserFan3D:
case carto::sensor::Data::Type::kLaserFan3D:
if (options_.map_builder_options.use_trajectory_builder_2d()) {
trajectory_builder->AddHorizontalLaserFan(time,
sensor_data->laser_fan_3d);
@ -520,7 +521,7 @@ void Node::HandleSensorData(const int64 timestamp,
}
return;
case SensorType::kOdometry:
case carto::sensor::Data::Type::kOdometry:
trajectory_builder->AddOdometerPose(time, sensor_data->odometry.pose,
sensor_data->odometry.covariance);
return;

34
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -26,6 +26,17 @@ namespace carto = ::cartographer;
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(
carto::common::LuaParameterDictionary* const lua_parameter_dictionary) {
SensorBridgeOptions options;
@ -48,7 +59,7 @@ SensorBridgeOptions CreateSensorBridgeOptions(
SensorBridge::SensorBridge(
const SensorBridgeOptions& options, const TfBridge* const tf_bridge,
const int trajectory_id,
carto::mapping::SensorCollator<SensorData>* const sensor_collator)
carto::mapping::SensorCollator<carto::sensor::Data>* const sensor_collator)
: options_(options),
tf_bridge_(tf_bridge),
trajectory_id_(trajectory_id),
@ -74,9 +85,9 @@ void SensorBridge::HandleOdometryMessage(
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{
carto::common::make_unique<::cartographer::sensor::Data>(
CheckNoLeadingSlash(msg->child_frame_id),
::cartographer::sensor::Data::Odometry{
ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(),
covariance}));
}
@ -96,9 +107,9 @@ void SensorBridge::HandleImuMessage(const string& topic,
"otherwise be imprecise.";
sensor_collator_->AddSensorData(
trajectory_id_, carto::common::ToUniversal(time), topic,
carto::common::make_unique<SensorData>(
msg->header.frame_id,
SensorData::Imu{
carto::common::make_unique<::cartographer::sensor::Data>(
CheckNoLeadingSlash(msg->header.frame_id),
::cartographer::sensor::Data::Imu{
sensor_to_tracking->rotation() *
ToEigen(msg->linear_acceleration),
sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity),
@ -128,8 +139,8 @@ void SensorBridge::HandlePointCloud2Message(
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::common::make_unique<::cartographer::sensor::Data>(
CheckNoLeadingSlash(msg->header.frame_id),
carto::sensor::TransformLaserFan3D(
carto::sensor::FromProto(ToCartographer(pcl_point_cloud)),
sensor_to_tracking->cast<float>())));
@ -148,8 +159,9 @@ void SensorBridge::HandleLaserScanProto(
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::common::make_unique<::cartographer::sensor::Data>(
CheckNoLeadingSlash(frame_id),
carto::sensor::TransformLaserFan3D(
carto::sensor::ToLaserFan3D(laser_fan_2d),
sensor_to_tracking->cast<float>())));
}

8
cartographer_ros/cartographer_ros/sensor_bridge.h
View File

@ -18,9 +18,9 @@
#define CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_
#include "cartographer/mapping/sensor_collator.h"
#include "cartographer/sensor/data.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "cartographer_ros/sensor_data.h"
#include "cartographer_ros/tf_bridge.h"
#include "geometry_msgs/Transform.h"
#include "geometry_msgs/TransformStamped.h"
@ -50,7 +50,8 @@ class SensorBridge {
explicit SensorBridge(
const SensorBridgeOptions& options, const TfBridge* tf_bridge,
int trajectory_id,
::cartographer::mapping::SensorCollator<SensorData>* sensor_collator);
::cartographer::mapping::SensorCollator<::cartographer::sensor::Data>*
sensor_collator);
SensorBridge(const SensorBridge&) = delete;
SensorBridge& operator=(const SensorBridge&) = delete;
@ -76,7 +77,8 @@ class SensorBridge {
const SensorBridgeOptions options_;
const TfBridge* const tf_bridge_;
const int trajectory_id_;
::cartographer::mapping::SensorCollator<SensorData>* const sensor_collator_;
::cartographer::mapping::SensorCollator<::cartographer::sensor::Data>* const
sensor_collator_;
};
} // namespace cartographer_ros

76
cartographer_ros/cartographer_ros/sensor_data.h
View File

@ -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_