From 9431e646809b1d8f51b5c776ca999264eaffe98f Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Mon, 31 Jul 2023 14:40:21 +0900 Subject: [PATCH 1/4] planner_cspace: avoid multiple goal pose relocation --- planner_cspace/src/planner_3d.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp index 84be5afe..b9687bc7 100644 --- a/planner_cspace/src/planner_3d.cpp +++ b/planner_cspace/src/planner_3d.cpp @@ -509,8 +509,8 @@ class Planner3dNode s.cycleUnsigned(map_info_.angle); grid_metric_converter::metric2Grid( map_info_, e[0], e[1], e[2], - goal_.pose.position.x, goal_.pose.position.y, - tf2::getYaw(goal_.pose.orientation)); + goal_raw_.pose.position.x, goal_raw_.pose.position.y, + tf2::getYaw(goal_raw_.pose.orientation)); e.cycleUnsigned(map_info_.angle); if (goal_changed) { @@ -554,6 +554,7 @@ class Planner3dNode goal_.pose.position.y = y; break; default: + goal_ = goal_raw_; break; } const auto ts = boost::chrono::high_resolution_clock::now(); From d7f414be4798da88f16ef8919ad87a49f2dd514e Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Mon, 31 Jul 2023 14:40:42 +0900 Subject: [PATCH 2/4] Fix relocation outputs --- planner_cspace/src/planner_3d.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp index b9687bc7..453d2a4c 100644 --- a/planner_cspace/src/planner_3d.cpp +++ b/planner_cspace/src/planner_3d.cpp @@ -514,10 +514,9 @@ class Planner3dNode e.cycleUnsigned(map_info_.angle); if (goal_changed) { - ROS_INFO( - "New goal received (%d, %d, %d)", - e[0], e[1], e[2]); - + ROS_INFO("New goal received. Metric: (%f, %f, %f), Grid: (%d, %d, %d)", + goal_raw_.pose.position.x, goal_raw_.pose.position.y, tf2::getYaw(goal_raw_.pose.orientation), + e[0], e[1], e[2]); clearHysteresis(); has_hysteresis_map_ = false; } @@ -546,12 +545,12 @@ class Planner3dNode ++cnt_stuck_; return true; case DiscretePoseStatus::RELOCATED: - ROS_INFO("Goal moved (%d, %d, %d)", e[0], e[1], e[2]); float x, y, yaw; grid_metric_converter::grid2Metric(map_info_, e[0], e[1], e[2], x, y, yaw); goal_.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw)); goal_.pose.position.x = x; goal_.pose.position.y = y; + ROS_INFO("Goal moved. Metric: (%f, %f, %f), Grid: (%d, %d, %d)", x, y, yaw, e[0], e[1], e[2]); break; default: goal_ = goal_raw_; From d509fe37c1acf3d68277cb8fe198ef23f46f42c9 Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Mon, 31 Jul 2023 14:54:11 +0900 Subject: [PATCH 3/4] Add message when the goal is reverted --- planner_cspace/src/planner_3d.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp index 453d2a4c..431b1bcb 100644 --- a/planner_cspace/src/planner_3d.cpp +++ b/planner_cspace/src/planner_3d.cpp @@ -553,6 +553,17 @@ class Planner3dNode ROS_INFO("Goal moved. Metric: (%f, %f, %f), Grid: (%d, %d, %d)", x, y, yaw, e[0], e[1], e[2]); break; default: + Astar::Vec e_prev; + grid_metric_converter::metric2Grid( + map_info_, e_prev[0], e_prev[1], e_prev[2], + goal_.pose.position.x, goal_.pose.position.y, + tf2::getYaw(goal_.pose.orientation)); + if (e[0] != e_prev[0] || e[1] != e_prev[1] || e[2] != e_prev[2]) + { + ROS_INFO("Goal reverted. Metric: (%f, %f, %f), Grid: (%d, %d, %d)", + goal_raw_.pose.position.x, goal_raw_.pose.position.y, tf2::getYaw(goal_raw_.pose.orientation), + e[0], e[1], e[2]); + } goal_ = goal_raw_; break; } From 01caafe7d1dfb2714053761b4200f6af1f9aef7c Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Mon, 31 Jul 2023 16:10:10 +0900 Subject: [PATCH 4/4] Fix digits --- planner_cspace/src/planner_3d.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp index 431b1bcb..105fafe8 100644 --- a/planner_cspace/src/planner_3d.cpp +++ b/planner_cspace/src/planner_3d.cpp @@ -514,7 +514,7 @@ class Planner3dNode e.cycleUnsigned(map_info_.angle); if (goal_changed) { - ROS_INFO("New goal received. Metric: (%f, %f, %f), Grid: (%d, %d, %d)", + ROS_INFO("New goal received. Metric: (%.3f, %.3f, %.3f), Grid: (%d, %d, %d)", goal_raw_.pose.position.x, goal_raw_.pose.position.y, tf2::getYaw(goal_raw_.pose.orientation), e[0], e[1], e[2]); clearHysteresis(); @@ -550,7 +550,7 @@ class Planner3dNode goal_.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw)); goal_.pose.position.x = x; goal_.pose.position.y = y; - ROS_INFO("Goal moved. Metric: (%f, %f, %f), Grid: (%d, %d, %d)", x, y, yaw, e[0], e[1], e[2]); + ROS_INFO("Goal moved. Metric: (%.3f, %.3f, %.3f), Grid: (%d, %d, %d)", x, y, yaw, e[0], e[1], e[2]); break; default: Astar::Vec e_prev; @@ -560,7 +560,7 @@ class Planner3dNode tf2::getYaw(goal_.pose.orientation)); if (e[0] != e_prev[0] || e[1] != e_prev[1] || e[2] != e_prev[2]) { - ROS_INFO("Goal reverted. Metric: (%f, %f, %f), Grid: (%d, %d, %d)", + ROS_INFO("Goal reverted. Metric: (%.3f, %.3f, %.3f), Grid: (%d, %d, %d)", goal_raw_.pose.position.x, goal_raw_.pose.position.y, tf2::getYaw(goal_raw_.pose.orientation), e[0], e[1], e[2]); }