Program Listing for File typedefs.hpp
↰ Return to documentation for file (/tmp/ws/src/septentrio_gnss_driver/include/septentrio_gnss_driver/abstraction/typedefs.hpp
)
// *****************************************************************************
//
// © Copyright 2020, Septentrio NV/SA.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// *****************************************************************************
#pragma once
// std includes
#include <any>
#include <iomanip>
#include <sstream>
#include <unordered_map>
// ROS includes
#include <rclcpp/rclcpp.hpp>
// tf2 includes
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#ifdef ROS2_VER_N250
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#else
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#endif
// ROS msg includes
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/twist_with_covariance.hpp>
#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
#include <gps_msgs/msg/gps_fix.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <sensor_msgs/msg/time_reference.hpp>
// GNSS msg includes
#include <septentrio_gnss_driver/msg/aim_plus_status.hpp>
#include <septentrio_gnss_driver/msg/att_cov_euler.hpp>
#include <septentrio_gnss_driver/msg/att_euler.hpp>
#include <septentrio_gnss_driver/msg/base_vector_cart.hpp>
#include <septentrio_gnss_driver/msg/base_vector_geod.hpp>
#include <septentrio_gnss_driver/msg/block_header.hpp>
#include <septentrio_gnss_driver/msg/gal_auth_status.hpp>
#include <septentrio_gnss_driver/msg/meas_epoch.hpp>
#include <septentrio_gnss_driver/msg/meas_epoch_channel_type1.hpp>
#include <septentrio_gnss_driver/msg/meas_epoch_channel_type2.hpp>
#include <septentrio_gnss_driver/msg/pos_cov_cartesian.hpp>
#include <septentrio_gnss_driver/msg/pos_cov_geodetic.hpp>
#include <septentrio_gnss_driver/msg/pvt_cartesian.hpp>
#include <septentrio_gnss_driver/msg/pvt_geodetic.hpp>
#include <septentrio_gnss_driver/msg/receiver_time.hpp>
#include <septentrio_gnss_driver/msg/rf_band.hpp>
#include <septentrio_gnss_driver/msg/rf_status.hpp>
#include <septentrio_gnss_driver/msg/vector_info_cart.hpp>
#include <septentrio_gnss_driver/msg/vector_info_geod.hpp>
#include <septentrio_gnss_driver/msg/vel_cov_cartesian.hpp>
#include <septentrio_gnss_driver/msg/vel_cov_geodetic.hpp>
// NMEA msg includes
#include <nmea_msgs/msg/gpgga.hpp>
#include <nmea_msgs/msg/gpgsa.hpp>
#include <nmea_msgs/msg/gpgsv.hpp>
#include <nmea_msgs/msg/gprmc.hpp>
// INS msg includes
#include <septentrio_gnss_driver/msg/ext_sensor_meas.hpp>
#include <septentrio_gnss_driver/msg/imu_setup.hpp>
#include <septentrio_gnss_driver/msg/ins_nav_cart.hpp>
#include <septentrio_gnss_driver/msg/ins_nav_geod.hpp>
#include <septentrio_gnss_driver/msg/vel_sensor_setup.hpp>
// Rosaic includes
#include <septentrio_gnss_driver/communication/settings.hpp>
#include <septentrio_gnss_driver/parsers/string_utilities.hpp>
// Timestamp in nanoseconds (Unix epoch)
typedef uint64_t Timestamp;
// ROS timestamp
typedef rclcpp::Time TimestampRos;
// ROS messages
typedef diagnostic_msgs::msg::DiagnosticArray DiagnosticArrayMsg;
typedef diagnostic_msgs::msg::DiagnosticStatus DiagnosticStatusMsg;
typedef geometry_msgs::msg::Quaternion QuaternionMsg;
typedef geometry_msgs::msg::PoseWithCovarianceStamped PoseWithCovarianceStampedMsg;
typedef geometry_msgs::msg::TwistWithCovarianceStamped TwistWithCovarianceStampedMsg;
typedef geometry_msgs::msg::TransformStamped TransformStampedMsg;
typedef gps_msgs::msg::GPSFix GpsFixMsg;
typedef gps_msgs::msg::GPSStatus GpsStatusMsg;
typedef sensor_msgs::msg::NavSatFix NavSatFixMsg;
typedef sensor_msgs::msg::NavSatStatus NavSatStatusMsg;
typedef sensor_msgs::msg::TimeReference TimeReferenceMsg;
typedef sensor_msgs::msg::Imu ImuMsg;
typedef nav_msgs::msg::Odometry LocalizationMsg;
// Septentrio GNSS SBF messages
typedef septentrio_gnss_driver::msg::AIMPlusStatus AimPlusStatusMsg;
typedef septentrio_gnss_driver::msg::BaseVectorCart BaseVectorCartMsg;
typedef septentrio_gnss_driver::msg::BaseVectorGeod BaseVectorGeodMsg;
typedef septentrio_gnss_driver::msg::BlockHeader BlockHeaderMsg;
typedef septentrio_gnss_driver::msg::GALAuthStatus GalAuthStatusMsg;
typedef septentrio_gnss_driver::msg::RFStatus RfStatusMsg;
typedef septentrio_gnss_driver::msg::RFBand RfBandMsg;
typedef septentrio_gnss_driver::msg::MeasEpoch MeasEpochMsg;
typedef septentrio_gnss_driver::msg::MeasEpochChannelType1 MeasEpochChannelType1Msg;
typedef septentrio_gnss_driver::msg::MeasEpochChannelType2 MeasEpochChannelType2Msg;
typedef septentrio_gnss_driver::msg::AttCovEuler AttCovEulerMsg;
typedef septentrio_gnss_driver::msg::AttEuler AttEulerMsg;
typedef septentrio_gnss_driver::msg::PVTCartesian PVTCartesianMsg;
typedef septentrio_gnss_driver::msg::PVTGeodetic PVTGeodeticMsg;
typedef septentrio_gnss_driver::msg::PosCovCartesian PosCovCartesianMsg;
typedef septentrio_gnss_driver::msg::PosCovGeodetic PosCovGeodeticMsg;
typedef septentrio_gnss_driver::msg::ReceiverTime ReceiverTimeMsg;
typedef septentrio_gnss_driver::msg::VectorInfoCart VectorInfoCartMsg;
typedef septentrio_gnss_driver::msg::VectorInfoGeod VectorInfoGeodMsg;
typedef septentrio_gnss_driver::msg::VelCovCartesian VelCovCartesianMsg;
typedef septentrio_gnss_driver::msg::VelCovGeodetic VelCovGeodeticMsg;
// NMEA message
typedef nmea_msgs::msg::Gpgga GpggaMsg;
typedef nmea_msgs::msg::Gpgsa GpgsaMsg;
typedef nmea_msgs::msg::Gpgsv GpgsvMsg;
typedef nmea_msgs::msg::Gprmc GprmcMsg;
;
// Septentrio INS+GNSS SBF messages
typedef septentrio_gnss_driver::msg::INSNavCart INSNavCartMsg;
typedef septentrio_gnss_driver::msg::INSNavGeod INSNavGeodMsg;
typedef septentrio_gnss_driver::msg::IMUSetup IMUSetupMsg;
typedef septentrio_gnss_driver::msg::VelSensorSetup VelSensorSetupMsg;
typedef septentrio_gnss_driver::msg::ExtSensorMeas ExtSensorMeasMsg;
inline TimestampRos timestampToRos(Timestamp ts) { return TimestampRos(ts); }
inline Timestamp timestampFromRos(const TimestampRos& tsr)
{
return tsr.nanoseconds();
}
namespace log_level {
enum LogLevel
{
DEBUG,
INFO,
WARN,
ERROR,
FATAL
};
} // namespace log_level
class ROSaicNodeBase : public rclcpp::Node
{
public:
ROSaicNodeBase(const rclcpp::NodeOptions& options) :
Node("septentrio_gnss", options), tf2Publisher_(this),
tfBuffer_(this->get_clock()), tfListener_(tfBuffer_)
{
}
virtual ~ROSaicNodeBase() {}
const Settings* settings() const { return &settings_; }
void registerSubscriber()
{
try
{
if (settings_.ins_vsm_ros_source == "odometry")
odometrySubscriber_ =
this->create_subscription<nav_msgs::msg::Odometry>(
"odometry_vsm", 10,
std::bind(&ROSaicNodeBase::callbackOdometry, this,
std::placeholders::_1));
else if (settings_.ins_vsm_ros_source == "twist")
twistSubscriber_ =
this->create_subscription<TwistWithCovarianceStampedMsg>(
"twist_vsm", 10,
std::bind(&ROSaicNodeBase::callbackTwist, this,
std::placeholders::_1));
} catch (const std::runtime_error& ex)
{
this->log(log_level::ERROR, "Subscriber initialization failed due to: " +
std::string(ex.what()) + ".");
}
}
bool getUint32Param(const std::string& name, uint32_t& val, uint32_t defaultVal)
{
int32_t tempVal;
if ((!this->param(name, tempVal, -1)) || (tempVal < 0))
{
val = defaultVal;
return false;
}
val = tempVal;
return true;
}
template <typename T>
bool param(const std::string& name, T& val, const T& defaultVal)
{
if (this->has_parameter(name))
this->undeclare_parameter(name);
try
{
val = this->declare_parameter<T>(name, defaultVal);
} catch (std::runtime_error& e)
{
RCLCPP_WARN_STREAM(this->get_logger(), e.what());
return false;
}
return true;
};
void log(log_level::LogLevel logLevel, const std::string& s) const
{
switch (logLevel)
{
case log_level::DEBUG:
RCLCPP_DEBUG_STREAM(this->get_logger(), s);
break;
case log_level::INFO:
RCLCPP_INFO_STREAM(this->get_logger(), s);
break;
case log_level::WARN:
RCLCPP_WARN_STREAM(this->get_logger(), s);
break;
case log_level::ERROR:
RCLCPP_ERROR_STREAM(this->get_logger(), s);
break;
case log_level::FATAL:
RCLCPP_FATAL_STREAM(this->get_logger(), s);
break;
default:
break;
}
}
Timestamp getTime() const { return this->now().nanoseconds(); }
template <typename M>
void publishMessage(const std::string& topic, const M& msg)
{
auto it = topicMap_.find(topic);
if (it != topicMap_.end())
{
typename rclcpp::Publisher<M>::SharedPtr ptr =
std::any_cast<typename rclcpp::Publisher<M>::SharedPtr>(it->second);
ptr->publish(msg);
} else
{
typename rclcpp::Publisher<M>::SharedPtr pub =
this->create_publisher<M>(topic, queueSize_);
topicMap_.insert(std::make_pair(topic, pub));
pub->publish(msg);
}
}
void publishTf(const LocalizationMsg& loc)
{
if (std::isnan(loc.pose.pose.orientation.w))
return;
Timestamp currentStamp = timestampFromRos(loc.header.stamp);
if (lastTfStamp_ == currentStamp)
return;
lastTfStamp_ = currentStamp;
geometry_msgs::msg::TransformStamped transformStamped;
transformStamped.header.stamp = loc.header.stamp;
transformStamped.header.frame_id = loc.header.frame_id;
transformStamped.child_frame_id = loc.child_frame_id;
transformStamped.transform.translation.x = loc.pose.pose.position.x;
transformStamped.transform.translation.y = loc.pose.pose.position.y;
transformStamped.transform.translation.z = loc.pose.pose.position.z;
transformStamped.transform.rotation.x = loc.pose.pose.orientation.x;
transformStamped.transform.rotation.y = loc.pose.pose.orientation.y;
transformStamped.transform.rotation.z = loc.pose.pose.orientation.z;
transformStamped.transform.rotation.w = loc.pose.pose.orientation.w;
if (settings_.insert_local_frame)
{
geometry_msgs::msg::TransformStamped T_l_b;
try
{
// try to get tf at timestamp of message
T_l_b = tfBuffer_.lookupTransform(
loc.child_frame_id, settings_.local_frame_id, loc.header.stamp);
} catch (const tf2::TransformException& ex)
{
try
{
RCLCPP_INFO_STREAM_THROTTLE(
this->get_logger(), *this->get_clock(), 10000,
": No transform for insertion of local frame at t="
<< std::to_string(currentStamp)
<< ". Exception: " << std::string(ex.what()));
// try to get latest tf
T_l_b = tfBuffer_.lookupTransform(loc.child_frame_id,
settings_.local_frame_id,
rclcpp::Time(0));
} catch (const tf2::TransformException& ex)
{
RCLCPP_WARN_STREAM_THROTTLE(
this->get_logger(), *this->get_clock(), 10000,
": No most recent transform for insertion of local frame. Exception: "
<< std::string(ex.what()));
return;
}
}
// T_l_g = T_b_l^-1 * T_b_g;
transformStamped =
tf2::eigenToTransform(tf2::transformToEigen(transformStamped) *
tf2::transformToEigen(T_l_b));
transformStamped.header.stamp = loc.header.stamp;
transformStamped.header.frame_id = loc.header.frame_id;
transformStamped.child_frame_id = settings_.local_frame_id;
}
tf2Publisher_.sendTransform(transformStamped);
}
void setIsIns() { capabilities_.is_ins = true; }
void setHasHeading() { capabilities_.has_heading = true; }
void setImprovedVsmHandling() { capabilities_.has_improved_vsm_handling = true; }
bool isIns() { return capabilities_.is_ins; }
bool hasHeading() { return capabilities_.has_heading; }
bool hasImprovedVsmHandling() { return capabilities_.has_improved_vsm_handling; }
private:
void callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr odo)
{
Timestamp stamp = timestampFromRos(odo->header.stamp);
processTwist(stamp, odo->twist);
}
void callbackTwist(const TwistWithCovarianceStampedMsg::SharedPtr twist)
{
Timestamp stamp = timestampFromRos(twist->header.stamp);
processTwist(stamp, twist->twist);
}
void processTwist(Timestamp stamp,
const geometry_msgs::msg::TwistWithCovariance& twist)
{
// in case stamp was not set
if (stamp == 0)
stamp = getTime();
static Eigen::Vector3d vel = Eigen::Vector3d::Zero();
static Eigen::Vector3d var = Eigen::Vector3d::Zero();
static uint64_t ctr = 0;
static Timestamp lastStamp = 0;
++ctr;
vel[0] += twist.twist.linear.x;
vel[1] += twist.twist.linear.y;
vel[2] += twist.twist.linear.z;
var[0] += twist.covariance[0];
var[1] += twist.covariance[7];
var[2] += twist.covariance[14];
// Rx expects averaged velocity at a rate of 2 Hz
if ((stamp - lastStamp) >= 495000000) // allow for 5 ms jitter
{
vel /= ctr;
var /= ctr;
time_t epochSeconds = stamp / 1000000000;
struct tm* tm_temp = std::gmtime(&epochSeconds);
std::stringstream timeUtc;
timeUtc << std::setfill('0') << std::setw(2)
<< std::to_string(tm_temp->tm_hour) << std::setw(2)
<< std::to_string(tm_temp->tm_min) << std::setw(2)
<< std::to_string(tm_temp->tm_sec) << "." << std::setw(3)
<< std::to_string((stamp - (stamp / 1000000000) * 1000000000) /
1000000);
std::string v_x;
std::string v_y;
std::string v_z;
std::string std_x;
std::string std_y;
std::string std_z;
if (settings_.ins_vsm_ros_config[0])
{
v_x = string_utilities::trimDecimalPlaces(vel[0]);
if (settings_.ins_vsm_ros_variances_by_parameter)
std_x = string_utilities::trimDecimalPlaces(
settings_.ins_vsm_ros_variances[0]);
else if (var[0] > 0.0)
std_x = string_utilities::trimDecimalPlaces(std::sqrt(var[0]));
else if (!capabilities_.has_improved_vsm_handling)
{
log(log_level::ERROR, "Invalid covariance value for v_x: " +
std::to_string(var[0]) +
". Ignoring measurement.");
v_x = "";
std_x = string_utilities::trimDecimalPlaces(1000000.0);
}
} else
std_x = std::to_string(1000000.0);
if (settings_.ins_vsm_ros_config[1])
{
if (settings_.use_ros_axis_orientation)
v_y = "-";
v_y += string_utilities::trimDecimalPlaces(vel[1]);
if (settings_.ins_vsm_ros_variances_by_parameter)
std_y = string_utilities::trimDecimalPlaces(
settings_.ins_vsm_ros_variances[1]);
else if (var[1] > 0.0)
std_y = string_utilities::trimDecimalPlaces(std::sqrt(var[1]));
else if (!capabilities_.has_improved_vsm_handling)
{
log(log_level::ERROR, "Invalid covariance value for v_y: " +
std::to_string(var[1]) +
". Ignoring measurement.");
v_y = "";
std_y = string_utilities::trimDecimalPlaces(1000000.0);
}
} else
std_y = string_utilities::trimDecimalPlaces(1000000.0);
if (settings_.ins_vsm_ros_config[2])
{
if (settings_.use_ros_axis_orientation)
v_z = "-";
v_z += string_utilities::trimDecimalPlaces(vel[2]);
if (settings_.ins_vsm_ros_variances_by_parameter)
std_z = string_utilities::trimDecimalPlaces(
settings_.ins_vsm_ros_variances[2]);
else if (var[2] > 0.0)
std_z = string_utilities::trimDecimalPlaces(std::sqrt(var[2]));
else if (!capabilities_.has_improved_vsm_handling)
{
log(log_level::ERROR, "Invalid covariance value for v_z: " +
std::to_string(var[2]) +
". Ignoring measurement.");
v_z = "";
std_z = string_utilities::trimDecimalPlaces(1000000.0);
}
} else
std_z = string_utilities::trimDecimalPlaces(1000000.0);
std::string velNmea = "$PSSN,VSM," + timeUtc.str() + "," + v_x + "," +
v_y + "," + std_x + "," + std_y + "," + v_z + "," +
std_z;
char crc = std::accumulate(velNmea.begin() + 1, velNmea.end(), 0,
[](char sum, char ch) { return sum ^ ch; });
std::stringstream crcss;
crcss << std::hex << static_cast<int32_t>(crc);
velNmea += "*" + crcss.str() + "\r\n";
sendVelocity(velNmea);
vel = Eigen::Vector3d::Zero();
var = Eigen::Vector3d::Zero();
ctr = 0;
lastStamp = stamp;
}
}
protected:
Settings settings_;
virtual void sendVelocity(const std::string& velNmea) = 0;
private:
std::unordered_map<std::string, std::any> topicMap_;
uint32_t queueSize_ = 1;
tf2_ros::TransformBroadcaster tf2Publisher_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometrySubscriber_;
rclcpp::Subscription<TwistWithCovarianceStampedMsg>::SharedPtr twistSubscriber_;
Timestamp lastTfStamp_ = 0;
tf2_ros::Buffer tfBuffer_;
// tf listener
tf2_ros::TransformListener tfListener_;
// Capabilities of Rx
Capabilities capabilities_;
};