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.
|
#include <GNSSToMapConvertor.hpp>
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_ |
Definition at line 50 of file GNSSToMapConvertor.hpp.
using gnss_to_map_convertor::GNSSToMapConvertor::PosePubCallback = std::function<void(geometry_msgs::msg::PoseStamped)> |
Definition at line 53 of file GNSSToMapConvertor.hpp.
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.
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.
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 in the map frame. |
Definition at line 24 of file GNSSToMapConvertor.cpp.
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.)
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 95 of file GNSSToMapConvertor.cpp.
References georeference_, logger_, map_projector_, and ned_in_map_rotation_.
Referenced by gnss_to_map_convertor::Node::handle_on_configure().
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.
References map_projector_.
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.
References ned_in_map_rotation_.
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.
fix_msg | The message to convert to the map frame |
Definition at line 36 of file GNSSToMapConvertor.cpp.
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().
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.
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 |
Definition at line 128 of file GNSSToMapConvertor.cpp.
References logger_.
Referenced by gnssFixCb().
|
private |
Definition at line 137 of file GNSSToMapConvertor.hpp.
Referenced by gnssFixCb().
|
private |
Definition at line 152 of file GNSSToMapConvertor.hpp.
Referenced by gnssFixCb().
|
private |
Definition at line 139 of file GNSSToMapConvertor.hpp.
Referenced by geoReferenceCallback().
|
private |
Definition at line 138 of file GNSSToMapConvertor.hpp.
Referenced by gnssFixCb().
|
private |
Definition at line 156 of file GNSSToMapConvertor.hpp.
Referenced by geoReferenceCallback(), gnssFixCb(), and poseFromGnss().
|
private |
Definition at line 136 of file GNSSToMapConvertor.hpp.
Referenced by gnssFixCb().
|
private |
Definition at line 148 of file GNSSToMapConvertor.hpp.
Referenced by geoReferenceCallback(), getMapProjector(), and gnssFixCb().
|
private |
Definition at line 143 of file GNSSToMapConvertor.hpp.
Referenced by geoReferenceCallback(), getNedInMapRotation(), and gnssFixCb().
|
private |
Definition at line 134 of file GNSSToMapConvertor.hpp.
Referenced by gnssFixCb().
|
private |
Definition at line 146 of file GNSSToMapConvertor.hpp.
Referenced by gnssFixCb().
|
private |
Definition at line 135 of file GNSSToMapConvertor.hpp.
Referenced by gnssFixCb().