From 23f99a74585658e5da6b6940222034e19b2d5fec Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 2 May 2024 20:15:43 +0900 Subject: [PATCH] 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 * refactoring to a function Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/mission_planner/mission_planner.cpp | 17 +++++++++++++++++ .../src/mission_planner/mission_planner.hpp | 2 ++ 2 files changed, 19 insertions(+) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index e9a0108cdaa24..4cc646526a0ac 100644 --- 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(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 index 8016851d5a7d3..6752ceb4eaa8b 100644 --- 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 & 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();