typedefs.hpp

Go to the documentation of this file.

// *****************************************************************************
//
// © 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 <numeric>
#include <unordered_map>
// ROS includes
#include <ros/ros.h>
// tf2 includes
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
// ROS msg includes
#include <diagnostic_msgs/DiagnosticArray.h>
#include <diagnostic_msgs/DiagnosticStatus.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <gps_common/GPSFix.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/TimeReference.h>
// GNSS msg includes
#include <septentrio_gnss_driver/AttCovEuler.h>
#include <septentrio_gnss_driver/AttEuler.h>
#include <septentrio_gnss_driver/BaseVectorCart.h>
#include <septentrio_gnss_driver/BaseVectorGeod.h>
#include <septentrio_gnss_driver/BlockHeader.h>
#include <septentrio_gnss_driver/MeasEpoch.h>
#include <septentrio_gnss_driver/MeasEpochChannelType1.h>
#include <septentrio_gnss_driver/MeasEpochChannelType2.h>
#include <septentrio_gnss_driver/PVTCartesian.h>
#include <septentrio_gnss_driver/PVTGeodetic.h>
#include <septentrio_gnss_driver/PosCovCartesian.h>
#include <septentrio_gnss_driver/PosCovGeodetic.h>
#include <septentrio_gnss_driver/ReceiverTime.h>
#include <septentrio_gnss_driver/VectorInfoCart.h>
#include <septentrio_gnss_driver/VectorInfoGeod.h>
#include <septentrio_gnss_driver/VelCovCartesian.h>
#include <septentrio_gnss_driver/VelCovGeodetic.h>
// NMEA msg includes
#include <nmea_msgs/Gpgga.h>
#include <nmea_msgs/Gpgsa.h>
#include <nmea_msgs/Gpgsv.h>
#include <nmea_msgs/Gprmc.h>
// INS msg includes
#include <septentrio_gnss_driver/ExtSensorMeas.h>
#include <septentrio_gnss_driver/IMUSetup.h>
#include <septentrio_gnss_driver/INSNavCart.h>
#include <septentrio_gnss_driver/INSNavGeod.h>
#include <septentrio_gnss_driver/VelSensorSetup.h>
// Rosaic includes
#include <septentrio_gnss_driver/communication/settings.h>
#include <septentrio_gnss_driver/parsers/string_utilities.h>

// Timestamp in nanoseconds (Unix epoch)
typedef uint64_t Timestamp;
// ROS timestamp
typedef ros::Time TimestampRos;

// ROS messages
typedef diagnostic_msgs::DiagnosticArray DiagnosticArrayMsg;
typedef diagnostic_msgs::DiagnosticStatus DiagnosticStatusMsg;
typedef geometry_msgs::Quaternion QuaternionMsg;
typedef geometry_msgs::PoseWithCovarianceStamped PoseWithCovarianceStampedMsg;
typedef geometry_msgs::TwistWithCovarianceStamped TwistWithCovarianceStampedMsg;
typedef geometry_msgs::TransformStamped TransformStampedMsg;
typedef gps_common::GPSFix GPSFixMsg;
typedef gps_common::GPSStatus GPSStatusMsg;
typedef sensor_msgs::NavSatFix NavSatFixMsg;
typedef sensor_msgs::NavSatStatus NavSatStatusMsg;
typedef sensor_msgs::TimeReference TimeReferenceMsg;
typedef sensor_msgs::Imu ImuMsg;
typedef nav_msgs::Odometry LocalizationUtmMsg;

// Septentrio GNSS SBF messages
typedef septentrio_gnss_driver::BaseVectorCart BaseVectorCartMsg;
typedef septentrio_gnss_driver::BaseVectorGeod BaseVectorGeodMsg;
typedef septentrio_gnss_driver::BlockHeader BlockHeaderMsg;
typedef septentrio_gnss_driver::MeasEpoch MeasEpochMsg;
typedef septentrio_gnss_driver::MeasEpochChannelType1 MeasEpochChannelType1Msg;
typedef septentrio_gnss_driver::MeasEpochChannelType2 MeasEpochChannelType2Msg;
typedef septentrio_gnss_driver::AttCovEuler AttCovEulerMsg;
typedef septentrio_gnss_driver::AttEuler AttEulerMsg;
typedef septentrio_gnss_driver::PVTCartesian PVTCartesianMsg;
typedef septentrio_gnss_driver::PVTGeodetic PVTGeodeticMsg;
typedef septentrio_gnss_driver::PosCovCartesian PosCovCartesianMsg;
typedef septentrio_gnss_driver::PosCovGeodetic PosCovGeodeticMsg;
typedef septentrio_gnss_driver::ReceiverTime ReceiverTimeMsg;
typedef septentrio_gnss_driver::VectorInfoCart VectorInfoCartMsg;
typedef septentrio_gnss_driver::VectorInfoGeod VectorInfoGeodMsg;
typedef septentrio_gnss_driver::VelCovCartesian VelCovCartesianMsg;
typedef septentrio_gnss_driver::VelCovGeodetic VelCovGeodeticMsg;

// NMEA messages
typedef nmea_msgs::Gpgga GpggaMsg;
typedef nmea_msgs::Gpgsa GpgsaMsg;
typedef nmea_msgs::Gpgsv GpgsvMsg;
typedef nmea_msgs::Gprmc GprmcMsg;

// Septentrio INS+GNSS SBF messages
typedef septentrio_gnss_driver::INSNavCart INSNavCartMsg;
typedef septentrio_gnss_driver::INSNavGeod INSNavGeodMsg;
typedef septentrio_gnss_driver::IMUSetup IMUSetupMsg;
typedef septentrio_gnss_driver::VelSensorSetup VelSensorSetupMsg;
typedef septentrio_gnss_driver::ExtSensorMeas ExtSensorMeasMsg;

inline TimestampRos timestampToRos(Timestamp ts)
{
    TimestampRos tsr;
    tsr.fromNSec(ts);
    return tsr;
}

inline Timestamp timestampFromRos(const TimestampRos& tsr) { return tsr.toNSec(); }

enum LogLevel
{
    DEBUG,
    INFO,
    WARN,
    ERROR,
    FATAL
};

class ROSaicNodeBase
{
public:
    ROSaicNodeBase() : pNh_(new ros::NodeHandle("~")), tfListener_(tfBuffer_) {}

    virtual ~ROSaicNodeBase() {}

    void registerSubscriber()
    {
        ros::NodeHandle nh;
        if (settings_.ins_vsm_ros_source == "odometry")
            odometrySubscriber_ = nh.subscribe<nav_msgs::Odometry>(
                "odometry_vsm", 10, &ROSaicNodeBase::callbackOdometry, this);
        else if (settings_.ins_vsm_ros_source == "twist")
            twistSubscriber_ = nh.subscribe<TwistWithCovarianceStampedMsg>(
                "twist_vsm", 10, &ROSaicNodeBase::callbackTwist, this);
    }

    bool getUint32Param(const std::string& name, uint32_t& val, uint32_t defaultVal)
    {
        int32_t tempVal;
        if ((!pNh_->getParam(name, tempVal)) || (tempVal < 0))
        {
            val = defaultVal;
            return false;
        }
        val = tempVal;
        return true;
    }

    template <typename T>
    bool param(const std::string& name, T& val, const T& defaultVal)
    {
        return pNh_->param(name, val, defaultVal);
    };

    void log(LogLevel logLevel, const std::string& s)
    {
        switch (logLevel)
        {
        case LogLevel::DEBUG:
            ROS_DEBUG_STREAM(ros::this_node::getName() << ": " << s);
            break;
        case LogLevel::INFO:
            ROS_INFO_STREAM(ros::this_node::getName() << ": " << s);
            break;
        case LogLevel::WARN:
            ROS_WARN_STREAM(ros::this_node::getName() << ": " << s);
            break;
        case LogLevel::ERROR:
            ROS_ERROR_STREAM(ros::this_node::getName() << ": " << s);
            break;
        case LogLevel::FATAL:
            ROS_FATAL_STREAM(ros::this_node::getName() << ": " << s);
            break;
        default:
            break;
        }
    }

    Timestamp getTime() { return ros::Time::now().toNSec(); }

    template <typename M>
    void publishMessage(const std::string& topic, const M& msg)
    {
        auto it = topicMap_.find(topic);
        if (it != topicMap_.end())
        {
            it->second.publish(msg);
        } else
        {
            ros::Publisher pub = pNh_->advertise<M>(topic, queueSize_);
            topicMap_.insert(std::make_pair(topic, pub));
            pub.publish(msg);
        }
    }

    void publishTf(const LocalizationUtmMsg& loc)
    {
        if (std::isnan(loc.pose.pose.orientation.w))
            return;

        if (lastTfStamp_ == loc.header.stamp)
            return;
        lastTfStamp_ = loc.header.stamp;

        geometry_msgs::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::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, lastTfStamp_);
            } catch (const tf2::TransformException& ex)
            {
                try
                {
                    ROS_INFO_STREAM_THROTTLE(
                        10.0,
                        ros::this_node::getName()
                            << ": No transform for insertion of local frame at t="
                            << lastTfStamp_.toNSec()
                            << ". Exception: " << std::string(ex.what()));
                    // try to get latest tf
                    T_l_b = tfBuffer_.lookupTransform(
                        loc.child_frame_id, settings_.local_frame_id, ros::Time(0));
                } catch (const tf2::TransformException& ex)
                {
                    ROS_WARN_STREAM_THROTTLE(
                        10.0,
                        ros::this_node::getName()
                            << ": No most recent transform for insertion of local frame. Exception: "
                            << std::string(ex.what()));
                    return;
                }
            }

            // T_l_g = T_b_g * T_l_b;
            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);
    }

private:
    void callbackOdometry(const nav_msgs::Odometry::ConstPtr& odo)
    {
        Timestamp stamp = timestampFromRos(odo->header.stamp);

        processTwist(stamp, odo->twist);
    }

    void callbackTwist(const TwistWithCovarianceStampedMsg::ConstPtr& twist)
    {
        Timestamp stamp = timestampFromRos(twist->header.stamp);

        processTwist(stamp, twist->twist);
    }

    void processTwist(Timestamp stamp,
                      const geometry_msgs::TwistWithCovariance& twist)
    {
        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(twist.twist.linear.x);
            if (settings_.ins_vsm_ros_variances_by_parameter)
                std_x = string_utilities::trimDecimalPlaces(
                    settings_.ins_vsm_ros_variances[0]);
            else if (twist.covariance[0] > 0.0)
                std_x = string_utilities::trimDecimalPlaces(
                    std::sqrt(twist.covariance[0]));
            else
            {
                ROS_ERROR_STREAM("Invalid covariance value for v_x: "
                                 << std::to_string(twist.covariance[0])
                                 << ". Ignoring measurement.");
            }
        } 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(twist.twist.linear.y);
            if (settings_.ins_vsm_ros_variances_by_parameter)
                std_y = string_utilities::trimDecimalPlaces(
                    settings_.ins_vsm_ros_variances[1]);
            else if (twist.covariance[7] > 0.0)
                std_y = string_utilities::trimDecimalPlaces(
                    std::sqrt(twist.covariance[7]));
            else
            {
                ROS_ERROR_STREAM("Invalid covariance value for v_y: "
                                 << std::to_string(twist.covariance[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(twist.twist.linear.z);
            if (settings_.ins_vsm_ros_variances_by_parameter)
                std_z = string_utilities::trimDecimalPlaces(
                    settings_.ins_vsm_ros_variances[2]);
            else if (twist.covariance[14] > 0.0)
                std_z = string_utilities::trimDecimalPlaces(
                    std::sqrt(twist.covariance[14]));
            else
            {
                ROS_ERROR_STREAM("Invalid covariance value for v_z: "
                                 << std::to_string(twist.covariance[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);
    }

protected:
    std::shared_ptr<ros::NodeHandle> pNh_;
    Settings settings_;
    virtual void sendVelocity(const std::string& velNmea) = 0;

private:
    std::unordered_map<std::string, ros::Publisher> topicMap_;
    uint32_t queueSize_ = 1;
    tf2_ros::TransformBroadcaster tf2Publisher_;
    ros::Subscriber odometrySubscriber_;
    ros::Subscriber twistSubscriber_;
    TimestampRos lastTfStamp_;
    tf2_ros::Buffer tfBuffer_;
    // tf listener
    tf2_ros::TransformListener tfListener_;
};