sensor_bridge.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
19 #include "cartographer/kalman_filter/pose_tracker.h"
22 
23 namespace cartographer_ros {
24 
25 namespace carto = ::cartographer;
26 
27 using carto::transform::Rigid3d;
28 
29 namespace {
30 
31 const string& CheckNoLeadingSlash(const string& frame_id) {
32  if (frame_id.size() > 0) {
33  CHECK_NE(frame_id[0], '/');
34  }
35  return frame_id;
36 }
37 
38 } // namespace
39 
41  const string& tracking_frame, const double lookup_transform_timeout_sec,
42  tf2_ros::Buffer* const tf_buffer,
43  carto::mapping::TrajectoryBuilder* const trajectory_builder)
44  : tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
45  trajectory_builder_(trajectory_builder) {}
46 
48  const string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
49  const carto::common::Time time = FromRos(msg->header.stamp);
50  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
51  time, CheckNoLeadingSlash(msg->child_frame_id));
52  if (sensor_to_tracking != nullptr) {
53  trajectory_builder_->AddOdometerData(
54  sensor_id, time,
55  ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse());
56  }
57 }
58 
59 void SensorBridge::HandleImuMessage(const string& sensor_id,
60  const sensor_msgs::Imu::ConstPtr& msg) {
61  CHECK_NE(msg->linear_acceleration_covariance[0], -1)
62  << "Your IMU data claims to not contain linear acceleration measurements "
63  "by setting linear_acceleration_covariance[0] to -1. Cartographer "
64  "requires this data to work. See "
65  "http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";
66  CHECK_NE(msg->angular_velocity_covariance[0], -1)
67  << "Your IMU data claims to not contain angular velocity measurements "
68  "by setting angular_velocity_covariance[0] to -1. Cartographer "
69  "requires this data to work. See "
70  "http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";
71 
72  const carto::common::Time time = FromRos(msg->header.stamp);
73  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
74  time, CheckNoLeadingSlash(msg->header.frame_id));
75  if (sensor_to_tracking != nullptr) {
76  CHECK(sensor_to_tracking->translation().norm() < 1e-5)
77  << "The IMU frame must be colocated with the tracking frame. "
78  "Transforming linear acceleration into the tracking frame will "
79  "otherwise be imprecise.";
80  trajectory_builder_->AddImuData(
81  sensor_id, time,
82  sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
83  sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity));
84  }
85 }
86 
88  const string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
89  HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
90  ToPointCloudWithIntensities(*msg).points);
91 }
92 
94  const string& sensor_id,
95  const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
96  HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
97  ToPointCloudWithIntensities(*msg).points);
98 }
99 
101  const string& sensor_id, const sensor_msgs::PointCloud2::ConstPtr& msg) {
102  pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
103  pcl::fromROSMsg(*msg, pcl_point_cloud);
104  carto::sensor::PointCloud point_cloud;
105  for (const auto& point : pcl_point_cloud) {
106  point_cloud.emplace_back(point.x, point.y, point.z);
107  }
108  HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
109  point_cloud);
110 }
111 
112 const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
113 
114 void SensorBridge::HandleRangefinder(const string& sensor_id,
115  const carto::common::Time time,
116  const string& frame_id,
117  const carto::sensor::PointCloud& ranges) {
118  const auto sensor_to_tracking =
119  tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
120  if (sensor_to_tracking != nullptr) {
121  trajectory_builder_->AddRangefinderData(
122  sensor_id, time, sensor_to_tracking->translation().cast<float>(),
123  carto::sensor::TransformPointCloud(ranges,
124  sensor_to_tracking->cast<float>()));
125  }
126 }
127 
128 } // namespace cartographer_ros
void HandlePointCloud2Message(const string &sensor_id, const sensor_msgs::PointCloud2::ConstPtr &msg)
Rigid3d ToRigid3d(const geometry_msgs::TransformStamped &transform)
const TfBridge & tf_bridge() const
PointCloudWithIntensities ToPointCloudWithIntensities(const sensor_msgs::LaserScan &msg)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
const common::Time time
void HandleRangefinder(const string &sensor_id, const ::cartographer::common::Time time, const string &frame_id, const ::cartographer::sensor::PointCloud &ranges)
void HandleMultiEchoLaserScanMessage(const string &sensor_id, const sensor_msgs::MultiEchoLaserScan::ConstPtr &msg)
Eigen::Vector3d ToEigen(const geometry_msgs::Vector3 &vector3)
SensorBridge(const string &tracking_frame, double lookup_transform_timeout_sec, tf2_ros::Buffer *tf_buffer,::cartographer::mapping::TrajectoryBuilder *trajectory_builder)
void HandleLaserScanMessage(const string &sensor_id, const sensor_msgs::LaserScan::ConstPtr &msg)
void HandleImuMessage(const string &sensor_id, const sensor_msgs::Imu::ConstPtr &msg)
void HandleOdometryMessage(const string &sensor_id, const nav_msgs::Odometry::ConstPtr &msg)
::cartographer::common::Time FromRos(const ::ros::Time &time)


cartographer_ros
Author(s):
autogenerated on Mon Jun 10 2019 12:59:40