Carma-platform v4.2.0
CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication.
gnss_to_map_convertor::GNSSToMapConvertor Class Reference

#include <GNSSToMapConvertor.hpp>

Collaboration diagram for gnss_to_map_convertor::GNSSToMapConvertor:
Collaboration graph

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, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger)
 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_
 
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_
 

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,
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr  logger 
)

Constructor.

Parameters
pose_pubA function to publish a converted gps fix as a pose to the rest of the system
tf_lookupA function which will lookup the most recent transform between the two provided frames
map_frame_idThe frame id of the frame which the output pose should be considered in
base_link_frame_idThe frame id of the frame which the output pose defines the position and orientation of
heading_frame_idThe frame id of the frame which the heading report aligns with an NED frame in the map frame.

Definition at line 24 of file GNSSToMapConvertor.cpp.

27 : pose_pub_(pose_pub)
28 , tf_lookup_(tf_lookup)
29 , map_frame_id_(map_frame_id)
30 , base_link_frame_id_(base_link_frame_id)
31 , heading_frame_id_(heading_frame_id)
32 , logger_(logger)
33{
34}
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_

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_refThe proj string which defines the geodetic projection of the map frame which is used to convert between GNSS and Map.

Definition at line 95 of file GNSSToMapConvertor.cpp.

96{
97 if (georeference_ != geo_ref->data)
98 {
99 georeference_ = geo_ref->data;
100 map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(
101 geo_ref->data.c_str()); // Build projector from proj string
102
103 RCLCPP_INFO_STREAM(logger_->get_logger(), "Received map georeference: " << geo_ref->data);
104
105 std::string axis = wgs84_utils::proj_tools::getAxisFromProjString(geo_ref->data); // Extract axis for orientation calc
106
107 RCLCPP_INFO_STREAM(logger_->get_logger(), "Extracted Axis: " << axis);
108
109 ned_in_map_rotation_ = wgs84_utils::proj_tools::getRotationOfNEDFromProjAxis(axis); // Extract map rotation from axis
110
111 RCLCPP_INFO_STREAM(logger_->get_logger(), "Extracted NED in Map Rotation (x,y,z,w) : ( "
112 << ned_in_map_rotation_.get().x() << ", " << ned_in_map_rotation_.get().y() << ", "
113 << ned_in_map_rotation_.get().z() << ", " << ned_in_map_rotation_.get().w());
114 }
115}
std::shared_ptr< lanelet::projection::LocalFrameProjector > map_projector_
boost::optional< tf2::Quaternion > ned_in_map_rotation_

References georeference_, logger_, map_projector_, and ned_in_map_rotation_.

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 122 of file GNSSToMapConvertor.cpp.

123{
124 return map_projector_;
125}

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 117 of file GNSSToMapConvertor.cpp.

118{
120}

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_msgThe message to convert to the map frame

Definition at line 36 of file GNSSToMapConvertor.cpp.

37{
38 if (!baselink_in_sensor_) // Extract the assumed static transform between baselink and the sensor if it was not
39 // already optained
40 {
41 auto tf_msg = tf_lookup_(fix_msg->header.frame_id, base_link_frame_id_);
42 if (!tf_msg) // Failed to get transform
43 {
44 RCLCPP_WARN_STREAM(logger_->get_logger(),"Ignoring fix message: Could not locate static transform between "
45 << fix_msg->header.frame_id << " and " << base_link_frame_id_);
46 return;
47 }
48
49 tf2::Transform tf;
50 tf2::convert(tf_msg.get().transform, tf);
51
53 }
54
55 if (!sensor_in_ned_heading_rotation_) // Extract the assumed static transform between heading frame and the position
56 // sensor frame if it was not already optained
57 {
58 auto tf_msg = tf_lookup_(fix_msg->header.frame_id, heading_frame_id_);
59 if (!tf_msg) // Failed to get transform
60 {
61 RCLCPP_WARN_STREAM(logger_->get_logger(), "Ignoring fix message: Could not locate static transform between " << heading_frame_id_ << " and "
62 << fix_msg->header.frame_id);
63 return;
64 }
65
66 tf2::Transform tf;
67 tf2::convert(tf_msg.get().transform, tf);
68
69 if (tf.getOrigin().x() != 0.0 || tf.getOrigin().y() != 0.0 || tf.getOrigin().z() != 0.0)
70 {
71 RCLCPP_WARN_STREAM(logger_->get_logger(), "Heading frame does not have rotation only transform with sensor frame. The translation will not "
72 "be handled by the GNSS convertor");
73 }
74
75 sensor_in_ned_heading_rotation_ = tf.getRotation();
76 }
77
78 if (!map_projector_ || !ned_in_map_rotation_) // Check if map projection is available
79 {
80 RCLCPP_WARN_STREAM(logger_->get_logger(), "Ignoring fix message as no map projection has been recieved.");
81 return;
82 }
83
84 geometry_msgs::msg::PoseWithCovarianceStamped pose_msg =
86
87 geometry_msgs::msg::PoseStamped msg; // TODO until covariance is added drop it from the output
88 msg.header = pose_msg.header;
89 msg.header.frame_id = map_frame_id_;
90 msg.pose = pose_msg.pose.pose;
91
92 pose_pub_(msg);
93}
boost::optional< tf2::Quaternion > sensor_in_ned_heading_rotation_
boost::optional< tf2::Transform > baselink_in_sensor_
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 provide...
void convert(const carma_v2x_msgs::msg::PSM &in_msg, carma_perception_msgs::msg::ExternalObject &out_msg, const std::string &map_frame_id, double pred_period, double pred_step_size, const lanelet::projection::LocalFrameProjector &map_projector, const tf2::Quaternion &ned_in_map_rotation, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock)

References base_link_frame_id_, baselink_in_sensor_, motion_computation::conversion::convert(), heading_frame_id_, logger_, map_frame_id_, map_projector_, ned_in_map_rotation_, 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_sensorA transform describing the location of the desried output frame (baselink) with respect to the GNSS sensor frame.
sensor_in_ned_heading_rotationA rotation describing the orientation of the heading frame with respect to the position sensor frame
projectorA projector using the proj library which can convert lat/lon points into the map frame projection
ned_in_map_rotationA rotation describibing the orientation of an NED frame located at the map origin with respect to the map frame.
fix_msgThe 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 128 of file GNSSToMapConvertor.cpp.

134{
136 const double lat = fix_msg.latitude;
137 const double lon = fix_msg.longitude;
138 const double alt = fix_msg.altitude;
139
140 lanelet::BasicPoint3d map_point = projector.forward({ lat, lon, alt });
141 RCLCPP_DEBUG_STREAM(logger_->get_logger(), "map_point: " << map_point.x() << ", " << map_point.y() << ", " << map_point.z());
142
143 if (fabs(map_point.x()) > 10000.0 || fabs(map_point.y()) > 10000.0)
144 { // Above 10km from map origin earth curvature will start to have a negative impact on system performance
145
146 RCLCPP_WARN_STREAM(logger_->get_logger(), "Distance from map origin is larger than supported by system assumptions. Strongly advise "
147 "alternative map origin be used. ");
148 }
149
151 // This logic assumes that the orientation difference between an NED frame located at the map origin and an NED frame
152 // located at the GNSS point are sufficiently small that they can be ignored. Therefore it is assumed the heading
153 // report of the GNSS system reguardless of its poition in the map without change in its orientation will give the
154 // same result (as far as we are concered).
155
156 tf2::Quaternion R_m_n(ned_in_map_rotation); // Rotation of NED frame in map frame
157 tf2::Quaternion R_n_h; // Rotation of sensor heading report in NED frame
158 R_n_h.setRPY(0, 0, fix_msg.track * wgs84_utils::DEG2RAD);
159
160 tf2::Quaternion R_h_s = sensor_in_ned_heading_rotation; // Rotation of heading report in sensor frame
161
162 tf2::Quaternion R_m_s =
163 R_m_n * R_n_h * R_h_s; // Rotation of sensor in map frame under assumption that distance from map origin is
164 // sufficiently small so as to ignore local changes in NED orientation
165
166 RCLCPP_DEBUG_STREAM(logger_->get_logger(), "R_m_n (x,y,z,w) : ( "
167 << R_m_n.x() << ", " << R_m_n.y() << ", "
168 << R_m_n.z() << ", " << R_m_n.w());
169
170 RCLCPP_DEBUG_STREAM(logger_->get_logger(), "R_n_h (x,y,z,w) : ( "
171 << R_n_h.x() << ", " << R_n_h.y() << ", "
172 << R_n_h.z() << ", " << R_n_h.w());
173
174 RCLCPP_DEBUG_STREAM(logger_->get_logger(), "R_h_s (x,y,z,w) : ( "
175 << R_h_s.x() << ", " << R_h_s.y() << ", "
176 << R_h_s.z() << ", " << R_h_s.w());
177
178 RCLCPP_DEBUG_STREAM(logger_->get_logger(), "R_m_s (x,y,z,w) : ( "
179 << R_m_s.x() << ", " << R_m_s.y() << ", "
180 << R_m_s.z() << ", " << R_m_s.w());
181
182 tf2::Transform T_m_s(R_m_s,
183 tf2::Vector3(map_point.x(), map_point.y(), map_point.z())); // Reported position and orientation
184 // of sensor frame in map frame
185 tf2::Transform T_s_b(baselink_in_sensor); // Transform between sensor and baselink frame
186 tf2::Transform T_m_b = T_m_s * T_s_b; // Transform between map and baselink frame
187
188 RCLCPP_DEBUG_STREAM(logger_->get_logger(), "T_m_s (x,y,z,w) : ( "
189 << T_m_s.getRotation().x() << ", " << T_m_s.getRotation().y() << ", "
190 << T_m_s.getRotation().z() << ", " << T_m_s.getRotation().w());
191 RCLCPP_DEBUG_STREAM(logger_->get_logger(), "T_s_b (x,y,z,w) : ( "
192 << T_s_b.getRotation().x() << ", " << T_s_b.getRotation().y() << ", "
193 << T_s_b.getRotation().z() << ", " << T_s_b.getRotation().w());
194 RCLCPP_DEBUG_STREAM(logger_->get_logger(), "T_m_b (x,y,z,w) : ( "
195 << T_m_b.getRotation().x() << ", " << T_m_b.getRotation().y() << ", "
196 << T_m_b.getRotation().z() << ", " << T_m_b.getRotation().w());
197
198 // TODO handle covariance
199
200 // Populate message
201 geometry_msgs::msg::PoseWithCovarianceStamped pose;
202 pose.header = fix_msg.header;
203
204 pose.pose.pose.position.x = T_m_b.getOrigin().getX();
205 pose.pose.pose.position.y = T_m_b.getOrigin().getY();
206 pose.pose.pose.position.z = T_m_b.getOrigin().getZ();
207
208 pose.pose.pose.orientation.x = T_m_b.getRotation().getX();
209 pose.pose.pose.orientation.y = T_m_b.getRotation().getY();
210 pose.pose.pose.orientation.z = T_m_b.getRotation().getZ();
211 pose.pose.pose.orientation.w = T_m_b.getRotation().getW();
212
213 return pose;
214}

References logger_.

Referenced by gnssFixCb().

Here is the caller graph for this function:

Member Data Documentation

◆ base_link_frame_id_

std::string gnss_to_map_convertor::GNSSToMapConvertor::base_link_frame_id_
private

Definition at line 137 of file GNSSToMapConvertor.hpp.

Referenced by gnssFixCb().

◆ baselink_in_sensor_

boost::optional<tf2::Transform> gnss_to_map_convertor::GNSSToMapConvertor::baselink_in_sensor_
private

Definition at line 152 of file GNSSToMapConvertor.hpp.

Referenced by gnssFixCb().

◆ georeference_

std::string gnss_to_map_convertor::GNSSToMapConvertor::georeference_ {""}
private

Definition at line 139 of file GNSSToMapConvertor.hpp.

Referenced by geoReferenceCallback().

◆ heading_frame_id_

std::string gnss_to_map_convertor::GNSSToMapConvertor::heading_frame_id_
private

Definition at line 138 of file GNSSToMapConvertor.hpp.

Referenced by gnssFixCb().

◆ logger_

rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr gnss_to_map_convertor::GNSSToMapConvertor::logger_
private

Definition at line 156 of file GNSSToMapConvertor.hpp.

Referenced by geoReferenceCallback(), gnssFixCb(), and poseFromGnss().

◆ map_frame_id_

std::string gnss_to_map_convertor::GNSSToMapConvertor::map_frame_id_
private

Definition at line 136 of file GNSSToMapConvertor.hpp.

Referenced by gnssFixCb().

◆ map_projector_

std::shared_ptr<lanelet::projection::LocalFrameProjector> gnss_to_map_convertor::GNSSToMapConvertor::map_projector_
private

Definition at line 148 of file GNSSToMapConvertor.hpp.

Referenced by geoReferenceCallback(), getMapProjector(), and gnssFixCb().

◆ ned_in_map_rotation_

boost::optional<tf2::Quaternion> gnss_to_map_convertor::GNSSToMapConvertor::ned_in_map_rotation_
private

Definition at line 143 of file GNSSToMapConvertor.hpp.

Referenced by geoReferenceCallback(), getNedInMapRotation(), and gnssFixCb().

◆ pose_pub_

PosePubCallback gnss_to_map_convertor::GNSSToMapConvertor::pose_pub_
private

Definition at line 134 of file GNSSToMapConvertor.hpp.

Referenced by gnssFixCb().

◆ sensor_in_ned_heading_rotation_

boost::optional<tf2::Quaternion> gnss_to_map_convertor::GNSSToMapConvertor::sensor_in_ned_heading_rotation_
private

Definition at line 146 of file GNSSToMapConvertor.hpp.

Referenced by gnssFixCb().

◆ tf_lookup_

TransformLookupCallback gnss_to_map_convertor::GNSSToMapConvertor::tf_lookup_
private

Definition at line 135 of file GNSSToMapConvertor.hpp.

Referenced by gnssFixCb().


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