diff --git a/README.md b/README.md
index 852cccd4..49c19371 100755
--- a/README.md
+++ b/README.md
@@ -171,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/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_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_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/fix2pose/CMakeLists.txt b/eagleye_util/geo_pose_converter/CMakeLists.txt
similarity index 80%
rename from eagleye_util/fix2pose/CMakeLists.txt
rename to eagleye_util/geo_pose_converter/CMakeLists.txt
index 24c2ef71..c7346092 100644
--- a/eagleye_util/fix2pose/CMakeLists.txt
+++ b/eagleye_util/geo_pose_converter/CMakeLists.txt
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.5)
-project(eagleye_fix2pose)
+project(eagleye_geo_pose_converter)
# Default to C99
if(NOT CMAKE_C_STANDARD)
@@ -15,17 +15,13 @@ 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()
include_directories(include)
-ament_auto_add_executable(fix2pose
- src/fix2pose.cpp
+ament_auto_add_executable(geo_pose_converter
+ src/geo_pose_converter.cpp
)
install(TARGETS
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 77%
rename from eagleye_util/fix2pose/package.xml
rename to eagleye_util/geo_pose_converter/package.xml
index 5f022163..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
+ 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/geo_pose_fusion/CMakeLists.txt b/eagleye_util/geo_pose_fusion/CMakeLists.txt
new file mode 100644
index 00000000..7780d41a
--- /dev/null
+++ b/eagleye_util/geo_pose_fusion/CMakeLists.txt
@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.5)
+project(eagleye_geo_pose_fusion)
+
+# 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_fusion
+ src/geo_pose_fusion.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/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;
+}