Fix height in eagleye_fix (#307)

eagleye_rt/src/smoothing_node.cpp

@@ -38,11 +38,17 @@ static geometry_msgs::msg::TwistStamped velocity;
 static eagleye_msgs::msg::StatusStamped velocity_status;
 rclcpp::Publisher<eagleye_msgs::msg::Position>::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<rclcpp::Clock>(clock_));
+
+const std::string node_name = "eagleye_smoothing";
+
 void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg)
 {
   velocity = *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<bool>();
 
+    position_parameter.tf_gnss_parent_frame = conf["/**"]["ros__parameters"]["tf_gnss_frame"]["parent"].as<std::string>();
+    position_parameter.tf_gnss_child_frame = conf["/**"]["ros__parameters"]["tf_gnss_frame"]["child"].as<std::string>();
+
     smoothing_parameter.ecef_base_pos_x = conf["/**"]["ros__parameters"]["ecef_base_pos"]["x"].as<double>();
     smoothing_parameter.ecef_base_pos_y = conf["/**"]["ros__parameters"]["ecef_base_pos"]["y"].as<double>();
     smoothing_parameter.ecef_base_pos_z = conf["/**"]["ros__parameters"]["ecef_base_pos"]["z"].as<double>();
@@ -115,6 +148,16 @@ int main(int argc, char** argv)
   auto sub3 = node->create_subscription<rtklib_msgs::msg::RtklibNav>(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback);
   pub = node->create_publisher<eagleye_msgs::msg::Position>("gnss_smooth_pos_enu", rclcpp::QoS(10));
 
+  const auto period_ns =
+      std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(0.5));
+  auto timer_callback = std::bind(on_timer);
+  auto timer = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
+    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;