diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index 68a3eb2..f978782 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -81,6 +81,10 @@ add_executable(cartographer_node src/msg_conversion.h src/msg_conversion.cc src/time_conversion.h + src/sensor_data.cc + src/sensor_data.h + src/sensor_data_producer.cc + src/sensor_data_producer.h src/time_conversion.cc ) target_link_libraries(cartographer_node @@ -89,7 +93,6 @@ target_link_libraries(cartographer_node ${PCL_LIBRARIES} gflags # TODO(whess): Use or remove gflags_catkin. ) - add_dependencies(cartographer_node ${catkin_EXPORTED_TARGETS} ) diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index 8af12b5..34d5fa8 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/cartographer_ros/src/cartographer_node_main.cc @@ -49,21 +49,13 @@ #include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapQuery.h" #include "cartographer_ros_msgs/TrajectorySubmapList.h" -#include "geometry_msgs/Transform.h" -#include "geometry_msgs/TransformStamped.h" #include "gflags/gflags.h" #include "glog/log_severity.h" #include "glog/logging.h" #include "nav_msgs/OccupancyGrid.h" #include "nav_msgs/Odometry.h" -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -#include "pcl_conversions/pcl_conversions.h" #include "ros/ros.h" #include "ros/serialization.h" -#include "sensor_msgs/Imu.h" -#include "sensor_msgs/LaserScan.h" -#include "sensor_msgs/MultiEchoLaserScan.h" #include "sensor_msgs/PointCloud2.h" #include "tf2_eigen/tf2_eigen.h" #include "tf2_ros/transform_broadcaster.h" @@ -71,6 +63,8 @@ #include "msg_conversion.h" #include "node_constants.h" +#include "sensor_data.h" +#include "sensor_data_producer.h" #include "time_conversion.h" DEFINE_string(configuration_directory, "", @@ -103,92 +97,6 @@ constexpr char kImuTopic[] = "/imu"; constexpr char kOdometryTopic[] = "/odom"; constexpr char kOccupancyGridTopic[] = "/map"; -const string& CheckNoLeadingSlash(const string& frame_id) { - if (frame_id.size() > 0) { - CHECK_NE(frame_id[0], '/'); - } - return frame_id; -} - -Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) { - return Rigid3d(Eigen::Vector3d(transform.transform.translation.x, - transform.transform.translation.y, - transform.transform.translation.z), - Eigen::Quaterniond(transform.transform.rotation.w, - transform.transform.rotation.x, - transform.transform.rotation.y, - transform.transform.rotation.z)); -} - -Rigid3d ToRigid3d(const geometry_msgs::Pose& pose) { - return Rigid3d( - Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z), - Eigen::Quaterniond(pose.orientation.w, pose.orientation.x, - pose.orientation.y, pose.orientation.z)); -} - -PoseCovariance ToPoseCovariance(const boost::array& covariance) { - return Eigen::Map>(covariance.data()); -} - -// TODO(hrapp): move to msg_conversion.cc. -geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d& rigid) { - geometry_msgs::Transform transform; - transform.translation.x = rigid.translation().x(); - transform.translation.y = rigid.translation().y(); - transform.translation.z = rigid.translation().z(); - transform.rotation.w = rigid.rotation().w(); - transform.rotation.x = rigid.rotation().x(); - transform.rotation.y = rigid.rotation().y(); - transform.rotation.z = rigid.rotation().z(); - return transform; -} - -geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid) { - geometry_msgs::Pose pose; - pose.position.x = rigid.translation().x(); - pose.position.y = rigid.translation().y(); - pose.position.z = rigid.translation().z(); - pose.orientation.w = rigid.rotation().w(); - pose.orientation.x = rigid.rotation().x(); - pose.orientation.y = rigid.rotation().y(); - pose.orientation.z = rigid.rotation().z(); - return pose; -} - -// This type is a logical union, i.e. only one proto is actually filled in. It -// is only used for time ordering sensor data before passing it on. -enum class SensorType { kImu, kLaserScan, kLaserFan3D, kOdometry }; -struct SensorData { - SensorData(const string& frame_id, proto::Imu imu) - : type(SensorType::kImu), - frame_id(CheckNoLeadingSlash(frame_id)), - imu(imu) {} - SensorData(const string& frame_id, proto::LaserScan laser_scan) - : type(SensorType::kLaserScan), - frame_id(CheckNoLeadingSlash(frame_id)), - laser_scan(laser_scan) {} - SensorData(const string& frame_id, proto::LaserFan3D laser_fan_3d) - : type(SensorType::kLaserFan3D), - frame_id(CheckNoLeadingSlash(frame_id)), - laser_fan_3d(laser_fan_3d) {} - SensorData(const string& frame_id, const Rigid3d& pose, - const PoseCovariance& covariance) - : type(SensorType::kOdometry), - frame_id(frame_id), - odometry{pose, covariance} {} - - SensorType type; - string frame_id; - proto::Imu imu; - proto::LaserScan laser_scan; - proto::LaserFan3D laser_fan_3d; - struct { - Rigid3d pose; - PoseCovariance covariance; - } odometry; -}; - // Node that listens to all the sensor data that we are interested in and wires // it up to the SLAM. class Node { @@ -204,13 +112,6 @@ class Node { private: void HandleSensorData(int64 timestamp, std::unique_ptr sensor_data); - void OdometryMessageCallback(const nav_msgs::Odometry::ConstPtr& msg); - void ImuMessageCallback(const sensor_msgs::Imu::ConstPtr& msg); - void LaserScanMessageCallback(const sensor_msgs::LaserScan::ConstPtr& msg); - void MultiEchoLaserScanMessageCallback( - const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); - void PointCloud2MessageCallback( - const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg); void AddOdometry(int64 timestamp, const string& frame_id, const Rigid3d& pose, const PoseCovariance& covariance); @@ -236,6 +137,7 @@ class Node { // TODO(hrapp): Pull out the common functionality between this and MapWriter // into an open sourcable MapWriter. carto::mapping::SensorCollator sensor_collator_; + SensorDataProducer sensor_data_producer_; ros::NodeHandle node_handle_; ros::Subscriber imu_subscriber_; ros::Subscriber laser_subscriber_2d_; @@ -281,7 +183,8 @@ class Node { }; Node::Node() - : node_handle_("~"), + : sensor_data_producer_(kTrajectoryId, &sensor_collator_), + node_handle_("~"), tf_buffer_(ros::Duration(1000)), tf_(tf_buffer_), thread_pool_(10) {} @@ -303,29 +206,12 @@ Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time, ros::Duration(lookup_transform_timeout_))); } -void Node::OdometryMessageCallback(const nav_msgs::Odometry::ConstPtr& msg) { - auto sensor_data = carto::common::make_unique( - msg->header.frame_id, ToRigid3d(msg->pose.pose), - ToPoseCovariance(msg->pose.covariance)); - sensor_collator_.AddSensorData( - kTrajectoryId, carto::common::ToUniversal(FromRos(msg->header.stamp)), - kOdometryTopic, std::move(sensor_data)); -} - void Node::AddOdometry(int64 timestamp, const string& frame_id, const Rigid3d& pose, const PoseCovariance& covariance) { const carto::common::Time time = carto::common::FromUniversal(timestamp); trajectory_builder_->AddOdometerPose(time, pose, covariance); } -void Node::ImuMessageCallback(const sensor_msgs::Imu::ConstPtr& msg) { - auto sensor_data = carto::common::make_unique( - msg->header.frame_id, ToCartographer(*msg)); - sensor_collator_.AddSensorData( - kTrajectoryId, carto::common::ToUniversal(FromRos(msg->header.stamp)), - kImuTopic, std::move(sensor_data)); -} - void Node::AddImu(const int64 timestamp, const string& frame_id, const proto::Imu& imu) { const carto::common::Time time = carto::common::FromUniversal(timestamp); @@ -346,16 +232,6 @@ void Node::AddImu(const int64 timestamp, const string& frame_id, << ": " << ex.what(); } } - -void Node::LaserScanMessageCallback( - const sensor_msgs::LaserScan::ConstPtr& msg) { - auto sensor_data = carto::common::make_unique( - msg->header.frame_id, ToCartographer(*msg)); - sensor_collator_.AddSensorData( - kTrajectoryId, carto::common::ToUniversal(FromRos(msg->header.stamp)), - kLaserScanTopic, std::move(sensor_data)); -} - void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id, const proto::LaserScan& laser_scan) { const carto::common::Time time = carto::common::FromUniversal(timestamp); @@ -376,27 +252,6 @@ void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id, } } -void Node::MultiEchoLaserScanMessageCallback( - const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { - auto sensor_data = carto::common::make_unique( - msg->header.frame_id, ToCartographer(*msg)); - sensor_collator_.AddSensorData( - kTrajectoryId, carto::common::ToUniversal(FromRos(msg->header.stamp)), - kMultiEchoLaserScanTopic, std::move(sensor_data)); -} - -void Node::PointCloud2MessageCallback( - const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg) { - pcl::PointCloud pcl_points; - pcl::fromROSMsg(*msg, pcl_points); - - auto sensor_data = carto::common::make_unique( - msg->header.frame_id, ToCartographer(pcl_points)); - sensor_collator_.AddSensorData( - kTrajectoryId, carto::common::ToUniversal(FromRos(msg->header.stamp)), - topic, std::move(sensor_data)); -} - 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); @@ -458,15 +313,22 @@ void Node::Initialize() { "'num_lasers_3d' are mutually exclusive, but one is required."; if (has_laser_scan_2d) { - laser_subscriber_2d_ = - node_handle_.subscribe(kLaserScanTopic, kSubscriberQueueSize, - &Node::LaserScanMessageCallback, this); + laser_subscriber_2d_ = node_handle_.subscribe( + kLaserScanTopic, kSubscriberQueueSize, + boost::function( + [this](const sensor_msgs::LaserScan::ConstPtr& msg) { + sensor_data_producer_.AddLaserScanMessage(kLaserScanTopic, msg); + })); expected_sensor_identifiers.insert(kLaserScanTopic); } if (has_multi_echo_laser_scan_2d) { - laser_subscriber_2d_ = - node_handle_.subscribe(kMultiEchoLaserScanTopic, kSubscriberQueueSize, - &Node::MultiEchoLaserScanMessageCallback, this); + laser_subscriber_2d_ = node_handle_.subscribe( + kMultiEchoLaserScanTopic, kSubscriberQueueSize, + boost::function( + [this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { + sensor_data_producer_.AddMultiEchoLaserScanMessage( + kMultiEchoLaserScanTopic, msg); + })); expected_sensor_identifiers.insert(kMultiEchoLaserScanTopic); } @@ -498,7 +360,7 @@ void Node::Initialize() { topic, kSubscriberQueueSize, boost::function( [this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { - PointCloud2MessageCallback(topic, msg); + sensor_data_producer_.AddPointCloud2Message(topic, msg); }))); expected_sensor_identifiers.insert(topic); } @@ -520,15 +382,22 @@ void Node::Initialize() { // Maybe subscribe to the IMU. if (expect_imu_data) { - imu_subscriber_ = node_handle_.subscribe(kImuTopic, kSubscriberQueueSize, - &Node::ImuMessageCallback, this); + imu_subscriber_ = node_handle_.subscribe( + kImuTopic, kSubscriberQueueSize, + boost::function( + [this](const sensor_msgs::Imu::ConstPtr& msg) { + sensor_data_producer_.AddImuMessage(kImuTopic, msg); + })); expected_sensor_identifiers.insert(kImuTopic); } if (expect_odometry_data_) { - odometry_subscriber_ = - node_handle_.subscribe(kOdometryTopic, kSubscriberQueueSize, - &Node::OdometryMessageCallback, this); + odometry_subscriber_ = node_handle_.subscribe( + kOdometryTopic, kSubscriberQueueSize, + boost::function( + [this](const nav_msgs::Odometry::ConstPtr& msg) { + sensor_data_producer_.AddOdometryMessage(kOdometryTopic, msg); + })); expected_sensor_identifiers.insert(kOdometryTopic); } diff --git a/cartographer_ros/src/msg_conversion.cc b/cartographer_ros/src/msg_conversion.cc index e766847..2f1126c 100644 --- a/cartographer_ros/src/msg_conversion.cc +++ b/cartographer_ros/src/msg_conversion.cc @@ -44,6 +44,9 @@ namespace { // properly. constexpr float kPointCloudComponentFourMagic = 1.; +using ::cartographer::transform::Rigid3d; +using ::cartographer::kalman_filter::PoseCovariance; + void ToMessage(const ::cartographer::transform::proto::Vector3d& proto, geometry_msgs::Vector3* vector) { vector->x = proto.x(); @@ -284,4 +287,49 @@ sensor_msgs::PointCloud2 ToPointCloud2Message( return proto; } +Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) { + return Rigid3d(Eigen::Vector3d(transform.transform.translation.x, + transform.transform.translation.y, + transform.transform.translation.z), + Eigen::Quaterniond(transform.transform.rotation.w, + transform.transform.rotation.x, + transform.transform.rotation.y, + transform.transform.rotation.z)); +} + +Rigid3d ToRigid3d(const geometry_msgs::Pose& pose) { + return Rigid3d( + Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z), + Eigen::Quaterniond(pose.orientation.w, pose.orientation.x, + pose.orientation.y, pose.orientation.z)); +} + +PoseCovariance ToPoseCovariance(const boost::array& covariance) { + return Eigen::Map>(covariance.data()); +} + +geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d& rigid) { + geometry_msgs::Transform transform; + transform.translation.x = rigid.translation().x(); + transform.translation.y = rigid.translation().y(); + transform.translation.z = rigid.translation().z(); + transform.rotation.w = rigid.rotation().w(); + transform.rotation.x = rigid.rotation().x(); + transform.rotation.y = rigid.rotation().y(); + transform.rotation.z = rigid.rotation().z(); + return transform; +} + +geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid) { + geometry_msgs::Pose pose; + pose.position.x = rigid.translation().x(); + pose.position.y = rigid.translation().y(); + pose.position.z = rigid.translation().z(); + pose.orientation.w = rigid.rotation().w(); + pose.orientation.x = rigid.rotation().x(); + pose.orientation.y = rigid.rotation().y(); + pose.orientation.z = rigid.rotation().z(); + return pose; +} + } // namespace cartographer_ros diff --git a/cartographer_ros/src/msg_conversion.h b/cartographer_ros/src/msg_conversion.h index b069e4d..12f691f 100644 --- a/cartographer_ros/src/msg_conversion.h +++ b/cartographer_ros/src/msg_conversion.h @@ -18,9 +18,15 @@ #define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_MSG_CONVERSION_H_ #include "cartographer/common/port.h" +#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/transform/rigid_transform.h" +#include "geometry_msgs/Pose.h" +#include "geometry_msgs/Transform.h" +#include "geometry_msgs/TransformStamped.h" #include "pcl/point_cloud.h" #include "pcl/point_types.h" +#include "pcl_conversions/pcl_conversions.h" #include "sensor_msgs/Imu.h" #include "sensor_msgs/LaserScan.h" #include "sensor_msgs/MultiEchoLaserScan.h" @@ -44,6 +50,12 @@ sensor_msgs::PointCloud2 ToPointCloud2Message( int64 timestamp, const string& frame_id, const ::cartographer::sensor::proto::LaserFan3D& laser_scan_3d); +geometry_msgs::Transform ToGeometryMsgTransform( + const ::cartographer::transform::Rigid3d& rigid); + +geometry_msgs::Pose ToGeometryMsgPose( + const ::cartographer::transform::Rigid3d& rigid); + ::cartographer::sensor::proto::Imu ToCartographer(const sensor_msgs::Imu& msg); ::cartographer::sensor::proto::LaserScan ToCartographer( @@ -55,6 +67,14 @@ sensor_msgs::PointCloud2 ToPointCloud2Message( ::cartographer::sensor::proto::LaserFan3D ToCartographer( const pcl::PointCloud& pcl_points); +::cartographer::transform::Rigid3d ToRigid3d( + const geometry_msgs::TransformStamped& transform); + +::cartographer::transform::Rigid3d ToRigid3d(const geometry_msgs::Pose& pose); + +::cartographer::kalman_filter::PoseCovariance ToPoseCovariance( + const boost::array& covariance); + } // namespace cartographer_ros #endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_MSG_CONVERSION_H_ diff --git a/cartographer_ros/src/sensor_data.cc b/cartographer_ros/src/sensor_data.cc new file mode 100644 index 0000000..064029c --- /dev/null +++ b/cartographer_ros/src/sensor_data.cc @@ -0,0 +1,62 @@ +/* + * 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 "sensor_data.h" + +#include + +#include "glog/logging.h" + +namespace cartographer_ros { + +namespace { + +namespace proto = ::cartographer::sensor::proto; + +using ::cartographer::transform::Rigid3d; +using ::cartographer::kalman_filter::PoseCovariance; + +const string& CheckNoLeadingSlash(const string& frame_id) { + if (frame_id.size() > 0) { + CHECK_NE(frame_id[0], '/'); + } + return frame_id; +} + +} // namespace + +SensorData::SensorData(const string& frame_id, proto::Imu imu) + : type(SensorType::kImu), + frame_id(CheckNoLeadingSlash(frame_id)), + imu(imu) {} + +SensorData::SensorData(const string& frame_id, proto::LaserScan laser_scan) + : type(SensorType::kLaserScan), + frame_id(CheckNoLeadingSlash(frame_id)), + laser_scan(laser_scan) {} + +SensorData::SensorData(const string& frame_id, proto::LaserFan3D laser_fan_3d) + : type(SensorType::kLaserFan3D), + frame_id(CheckNoLeadingSlash(frame_id)), + laser_fan_3d(laser_fan_3d) {} + +SensorData::SensorData(const string& frame_id, const Rigid3d& pose, + const PoseCovariance& covariance) + : type(SensorType::kOdometry), + frame_id(frame_id), + odometry{pose, covariance} {} + +} // namespace cartorapher_ros diff --git a/cartographer_ros/src/sensor_data.h b/cartographer_ros/src/sensor_data.h new file mode 100644 index 0000000..0722698 --- /dev/null +++ b/cartographer_ros/src/sensor_data.h @@ -0,0 +1,54 @@ +/* + * 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 CARTOGRAHPER_ROS_SENSOR_DATA_H_ +#define CARTOGRAHPER_ROS_SENSOR_DATA_H_ + +#include + +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer_ros { + +// This type is a logical union, i.e. only one proto is actually filled in. It +// is only used for time ordering sensor data before passing it on. +enum class SensorType { kImu, kLaserScan, kLaserFan3D, kOdometry }; +struct SensorData { + SensorData(const string& frame_id, ::cartographer::sensor::proto::Imu imu); + SensorData(const string& frame_id, + ::cartographer::sensor::proto::LaserScan laser_scan); + SensorData(const string& frame_id, + ::cartographer::sensor::proto::LaserFan3D laser_fan_3d); + SensorData(const string& frame_id, + const ::cartographer::transform::Rigid3d& pose, + const ::cartographer::kalman_filter::PoseCovariance& covariance); + + SensorType type; + string frame_id; + ::cartographer::sensor::proto::Imu imu; + ::cartographer::sensor::proto::LaserScan laser_scan; + ::cartographer::sensor::proto::LaserFan3D laser_fan_3d; + struct { + ::cartographer::transform::Rigid3d pose; + ::cartographer::kalman_filter::PoseCovariance covariance; + } odometry; +}; + +} // namespace cartographer_ros + +#endif // CARTOGRAHPER_ROS_SENSOR_DATA_H_ diff --git a/cartographer_ros/src/sensor_data_producer.cc b/cartographer_ros/src/sensor_data_producer.cc new file mode 100644 index 0000000..a6896b0 --- /dev/null +++ b/cartographer_ros/src/sensor_data_producer.cc @@ -0,0 +1,83 @@ +/* + * 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 "sensor_data_producer.h" + +#include "msg_conversion.h" +#include "time_conversion.h" + +namespace cartographer_ros { + +SensorDataProducer::SensorDataProducer( + const int trajectory_id, + ::cartographer::mapping::SensorCollator* sensor_collator) + : trajectory_id_(trajectory_id), sensor_collator_(sensor_collator) {} + +void SensorDataProducer::AddOdometryMessage( + const string& topic, const nav_msgs::Odometry::ConstPtr& msg) { + auto sensor_data = ::cartographer::common::make_unique( + msg->header.frame_id, ToRigid3d(msg->pose.pose), + ToPoseCovariance(msg->pose.covariance)); + sensor_collator_->AddSensorData( + trajectory_id_, + ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic, + std::move(sensor_data)); +} + +void SensorDataProducer::AddImuMessage(const string& topic, + const sensor_msgs::Imu::ConstPtr& msg) { + auto sensor_data = ::cartographer::common::make_unique( + msg->header.frame_id, ToCartographer(*msg)); + sensor_collator_->AddSensorData( + trajectory_id_, + ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic, + std::move(sensor_data)); +} + +void SensorDataProducer::AddLaserScanMessage( + const string& topic, const sensor_msgs::LaserScan::ConstPtr& msg) { + auto sensor_data = ::cartographer::common::make_unique( + msg->header.frame_id, ToCartographer(*msg)); + sensor_collator_->AddSensorData( + trajectory_id_, + ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic, + std::move(sensor_data)); +} + +void SensorDataProducer::AddMultiEchoLaserScanMessage( + const string& topic, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { + auto sensor_data = ::cartographer::common::make_unique( + msg->header.frame_id, ToCartographer(*msg)); + sensor_collator_->AddSensorData( + trajectory_id_, + ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic, + std::move(sensor_data)); +} + +void SensorDataProducer::AddPointCloud2Message( + const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg) { + pcl::PointCloud pcl_points; + pcl::fromROSMsg(*msg, pcl_points); + + auto sensor_data = ::cartographer::common::make_unique( + msg->header.frame_id, ToCartographer(pcl_points)); + sensor_collator_->AddSensorData( + trajectory_id_, + ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic, + std::move(sensor_data)); +} + +} // namespace cartographer_ros diff --git a/cartographer_ros/src/sensor_data_producer.h b/cartographer_ros/src/sensor_data_producer.h new file mode 100644 index 0000000..a3a31bf --- /dev/null +++ b/cartographer_ros/src/sensor_data_producer.h @@ -0,0 +1,63 @@ +/* + * 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 CARTOGRAHPER_ROS_SENSOR_DATA_PRODUCER_H_ +#define CARTOGRAHPER_ROS_SENSOR_DATA_PRODUCER_H_ + +#include "cartographer/mapping/sensor_collator.h" +#include "geometry_msgs/Transform.h" +#include "geometry_msgs/TransformStamped.h" +#include "nav_msgs/OccupancyGrid.h" +#include "nav_msgs/Odometry.h" +#include "sensor_data.h" +#include "sensor_msgs/Imu.h" +#include "sensor_msgs/LaserScan.h" +#include "sensor_msgs/MultiEchoLaserScan.h" +#include "sensor_msgs/PointCloud2.h" + +namespace cartographer_ros { + +// A wrapper around SensorCollator that converts ROS messages into our internal +// representation and passes them on to the 'sensor_collator'. +class SensorDataProducer { + public: + explicit SensorDataProducer( + int trajectory_id, + ::cartographer::mapping::SensorCollator* sensor_collator); + + SensorDataProducer(const SensorDataProducer&) = delete; + SensorDataProducer& operator=(const SensorDataProducer&) = delete; + + void AddOdometryMessage(const string& topic, + const nav_msgs::Odometry::ConstPtr& msg); + void AddImuMessage(const string& topic, + const sensor_msgs::Imu::ConstPtr& msg); + void AddLaserScanMessage(const string& topic, + const sensor_msgs::LaserScan::ConstPtr& msg); + void AddMultiEchoLaserScanMessage( + const string& topic, + const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); + void AddPointCloud2Message(const string& topic, + const sensor_msgs::PointCloud2::ConstPtr& msg); + + private: + const int trajectory_id_; + ::cartographer::mapping::SensorCollator* sensor_collator_; +}; + +} // namespace cartographer_ros + +#endif // CARTOGRAHPER_ROS_SENSOR_DATA_PRODUCER_H_