change testpose, add utm.

src/ros2/src/testpose/CMakeLists.txt

@@ -24,6 +24,7 @@ find_package(sensor_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(autoware_auto_control_msgs REQUIRED)
 
+
 include_directories(
 ##  INCLUDE_DIRS include
   ${CMAKE_CURRENT_BINARY_DIR}/..
@@ -36,6 +37,8 @@ add_executable(testpose
   src/testpose_core.cpp
 )
 
+target_link_libraries(testpose Geographic)
+
 ament_target_dependencies(testpose rclcpp std_msgs geometry_msgs 
   tf2_geometry_msgs nav_msgs sensor_msgs autoware_auto_control_msgs autoware_auto_geometry_msgs
   autoware_auto_mapping_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs)

src/ros2/src/testpose/include/testpose/testpose_core.hpp

@@ -123,6 +123,9 @@ private:
   void publish_velocity(const VelocityReport & velocity);
 
 
+  void CalcXY(double flon,double flat,double & fx,double & fy);
+
+
 private:
   std::string simulated_frame_id_;  //!< @brief simulated vehicle frame id
   std::string origin_frame_id_;     //!< @brief map frame_id

src/ros2/src/testpose/src/testpose_core.cpp

@@ -29,6 +29,9 @@
 
 #include <iostream>
 
+#include <GeographicLib/MGRS.hpp>
+#include <GeographicLib/UTMUPS.hpp>
+
 using namespace std;
 
 
@@ -200,13 +203,18 @@ void TestPose::callbackTimer()
 
     nav_msgs::msg::Odometry msg;
 
+    double flon,flat;
+    flon = 117.0857970;
+    flat = 39.1366668;
 
+    double fx,fy;
+    CalcXY(flon,flat,fx,fy);
 
-    static double rel_x = 3705;
-    static double rel_y = 73765;
+    static double rel_x = fx;//7600;
+    static double rel_y = fy;//32100;
     static double rel_z = 0;
     static double vn = 0;
-    static double ve = 0;
+    static double ve = 1;
    // rel_x+=0.01;
 
     msg.pose.pose.position.x = rel_x;
@@ -271,4 +279,26 @@ void TestPose::publish_velocity(const VelocityReport & velocity)
   pub_velocity_->publish(msg);
 }
 
+void TestPose::CalcXY(double flon,double flat,double & fx,double & fy)
+{
+    int zone{};
+    bool is_north{};
+    std::string mgrs_code;
+
+    double utm_x,utm_y;
+
+    try {
+        GeographicLib::UTMUPS::Forward(flat, flon, zone, is_north, utm_x, utm_y);
+        GeographicLib::MGRS::Forward(
+            zone, is_north, utm_x, utm_y, flat, 3, mgrs_code);
+    } catch (const GeographicLib::GeographicErr & err) {
+        std::cerr << err.what() << std::endl;
+        return;
+
+    }
+    fx = fmod(utm_x,1e5);
+    fy = fmod(utm_y,1e5);
+    return;
+}
+