msg_conversion.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H
00018 #define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H
00019 
00020 #include "cartographer/common/time.h"
00021 #include "cartographer/io/submap_painter.h"
00022 #include "cartographer/sensor/landmark_data.h"
00023 #include "cartographer/sensor/point_cloud.h"
00024 #include "cartographer/transform/rigid_transform.h"
00025 #include "cartographer_ros_msgs/LandmarkList.h"
00026 #include "geometry_msgs/Pose.h"
00027 #include "geometry_msgs/Transform.h"
00028 #include "geometry_msgs/TransformStamped.h"
00029 #include "nav_msgs/OccupancyGrid.h"
00030 #include "sensor_msgs/Imu.h"
00031 #include "sensor_msgs/LaserScan.h"
00032 #include "sensor_msgs/MultiEchoLaserScan.h"
00033 #include "sensor_msgs/PointCloud2.h"
00034 
00035 namespace cartographer_ros {
00036 
00037 sensor_msgs::PointCloud2 ToPointCloud2Message(
00038     int64_t timestamp, const std::string& frame_id,
00039     const ::cartographer::sensor::TimedPointCloud& point_cloud);
00040 
00041 geometry_msgs::Transform ToGeometryMsgTransform(
00042     const ::cartographer::transform::Rigid3d& rigid3d);
00043 
00044 geometry_msgs::Pose ToGeometryMsgPose(
00045     const ::cartographer::transform::Rigid3d& rigid3d);
00046 
00047 geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d);
00048 
00049 // Converts ROS message to point cloud. Returns the time when the last point
00050 // was acquired (different from the ROS timestamp). Timing of points is given in
00051 // the fourth component of each point relative to `Time`.
00052 std::tuple<::cartographer::sensor::PointCloudWithIntensities,
00053            ::cartographer::common::Time>
00054 ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg);
00055 
00056 std::tuple<::cartographer::sensor::PointCloudWithIntensities,
00057            ::cartographer::common::Time>
00058 ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg);
00059 
00060 std::tuple<::cartographer::sensor::PointCloudWithIntensities,
00061            ::cartographer::common::Time>
00062 ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg);
00063 
00064 ::cartographer::sensor::LandmarkData ToLandmarkData(
00065     const cartographer_ros_msgs::LandmarkList& landmark_list);
00066 
00067 ::cartographer::transform::Rigid3d ToRigid3d(
00068     const geometry_msgs::TransformStamped& transform);
00069 
00070 ::cartographer::transform::Rigid3d ToRigid3d(const geometry_msgs::Pose& pose);
00071 
00072 Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3);
00073 
00074 Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion);
00075 
00076 // Converts from WGS84 (latitude, longitude, altitude) to ECEF.
00077 Eigen::Vector3d LatLongAltToEcef(double latitude, double longitude,
00078                                  double altitude);
00079 
00080 // Returns a transform that takes ECEF coordinates from nearby points to a local
00081 // frame that has z pointing upwards.
00082 cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(double latitude,
00083                                                               double longitude);
00084 
00085 // Points to an occupancy grid message at a specific resolution from painted
00086 // submap slices obtained via ::cartographer::io::PaintSubmapSlices(...).
00087 std::unique_ptr<nav_msgs::OccupancyGrid> CreateOccupancyGridMsg(
00088     const cartographer::io::PaintSubmapSlicesResult& painted_slices,
00089     const double resolution, const std::string& frame_id,
00090     const ros::Time& time);
00091 
00092 }  // namespace cartographer_ros
00093 
00094 #endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H


cartographer_ros
Author(s): The Cartographer Authors
autogenerated on Wed Jul 10 2019 04:10:28