#include <GNSSToMapConvertor.hpp>

Collaboration diagram for gnss_to_map_convertor::GNSSToMapConvertor:

Public Types
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 &)>

Public Member Functions
	GNSSToMapConvertor (PosePubCallback pose_pub, TransformLookupCallback tf_lookup, std::string map_frame_id, std::string base_link_frame_id, std::string heading_frame_id, std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > nh)
	Constructor. More...

void	gnssFixCb (gps_msgs::msg::GPSFix::UniquePtr fix_msg)
	GNSS Fix callback which will publish a pose representing that fix in the map frame if the required transforms are available. More...

void	geoReferenceCallback (std_msgs::msg::String::UniquePtr geo_ref)
	Map georeference callback The geodetic description of the map frame should be provided to this callback as a proj library string. The frame orientation must be right handed and aligned with lat/lon however the orientation of this alignment does not matter (enu, ned, etc.) More...

boost::optional< tf2::Quaternion >	getNedInMapRotation ()
	Get the rotation computed from the recieved georeference or boost::none if unset. More...

std::shared_ptr< lanelet::projection::LocalFrameProjector >	getMapProjector ()
	Get the projector built from the provided georeference via the callback. More...

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)
	Converts a provided GNSS fix message into a pose message for the map frame describibed by the provided projector. More...

Private Attributes
PosePubCallback	pose_pub_

TransformLookupCallback	tf_lookup_

std::string	map_frame_id_

std::string	base_link_frame_id_

std::string	heading_frame_id_

std::string	georeference_ {""}

boost::optional< tf2::Quaternion >	ned_in_map_rotation_

boost::optional< tf2::Quaternion >	sensor_in_ned_heading_rotation_

std::shared_ptr< lanelet::projection::LocalFrameProjector >	map_projector_

boost::optional< tf2::Transform >	baselink_in_sensor_

std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode >	nh_

Detailed Description

Definition at line 50 of file GNSSToMapConvertor.hpp.

Member Typedef Documentation

◆ PosePubCallback

using gnss_to_map_convertor::GNSSToMapConvertor::PosePubCallback = std::function<void(geometry_msgs::msg::PoseStamped)>

Definition at line 53 of file GNSSToMapConvertor.hpp.

◆ TransformLookupCallback

using gnss_to_map_convertor::GNSSToMapConvertor::TransformLookupCallback = std::function<boost::optional<geometry_msgs::msg::TransformStamped>(const std::string&, const std::string&)>

Function which will return the most recent transform between the provided frames First frame is target second is source If the transform does not exist or cannot be computed the optional returns false

Definition at line 60 of file GNSSToMapConvertor.hpp.

Constructor & Destructor Documentation

◆ GNSSToMapConvertor()

GNSSToMapConvertor::GNSSToMapConvertor	(	PosePubCallback	pose_pub,
		TransformLookupCallback	tf_lookup,
		std::string	map_frame_id,
		std::string	base_link_frame_id,
		std::string	heading_frame_id,
		std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode >	nh
	)

Constructor.

Parameters

pose_pub	A function to publish a converted gps fix as a pose to the rest of the system
tf_lookup	A function which will lookup the most recent transform between the two provided frames
map_frame_id	The frame id of the frame which the output pose should be considered in
base_link_frame_id	The frame id of the frame which the output pose defines the position and orientation of
heading_frame_id	The frame id of the frame which the heading report aligns with an NED frame
nh	A shared pointer to the node handle which will be used to log messages and get clock in the map frame.

Definition at line 24 of file GNSSToMapConvertor.cpp.

  : pose_pub_(pose_pub)
  , tf_lookup_(tf_lookup)
  , map_frame_id_(map_frame_id)
  , base_link_frame_id_(base_link_frame_id)
  , heading_frame_id_(heading_frame_id)
  , nh_(nh)
{
}

Member Function Documentation

◆ geoReferenceCallback()

void GNSSToMapConvertor::geoReferenceCallback ( std_msgs::msg::String::UniquePtr geo_ref )

Map georeference callback The geodetic description of the map frame should be provided to this callback as a proj library string. The frame orientation must be right handed and aligned with lat/lon however the orientation of this alignment does not matter (enu, ned, etc.)

Parameters

geo_ref The proj string which defines the geodetic projection of the map frame which is used to convert between GNSS and Map.

Definition at line 100 of file GNSSToMapConvertor.cpp.

{
  if (georeference_ != geo_ref->data)
  {
    georeference_ = geo_ref->data;
    map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(
      geo_ref->data.c_str());  // Build projector from proj string
 
    RCLCPP_INFO_STREAM(nh_->get_logger(), "Received map georeference: " << geo_ref->data);
 
    std::string axis = wgs84_utils::proj_tools::getAxisFromProjString(geo_ref->data);  // Extract axis for orientation calc
 
    RCLCPP_INFO_STREAM(nh_->get_logger(), "Extracted Axis: " << axis);
 
    ned_in_map_rotation_ = wgs84_utils::proj_tools::getRotationOfNEDFromProjAxis(axis);  // Extract map rotation from axis
 
    RCLCPP_INFO_STREAM(nh_->get_logger(), "Extracted NED in Map Rotation (x,y,z,w) : ( "
                    << ned_in_map_rotation_.get().x() << ", " << ned_in_map_rotation_.get().y() << ", "
                    << ned_in_map_rotation_.get().z() << ", " << ned_in_map_rotation_.get().w());
  }
}

References georeference_, map_projector_, ned_in_map_rotation_, and nh_.

Referenced by gnss_to_map_convertor::Node::handle_on_configure().

Here is the caller graph for this function:

◆ getMapProjector()

std::shared_ptr< lanelet::projection::LocalFrameProjector > GNSSToMapConvertor::getMapProjector ( )

Get the projector built from the provided georeference via the callback.

Definition at line 127 of file GNSSToMapConvertor.cpp.

{
  return map_projector_;
}

References map_projector_.

◆ getNedInMapRotation()

boost::optional< tf2::Quaternion > GNSSToMapConvertor::getNedInMapRotation ( )

Get the rotation computed from the recieved georeference or boost::none if unset.

Definition at line 122 of file GNSSToMapConvertor.cpp.

{
  return ned_in_map_rotation_;
}

References ned_in_map_rotation_.

◆ gnssFixCb()

void GNSSToMapConvertor::gnssFixCb ( gps_msgs::msg::GPSFix::UniquePtr fix_msg )

GNSS Fix callback which will publish a pose representing that fix in the map frame if the required transforms are available.

Parameters

fix_msg The message to convert to the map frame

Definition at line 37 of file GNSSToMapConvertor.cpp.

{
  if (!baselink_in_sensor_)  // Extract the assumed static transform between baselink and the sensor if it was not
                             // already optained
  {
    auto tf_msg = tf_lookup_(fix_msg->header.frame_id, base_link_frame_id_);
    if (!tf_msg)  // Failed to get transform
    {
      RCLCPP_WARN_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1000,
        "Throttled Log 1s: Ignoring fix message: Could not locate static transform between "
        << fix_msg->header.frame_id << " and " << base_link_frame_id_);
      return;
    }
 
    tf2::Transform tf;
    tf2::convert(tf_msg.get().transform, tf);
 
    baselink_in_sensor_ = tf;
  }
 
  if (!sensor_in_ned_heading_rotation_)  // Extract the assumed static transform between heading frame and the position
                                         // sensor frame if it was not already optained
  {
    auto tf_msg = tf_lookup_(fix_msg->header.frame_id, heading_frame_id_);
    if (!tf_msg)  // Failed to get transform
    {
      RCLCPP_WARN_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1000,
        "Throttled Log 1s: Ignoring fix message: Could not locate static transform between "
        << heading_frame_id_ << " and "
        << fix_msg->header.frame_id);
      return;
    }
 
    tf2::Transform tf;
    tf2::convert(tf_msg.get().transform, tf);
 
    if (tf.getOrigin().x() != 0.0 || tf.getOrigin().y() != 0.0 || tf.getOrigin().z() != 0.0)
    {
      RCLCPP_WARN_STREAM(nh_->get_logger(), "Heading frame does not have rotation only transform with sensor frame. The translation will not "
                     "be handled by the GNSS convertor");
    }
 
    sensor_in_ned_heading_rotation_ = tf.getRotation();
  }
 
  if (!map_projector_ || !ned_in_map_rotation_)  // Check if map projection is available
  {
    RCLCPP_WARN_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1000,
      "Throttled Log 1s: Ignoring fix message as no map projection has been received.");
    return;
  }
 
  geometry_msgs::msg::PoseWithCovarianceStamped pose_msg =
      poseFromGnss(baselink_in_sensor_.get(), sensor_in_ned_heading_rotation_.get(), *map_projector_, ned_in_map_rotation_.get(), *fix_msg);  // Convert to pose
 
  geometry_msgs::msg::PoseStamped msg;  // TODO until covariance is added drop it from the output
  msg.header = pose_msg.header;
  msg.header.frame_id = map_frame_id_;
  msg.pose = pose_msg.pose.pose;
 
  pose_pub_(msg);
}

References base_link_frame_id_, baselink_in_sensor_, motion_computation::conversion::convert(), heading_frame_id_, map_frame_id_, map_projector_, ned_in_map_rotation_, nh_, pose_pub_, poseFromGnss(), sensor_in_ned_heading_rotation_, and tf_lookup_.

Referenced by gnss_to_map_convertor::Node::handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ poseFromGnss()

geometry_msgs::msg::PoseWithCovarianceStamped GNSSToMapConvertor::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
	)

Converts a provided GNSS fix message into a pose message for the map frame describibed by the provided projector.

ASSUMPTION: This logic assumes that the orientation difference between an NED frame located at the map origin and an NED frame located at the GNSS point are sufficiently small that they can be ignored. Therefore it is assumed the heading report of the GNSS system reguardless of its poition in the map without change in its orientation will give the same result (as far as we are concered). This assumption will break down as the distance between the GNSS recieved and the map origin grows. It is recommended this distance be kept under 10km. If larger distances are required then the map origin should probably be periodically updated with a new georeference.

Parameters

baselink_in_sensor	A transform describing the location of the desried output frame (baselink) with respect to the GNSS sensor frame.
sensor_in_ned_heading_rotation	A rotation describing the orientation of the heading frame with respect to the position sensor frame
projector	A projector using the proj library which can convert lat/lon points into the map frame projection
ned_in_map_rotation	A rotation describibing the orientation of an NED frame located at the map origin with respect to the map frame.
fix_msg	The GNSS message to be converted into the pose in the map frame

Returns: A pose message describing the location and orientation of the baselink frame in the map frame. TODO handle covariance which is not currently included

Definition at line 133 of file GNSSToMapConvertor.cpp.

{
  const double lat = fix_msg.latitude;
  const double lon = fix_msg.longitude;
  const double alt = fix_msg.altitude;
 
  lanelet::BasicPoint3d map_point = projector.forward({ lat, lon, alt });
  RCLCPP_DEBUG_STREAM(nh_->get_logger(), "map_point: " << map_point.x() << ", " << map_point.y() << ", " << map_point.z());
 
  if (fabs(map_point.x()) > 10000.0 || fabs(map_point.y()) > 10000.0)
  {  // Above 10km from map origin earth curvature will start to have a negative impact on system performance
 
    RCLCPP_WARN_STREAM(nh_->get_logger(), "Distance from map origin is larger than supported by system assumptions. Strongly advise "
                   "alternative map origin be used. ");
  }
 
  // This logic assumes that the orientation difference between an NED frame located at the map origin and an NED frame
  // located at the GNSS point are sufficiently small that they can be ignored. Therefore it is assumed the heading
  // report of the GNSS system reguardless of its poition in the map without change in its orientation will give the
  // same result (as far as we are concered).
 
  tf2::Quaternion R_m_n(ned_in_map_rotation);  // Rotation of NED frame in map frame
  tf2::Quaternion R_n_h;                       // Rotation of sensor heading report in NED frame
  R_n_h.setRPY(0, 0, fix_msg.track * wgs84_utils::DEG2RAD);
 
  tf2::Quaternion R_h_s = sensor_in_ned_heading_rotation;  // Rotation of heading report in sensor frame
 
  tf2::Quaternion R_m_s =
      R_m_n * R_n_h * R_h_s;  // Rotation of sensor in map frame under assumption that distance from map origin is
                              // sufficiently small so as to ignore local changes in NED orientation
 
    RCLCPP_DEBUG_STREAM(nh_->get_logger(), "R_m_n (x,y,z,w) : ( "
                  << R_m_n.x() << ", " << R_m_n.y() << ", "
                  << R_m_n.z() << ", " << R_m_n.w());
 
    RCLCPP_DEBUG_STREAM(nh_->get_logger(), "R_n_h (x,y,z,w) : ( "
                  << R_n_h.x() << ", " << R_n_h.y() << ", "
                  << R_n_h.z() << ", " << R_n_h.w());
 
    RCLCPP_DEBUG_STREAM(nh_->get_logger(), "R_h_s (x,y,z,w) : ( "
                  << R_h_s.x() << ", " << R_h_s.y() << ", "
                  << R_h_s.z() << ", " << R_h_s.w());
 
    RCLCPP_DEBUG_STREAM(nh_->get_logger(), "R_m_s (x,y,z,w) : ( "
                  << R_m_s.x() << ", " << R_m_s.y() << ", "
                  << R_m_s.z() << ", " << R_m_s.w());
 
  tf2::Transform T_m_s(R_m_s,
                       tf2::Vector3(map_point.x(), map_point.y(), map_point.z()));  // Reported position and orientation
                                                                                    // of sensor frame in map frame
  tf2::Transform T_s_b(baselink_in_sensor);  // Transform between sensor and baselink frame
  tf2::Transform T_m_b = T_m_s * T_s_b;      // Transform between map and baselink frame
 
  RCLCPP_DEBUG_STREAM(nh_->get_logger(), "T_m_s (x,y,z,w) : ( "
               << T_m_s.getRotation().x() << ", " << T_m_s.getRotation().y() << ", "
               << T_m_s.getRotation().z() << ", " << T_m_s.getRotation().w());
  RCLCPP_DEBUG_STREAM(nh_->get_logger(), "T_s_b (x,y,z,w) : ( "
               << T_s_b.getRotation().x() << ", " << T_s_b.getRotation().y() << ", "
               << T_s_b.getRotation().z() << ", " << T_s_b.getRotation().w());
  RCLCPP_DEBUG_STREAM(nh_->get_logger(), "T_m_b (x,y,z,w) : ( "
               << T_m_b.getRotation().x() << ", " << T_m_b.getRotation().y() << ", "
               << T_m_b.getRotation().z() << ", " << T_m_b.getRotation().w());
 
  // TODO handle covariance
 
  // Populate message
  geometry_msgs::msg::PoseWithCovarianceStamped pose;
  pose.header = fix_msg.header;
 
  pose.pose.pose.position.x = T_m_b.getOrigin().getX();
  pose.pose.pose.position.y = T_m_b.getOrigin().getY();
  pose.pose.pose.position.z = T_m_b.getOrigin().getZ();
 
  pose.pose.pose.orientation.x = T_m_b.getRotation().getX();
  pose.pose.pose.orientation.y = T_m_b.getRotation().getY();
  pose.pose.pose.orientation.z = T_m_b.getRotation().getZ();
  pose.pose.pose.orientation.w = T_m_b.getRotation().getW();
 
  return pose;
}

References osm_transform::lat, osm_transform::lon, and nh_.

Referenced by gnssFixCb().

Here is the caller graph for this function:

Referenced by gnssFixCb().

◆ tf_lookup_

TransformLookupCallback gnss_to_map_convertor::GNSSToMapConvertor::tf_lookup_

private

Definition at line 137 of file GNSSToMapConvertor.hpp.

Referenced by gnssFixCb().

The documentation for this class was generated from the following files:

gnss_to_map_convertor/include/gnss_to_map_convertor/GNSSToMapConvertor.hpp
gnss_to_map_convertor/src/GNSSToMapConvertor.cpp

Public Types

Public Member Functions

Private Attributes

Detailed Description

Member Typedef Documentation

◆ PosePubCallback

◆ TransformLookupCallback

Constructor & Destructor Documentation

◆ GNSSToMapConvertor()

Member Function Documentation

◆ geoReferenceCallback()

◆ getMapProjector()

◆ getNedInMapRotation()

◆ gnssFixCb()

◆ poseFromGnss()

Member Data Documentation

◆ base_link_frame_id_

◆ baselink_in_sensor_

◆ georeference_

◆ heading_frame_id_

◆ map_frame_id_

◆ map_projector_

◆ ned_in_map_rotation_

◆ nh_

◆ pose_pub_

◆ sensor_in_ned_heading_rotation_

◆ tf_lookup_