GNSSToMapConvertor.hpp

Go to the documentation of this file.

#pragma once
 
/*
 * Copyright (C) 2018-2021 LEIDOS.
 *
 * 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 <rclcpp/rclcpp.hpp>
#include <functional>
 
#include <std_msgs/msg/string.hpp>
#include <std_srvs/srv/empty.hpp>
 
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
 
#include <tf2/LinearMath/Transform.h>
 
#include <carma_ros2_utils/carma_lifecycle_node.hpp>
 
#include <boost/optional.hpp>
#include <memory>
 
#include <gps_msgs/msg/gps_fix.hpp>
#include <wgs84_utils/wgs84_utils.h>
 
#include <lanelet2_extension/projection/local_frame_projector.h>
 
namespace gnss_to_map_convertor
{
class GNSSToMapConvertor
{
public:
  using PosePubCallback = std::function<void(geometry_msgs::msg::PoseStamped)>;
 
  using TransformLookupCallback =
      std::function<boost::optional<geometry_msgs::msg::TransformStamped>(const std::string&, const std::string&)>;
 
  GNSSToMapConvertor(PosePubCallback pose_pub, TransformLookupCallback tf_lookup, std::string map_frame_id,
                     std::string base_link_frame_id, std::string heading_frame_id, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger);
 
  void gnssFixCb(gps_msgs::msg::GPSFix::UniquePtr fix_msg);
 
  void geoReferenceCallback(std_msgs::msg::String::UniquePtr geo_ref);
 
  boost::optional<tf2::Quaternion> getNedInMapRotation();
 
  std::shared_ptr<lanelet::projection::LocalFrameProjector> getMapProjector();
 
  geometry_msgs::msg::PoseWithCovarianceStamped poseFromGnss(const tf2::Transform& baselink_in_sensor,
                                                        const tf2::Quaternion& sensor_in_ned_heading_rotation,
                                                        const lanelet::projection::LocalFrameProjector& projector,
                                                        const tf2::Quaternion& ned_in_map_rotation,
                                                        gps_msgs::msg::GPSFix fix_msg);
 
private:
  PosePubCallback pose_pub_;           // Function which can forward the output from this component
  TransformLookupCallback tf_lookup_;  // Function which accesses a buffer of transforms for query lookups
  std::string map_frame_id_;           // The frame id of the map which the output pose will be in.
  std::string base_link_frame_id_;     // The frame id of the final reported pose
  std::string heading_frame_id_;       // The frame id of the heading frame
  std::string georeference_{""};
 
  // Rotation describing the orientation of an NED frame relative to the map frame located at the map origin.
  // This is derived from the received georeference
  boost::optional<tf2::Quaternion> ned_in_map_rotation_;
 
  // Rotation describing orientation of sensor in heading frame
  boost::optional<tf2::Quaternion> sensor_in_ned_heading_rotation_;
 
  std::shared_ptr<lanelet::projection::LocalFrameProjector> map_projector_;  // Must be shared pointer instead of
                                                                             // optional since for some reason optional
                                                                             // does not work with this class
 
  boost::optional<tf2::Transform> baselink_in_sensor_;  // A transform describing the relation of the baselink frame
                                                        // with the frame provided by the gnss message
 
  // Logger interface
  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_;
 
};
 
};  // namespace gnss_to_map_convertor