Nova gps update

Author: szepilot

launch/drivers/gps_nova.launch.py

@@ -41,8 +41,8 @@ def arg(name, default_value, description):
 def generate_launch_description():
 
     nova_node = Node(
-        package = PKG,
-        namespace = lx_nova_namespace,
+        package=PKG,
+        namespace=lx_nova_namespace,
         name='main',
         executable='novatel_oem7_driver_exe',
 
@@ -83,22 +83,31 @@ def generate_launch_description():
     )
 
     convert_node = Node(
-        package = "lexus_bringup",
-        namespace = lx_nova_namespace,
+        package="lexus_bringup",
+        namespace=lx_nova_namespace,
         name='gps_convert',
         executable='nova_oem7_to_tf',
         parameters=[
              {
-                   'use_sim_time': False,
-                   'x_coord_offset': -697237.0,
-                   'y_coord_offset': -5285644.0,
-                   'z_coord_exact_height': 1.9,
-                   'frame_id': 'map',
-                   'child_frame_id': 'lexus3/base_link',
-                   'z_coord_ref_switch': 'orig',
-                   'pose_topic_pub': 'current_pose',
-                   'utm_topic_sub': 'bestutm',
-                   'inspva_topic_sub': 'inspva'
+                'use_sim_time': False,
+                'x_coord_offset': -697237.0,  # map_gyor_0
+                # 'x_coord_offset': -639770.0, # map_zala_0
+                'y_coord_offset': -5285644.0,  # map_gyor_0
+                # 'y_coord_offset': -5195040.0, # map_zala_0
+                'z_coord_exact_height': 1.9,
+                'z_coord_offset_plus': -112.0,
+                'frame_id': 'map',
+                'child_frame_id': 'lexus3/base_link',
+                # z_coord_ref_switch can be exact / zero_based / orig / orig_offset
+                # exact: the Z coorindinate is always z_coord_exact_height param (must be set in this launch)
+                # zero_based: Z coordinate starts from 0 and relative
+                # orig: the original Z provided by sensor
+                # orig_offset: the original Z plus a fixed offset
+                'z_coord_ref_switch': 'orig_offset',
+                'pose_topic_pub': 'current_pose',
+                'utm_topic_sub': 'bestutm',
+                'inspva_topic_sub': 'inspva',
+                'debug_publish': False,
              }
         ]
     )

src/nova_oem7_to_tf.cpp

@@ -4,14 +4,17 @@
 #include <chrono>
 #include <memory>
 #include <string>
+#include <cmath>
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
 
 #include "tf2/LinearMath/Quaternion.h"
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include "tf2_ros/transform_broadcaster.h"
 #include "tf2_ros/buffer.h"
+#include "tf2/utils.h"
 
 #include "novatel_oem7_msgs/msg/bestutm.hpp"
 #include "novatel_oem7_msgs/msg/inspva.hpp"
@@ -28,26 +31,38 @@ class CurrentPoseFromTf : public rclcpp::Node
         this->declare_parameter<float>("x_coord_offset", -697237.0);
         this->declare_parameter<float>("y_coord_offset", -5285644.0);
         this->declare_parameter<float>("z_coord_exact_height", 1.9);
+        this->declare_parameter<float>("z_coord_offset_plus", -10.0);
         this->declare_parameter<std::string>("frame_id", "map");
         this->declare_parameter<std::string>("child_frame_id", "lexus3/base_link");
+        // z_coord_ref_switch can be exact / zero_based / orig / orig_offset
+        // exact: the Z coorindinate is always z_coord_exact_height param (must be set in this launch)
+        // zero_based: Z coordinate starts from 0 and relative
+        // orig: the original Z provided by the sensor
+        // orig_offset: the original Z plus a fixed offset
         this->declare_parameter<std::string>("z_coord_ref_switch", "orig");
         this->declare_parameter<std::string>("pose_topic_pub", "lexus3/gps/nova/current_pose");
         this->declare_parameter<std::string>("utm_topic_sub", "lexus3/gps/nova/bestutm");
         this->declare_parameter<std::string>("inspva_topic_sub", "lexus3/gps/nova/inspva");
+        this->declare_parameter<bool>("debug_publish", false);
 
         this->get_parameter("x_coord_offset", m_x_coord_offset_);
         this->get_parameter("y_coord_offset", m_y_coord_offset_);
         this->get_parameter("z_coord_exact_height", m_z_coord_exact_height_);
+        this->get_parameter("z_coord_offset_plus", m_z_coord_orig_offset_);
         this->get_parameter("frame_id", m_frame_id_);
         this->get_parameter("child_frame_id", m_child_frame_id_);
         this->get_parameter("z_coord_ref_switch", m_z_coord_ref_switch_);
         this->get_parameter("pose_topic_pub", m_pose_topic_);
         this->get_parameter("utm_topic_sub", m_utm_topic_);
         this->get_parameter("inspva_topic_sub", m_inspva_topic_);
+        this->get_parameter("debug_publish", debug_pub);
 
         m_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>(m_pose_topic_, 10);
         // additional publisher for translated pose
-        m_pose_pub2_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("current_pose2", 10); // TODO: this is only temporary
+        if (debug_pub)
+        {
+            m_pose_pub_debug_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("current_pose_debug_original", 10); // TODO: this is only temporary
+        }
         m_utm_sub_ = this->create_subscription<novatel_oem7_msgs::msg::BESTUTM>(m_utm_topic_, 10, std::bind(&CurrentPoseFromTf::utm_callback, this, _1));
         m_inspva_sub_ = this->create_subscription<novatel_oem7_msgs::msg::INSPVA>(m_inspva_topic_, 10, std::bind(&CurrentPoseFromTf::inspva_callback, this, _1));
         m_tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
@@ -60,17 +75,29 @@ class CurrentPoseFromTf : public rclcpp::Node
         {
             RCLCPP_INFO_STREAM(this->get_logger(), "nova exact height (z): " << m_z_coord_exact_height_);
         }
-        else
+        else if (m_z_coord_ref_switch_.compare("zero_based") == 0)
+        {
+            RCLCPP_INFO_STREAM(this->get_logger(), "nova zero based height (z)");
+        }
+        else if (m_z_coord_ref_switch_.compare("orig") == 0)
         {
             RCLCPP_INFO_STREAM(this->get_logger(), "nova original height (z)");
         }
+        else
+        {
+            RCLCPP_INFO_STREAM(this->get_logger(), "nova orig offset height (z): " << m_z_coord_orig_offset_);
+        }
     }
 
 private:
     // Callback function for the subscriber novatel_oem7_msgs
     void utm_callback(const novatel_oem7_msgs::msg::BESTUTM::SharedPtr msg)
     {
-
+        if (first_run_z_coord)
+        {
+            m_z_coord_start_ = msg->height;
+            first_run_z_coord = false;
+        }
         // create current pose
         m_currentPose.header.stamp = msg->header.stamp;
         m_currentPose.header.frame_id = m_frame_id_;
@@ -84,9 +111,17 @@ class CurrentPoseFromTf : public rclcpp::Node
         {
             m_currentPose.pose.position.z = m_z_coord_exact_height_;
         }
+        else if (m_z_coord_ref_switch_.compare("zero_based") == 0)
+        {
+            m_currentPose.pose.position.z = msg->height - m_z_coord_start_;
+        }
+        else if (m_z_coord_ref_switch_.compare("orig") == 0)
+        {
+            m_currentPose.pose.position.z = msg->height;
+        }
         else
         {
-            m_currentPose.pose.position.z = msg->height; // original height
+            m_currentPose.pose.position.z = msg->height + m_z_coord_orig_offset_;
         }
 
         geometry_msgs::msg::TransformStamped transformStamped;
@@ -108,13 +143,25 @@ class CurrentPoseFromTf : public rclcpp::Node
         // Publish tf
         m_tfBroadcaster_->sendTransform(transformStamped);
         // Publish the current pose
-        m_pose_pub_->publish(m_currentPose);
         // Publish a translated copy of the current pose on current_pose2
         geometry_msgs::msg::PoseStamped translated_pose = m_currentPose;
-        // Apply translation: x = x - 1.5, y = y + 0.2
-        translated_pose.pose.position.x = translated_pose.pose.position.x - 1.585;
-        translated_pose.pose.position.y = translated_pose.pose.position.y + 0.2755;
-        m_pose_pub2_->publish(translated_pose);
+        // Apply translation in the pose local frame.
+        const double yaw = tf2::getYaw(translated_pose.pose.orientation);
+
+        constexpr double offset_x = -1.585;   // local forward/backward
+        constexpr double offset_y = 0.2755;   // local left/right
+
+        translated_pose.pose.position.x += offset_x * std::cos(yaw) - offset_y * std::sin(yaw);
+        translated_pose.pose.position.y += offset_x * std::sin(yaw) + offset_y * std::cos(yaw);
+
+        // // Apply translation: x = x - 1.5, y = y + 0.2 Faulty logic
+        // translated_pose.pose.position.x = translated_pose.pose.position.x - 1.585;
+        // translated_pose.pose.position.y = translated_pose.pose.position.y + 0.2755;
+        if (debug_pub)
+        {
+            m_pose_pub_debug_->publish(m_currentPose);
+        }
+        m_pose_pub_->publish(translated_pose);
     }
 
     void inspva_callback(const novatel_oem7_msgs::msg::INSPVA::SharedPtr msg)
@@ -136,7 +183,7 @@ class CurrentPoseFromTf : public rclcpp::Node
     }
 
     rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr m_pose_pub_;
-    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr m_pose_pub2_;
+    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr m_pose_pub_debug_;
     rclcpp::Subscription<novatel_oem7_msgs::msg::BESTUTM>::SharedPtr m_utm_sub_;
     rclcpp::Subscription<novatel_oem7_msgs::msg::INSPVA>::SharedPtr m_inspva_sub_;
     std::shared_ptr<tf2_ros::TransformBroadcaster> m_tfBroadcaster_;
@@ -148,9 +195,13 @@ class CurrentPoseFromTf : public rclcpp::Node
     double m_x_coord_offset_ = 0.0;
     double m_y_coord_offset_ = 0.0;
     double m_z_coord_exact_height_ = 1.9;
+    double m_z_coord_start_ = 0.0;
+    double m_z_coord_orig_offset_ = 0.0;
     std::string m_pose_topic_ = "/lexus3/gps/nova/current_pose";
     std::string m_utm_topic_ = "/lexus3/nova/bestutm";
     std::string m_inspva_topic_ = "/lexus3/nova/inspva";
+    bool debug_pub = false;
+    bool first_run_z_coord = true;
 };
 
 int main(int argc, char **argv)