S^Cit fix2pose into two separate nodes

MapIV · Nov 29, 2023 · 2e46a21 · 2e46a21
1 parent dddf471
commit 2e46a21
Show file tree

Hide file tree

Showing 13 changed files with 521 additions and 438 deletions.
diff --git 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
@@ -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
@@ -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
@@ -145,7 +145,9 @@
         </include>
         <include file="$(find-pkg-share eagleye_tf)/launch/tf.launch.xml" if="$(var use_tf)"/>
 
-        <include file="$(find-pkg-share eagleye_fix2pose)/launch/fix2pose.launch.xml">
+        <include file="$(find-pkg-share eagleye_geo_pose_fusion)/launch/geo_pose_fusion.launch.xml">
+        </include>
+        <include file="$(find-pkg-share eagleye_geo_pose_converter)/launch/geo_pose_converter.launch.xml">
         </include>
   </group>