From a527d20943229884037045e0e68bf63c0d8544f8 Mon Sep 17 00:00:00 2001 From: Ryohei Sasaki Date: Sun, 7 Apr 2024 10:19:42 +0900 Subject: [PATCH 1/2] Add eagleye.repos (#329) --- README.md | 9 +++------ eagleye.repos | 17 +++++++++++++++++ 2 files changed, 20 insertions(+), 6 deletions(-) create mode 100644 eagleye.repos diff --git a/README.md b/README.md index 51aa0a6a..6887f285 100755 --- a/README.md +++ b/README.md @@ -40,18 +40,15 @@ Clone and Build MapIV's fork of [RTKLIB](https://github.com/MapIV/rtklib_ros_bri cd $HOME/RTKLIB/lib/iers/gcc/ make cd $HOME/RTKLIB/app/consapp - make + make ### ROS Packages Clone and build the necessary packages for Eagleye. ([rtklib_ros_bridge](https://github.com/MapIV/rtklib_ros_bridge/tree/ros2-v0.1.0), [nmea_ros_bridge](https://github.com/MapIV/nmea_ros_bridge/tree/ros2-v0.1.0)) cd $HOME/catkin_ws/src - git clone https://github.com/MapIV/eagleye.git -b main-ros2 --recursive - git clone https://github.com/MapIV/rtklib_ros_bridge.git -b ros2-v0.1.0 - git clone https://github.com/MapIV/llh_converter.git -b ros2 - git clone https://github.com/MapIV/nmea_ros_bridge.git -b ros2-v0.1.0 - git clone https://github.com/MapIV/gnss_compass_ros.git -b main-ros2 + git clone https://github.com/MapIV/eagleye.git -b main-ros2 + vcs import . < eagleye/eagleye.repos sudo apt-get install -y libgeographic-dev geographiclib-tools geographiclib-doc sudo geographiclib-get-geoids best sudo mkdir /usr/share/GSIGEO diff --git a/eagleye.repos b/eagleye.repos new file mode 100644 index 00000000..8e1d06b8 --- /dev/null +++ b/eagleye.repos @@ -0,0 +1,17 @@ +repositories: + rtklib_ros_bridge: + type: git + url: https://github.com/MapIV/rtklib_ros_bridge.git + version: ros2-v0.1.0 + llh_converter: + type: git + url: https://github.com/MapIV/llh_converter.git + version: ros2 + nmea_ros_bridge: + type: git + url: https://github.com/MapIV/nmea_ros_bridge.git + version: ros2-v0.1.0 + gnss_compass_ros: + type: git + url: https://github.com/MapIV/gnss_compass_ros.git + version: main-ros2 \ No newline at end of file From b2a10bde58f80e4730af957f56bc9ce31972497f Mon Sep 17 00:00:00 2001 From: Ryohei Sasaki Date: Mon, 22 Jul 2024 10:51:53 +0900 Subject: [PATCH 2/2] Add setentrio msgs for velocity source (#336) --- eagleye_rt/config/README.md | 2 +- eagleye_rt/config/eagleye_config.yaml | 2 +- eagleye_util/gnss_converter/CMakeLists.txt | 1 + eagleye_util/gnss_converter/package.xml | 5 ++- .../src/gnss_converter_node.cpp | 37 +++++++++++++++++++ 5 files changed, 44 insertions(+), 3 deletions(-) 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");