From 0a76d10546eda93ba6f3b34fbad809e6e8b21a8e Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Thu, 13 Oct 2016 14:38:00 +0200 Subject: [PATCH] Moves SensorData struct into Cartographer. (#110) --- .../cartographer_ros/CMakeLists.txt | 8 -- .../cartographer_ros/node_main.cc | 17 +++-- .../cartographer_ros/sensor_bridge.cc | 38 ++++++---- .../cartographer_ros/sensor_bridge.h | 8 +- .../cartographer_ros/sensor_data.h | 76 ------------------- 5 files changed, 39 insertions(+), 108 deletions(-) delete mode 100644 cartographer_ros/cartographer_ros/sensor_data.h diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index a77b82b..7a9e219 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -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 ) diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 0f897a9..3966297 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -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 sensor_data); + std::unique_ptr sensor_data); bool HandleSubmapQuery( ::cartographer_ros_msgs::SubmapQuery::Request& request, ::cartographer_ros_msgs::SubmapQuery::Response& response); @@ -145,7 +145,7 @@ class Node { std::deque constant_data_ GUARDED_BY(mutex_); carto::mapping::MapBuilder map_builder_ GUARDED_BY(mutex_); - carto::mapping::SensorCollator sensor_collator_ + carto::mapping::SensorCollator 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 sensor_data) { + [this](const int64 timestamp, + std::unique_ptr sensor_data) { HandleSensorData(timestamp, std::move(sensor_data)); }); @@ -481,7 +482,7 @@ void Node::SpinOccupancyGridThreadForever() { } void Node::HandleSensorData(const int64 timestamp, - std::unique_ptr sensor_data) { + std::unique_ptr 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; diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 94747d8..3791f8f 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -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* const sensor_collator) + carto::mapping::SensorCollator* 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( - 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( - 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( - 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()))); @@ -148,10 +159,11 @@ void SensorBridge::HandleLaserScanProto( if (sensor_to_tracking != nullptr) { sensor_collator_->AddSensorData( trajectory_id_, carto::common::ToUniversal(time), topic, - carto::common::make_unique( - frame_id, carto::sensor::TransformLaserFan3D( - carto::sensor::ToLaserFan3D(laser_fan_2d), - sensor_to_tracking->cast()))); + carto::common::make_unique<::cartographer::sensor::Data>( + CheckNoLeadingSlash(frame_id), + carto::sensor::TransformLaserFan3D( + carto::sensor::ToLaserFan3D(laser_fan_2d), + sensor_to_tracking->cast()))); } } diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/cartographer_ros/sensor_bridge.h index d75d1b0..d1f513a 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.h +++ b/cartographer_ros/cartographer_ros/sensor_bridge.h @@ -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* 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* const sensor_collator_; + ::cartographer::mapping::SensorCollator<::cartographer::sensor::Data>* const + sensor_collator_; }; } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/sensor_data.h b/cartographer_ros/cartographer_ros/sensor_data.h deleted file mode 100644 index e4d6ab3..0000000 --- a/cartographer_ros/cartographer_ros/sensor_data.h +++ /dev/null @@ -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 - -#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_