feat(mission_planner): publish initial and goal poses to logs (#6918)

* feat(mission_planner): publish initial and goal poses to logs Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * refactoring to a function Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> --------- Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
autowarefoundation · May 2, 2024 · 23f99a7 · 23f99a7
1 parent 52f4b3e
commit 23f99a7
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 0 deletions.
diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp
@@ -87,6 +87,17 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
 }
 
+void MissionPlanner::publish_pose_log(const Pose & pose, const std::string & pose_type)
+{
+  const auto & p = pose.position;
+  RCLCPP_INFO(
+    this->get_logger(), "%s pose - x: %f, y: %f, z: %f", pose_type.c_str(), p.x, p.y, p.z);
+  const auto & quaternion = pose.orientation;
+  RCLCPP_INFO(
+    this->get_logger(), "%s orientation - qx: %f, qy: %f, qz: %f, qw: %f", pose_type.c_str(),
+    quaternion.x, quaternion.y, quaternion.z, quaternion.w);
+}
+
 void MissionPlanner::check_initialization()
 {
   auto logger = get_logger();
@@ -247,6 +258,9 @@ void MissionPlanner::on_set_lanelet_route(
   change_route(route);
   change_state(RouteState::SET);
   res->status.success = true;
+
+  publish_pose_log(odometry_->pose.pose, "initial");
+  publish_pose_log(req->goal_pose, "goal");
 }
 
 void MissionPlanner::on_set_waypoint_route(
@@ -292,6 +306,9 @@ void MissionPlanner::on_set_waypoint_route(
   change_route(route);
   change_state(RouteState::SET);
   res->status.success = true;
+
+  publish_pose_log(odometry_->pose.pose, "initial");
+  publish_pose_log(req->goal_pose, "goal");
 }
 
 void MissionPlanner::change_route()

diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp
@@ -122,6 +122,8 @@ class MissionPlanner : public rclcpp::Node
     const Header & header, const std::vector<Pose> & waypoints, const Pose & goal_pose,
     const UUID & uuid, const bool allow_goal_modification);
 
+  void publish_pose_log(const Pose & pose, const std::string & pose_type);
+
   rclcpp::TimerBase::SharedPtr data_check_timer_;
   void check_initialization();