From 92ae3d24bf66be11b31f8ddac3ac8458d8b9f5f6 Mon Sep 17 00:00:00 2001 From: Ryohei Sasaki Date: Wed, 3 Apr 2024 09:32:40 +0900 Subject: [PATCH] v1.7.2-ros2 (#327) * Add static initilization skip mode (#271) * Add covariance in nmea2fix (#272) * Galactit build (#276) * Add velocity subscriver in slip_angle_node (#278) * fixed stop judgment bug (#290) * Change of coordinate system for Eagleye's standard input IMU (#296) * Revert "Change of coordinate system for Eagleye's standard input IMU (#296)" (#297) This reverts commit 03f9e1f627b0d785a79e6815397c5da449355630. * Change of coordinate system for Eagleye's standard input IMU (#299) * Change of coordinate system for Eagleye's standard input IMU * Correction of Response * fix compile warning (#305) * add missing dependencies in package.xml (#306) * Fix height in eagleye_fix (#307) * Add architecture svg (#308) * S^Cit fix2pose into two separate nodes (#316) * [ROS2]Fixed heading estimation bug in multi-antenna mode at standstill. (#322) * Fixed heading estimation bug in multi-antenna mode at standstill. * Add comment about is_first_correction_velocity in multi-antenna mode --------- Co-authored-by: Aoki-Takanose <49303804+Aoki-Takanose@users.noreply.github.com> --- README.md | 17 +- docs/eagleye_architecture.drawio.svg | 4 + eagleye_core/navigation/CMakeLists.txt | 4 + .../src/angular_velocity_offset_stop.cpp | 2 +- eagleye_core/navigation/src/position.cpp | 37 +- .../navigation/src/rtk_dead_reckoning.cpp | 25 -- .../navigation/src/yaw_rate_offset_stop.cpp | 2 +- eagleye_rt/CMakeLists.txt | 4 + eagleye_rt/config/README.md | 2 + eagleye_rt/config/eagleye_config.yaml | 4 +- eagleye_rt/launch/eagleye_rt.launch.xml | 4 +- eagleye_rt/src/heading_node.cpp | 38 +- eagleye_rt/src/rolling_node.cpp | 10 +- eagleye_rt/src/rtk_heading_node.cpp | 4 +- eagleye_rt/src/slip_angle_node.cpp | 5 +- eagleye_rt/src/slip_coefficient_node.cpp | 2 +- eagleye_rt/src/smoothing_node.cpp | 47 ++- eagleye_rt/src/tf_converted_imu.cpp | 24 +- eagleye_rt/src/trajectory_node.cpp | 2 +- eagleye_rt/src/twist_relay.cpp | 8 +- eagleye_rt/src/velocity_estimator_node.cpp | 6 +- eagleye_rt/src/yaw_rate_offset_node.cpp | 2 +- eagleye_util/fix2pose/src/fix2pose.cpp | 348 ------------------ .../geo_pose_converter/CMakeLists.txt | 40 ++ .../launch/geo_pose_converter.launch.xml} | 18 +- .../package.xml | 11 +- .../src/geo_pose_converter.cpp | 245 ++++++++++++ .../CMakeLists.txt | 6 +- .../launch/geo_pose_fusion.launch.xml | 20 + eagleye_util/geo_pose_fusion/package.xml | 31 ++ .../geo_pose_fusion/src/geo_pose_fusion.cpp | 165 +++++++++ eagleye_util/gnss_converter/CMakeLists.txt | 4 + .../gnss_converter/src/nmea2fix_core.cpp | 6 + 33 files changed, 670 insertions(+), 477 deletions(-) create mode 100644 docs/eagleye_architecture.drawio.svg delete mode 100644 eagleye_util/fix2pose/src/fix2pose.cpp create mode 100644 eagleye_util/geo_pose_converter/CMakeLists.txt rename eagleye_util/{fix2pose/launch/fix2pose.launch.xml => geo_pose_converter/launch/geo_pose_converter.launch.xml} (50%) rename eagleye_util/{fix2pose => geo_pose_converter}/package.xml (81%) create mode 100644 eagleye_util/geo_pose_converter/src/geo_pose_converter.cpp rename eagleye_util/{fix2pose => geo_pose_fusion}/CMakeLists.txt (87%) create mode 100644 eagleye_util/geo_pose_fusion/launch/geo_pose_fusion.launch.xml create mode 100644 eagleye_util/geo_pose_fusion/package.xml create mode 100644 eagleye_util/geo_pose_fusion/src/geo_pose_fusion.cpp diff --git a/README.md b/README.md index f195fa7e..51aa0a6a 100755 --- a/README.md +++ b/README.md @@ -12,6 +12,10 @@ Eagleye is an open-source software for vehicle localization utilizing GNSS and I ![Flowchart of Eagleye](docs/flowchart.png) +## Architecture + +![Architecture of Eagleye](docs/eagleye_architecture.drawio.svg) + ## Recommended Sensors **GNSS receiver** * [Septentrio Mosaic development kit with GNSS antenna](https://shop.septentrio.com/en/shop/mosaic-x5-devkit) @@ -76,6 +80,16 @@ Access mosaic's web ui and upload the following file in Admin/Configuration. https://www.dropbox.com/s/uckt9 +### IMU + +1. IMU settings. + +* Output rate 50Hz + +2. Please be careful with the coordinate system when using the [tamagawa ros driver](https://github.com/MapIV/tamagawa_imu_driver) created by MAP IV. If the x-direction indicated on the IMU is set to the vehicle's direction and the y-direction to the right side of the vehicle, please set the `roll` in the `eagleye/eagleye_util/tf/config/sensors_tf.yaml file` to `3.14159`. + + roll: 3.14159 + ### Eagleye parameters The parameters of eagleye can be set in the [eagleye_config.yaml](https://github.com/MapIV/eagleye/tree/ros2-galactic-v1.1.5/eagleye_rt/config/eagleye_config.yaml). The default settings are 5Hz for GNSS and 50Hz for IMU. @@ -157,7 +171,8 @@ To visualize the eagleye output location /eagleye/fix, for example, use the foll To convert from eagleye/fix to eagleye/pose, use the following command　 - ros2 launch eagleye_fix2pose fix2pose.xml + ros2 launch eagleye_geo_pose_fusion geo_pose_fusion.launch.xml + ros2 launch eagleye_geo_pose_converter geo_pose_converter.launch.xml ## Sample data ### ROSBAG(ROS1) diff --git a/docs/eagleye_architecture.drawio.svg b/docs/eagleye_architecture.drawio.svg new file mode 100644 index 00000000..936a36bd --- /dev/null +++ b/docs/eagleye_architecture.drawio.svg @@ -0,0 +1,4 @@ + + + +
/smoothing_node
/smoothing_node
/velocity_scale_factor
/velocity_scale_fact...

rtk
rtk
gnss
gnss
imu
imu
/yawrate_offset_stop
/yawrate_offset_stop
twist
twist
/position_interpolate
/position_interpolate
/yawrate_offset_1st
/yawrate_offset_1st
/velocity
/velocity_scale_factor
/velocity...
/position
/position
/enu_absolute_pos
/enu_absolute_pos
/trajectory
/trajectory
/enu_relative_pos
/enu_vel
/twist
/enu_relative_pos...
/heading_2nd
/heading_2nd
/heading_2nd
/heading_2nd
/heading_interpolate_2nd
/heading_interpolate_2nd
/heading_interpolate_2nd
/heading_interpolate...
/yawrate_offset_1st
/yawrate_offset_1st
/heading_interpolate_3rd
/heading_interpolate_3rd
/heading_interpolate_3rd
/heading_interpolate...
/yawrawte_offset_2nd
/yawrawte_offset_2nd
/yawrate_offset_2nd
/yawrate_offset_2nd
/heading_interpolate_1st
/heading_interpolate_1st
/heading_interpolate_1st
/heading_interpolate...
/heading_1st
/heading_1st
/heading_1st
/heading_1st
/heading_node_3rd
/heading_node_3rd

YawrateOffsetStop

Output the average angular velocity recorded in a section where the speed of the vehicle is below a certain value.

YawrateOffsetStop...

VelocityScaleFactor

Estimating the wheel speed scale factor using GNSS Doppler velocity during straight-line driving.

VelocityScaleFactor...

Height

Using RTK FIX solutions and x-direction acceleration, estimate acceleration errors, eliminate mis-FIX, and estimate height. Estimate the pitch angle as well.

Height...

SlipAngle

To calculate the slip angle, multiply the yaw rate by the velocity and then multiply the result by the coefficient previously determined by slip_coefficient_node.

It is necessary for velocity_scale_factor, yawrate_offset_stop, and yawrate_offset_2nd to be activated.

By default, these are deactivated.

SlipAngle...

Distance

Calculate the driving distance

Distance...

Trajectory

Calculate the relative trajectory on the ENU coordinate system using heading_interpolate_3rd, angular velocity, and wheel speed


TrajectoryCalculate the relative trajectory on th...

Position

Integrating the absolute position obtained from GNSS with the relative travel trajectory, eliminating outliers caused by multipath, to estimate a high-precision absolute position.

Position...

Smoothing

Averaging the GNSS position while taking into account the speed. The output is only the position. Only the Z value will be used in the subsequent stages.

Smoothing...

PositionInterpolate

Due to time-consuming processes in the earlier stages, position delay is ensured through extrapolation.

PositionInterpolate...

Heading

When moving in a straight line, eliminate observations where the difference between the integrated yaw rate and the azimuth derived from the Doppler speed is too significant. A suitable heading is then outputted. Without estimating YawrateOffsetStop, heading estimation is not possible.


Heading...

HeadingInterpolate

Due to time-consuming processes in the earlier stages, heading delay is ensured through extrapolation.

HeadingInterpolate...

YawrateOffset

Using the least squares method to estimate yawrate_offset. If the estimated value is abnormally large, it is overwritten with yawrate_offset_stop for output.
YawrateOffset...
/slip_angle_node
/slip_angle_node
imu
imu
imu
imu
/distance_node
/distance_node
/height_node
/height_node
gnss
gnss
imu
imu
twist
twist
imu
imu
imu
imu
/fix2pose
/fix2pose
Text
Text
/pose
/pose_with_covanraice
/pose...

Fix2Pose

Output pose information from latitude-longitude-height data and rpy information.

Fix2Pose...
twist
twist
/fix
/enu_absolute_pos_interpolate
/fix...
/yawrate_offset_stop
/yawrate_offset_stop
gnss
gnss

gnss
gnss
/gnss_smooth_pos_enu
/gnss_smooth_pos_enu
/height
/pitching
/height...
/distance
/distance
/slip_angle
/slip_angleText is not SVG - cannot display \ No newline at end of file diff --git a/eagleye_core/navigation/CMakeLists.txt b/eagleye_core/navigation/CMakeLists.txt index 5a45e60c..fa609835 100755 --- a/eagleye_core/navigation/CMakeLists.txt +++ b/eagleye_core/navigation/CMakeLists.txt @@ -15,6 +15,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") # add_compile_options(-Wall -Wextra -Wpedantic) endif() +if($ENV{ROS_DISTRO} STREQUAL "galactic") + add_definitions(-DROS_DISTRO_GALACTIC) +endif() + # find dependencies find_package(ament_cmake_auto REQUIRED) find_package(eagleye_coordinate REQUIRED) diff --git a/eagleye_core/navigation/src/angular_velocity_offset_stop.cpp b/eagleye_core/navigation/src/angular_velocity_offset_stop.cpp index 9e47db81..81f3d7f7 100755 --- a/eagleye_core/navigation/src/angular_velocity_offset_stop.cpp +++ b/eagleye_core/navigation/src/angular_velocity_offset_stop.cpp @@ -70,7 +70,7 @@ void angular_velocity_offset_stop_estimate(const geometry_msgs::msg::TwistStampe angular_velocity_stop_status->yaw_rate_buffer.erase(angular_velocity_stop_status->yaw_rate_buffer.begin()); } - if (velocity.twist.linear.x < angular_velocity_stop_parameter.stop_judgment_threshold) + if (std::abs(velocity.twist.linear.x) < angular_velocity_stop_parameter.stop_judgment_threshold) { ++angular_velocity_stop_status->stop_count; } diff --git a/eagleye_core/navigation/src/position.cpp b/eagleye_core/navigation/src/position.cpp index 7eaf78ca..9e672719 100755 --- a/eagleye_core/navigation/src/position.cpp +++ b/eagleye_core/navigation/src/position.cpp @@ -60,42 +60,7 @@ void position_estimate_(geometry_msgs::msg::TwistStamped velocity,eagleye_msgs:: enu_pos[1] = position_status->enu_pos[1]; enu_pos[2] = position_status->enu_pos[2]; - if(!position_status->gnss_update_failure) - { - gnss_status = true; - - geometry_msgs::msg::PoseStamped pose; - - pose.pose.position.x = enu_pos[0]; - pose.pose.position.y = enu_pos[1]; - pose.pose.position.z = enu_pos[2]; - - heading_interpolate_3rd.heading_angle = fmod(heading_interpolate_3rd.heading_angle,2*M_PI); - tf2::Transform transform; - tf2::Quaternion q; - transform.setOrigin(tf2::Vector3(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)); - q.setRPY(0, 0, (90* M_PI / 180)-heading_interpolate_3rd.heading_angle); - transform.setRotation(q); - - tf2::Transform transform2, transform3; - tf2::Quaternion q2(position_parameter.tf_gnss_rotation_x,position_parameter.tf_gnss_rotation_y, - position_parameter.tf_gnss_rotation_z,position_parameter.tf_gnss_rotation_w); - transform2.setOrigin(tf2::Vector3(position_parameter.tf_gnss_translation_x, position_parameter.tf_gnss_translation_y, - position_parameter.tf_gnss_translation_z)); - transform2.setRotation(q2); - transform3 = transform * transform2.inverse(); - - tf2::Vector3 tmp_pos; - tmp_pos = transform3.getOrigin(); - - enu_pos[0] = tmp_pos.getX(); - enu_pos[1] = tmp_pos.getY(); - enu_pos[2] = tmp_pos.getZ(); - } - else - { - gnss_status = false; - } + gnss_status = !position_status->gnss_update_failure; if (heading_interpolate_3rd.status.estimate_status == true && velocity_status.status.enabled_status == true) { diff --git a/eagleye_core/navigation/src/rtk_dead_reckoning.cpp b/eagleye_core/navigation/src/rtk_dead_reckoning.cpp index 05cf1bf9..649d2206 100755 --- a/eagleye_core/navigation/src/rtk_dead_reckoning.cpp +++ b/eagleye_core/navigation/src/rtk_dead_reckoning.cpp @@ -64,31 +64,6 @@ void rtk_dead_reckoning_estimate_(geometry_msgs::msg::Vector3Stamped enu_vel, nm llh2xyz(llh_rtk,ecef_rtk); xyz2enu(ecef_rtk,ecef_base_pos,enu_rtk); - geometry_msgs::msg::PoseStamped pose; - - pose.pose.position.x = enu_rtk[0]; - pose.pose.position.y = enu_rtk[1]; - pose.pose.position.z = enu_rtk[2]; - - heading.heading_angle = fmod(heading.heading_angle,2*M_PI); - tf2::Transform transform; - tf2::Quaternion q; - transform.setOrigin(tf2::Vector3(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)); - q.setRPY(0, 0, (90* M_PI / 180)-heading.heading_angle); - transform.setRotation(q); - - tf2::Transform transform2; - tf2::Quaternion q2(rtk_dead_reckoning_parameter.tf_gnss_rotation_x,rtk_dead_reckoning_parameter.tf_gnss_rotation_y,rtk_dead_reckoning_parameter.tf_gnss_rotation_z,rtk_dead_reckoning_parameter.tf_gnss_rotation_w); - transform2.setOrigin(transform*tf2::Vector3(-rtk_dead_reckoning_parameter.tf_gnss_translation_x, -rtk_dead_reckoning_parameter.tf_gnss_translation_y,-rtk_dead_reckoning_parameter.tf_gnss_translation_z)); - transform2.setRotation(transform*q2); - - tf2::Vector3 tmp_pos; - tmp_pos = transform2.getOrigin(); - - enu_rtk[0] = tmp_pos.getX(); - enu_rtk[1] = tmp_pos.getY(); - enu_rtk[2] = tmp_pos.getZ(); - if (rtk_dead_reckoning_status->position_stamp_last != gga_time && gga.gps_qual == 4) { rtk_dead_reckoning_status->provisional_enu_pos_x = enu_rtk[0]; diff --git a/eagleye_core/navigation/src/yaw_rate_offset_stop.cpp b/eagleye_core/navigation/src/yaw_rate_offset_stop.cpp index cc6da583..2eff6e31 100755 --- a/eagleye_core/navigation/src/yaw_rate_offset_stop.cpp +++ b/eagleye_core/navigation/src/yaw_rate_offset_stop.cpp @@ -59,7 +59,7 @@ void yaw_rate_offset_stop_estimate(const geometry_msgs::msg::TwistStamped veloci yaw_rate_offset_stop_status->yaw_rate_buffer.erase(yaw_rate_offset_stop_status->yaw_rate_buffer.begin()); } - if (velocity.twist.linear.x < yaw_rate_offset_stop_parameter.stop_judgment_threshold) + if (std::abs(velocity.twist.linear.x) < yaw_rate_offset_stop_parameter.stop_judgment_threshold) { ++yaw_rate_offset_stop_status->stop_count; } diff --git a/eagleye_rt/CMakeLists.txt b/eagleye_rt/CMakeLists.txt index 37a85a04..c4664583 100644 --- a/eagleye_rt/CMakeLists.txt +++ b/eagleye_rt/CMakeLists.txt @@ -10,6 +10,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +if($ENV{ROS_DISTRO} STREQUAL "galactic") + add_definitions(-DROS_DISTRO_GALACTIC) +endif() + # find dependencies find_package(ament_cmake_auto REQUIRED) find_package(PkgConfig REQUIRED) diff --git a/eagleye_rt/config/README.md b/eagleye_rt/config/README.md index 4ffa5933..7e0046c0 100644 --- a/eagleye_rt/config/README.md +++ b/eagleye_rt/config/README.md @@ -111,6 +111,8 @@ Figure shows the relationship between these parameters. | outlier_ratio_threshold | double | Ratio of allowable outliers in the interval (Value from 0~1) | 0.5 | | curve_judgment_threshold | double | Yaw rate threshold for curve determination [rad/s] | 0.0873 (5 deg/s) | | init_STD | double | Standard deviation of Doppler azimuth angle [rad] | 0.0035 (0.2 deg) | +| skip_static_initialization | bool | Whether to skip initial static estimation of yaw rate offset | false | +| yaw_rate_offset_stop_in_skip_mode | double | Initial yaw rate offset stop in skip mode [rad/sec] | 0.0 | ### heading_interpolate diff --git a/eagleye_rt/config/eagleye_config.yaml b/eagleye_rt/config/eagleye_config.yaml index 398e2fa2..fa858856 100644 --- a/eagleye_rt/config/eagleye_config.yaml +++ b/eagleye_rt/config/eagleye_config.yaml @@ -31,8 +31,6 @@ z : 0.0 use_ecef_base_position : false - reverse_imu_wz: false - # Eagleye Navigation Parameters # Basic Navigation Functions common: @@ -72,6 +70,8 @@ outlier_ratio_threshold: 0.5 curve_judgment_threshold: 0.0873 init_STD: 0.0035 #[rad] (= 0.2 [deg]) + skip_static_initialization: false + yaw_rate_offset_stop_in_skip_mode: 0.0 heading_interpolate: sync_search_period: 2 diff --git a/eagleye_rt/launch/eagleye_rt.launch.xml b/eagleye_rt/launch/eagleye_rt.launch.xml index 8408db42..ec9d50d8 100644 --- a/eagleye_rt/launch/eagleye_rt.launch.xml +++ b/eagleye_rt/launch/eagleye_rt.launch.xml @@ -145,7 +145,9 @@ - + + + diff --git a/eagleye_rt/src/heading_node.cpp b/eagleye_rt/src/heading_node.cpp index 3a8734f5..3543c877 100644 --- a/eagleye_rt/src/heading_node.cpp +++ b/eagleye_rt/src/heading_node.cpp @@ -55,6 +55,9 @@ static bool use_multi_antenna_mode; bool is_first_correction_velocity = false; +bool skip_static_initialization = false; +double yaw_rate_offset_stop_in_skip_mode = 0.0; + std::string node_name = "eagleye_heading"; void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) @@ -65,6 +68,8 @@ void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { velocity = *msg; + // To avoid unnecessary buffering when it's just sitting there right after start-up, we're making it so it doesn't buffer. + // Multi-antenna mode is an exception. if (is_first_correction_velocity == false && msg->twist.linear.x > heading_parameter.moving_judgment_threshold) { is_first_correction_velocity = true; @@ -115,13 +120,28 @@ void rmc_callback(const nmea_msgs::msg::Gprmc::ConstSharedPtr msg) void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { - if (!is_first_correction_velocity) return; - if(use_can_less_mode && !velocity_status.status.enabled_status) return; - if(!yaw_rate_offset_stop.status.enabled_status) + if (!is_first_correction_velocity) + { + RCLCPP_WARN(rclcpp::get_logger(node_name), "is_first_correction_velocity is false."); + return; + } + if(use_can_less_mode && !velocity_status.status.enabled_status) { - RCLCPP_WARN(rclcpp::get_logger(node_name), "Heading estimation is not started because the stop calibration is not yet completed."); + RCLCPP_WARN(rclcpp::get_logger(node_name), "velocity_status is not enabled."); return; } + if(!yaw_rate_offset_stop.status.enabled_status) + { + if(skip_static_initialization) + { + yaw_rate_offset_stop.yaw_rate_offset = yaw_rate_offset_stop_in_skip_mode; + } + else + { + RCLCPP_WARN(rclcpp::get_logger(node_name), "Heading estimation is not started because the stop calibration is not yet completed."); + return; + } + } imu = *msg; heading.header = msg->header; @@ -176,6 +196,8 @@ int main(int argc, char** argv) heading_parameter.outlier_ratio_threshold = conf["/**"]["ros__parameters"]["heading"]["outlier_ratio_threshold"].as(); heading_parameter.curve_judgment_threshold = conf["/**"]["ros__parameters"]["heading"]["curve_judgment_threshold"].as(); heading_parameter.init_STD = conf["/**"]["ros__parameters"]["heading"]["init_STD"].as(); + skip_static_initialization = conf["/**"]["ros__parameters"]["heading"]["skip_static_initialization"].as(); + yaw_rate_offset_stop_in_skip_mode = conf["/**"]["ros__parameters"]["heading"]["yaw_rate_offset_stop_in_skip_mode"].as(); std::cout<< "use_gnss_mode " << use_gnss_mode << std::endl; @@ -194,6 +216,8 @@ int main(int argc, char** argv) std::cout << "outlier_ratio_threshold " << heading_parameter.outlier_ratio_threshold << std::endl; std::cout << "curve_judgment_threshold " << heading_parameter.curve_judgment_threshold << std::endl; std::cout << "init_STD " << heading_parameter.init_STD << std::endl; + std::cout << "skip_static_initialization " << skip_static_initialization << std::endl; + std::cout << "yaw_rate_offset_stop_in_skip_mode " << yaw_rate_offset_stop_in_skip_mode << std::endl; } catch (YAML::Exception& e) { @@ -237,6 +261,12 @@ int main(int argc, char** argv) rclcpp::shutdown(); } + if(use_multi_antenna_mode) + { + // When using multi-antenna mode, set is_first_correction_velocity to true even when stationary for the first time. + is_first_correction_velocity = true; + } + auto sub1 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); auto sub2 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); auto sub3 = node->create_subscription(subscribe_rmc_topic_name, 1000, rmc_callback); diff --git a/eagleye_rt/src/rolling_node.cpp b/eagleye_rt/src/rolling_node.cpp index 98286330..e029b26d 100644 --- a/eagleye_rt/src/rolling_node.cpp +++ b/eagleye_rt/src/rolling_node.cpp @@ -46,27 +46,27 @@ struct RollingStatus _rolling_status; static bool _use_can_less_mode; -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstPtr msg) +void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { _velocity_msg = *msg; } -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg) +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { _velocity_status_msg = *msg; } -void yaw_rate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstPtr msg) +void yaw_rate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { _yaw_rate_offset_stop_msg = *msg; } -void yaw_rate_offset_2nd_callback(const eagleye_msgs::msg::YawrateOffset::ConstPtr msg) +void yaw_rate_offset_2nd_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { _yaw_rate_offset_2nd_msg = *msg; } -void imu_callback(const sensor_msgs::msg::Imu::ConstPtr msg) +void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { if(_use_can_less_mode && !_velocity_status_msg.status.enabled_status) return; _imu_msg = *msg; diff --git a/eagleye_rt/src/rtk_heading_node.cpp b/eagleye_rt/src/rtk_heading_node.cpp index 7a535b28..bb0abfdb 100644 --- a/eagleye_rt/src/rtk_heading_node.cpp +++ b/eagleye_rt/src/rtk_heading_node.cpp @@ -50,7 +50,7 @@ struct RtkHeadingStatus heading_status; static bool use_can_less_mode; -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstPtr msg) +void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { velocity = *msg; } @@ -60,7 +60,7 @@ void gga_callback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) gga = *msg; } -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg) +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { velocity_status = *msg; } diff --git a/eagleye_rt/src/slip_angle_node.cpp b/eagleye_rt/src/slip_angle_node.cpp index 856ad16f..ed4374f6 100644 --- a/eagleye_rt/src/slip_angle_node.cpp +++ b/eagleye_rt/src/slip_angle_node.cpp @@ -46,12 +46,12 @@ struct SlipangleParameter slip_angle_parameter; static bool use_can_less_mode; -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstPtr msg) +void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { velocity = *msg; } -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg) +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { velocity_status = *msg; } @@ -124,6 +124,7 @@ int main(int argc, char** argv) auto sub2 = node->create_subscription("velocity_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback); //ros::TransportHints().tcpNoDelay() auto sub3 = node->create_subscription("yaw_rate_offset_stop", rclcpp::QoS(10), yaw_rate_offset_stop_callback); //ros::TransportHints().tcpNoDelay() auto sub4 = node->create_subscription("yaw_rate_offset_2nd", rclcpp::QoS(10), yaw_rate_offset_2nd_callback); //ros::TransportHints().tcpNoDelay() + auto sub5 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); //ros::TransportHints().tcpNoDelay() pub = node->create_publisher("slip_angle", rclcpp::QoS(10)); rclcpp::spin(node); diff --git a/eagleye_rt/src/slip_coefficient_node.cpp b/eagleye_rt/src/slip_coefficient_node.cpp index ec38294a..fb8b9612 100644 --- a/eagleye_rt/src/slip_coefficient_node.cpp +++ b/eagleye_rt/src/slip_coefficient_node.cpp @@ -66,7 +66,7 @@ void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr ms } } -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg) +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { velocity_status = *msg; } diff --git a/eagleye_rt/src/smoothing_node.cpp b/eagleye_rt/src/smoothing_node.cpp index 6d95baa8..3e5767e5 100644 --- a/eagleye_rt/src/smoothing_node.cpp +++ b/eagleye_rt/src/smoothing_node.cpp @@ -38,17 +38,23 @@ static geometry_msgs::msg::TwistStamped velocity; static eagleye_msgs::msg::StatusStamped velocity_status; rclcpp::Publisher::SharedPtr pub; +struct PositionParameter position_parameter; struct SmoothingParameter smoothing_parameter; struct SmoothingStatus smoothing_status; static bool use_can_less_mode; +rclcpp::Clock clock_(RCL_ROS_TIME); +tf2_ros::Buffer tfBuffer_(std::make_shared(clock_)); + +const std::string node_name = "eagleye_smoothing"; + void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { velocity = *msg; } -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg) +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { velocity_status = *msg; } @@ -61,13 +67,37 @@ void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) gnss_smooth_pos_enu.header = msg->header; gnss_smooth_pos_enu.header.frame_id = "base_link"; smoothing_estimate(rtklib_nav,velocity,smoothing_parameter,&smoothing_status,&gnss_smooth_pos_enu); + gnss_smooth_pos_enu.enu_pos.z -= position_parameter.tf_gnss_translation_z; pub->publish(gnss_smooth_pos_enu); } +void on_timer() +{ + geometry_msgs::msg::TransformStamped transformStamped; + try + { + transformStamped = tfBuffer_.lookupTransform(position_parameter.tf_gnss_parent_frame, position_parameter.tf_gnss_child_frame, tf2::TimePointZero); + + position_parameter.tf_gnss_translation_x = transformStamped.transform.translation.x; + position_parameter.tf_gnss_translation_y = transformStamped.transform.translation.y; + position_parameter.tf_gnss_translation_z = transformStamped.transform.translation.z; + position_parameter.tf_gnss_rotation_x = transformStamped.transform.rotation.x; + position_parameter.tf_gnss_rotation_y = transformStamped.transform.rotation.y; + position_parameter.tf_gnss_rotation_z = transformStamped.transform.rotation.z; + position_parameter.tf_gnss_rotation_w = transformStamped.transform.rotation.w; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(rclcpp::get_logger(node_name), "%s", ex.what()); + return; + } +} + + int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("eagleye_smoothing"); + auto node = rclcpp::Node::make_shared(node_name); std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; @@ -82,6 +112,9 @@ int main(int argc, char** argv) use_can_less_mode = conf["/**"]["ros__parameters"]["use_can_less_mode"].as(); + position_parameter.tf_gnss_parent_frame = conf["/**"]["ros__parameters"]["tf_gnss_frame"]["parent"].as(); + position_parameter.tf_gnss_child_frame = conf["/**"]["ros__parameters"]["tf_gnss_frame"]["child"].as(); + smoothing_parameter.ecef_base_pos_x = conf["/**"]["ros__parameters"]["ecef_base_pos"]["x"].as(); smoothing_parameter.ecef_base_pos_y = conf["/**"]["ros__parameters"]["ecef_base_pos"]["y"].as(); smoothing_parameter.ecef_base_pos_z = conf["/**"]["ros__parameters"]["ecef_base_pos"]["z"].as(); @@ -115,6 +148,16 @@ int main(int argc, char** argv) auto sub3 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); pub = node->create_publisher("gnss_smooth_pos_enu", rclcpp::QoS(10)); + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(0.5)); + auto timer_callback = std::bind(on_timer); + auto timer = std::make_shared>( + node->get_clock(), period_ns, std::move(timer_callback), + node->get_node_base_interface()->get_context()); + node->get_node_timers_interface()->add_timer(timer, nullptr); + + tf2_ros::TransformListener listener(tfBuffer_); + rclcpp::spin(node); return 0; diff --git a/eagleye_rt/src/tf_converted_imu.cpp b/eagleye_rt/src/tf_converted_imu.cpp index 8d22de35..bd742040 100644 --- a/eagleye_rt/src/tf_converted_imu.cpp +++ b/eagleye_rt/src/tf_converted_imu.cpp @@ -64,9 +64,7 @@ class TFConvertedIMU: public rclcpp::Node std::string tf_base_link_frame_; - bool reverse_imu_wz_; - - void imu_callback(const sensor_msgs::msg::Imu::ConstPtr msg); + void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg); }; @@ -82,17 +80,14 @@ TFConvertedIMU::TFConvertedIMU() : Node("eagleye_tf_converted_imu"), declare_parameter("imu_topic", subscribe_imu_topic_name); declare_parameter("publish_imu_topic", publish_imu_topic_name); declare_parameter("tf_gnss_frame.parent", tf_base_link_frame_); - declare_parameter("reverse_imu_wz", reverse_imu_wz_); get_parameter("imu_topic", subscribe_imu_topic_name); get_parameter("publish_imu_topic", publish_imu_topic_name); get_parameter("tf_gnss_frame.parent", tf_base_link_frame_); - get_parameter("reverse_imu_wz", reverse_imu_wz_); std::cout<< "subscribe_imu_topic_name: " << subscribe_imu_topic_name << std::endl; std::cout<< "publish_imu_topic_name: " << publish_imu_topic_name << std::endl; std::cout<< "tf_base_link_frame: " << tf_base_link_frame_ << std::endl; - std::cout<< "reverse_imu_wz: " << reverse_imu_wz_ << std::endl; sub_ = create_subscription(subscribe_imu_topic_name, rclcpp::QoS(10), std::bind(&TFConvertedIMU::imu_callback, this, std::placeholders::_1)); pub_ = create_publisher("imu/data_tf_converted", rclcpp::QoS(10)); @@ -100,7 +95,7 @@ TFConvertedIMU::TFConvertedIMU() : Node("eagleye_tf_converted_imu"), TFConvertedIMU::~TFConvertedIMU(){}; -void TFConvertedIMU::imu_callback(const sensor_msgs::msg::Imu::ConstPtr msg) +void TFConvertedIMU::imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { imu_ = *msg; tf_converted_imu_.header = imu_.header; @@ -121,14 +116,13 @@ void TFConvertedIMU::imu_callback(const sensor_msgs::msg::Imu::ConstPtr msg) tf2::doTransform(linear_acceleration, transformed_linear_acceleration, transform); tf_converted_imu_.angular_velocity = transformed_angular_velocity.vector; - if(reverse_imu_wz_) - { - tf_converted_imu_.angular_velocity.z = (-1) * transformed_angular_velocity.vector.z; - } - else - { - tf_converted_imu_.angular_velocity.z = transformed_angular_velocity.vector.z; - } + + // The latter part of the process assumes the coordinate system of the IMU output from tamagawa_imu_driver, so conversion is necessary. + tf_converted_imu_.angular_velocity.y = (-1) * transformed_angular_velocity.vector.y; + tf_converted_imu_.angular_velocity.z = (-1) * transformed_angular_velocity.vector.z; + tf_converted_imu_.linear_acceleration.y = (-1) * transformed_linear_acceleration.vector.y; + tf_converted_imu_.linear_acceleration.z = (-1) * transformed_linear_acceleration.vector.z; + tf_converted_imu_.linear_acceleration = transformed_linear_acceleration.vector; tf_converted_imu_.orientation = transformed_quaternion; diff --git a/eagleye_rt/src/trajectory_node.cpp b/eagleye_rt/src/trajectory_node.cpp index 70968900..966835a4 100644 --- a/eagleye_rt/src/trajectory_node.cpp +++ b/eagleye_rt/src/trajectory_node.cpp @@ -69,7 +69,7 @@ void correction_velocity_callback(const geometry_msgs::msg::TwistStamped::ConstS correction_velocity = *msg; } -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg) +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { velocity_status = *msg; } diff --git a/eagleye_rt/src/twist_relay.cpp b/eagleye_rt/src/twist_relay.cpp index 9f02f5cd..465170c8 100644 --- a/eagleye_rt/src/twist_relay.cpp +++ b/eagleye_rt/src/twist_relay.cpp @@ -46,8 +46,8 @@ class TwistRelay: public rclcpp::Node rclcpp::Logger logger_; rclcpp::Clock clock_; - void twist_callback(const geometry_msgs::msg::TwistStamped::ConstPtr msg); - void twist_with_covariance_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstPtr msg); + void twist_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg); + void twist_with_covariance_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr msg); }; @@ -90,12 +90,12 @@ TwistRelay::TwistRelay() : Node("eagleye_twist_relay"), TwistRelay::~TwistRelay(){}; -void TwistRelay::twist_callback(const geometry_msgs::msg::TwistStamped::ConstPtr msg) +void TwistRelay::twist_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { pub_->publish(*msg); }; -void TwistRelay::twist_with_covariance_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstPtr msg) +void TwistRelay::twist_with_covariance_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr msg) { geometry_msgs::msg::TwistStamped twist; twist.header.stamp = msg->header.stamp; diff --git a/eagleye_rt/src/velocity_estimator_node.cpp b/eagleye_rt/src/velocity_estimator_node.cpp index 0e8cbaf5..6e64d6c9 100644 --- a/eagleye_rt/src/velocity_estimator_node.cpp +++ b/eagleye_rt/src/velocity_estimator_node.cpp @@ -48,17 +48,17 @@ static std::string yaml_file; static std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; static std::string subscribe_gga_topic_name = "gnss/gga"; -void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstPtr msg) +void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) { rtklib_nav_msg = *msg; } -void gga_callback(const nmea_msgs::msg::Gpgga::ConstPtr msg) +void gga_callback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) { gga_msg = *msg; } -void imu_callback(const sensor_msgs::msg::Imu::ConstPtr msg) +void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { imu_msg = *msg; diff --git a/eagleye_rt/src/yaw_rate_offset_node.cpp b/eagleye_rt/src/yaw_rate_offset_node.cpp index ceaee9ea..9dc521e0 100644 --- a/eagleye_rt/src/yaw_rate_offset_node.cpp +++ b/eagleye_rt/src/yaw_rate_offset_node.cpp @@ -53,7 +53,7 @@ void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr ms _velocity = *msg; } -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg) +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { _velocity_status = *msg; } diff --git a/eagleye_util/fix2pose/src/fix2pose.cpp b/eagleye_util/fix2pose/src/fix2pose.cpp deleted file mode 100644 index 8ed4fe7f..00000000 --- a/eagleye_util/fix2pose/src/fix2pose.cpp +++ /dev/null @@ -1,348 +0,0 @@ -// Copyright (c) 2019, Map IV, Inc. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// * Neither the name of the Map IV, Inc. nor the names of its contributors -// may be used to endorse or promote products derived from this software -// without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -// DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY -// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -/* - * fix2pose.cpp - * Author MapIV Sekino - */ - -#include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/point_stamped.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" -#include "sensor_msgs/msg/nav_sat_fix.hpp" -#include "eagleye_msgs/msg/rolling.hpp" -#include "eagleye_msgs/msg/pitching.hpp" -#include "eagleye_msgs/msg/heading.hpp" -#include "eagleye_msgs/msg/position.hpp" -#include "tf2_ros/transform_broadcaster.h" -#include -#include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif -#include "eagleye_coordinate/eagleye_coordinate.hpp" -#include -#include - -#include -#include - -rclcpp::Clock _ros_clock(RCL_ROS_TIME); - -static eagleye_msgs::msg::Rolling _eagleye_rolling; -static eagleye_msgs::msg::Pitching _eagleye_pitching; -static eagleye_msgs::msg::Heading _eagleye_heading; -static geometry_msgs::msg::Quaternion _quat; - -rclcpp::Publisher::SharedPtr _pub; -rclcpp::Publisher::SharedPtr _pub2; -rclcpp::Publisher::SharedPtr _pub3; -std::shared_ptr _br; -std::shared_ptr _br2; -static geometry_msgs::msg::PoseStamped _pose; -static geometry_msgs::msg::PoseWithCovarianceStamped _pose_with_covariance; - -static std::string _parent_frame_id, _child_frame_id; -static std::string _base_link_frame_id, _gnss_frame_id; - -std::string geoid_file_path = ament_index_cpp::get_package_share_directory("llh_converter") + "/data/gsigeo2011_ver2_1.asc"; -llh_converter::LLHConverter _lc(geoid_file_path); -llh_converter::LLHParam _llh_param; - -bool _fix_only_publish = false; -int _fix_judgement_type = 0; -int _fix_gnss_status = 0; -double _fix_std_pos_thres = 0.1; // [m] - -std::string _node_name = "eagleye_fix2pose"; - -tf2_ros::Buffer _tf_buffer(std::make_shared(_ros_clock)); - -void heading_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) -{ - _eagleye_heading = *msg; -} - -void rolling_callback(const eagleye_msgs::msg::Rolling::ConstSharedPtr msg) -{ - _eagleye_rolling = *msg; -} - -void pitching_callback(const eagleye_msgs::msg::Pitching::ConstSharedPtr msg) -{ - _eagleye_pitching = *msg; -} - -void fix_callback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) -{ - if(!_eagleye_heading.status.enabled_status) - { - RCLCPP_WARN(rclcpp::get_logger(_node_name), "heading is not enabled to publish pose"); - return; - } - - if(_fix_only_publish) - { - if(_fix_judgement_type == 0) - { - if(!msg->status.status == 0) - { - RCLCPP_WARN(rclcpp::get_logger(_node_name), "status.status is not 0"); - return; - } - } - else if(_fix_judgement_type == 1) - { - if(msg->position_covariance[0] > _fix_std_pos_thres * _fix_std_pos_thres) - { - RCLCPP_WARN(rclcpp::get_logger(_node_name), "position_covariance[0] is over %f", _fix_std_pos_thres * _fix_std_pos_thres); - return; - } - } - else - { - RCLCPP_ERROR(rclcpp::get_logger(_node_name), "fix_judgement_type is not valid"); - rclcpp::shutdown(); - } - } - - double llh[3] = {0}; - double xyz[3] = {0}; - - llh[0] = msg->latitude * M_PI / 180; - llh[1] = msg->longitude* M_PI / 180; - llh[2] = msg->altitude; - - _lc.convertRad2XYZ(llh[0], llh[1], llh[2], xyz[0], xyz[1], xyz[2], _llh_param); - - double eagleye_heading = 0; - tf2::Quaternion localization_quat; - eagleye_heading = fmod((90* M_PI / 180)-_eagleye_heading.heading_angle,2*M_PI); - llh_converter::LLA lla_struct; - llh_converter::XYZ xyz_struct; - lla_struct.latitude = msg->latitude; - lla_struct.longitude = msg->longitude; - lla_struct.altitude = msg->altitude; - xyz_struct.x = xyz[0]; - xyz_struct.y = xyz[1]; - xyz_struct.z = xyz[2]; - double mca = llh_converter::getMeridianConvergence(lla_struct, xyz_struct, _lc, _llh_param); // meridian convergence angle - eagleye_heading += mca; - - localization_quat.setRPY(0, 0, eagleye_heading); - _quat = tf2::toMsg(localization_quat); - - - _pose.header = msg->header; - _pose.header.frame_id = "map"; - _pose.pose.position.x = xyz[0]; - _pose.pose.position.y = xyz[1]; - _pose.pose.position.z = xyz[2]; - _pose.pose.orientation = _quat; - - geometry_msgs::msg::PoseStamped::Ptr transformed_pose_msg_ptr( - new geometry_msgs::msg::PoseStamped); - - if(_fix_only_publish) - { - geometry_msgs::msg::TransformStamped::Ptr TF_sensor_to_base_ptr(new geometry_msgs::msg::TransformStamped); - try - { - *TF_sensor_to_base_ptr = _tf_buffer.lookupTransform(_gnss_frame_id, _base_link_frame_id, tf2::TimePointZero); - - tf2::Transform transform, transform2, transfrom3; - transform.setOrigin(tf2::Vector3(_pose.pose.position.x, _pose.pose.position.y, - _pose.pose.position.z)); - transform.setRotation(localization_quat); - tf2::Quaternion q2(TF_sensor_to_base_ptr->transform.rotation.x, TF_sensor_to_base_ptr->transform.rotation.y, - TF_sensor_to_base_ptr->transform.rotation.z, TF_sensor_to_base_ptr->transform.rotation.w); - transform2.setOrigin(tf2::Vector3(TF_sensor_to_base_ptr->transform.translation.x, - TF_sensor_to_base_ptr->transform.translation.y, TF_sensor_to_base_ptr->transform.translation.z)); - transform2.setRotation(q2); - transfrom3 = transform * transform2; - - _pose.header.frame_id = _parent_frame_id; - _pose.pose.position.x = transfrom3.getOrigin().getX(); - _pose.pose.position.y = transfrom3.getOrigin().getY(); - _pose.pose.position.z = transfrom3.getOrigin().getZ(); - _pose.pose.orientation.x = transfrom3.getRotation().getX(); - _pose.pose.orientation.y = transfrom3.getRotation().getY(); - _pose.pose.orientation.z = transfrom3.getRotation().getZ(); - _pose.pose.orientation.w = transfrom3.getRotation().getW(); - - } - catch (tf2::TransformException& ex) - { - RCLCPP_WARN(rclcpp::get_logger(_node_name), "%s", ex.what()); - return; - } - } - - _pub->publish(_pose); - - _pose_with_covariance.header = _pose.header; - _pose_with_covariance.pose.pose = _pose.pose; - // TODO(Map IV): temporary value - double std_dev_roll = 100; // [rad] - double std_dev_pitch = 100; // [rad] - double std_dev_yaw = 100; // [rad] - if(_eagleye_rolling.status.enabled_status) std_dev_roll = 0.5 / 180 * M_PI; - if(_eagleye_pitching.status.enabled_status) std_dev_pitch = 0.5 / 180 * M_PI; - if(_eagleye_heading.status.enabled_status) std_dev_yaw = std::sqrt(_eagleye_heading.variance); - _pose_with_covariance.pose.covariance[0] = msg->position_covariance[0]; - _pose_with_covariance.pose.covariance[7] = msg->position_covariance[4]; - _pose_with_covariance.pose.covariance[14] = msg->position_covariance[8]; - _pose_with_covariance.pose.covariance[21] = std_dev_roll * std_dev_roll; - _pose_with_covariance.pose.covariance[28] = std_dev_pitch * std_dev_pitch; - _pose_with_covariance.pose.covariance[35] = std_dev_yaw * std_dev_yaw; - _pub2->publish(_pose_with_covariance); - - tf2::Transform transform; - tf2::Quaternion q; - transform.setOrigin(tf2::Vector3(_pose.pose.position.x, _pose.pose.position.y, _pose.pose.position.z)); - q.setRPY(0, 0, (90* M_PI / 180)- _eagleye_heading.heading_angle); - transform.setRotation(q); - - geometry_msgs::msg::TransformStamped trans_msg; - trans_msg.header.stamp = msg->header.stamp; - trans_msg.header.frame_id = _parent_frame_id; - trans_msg.child_frame_id = _child_frame_id; - trans_msg.transform = tf2::toMsg(transform); - _br->sendTransform(trans_msg); -} - -int main(int argc, char** argv) -{ - int plane = 7; - int tf_num = 7; - int convert_height_num = 7; - int geoid_type = 0; - - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared(_node_name); - - node->declare_parameter("plane", plane); - node->declare_parameter("tf_num", tf_num); - node->declare_parameter("convert_height_num", convert_height_num); - node->declare_parameter("geoid_type", geoid_type); - node->declare_parameter("parent_frame_id", _parent_frame_id); - node->declare_parameter("child_frame_id", _child_frame_id); - node->declare_parameter("fix_only_publish", _fix_only_publish); - node->declare_parameter("fix_judgement_type", _fix_judgement_type); - node->declare_parameter("fix_gnss_status", _fix_gnss_status); - node->declare_parameter("fix_std_pos_thres", _fix_std_pos_thres); - node->declare_parameter("base_link_frame_id", _base_link_frame_id); - node->declare_parameter("gnss_frame_id", _gnss_frame_id); - - node->get_parameter("plane", plane); - node->get_parameter("tf_num", tf_num); - node->get_parameter("convert_height_num", convert_height_num); - node->get_parameter("geoid_type", geoid_type); - node->get_parameter("parent_frame_id", _parent_frame_id); - node->get_parameter("child_frame_id", _child_frame_id); - node->get_parameter("fix_only_publish", _fix_only_publish); - node->get_parameter("fix_judgement_type", _fix_judgement_type); - node->get_parameter("fix_gnss_status", _fix_gnss_status); - node->get_parameter("fix_std_pos_thres", _fix_std_pos_thres); - node->get_parameter("base_link_frame_id", _base_link_frame_id); - node->get_parameter("gnss_frame_id", _gnss_frame_id); - - std::cout<< "plane "<< plane<create_subscription("heading_interpolate_3rd", 1000, heading_callback); - auto sub3 = node->create_subscription("fix", 1000, fix_callback); - auto sub4 = node->create_subscription("rolling", 1000, rolling_callback); - auto sub5 = node->create_subscription("pitching", 1000, pitching_callback); - _pub = node->create_publisher("pose", 1000); - _pub2 = node->create_publisher("pose_with_covariance", 1000); - _br = std::make_shared(node, 100); - _br2 = std::make_shared(node, 100); - rclcpp::spin(node); - - return 0; -} diff --git a/eagleye_util/geo_pose_converter/CMakeLists.txt b/eagleye_util/geo_pose_converter/CMakeLists.txt new file mode 100644 index 00000000..c7346092 --- /dev/null +++ b/eagleye_util/geo_pose_converter/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.5) +project(eagleye_geo_pose_converter) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +include_directories(include) + +ament_auto_add_executable(geo_pose_converter + src/geo_pose_converter.cpp +) + +install(TARGETS + DESTINATION lib/$(PROJECT_NAME) +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/eagleye_util/fix2pose/launch/fix2pose.launch.xml b/eagleye_util/geo_pose_converter/launch/geo_pose_converter.launch.xml similarity index 50% rename from eagleye_util/fix2pose/launch/fix2pose.launch.xml rename to eagleye_util/geo_pose_converter/launch/geo_pose_converter.launch.xml index 9f130a90..b79016f9 100644 --- a/eagleye_util/fix2pose/launch/fix2pose.launch.xml +++ b/eagleye_util/geo_pose_converter/launch/geo_pose_converter.launch.xml @@ -1,10 +1,10 @@ + - - - + + @@ -12,23 +12,13 @@ - + - - - - - - - - - - diff --git a/eagleye_util/fix2pose/package.xml b/eagleye_util/geo_pose_converter/package.xml similarity index 81% rename from eagleye_util/fix2pose/package.xml rename to eagleye_util/geo_pose_converter/package.xml index f0ac7b32..b3169206 100644 --- a/eagleye_util/fix2pose/package.xml +++ b/eagleye_util/geo_pose_converter/package.xml @@ -1,35 +1,36 @@ - eagleye_fix2pose + eagleye_geo_pose_converter 1.0.0 - fix2pose package + geo_pose_converter package OsamuSekino + Koji Minoda BSD3 ament_cmake_auto rclcpp - rclpy std_msgs eagleye_msgs eagleye_navigation tf2_ros eagleye_coordinate + geographic_msgs rclcpp - rclpy std_msgs eagleye_msgs eagleye_navigation tf2_ros eagleye_coordinate + geographic_msgs rclcpp - rclpy std_msgs eagleye_msgs eagleye_navigation tf2_ros eagleye_coordinate + geographic_msgs llh_converter std_srvs diff --git a/eagleye_util/geo_pose_converter/src/geo_pose_converter.cpp b/eagleye_util/geo_pose_converter/src/geo_pose_converter.cpp new file mode 100644 index 00000000..59746d03 --- /dev/null +++ b/eagleye_util/geo_pose_converter/src/geo_pose_converter.cpp @@ -0,0 +1,245 @@ +// Copyright (c) 2019, Map IV, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of the Map IV, Inc. nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +/* + * geo_pose_converter.cpp + * Author MapIV Sekino + */ + +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include +#include + +#include + +rclcpp::Clock _ros_clock(RCL_ROS_TIME); + +rclcpp::Publisher::SharedPtr _pub; +rclcpp::Publisher::SharedPtr _pub2; +std::shared_ptr _br; + +static std::string _parent_frame_id, _child_frame_id; +static std::string _base_link_frame_id, _gnss_frame_id; + +std::string geoid_file_path = ament_index_cpp::get_package_share_directory("llh_converter") + "/data/gsigeo2011_ver2_1.asc"; +llh_converter::LLHConverter _lc(geoid_file_path); +llh_converter::LLHParam _llh_param; + +std::string _node_name = "eagleye_geo_pose_converter"; + +tf2_ros::Buffer _tf_buffer(std::make_shared(_ros_clock)); + +void geo_pose_callback(const geographic_msgs::msg::GeoPoseWithCovarianceStamped::ConstSharedPtr msg) +{ + double llh[3] = {0}; + double xyz[3] = {0}; + + llh[0] = msg->pose.pose.position.latitude * M_PI / 180; + llh[1] = msg->pose.pose.position.longitude* M_PI / 180; + llh[2] = msg->pose.pose.position.altitude; + + _lc.convertRad2XYZ(llh[0], llh[1], llh[2], xyz[0], xyz[1], xyz[2], _llh_param); + + geometry_msgs::msg::PoseStamped pose; + pose.header = msg->header; + pose.header.frame_id = "map"; + pose.pose.position.x = xyz[0]; + pose.pose.position.y = xyz[1]; + pose.pose.position.z = xyz[2]; + pose.pose.orientation = msg->pose.pose.orientation; + + const auto localization_quat = tf2::Quaternion(pose.pose.orientation.x, pose.pose.orientation.y, + pose.pose.orientation.z, pose.pose.orientation.w); + + geometry_msgs::msg::PoseStamped::SharedPtr transformed_pose_msg_ptr( + new geometry_msgs::msg::PoseStamped); + + geometry_msgs::msg::TransformStamped::SharedPtr TF_sensor_to_base_ptr(new geometry_msgs::msg::TransformStamped); + try + { + *TF_sensor_to_base_ptr = _tf_buffer.lookupTransform(_gnss_frame_id, _base_link_frame_id, tf2::TimePointZero); + + tf2::Transform transform, transform2, transfrom3; + transform.setOrigin(tf2::Vector3(pose.pose.position.x, pose.pose.position.y, + pose.pose.position.z)); + transform.setRotation(localization_quat); + tf2::Quaternion q2(TF_sensor_to_base_ptr->transform.rotation.x, TF_sensor_to_base_ptr->transform.rotation.y, + TF_sensor_to_base_ptr->transform.rotation.z, TF_sensor_to_base_ptr->transform.rotation.w); + transform2.setOrigin(tf2::Vector3(TF_sensor_to_base_ptr->transform.translation.x, + TF_sensor_to_base_ptr->transform.translation.y, TF_sensor_to_base_ptr->transform.translation.z)); + transform2.setRotation(q2); + transfrom3 = transform * transform2; + + pose.header.frame_id = _parent_frame_id; + pose.pose.position.x = transfrom3.getOrigin().getX(); + pose.pose.position.y = transfrom3.getOrigin().getY(); + pose.pose.position.z = transfrom3.getOrigin().getZ(); + pose.pose.orientation.x = transfrom3.getRotation().getX(); + pose.pose.orientation.y = transfrom3.getRotation().getY(); + pose.pose.orientation.z = transfrom3.getRotation().getZ(); + pose.pose.orientation.w = transfrom3.getRotation().getW(); + + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(rclcpp::get_logger(_node_name), "%s", ex.what()); + return; + } + + _pub->publish(pose); + + geometry_msgs::msg::PoseWithCovarianceStamped pose_with_covariance; + pose_with_covariance.header = pose.header; + pose_with_covariance.pose.pose = pose.pose; + + // Covariance in NavSatFix is in ENU coordinate while the one in GeoPoseWithCovariance is in Lat/Lon/Alt coordinate. + // In order to be consistent with the msg definition, we need to swap the covariance of x and y. + pose_with_covariance.pose.covariance[0] = msg->pose.covariance[7]; + pose_with_covariance.pose.covariance[7] = msg->pose.covariance[0]; + pose_with_covariance.pose.covariance[14] = msg->pose.covariance[14]; + pose_with_covariance.pose.covariance[21] = msg->pose.covariance[21]; + pose_with_covariance.pose.covariance[28] = msg->pose.covariance[28]; + pose_with_covariance.pose.covariance[35] = msg->pose.covariance[35]; + _pub2->publish(pose_with_covariance); + + tf2::Transform transform; + transform.setOrigin(tf2::Vector3(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)); + // NOTE: currently geo_pose_fuser, the node before this node, ignores roll and pitch for robust estimation results. + transform.setRotation(localization_quat); + + geometry_msgs::msg::TransformStamped trans_msg; + trans_msg.header.stamp = msg->header.stamp; + trans_msg.header.frame_id = _parent_frame_id; + trans_msg.child_frame_id = _child_frame_id; + trans_msg.transform = tf2::toMsg(transform); + _br->sendTransform(trans_msg); +} + +int main(int argc, char** argv) +{ + int plane = 7; + int tf_num = 7; + int convert_height_num = 7; + int geoid_type = 0; + + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared(_node_name); + + node->declare_parameter("plane", plane); + node->declare_parameter("tf_num", tf_num); + node->declare_parameter("convert_height_num", convert_height_num); + node->declare_parameter("geoid_type", geoid_type); + node->declare_parameter("parent_frame_id", _parent_frame_id); + node->declare_parameter("child_frame_id", _child_frame_id); + node->declare_parameter("base_link_frame_id", _base_link_frame_id); + node->declare_parameter("gnss_frame_id", _gnss_frame_id); + + node->get_parameter("plane", plane); + node->get_parameter("tf_num", tf_num); + node->get_parameter("convert_height_num", convert_height_num); + node->get_parameter("geoid_type", geoid_type); + node->get_parameter("parent_frame_id", _parent_frame_id); + node->get_parameter("child_frame_id", _child_frame_id); + node->get_parameter("base_link_frame_id", _base_link_frame_id); + node->get_parameter("gnss_frame_id", _gnss_frame_id); + + std::cout<< "plane "<< plane<create_subscription("eagleye/geo_pose_with_covariance", 1000, geo_pose_callback); + _pub = node->create_publisher("eagleye/pose", 1000); + _pub2 = node->create_publisher("eagleye/pose_with_covariance", 1000); + _br = std::make_shared(node, 100); + rclcpp::spin(node); + + return 0; +} diff --git a/eagleye_util/fix2pose/CMakeLists.txt b/eagleye_util/geo_pose_fusion/CMakeLists.txt similarity index 87% rename from eagleye_util/fix2pose/CMakeLists.txt rename to eagleye_util/geo_pose_fusion/CMakeLists.txt index c544ec90..7780d41a 100644 --- a/eagleye_util/fix2pose/CMakeLists.txt +++ b/eagleye_util/geo_pose_fusion/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(eagleye_fix2pose) +project(eagleye_geo_pose_fusion) # Default to C99 if(NOT CMAKE_C_STANDARD) @@ -20,8 +20,8 @@ ament_auto_find_build_dependencies() include_directories(include) -ament_auto_add_executable(fix2pose - src/fix2pose.cpp +ament_auto_add_executable(geo_pose_fusion + src/geo_pose_fusion.cpp ) install(TARGETS diff --git a/eagleye_util/geo_pose_fusion/launch/geo_pose_fusion.launch.xml b/eagleye_util/geo_pose_fusion/launch/geo_pose_fusion.launch.xml new file mode 100644 index 00000000..2224bf18 --- /dev/null +++ b/eagleye_util/geo_pose_fusion/launch/geo_pose_fusion.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/eagleye_util/geo_pose_fusion/package.xml b/eagleye_util/geo_pose_fusion/package.xml new file mode 100644 index 00000000..66c0170b --- /dev/null +++ b/eagleye_util/geo_pose_fusion/package.xml @@ -0,0 +1,31 @@ + + + eagleye_geo_pose_fusion + 1.0.0 + geo_pose_fusion package + + Ryohei Sasaki + Koji Minoda + + BSD3 + + ament_cmake_auto + rclcpp + eagleye_msgs + geographic_msgs + rclcpp + eagleye_msgs + geographic_msgs + rclcpp + eagleye_msgs + geographic_msgs + + ament_lint_auto + ament_lint_common + + tf2_geometry_msgs + + + ament_cmake + + diff --git a/eagleye_util/geo_pose_fusion/src/geo_pose_fusion.cpp b/eagleye_util/geo_pose_fusion/src/geo_pose_fusion.cpp new file mode 100644 index 00000000..d365c082 --- /dev/null +++ b/eagleye_util/geo_pose_fusion/src/geo_pose_fusion.cpp @@ -0,0 +1,165 @@ +// Copyright (c) 2019, Map IV, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of the Map IV, Inc. nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +/* + * geo_pose_fusion.cpp + * Author MapIV Sekino + */ + +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +rclcpp::Clock _ros_clock(RCL_ROS_TIME); + +static eagleye_msgs::msg::Rolling _eagleye_rolling; +static eagleye_msgs::msg::Pitching _eagleye_pitching; +static eagleye_msgs::msg::Heading _eagleye_heading; +static geometry_msgs::msg::Quaternion _quat; + +rclcpp::Publisher::SharedPtr _pub; +static geographic_msgs::msg::GeoPoseWithCovarianceStamped _geo_pose_with_covariance; + +bool _fix_only_publish = false; +int _fix_judgement_type = 0; +double _fix_std_pos_thres = 0.1; // [m] + +std::string _node_name = "eagleye_geo_pose_fusion"; + +void heading_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) +{ + _eagleye_heading = *msg; +} + +void rolling_callback(const eagleye_msgs::msg::Rolling::ConstSharedPtr msg) +{ + _eagleye_rolling = *msg; +} + +void pitching_callback(const eagleye_msgs::msg::Pitching::ConstSharedPtr msg) +{ + _eagleye_pitching = *msg; +} + +void fix_callback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) +{ + bool fix_flag = false; + if(_fix_judgement_type == 0) + { + if(msg->status.status == 0 && _eagleye_heading.status.enabled_status) fix_flag = true; + } + else if(_fix_judgement_type == 1) + { + if(msg->position_covariance[0] < _fix_std_pos_thres * _fix_std_pos_thres && _eagleye_heading.status.enabled_status) fix_flag = true; + } + else + { + RCLCPP_ERROR(rclcpp::get_logger(_node_name), "fix_judgement_type is not valid"); + rclcpp::shutdown(); + } + + if(_fix_only_publish && !fix_flag) + { + return; + } + + double eagleye_heading = 0; + tf2::Quaternion localization_quat; + if (_eagleye_heading.status.enabled_status) + { + // NOTE: currently geo_pose_fusion ignores roll and pitch for robust estimation results. + eagleye_heading = fmod((90* M_PI / 180)-_eagleye_heading.heading_angle,2*M_PI); + localization_quat.setRPY(0, 0, eagleye_heading); + } + else + { + tf2::Matrix3x3(localization_quat).setRPY(0, 0, 0); + } + _quat = tf2::toMsg(localization_quat); + + + _geo_pose_with_covariance.header = msg->header; + _geo_pose_with_covariance.header.frame_id = "map"; + _geo_pose_with_covariance.pose.pose.position.latitude = msg->latitude; + _geo_pose_with_covariance.pose.pose.position.longitude = msg->longitude; + _geo_pose_with_covariance.pose.pose.position.altitude = msg->altitude; + _geo_pose_with_covariance.pose.pose.orientation = _quat; + + // TODO(Map IV): temporary value + double std_dev_roll = 100; // [rad] + double std_dev_pitch = 100; // [rad] + double std_dev_yaw = 100; // [rad] + if(_eagleye_rolling.status.enabled_status) std_dev_roll = 0.5 / 180 * M_PI; + if(_eagleye_pitching.status.enabled_status) std_dev_pitch = 0.5 / 180 * M_PI; + if(_eagleye_heading.status.enabled_status) std_dev_yaw = std::sqrt(_eagleye_heading.variance); + + // Covariance in NavSatFix is in ENU coordinate while the one in GeoPoseWithCovariance is in Lat/Lon/Alt coordinate. + // In order to be consistent with the msg definition, we need to swap the covariance of x and y. + _geo_pose_with_covariance.pose.covariance[0] = msg->position_covariance[4]; + _geo_pose_with_covariance.pose.covariance[7] = msg->position_covariance[0]; + _geo_pose_with_covariance.pose.covariance[14] = msg->position_covariance[8]; + + _geo_pose_with_covariance.pose.covariance[21] = std_dev_roll * std_dev_roll; + _geo_pose_with_covariance.pose.covariance[28] = std_dev_pitch * std_dev_pitch; + _geo_pose_with_covariance.pose.covariance[35] = std_dev_yaw * std_dev_yaw; + _pub->publish(_geo_pose_with_covariance); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared(_node_name); + + node->declare_parameter("fix_only_publish", _fix_only_publish); + node->declare_parameter("fix_judgement_type", _fix_judgement_type); + node->declare_parameter("fix_std_pos_thres", _fix_std_pos_thres); + + node->get_parameter("fix_only_publish", _fix_only_publish); + node->get_parameter("fix_judgement_type", _fix_judgement_type); + node->get_parameter("fix_std_pos_thres", _fix_std_pos_thres); + + std::cout<< "fix_only_publish "<< _fix_only_publish<create_subscription("eagleye/heading_interpolate_3rd", 1000, heading_callback); + auto sub3 = node->create_subscription("eagleye/fix", 1000, fix_callback); + auto sub4 = node->create_subscription("eagleye/rolling", 1000, rolling_callback); + auto sub5 = node->create_subscription("eagleye/pitching", 1000, pitching_callback); + _pub = node->create_publisher("eagleye/geo_pose_with_covariance", 1000); + rclcpp::spin(node); + + return 0; +} diff --git a/eagleye_util/gnss_converter/CMakeLists.txt b/eagleye_util/gnss_converter/CMakeLists.txt index 1358ef4e..a3e0aec1 100644 --- a/eagleye_util/gnss_converter/CMakeLists.txt +++ b/eagleye_util/gnss_converter/CMakeLists.txt @@ -15,6 +15,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +if($ENV{ROS_DISTRO} STREQUAL "galactic") + add_definitions(-DROS_DISTRO_GALACTIC) +endif() + find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() diff --git a/eagleye_util/gnss_converter/src/nmea2fix_core.cpp b/eagleye_util/gnss_converter/src/nmea2fix_core.cpp index a3c3334c..de115cda 100644 --- a/eagleye_util/gnss_converter/src/nmea2fix_core.cpp +++ b/eagleye_util/gnss_converter/src/nmea2fix_core.cpp @@ -116,10 +116,16 @@ void gnss_converter_converter(const nmea_msgs::msg::Sentence sentence, sensor_ms if(gga->gps_qual == 4) { fix->status.status = 0; + fix->position_covariance[0] = 0.01; + fix->position_covariance[4] = 0.01; + fix->position_covariance[8] = 0.04; } else { fix->status.status = -1; + fix->position_covariance[0] = 100.0; + fix->position_covariance[4] = 100.0; + fix->position_covariance[8] = 100.0; } }