diff --git a/eagleye_rt/config/README.md b/eagleye_rt/config/README.md
index 7e0046c0..caae6846 100644
--- a/eagleye_rt/config/README.md
+++ b/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 |
diff --git a/eagleye_rt/config/eagleye_config.yaml b/eagleye_rt/config/eagleye_config.yaml
index fa858856..a6260ab2 100644
--- a/eagleye_rt/config/eagleye_config.yaml
+++ b/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
diff --git a/eagleye_util/gnss_converter/CMakeLists.txt b/eagleye_util/gnss_converter/CMakeLists.txt
index a3e0aec1..ecd20f84 100644
--- a/eagleye_util/gnss_converter/CMakeLists.txt
+++ b/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
diff --git a/eagleye_util/gnss_converter/package.xml b/eagleye_util/gnss_converter/package.xml
index 82374cc4..6c89bed7 100644
--- a/eagleye_util/gnss_converter/package.xml
+++ b/eagleye_util/gnss_converter/package.xml
@@ -13,6 +13,7 @@
sensor_msgs
nmea_msgs
ublox_msgs
+ septentrio_gnss_driver
rtklib_msgs
eagleye_coordinate
eagleye_navigation
@@ -22,6 +23,7 @@
sensor_msgs
nmea_msgs
ublox_msgs
+ septentrio_gnss_driver
rtklib_msgs
eagleye_coordinate
eagleye_navigation
@@ -31,12 +33,13 @@
sensor_msgs
nmea_msgs
ublox_msgs
+ septentrio_gnss_driver
rtklib_msgs
eagleye_coordinate
eagleye_navigation
tf2_ros
tf2_geometry_msgs
-
+
ament_lint_auto
ament_lint_common
diff --git a/eagleye_util/gnss_converter/src/gnss_converter_node.cpp b/eagleye_util/gnss_converter/src/gnss_converter_node.cpp
index 02a93e7e..741988be 100644
--- a/eagleye_util/gnss_converter/src/gnss_converter_node.cpp
+++ b/eagleye_util/gnss_converter/src/gnss_converter_node.cpp
@@ -2,6 +2,7 @@
#include "rclcpp/rclcpp.hpp"
#include "gnss_converter/nmea2fix.hpp"
#include
+#include
#include
#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::SharedPtr nmea_sentence_sub;
rclcpp::Subscription::SharedPtr navpvt_sub;
rclcpp::Subscription::SharedPtr gnss_velocity_sub;
+ rclcpp::Subscription::SharedPtr pvtgeodetic_sub;
rclcpp::Subscription::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(
velocity_source_topic, 1000, gnss_velocity_callback);
}
+ else if(velocity_source_type == 4)
+ {
+ pvtgeodetic_sub = node->create_subscription(
+ velocity_source_topic, 1000, pvtgeodetic_callback);
+ }
else
{
RCLCPP_ERROR(node->get_logger(),"Invalid velocity_source_type");