Add setentrio msgs for velocity source (#336)

Author: rsasaki0109

eagleye_rt/config/README.md

@@ -17,7 +17,7 @@ The parameters for estimation in Eagleye can be set in the `config/eagleye_confi
 | imu_topic                     | string | Topic name to be subscribed to in node (sensor_msgs/Imu.msg)            | /imu/data_raw            |
 | twist.twist_type                   | int | Topic type to be subscribed to in node (TwistStamped : 0, TwistWithCovarianceStamped: 1) | 0               |
 | twist.twist_topic                   | string | Topic name to be subscribed to in node | /can_twist               |
-| gnss.velocity_source_type              | int | Topic type to be subscribed to in node (rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, ublox_msgs/NavPVT: 2, geometry_msgs/TwistWithCovarianceStamped: 3)      | 0        |
+| gnss.velocity_source_type              | int | Topic type to be subscribed to in node (rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, ublox_msgs/NavPVT: 2, geometry_msgs/TwistWithCovarianceStamped: 3, septentrio_gnss_driver/PVTGeodetic: 4)      | 0        |
 | gnss.velocity_source_topic              | string | Topic name to be subscribed to in node      | /rtklib_nav        |
 | gnss.llh_source_type              | int | Topic type to be subscribed to in node (rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2)      | 0        |
 | gnss.llh_source_topic              | string | Topic name to be subscribed to in node   | /rtklib_nav        |

eagleye_rt/config/eagleye_config.yaml

@@ -10,7 +10,7 @@
       twist_topic: /can_twist
     imu_topic: /imu/data_raw
     gnss:
-      velocity_source_type: 0 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, ublox_msgs/NavPVT: 2, geometry_msgs/TwistWithCovarianceStamped: 3
+      velocity_source_type: 0 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, ublox_msgs/NavPVT: 2, geometry_msgs/TwistWithCovarianceStamped: 3, septentrio_gnss_driver/PVTGeodetic: 4
       velocity_source_topic: /rtklib_nav
       llh_source_type: 0 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2
       llh_source_topic:  /rtklib_nav

eagleye_util/gnss_converter/CMakeLists.txt

@@ -23,6 +23,7 @@ find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
 include_directories(include)
+find_package(Boost REQUIRED COMPONENTS system thread regex chrono)
 
 ament_auto_add_executable(gnss_converter
   src/nmea2fix_core.cpp

eagleye_util/gnss_converter/package.xml

@@ -13,6 +13,7 @@
   <build_depend>sensor_msgs</build_depend>
   <build_depend>nmea_msgs</build_depend>
   <build_depend>ublox_msgs</build_depend>
+  <build_depend>septentrio_gnss_driver</build_depend>
   <build_depend>rtklib_msgs</build_depend>
   <build_depend>eagleye_coordinate</build_depend>
   <build_depend>eagleye_navigation</build_depend>
@@ -22,6 +23,7 @@
   <build_export_depend>sensor_msgs</build_export_depend>
   <build_export_depend>nmea_msgs</build_export_depend>
   <build_export_depend>ublox_msgs</build_export_depend>
+  <build_export_depend>septentrio_gnss_driver</build_export_depend>
   <build_export_depend>rtklib_msgs</build_export_depend>
   <build_export_depend>eagleye_coordinate</build_export_depend>
   <build_export_depend>eagleye_navigation</build_export_depend>
@@ -31,12 +33,13 @@
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>nmea_msgs</exec_depend>
   <exec_depend>ublox_msgs</exec_depend>
+  <exec_depend>septentrio_gnss_driver</exec_depend>
   <exec_depend>rtklib_msgs</exec_depend>
   <exec_depend>eagleye_coordinate</exec_depend>
   <exec_depend>eagleye_navigation</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
   <exec_depend>tf2_geometry_msgs</exec_depend>
-  
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

eagleye_util/gnss_converter/src/gnss_converter_node.cpp

@@ -2,6 +2,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "gnss_converter/nmea2fix.hpp"
 #include <ublox_msgs/msg/nav_pvt.hpp>
+#include <septentrio_gnss_driver/msg/pvt_geodetic.hpp>
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 #include "eagleye_coordinate/eagleye_coordinate.hpp"
 #include "eagleye_navigation/eagleye_navigation.hpp"
@@ -107,6 +108,36 @@ void navpvt_callback(const ublox_msgs::msg::NavPVT::ConstSharedPtr msg)
   rtklib_nav_pub->publish(r);
 }
 
+void pvtgeodetic_callback(const septentrio_gnss_driver::msg::PVTGeodetic::ConstSharedPtr msg)
+{
+  rtklib_msgs::msg::RtklibNav r;
+  r.header.frame_id = "gps";
+  r.header.stamp = msg->header.stamp;
+  if (nav_msg_ptr != nullptr)
+    r.status = *nav_msg_ptr;
+  r.tow = msg->block_header.tow;
+
+  double llh[3];
+  llh[0] = msg->latitude;
+  llh[1] = msg->longitude;
+  llh[2] = msg->height;
+  double ecef_pos[3];
+  llh2xyz(llh, ecef_pos);
+
+  double enu_vel[3] = {msg->ve, msg->vn, msg->vu};
+  double ecef_vel[3];
+  enu2xyz_vel(enu_vel, ecef_pos, ecef_vel);
+
+  r.ecef_pos.x = ecef_pos[0];
+  r.ecef_pos.y = ecef_pos[1];
+  r.ecef_pos.z = ecef_pos[2];
+  r.ecef_vel.x = ecef_vel[0];
+  r.ecef_vel.y = ecef_vel[1];
+  r.ecef_vel.z = ecef_vel[2];
+
+  rtklib_nav_pub->publish(r);
+}
+
 void gnss_velocity_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr msg)
 {
   if(msg->twist.covariance[0] > twist_covariance_thresh)
@@ -161,6 +192,7 @@ int main(int argc, char** argv)
   rclcpp::Subscription<nmea_msgs::msg::Sentence>::SharedPtr nmea_sentence_sub;
   rclcpp::Subscription<ublox_msgs::msg::NavPVT>::SharedPtr navpvt_sub;
   rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr gnss_velocity_sub;
+  rclcpp::Subscription<septentrio_gnss_driver::msg::PVTGeodetic>::SharedPtr pvtgeodetic_sub;
   rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr navsatfix_sub;
 
   node->declare_parameter("is_sub_antenna",is_sub_antenna);
@@ -228,6 +260,11 @@ int main(int argc, char** argv)
       gnss_velocity_sub = node->create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
           velocity_source_topic, 1000, gnss_velocity_callback);
     }
+    else if(velocity_source_type == 4)
+    {
+      pvtgeodetic_sub = node->create_subscription<septentrio_gnss_driver::msg::PVTGeodetic>(
+        velocity_source_topic, 1000, pvtgeodetic_callback);
+    }
     else
     {
       RCLCPP_ERROR(node->get_logger(),"Invalid velocity_source_type");