How to get Odometry messages out of this driver ?

[chrissunny94]

I want the following

• Sensor_msgs/Odometry , i am able to compute the Pose from lat, long and altitude .
• I have issues with the Orientation (quaternion )

[chrissunny94]

Has anyone accumulated pointcloud data from a 3D lidar (such as VLP16) along with the 3DOF Pose from the UBLOX Gnss + IMU fusion .

I want to understand how accurate this was .

[tomlogan501]

You mean by using a SLAM to compare on a relative level ?

[chrissunny94]

Yes kind of SLAM , but using the Pointcloud for only building the map (and not for odometry )

The odometry output should be from GNSS/IMU fusion .

[chrissunny94]

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include "sensor_msgs/msg/imu.hpp"

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <gps_tools/conversions.h>
#include <ublox_msgs/msg/hnr_pvt.hpp>
#include <ublox_msgs/msg/nav_att.hpp>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"


using std::placeholders:: _1;

using namespace std::chrono_literals;

/* This example creates a subclass of Node and uses std::bind() to register a
* member function as a callback from the timer. */


/**
   * Get Geometry (UTM) point from GNSS position, assuming zero origin.
   */
  void UTMPointFromGnss(
          geometry_msgs::msg::Point& pt,
          double lat,
          double lon,
          double hgt)
  {
    pt.z = hgt;

    std::string zone; //unused
    gps_tools::LLtoUTM(lat, lon, pt.y, pt.x, zone);
  }


/***
   * Converts radians to degrees
   *
   * @return degrees
   */
  inline double radiansToDegrees(double radians)
  {
    return radians * 180.0 / M_PI;
  }

  /***
   * Converts degrees to Radians
   *
   * @return radians
   */
  inline double degreesToRadians(double degrees)
  {
    return degrees * M_PI / 180.0;
  }


double latitude , longitude , altitude , roll , pitch , azimuth;
nav_msgs::msg::Odometry odometry;
tf2::Quaternion Z90_DEG_ROTATION; ///< Rotate ENU to ROS frames.
    
class MinimalPublisher : public rclcpp::Node
{
  public:
    
    MinimalPublisher()
    : Node("minimal_publisher"), count_(0)
    {
      publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
      timer_ = this->create_wall_timer(
      500ms, std::bind(&MinimalPublisher::timer_callback, this));
      

      //publishers
      odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("/ublox/odom", 10);

      //Subscribers
      navatt_sub_ = this->create_subscription<ublox_msgs::msg::NavATT>(
        "/navatt",10,std::bind(&MinimalPublisher::sub_navatt, this,_1));
      navsatfix_subscription_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
        "/ublox_gps_node/fix",10,std::bind(&MinimalPublisher::sub_navfix, this,_1));

      subscription_ = this->create_subscription<std_msgs::msg::String>(
      "topic", 10, std::bind(&MinimalPublisher::topic_callback, this, _1));

      tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
      
    }

    void sub_navatt(const ublox_msgs::msg::NavATT::SharedPtr msg) const{
      roll = msg->roll/100000;
      pitch = msg->pitch/100000;
      azimuth = msg->heading/100000;
    }

    void sub_navfix(const sensor_msgs::msg::NavSatFix::SharedPtr msg) const{
      odometry.header.stamp.sec = msg->header.stamp.sec;
      odometry.header.stamp.nanosec = msg->header.stamp.nanosec;
      odometry.child_frame_id = msg->header.frame_id;
      odometry.header.frame_id = "map";
      //odometry.header.frame_id = msg->header.stamp;
      latitude = msg->latitude;
      longitude = msg->longitude;
      altitude = msg->altitude;
      //altitude = 0;
    }

    void sub_fix(const ublox_msgs::msg::NavATT::SharedPtr msg) const{
      std::cout<<"blah";
    }
    void topic_callback(const std_msgs::msg::String::SharedPtr msg) const
    {
      RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
    }

    void publishOdometry()
    {
      // latitude = 12.9631760;
      // longitude = 77.7200422;
      // altitude = 857.372;
      
      // roll = 209837/100000;
      // pitch = 915057/100000;
      // azimuth = 26930306/100000;
      RCLCPP_INFO(this->get_logger(), "ROLL_degree: %f , PITCH_degree: %f , YAW_degree: %f ", roll, pitch , azimuth);
      double ROLL_radian = degreesToRadians(-roll);
      double PITCH_radian = degreesToRadians(-pitch);
      double AZIMUTH_radian = degreesToRadians(-azimuth);
      RCLCPP_INFO(this->get_logger(), "ROLL_radian: %f , PITCH_radian: %f , YAW_radian: %f ", ROLL_radian, PITCH_radian , AZIMUTH_radian);
      

      
      UTMPointFromGnss(
            odometry.pose.pose.position,
            latitude,
            longitude,
            altitude);

        // odometry->pose.covariance[ 0] = gpsfix_->position_covariance[0];
        // odometry->pose.covariance[ 7] = gpsfix_->position_covariance[4];
        // odometry->pose.covariance[14] = gpsfix_->position_covariance[8];
      
        // INSPVA uses 'y-forward' ENU orientation;
        // ROS uses x-forward orientation.

        //roll = pitch = azimuth = 0;
        tf2::Quaternion enu_orientation;
        enu_orientation.setRPY( ROLL_radian , PITCH_radian , AZIMUTH_radian);

        Z90_DEG_ROTATION.setRPY(0, 0, degreesToRadians(90.0));
        tf2::Quaternion ros_orientation = Z90_DEG_ROTATION * enu_orientation;
        odometry.pose.pose.orientation = tf2::toMsg(ros_orientation);
        // odometry.pose.pose.orientation = tf2::toMsg(enu_orientation);

        //tf2::Transform velocity_transform(enu_orientation);
        //tf2::Vector3 local_frame_velocity = velocity_transform.inverse()(tf2::Vector3(inspva_->east_velocity, inspva_->north_velocity, inspva_->up_velocity));

        // FIXME
        //tf2::convert(local_frame_velocity, odometry->twist.twist.linear);
        // odometry->twist.twist.linear.x = local_frame_velocity.getX();
        // odometry->twist.twist.linear.y = local_frame_velocity.getY();
        // odometry->twist.twist.linear.z = local_frame_velocity.getZ();

        // odometry->pose.covariance[21] = std::pow(inspvax_->roll_stdev,      2);
        // odometry->pose.covariance[28] = std::pow(inspvax_->pitch_stdev,     2);
        // odometry->pose.covariance[35] = std::pow(inspvax_->azimuth_stdev,   2);

        // odometry->twist.covariance[0]  = std::pow(inspvax_->north_velocity_stdev, 2);
        // odometry->twist.covariance[7]  = std::pow(inspvax_->east_velocity_stdev,  2);
        // odometry->twist.covariance[14] = std::pow(inspvax_->up_velocity_stdev,    2);
      

      RCLCPP_INFO(this->get_logger(), "x: %f , y: %f , z: %f",odometry.pose.pose.position.x,odometry.pose.pose.position.y,odometry.pose.pose.position.z);
      RCLCPP_INFO(this->get_logger(), "rx: %f , ry: %f , rz: %f , rw: %f",odometry.pose.pose.orientation.x,odometry.pose.pose.orientation.y,odometry.pose.pose.orientation.z , odometry.pose.pose.orientation.w);
      //odometry.header = imu_.header;
      odom_pub_->publish(odometry);

      geometry_msgs::msg::TransformStamped t;

    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "map";
    t.child_frame_id = "gps";

    t.transform.translation.x = odometry.pose.pose.position.x;
    t.transform.translation.y = odometry.pose.pose.position.y;
    t.transform.translation.z = odometry.pose.pose.position.z;
    t.transform.rotation.x = odometry.pose.pose.orientation.x;
    t.transform.rotation.y = odometry.pose.pose.orientation.y;
    t.transform.rotation.z = odometry.pose.pose.orientation.z;
    t.transform.rotation.w = odometry.pose.pose.orientation.w;

    tf_static_broadcaster_->sendTransform(t);

    }

  private:
    void timer_callback()
    {
      auto message = std_msgs::msg::String();
      message.data = "Hello, world! " + std::to_string(count_++);
      RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
      publisher_->publish(message);
      publishOdometry();
    }
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
    rclcpp::Subscription<ublox_msgs::msg::NavATT>::SharedPtr navatt_sub_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr navsatfix_subscription_;
    sensor_msgs::msg::Imu imu_;
    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
    //for Odometry Publisher
    
    size_t count_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalPublisher>());
  rclcpp::shutdown();
  return 0;
}