feat: set start position from robot pose

navyu_planner/config/navyu_global_planner_params.yaml

@@ -1,5 +1,7 @@
 navyu_global_planner_node:
   ros__parameters:
-    map_frame_id: "map"
+    map_frame: "map"
 
-    lethal_costmap_threshold: 45
+    base_frame: "base_link"
+
+    lethal_cost_threshold: 45.0

navyu_planner/include/navyu_planner/astar_planner.hpp

@@ -7,7 +7,6 @@
 #include <Eigen/Core>
 
 #include <geometry_msgs/msg/pose.hpp>
-#include <nav_msgs/msg/detail/occupancy_grid__struct.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 
 #include <queue>

navyu_planner/include/navyu_planner/navyu_global_planner.hpp

@@ -7,10 +7,12 @@
 
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
-#include <nav_msgs/msg/detail/occupancy_grid__struct.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/path.hpp>
 
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_broadcaster.h>
+
 class NavyuGlobalPlanner : public rclcpp::Node
 {
 public:
@@ -28,14 +30,20 @@ class NavyuGlobalPlanner : public rclcpp::Node
 
   void wait_for_costmap();
 
+  bool get_robot_pose(geometry_msgs::msg::Pose & robot_pose);
+
 private:
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_pose_subscriber_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_subscriber_;
   rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_publisher_;
 
+  tf2_ros::Buffer tf_buffer_{get_clock()};
+  std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
+
   std::shared_ptr<AstarPlanner> planner_;
 
-  std::string map_frame_id_;
+  std::string map_frame_;
+  std::string base_frame_;
 
   bool costmap_initialized_{false};
 };

navyu_planner/src/navyu_global_planner.cpp

@@ -1,11 +1,15 @@
 #include "navyu_planner/navyu_global_planner.hpp"
 
+#include <geometry_msgs/msg/detail/transform_stamped__struct.hpp>
+
 NavyuGlobalPlanner::NavyuGlobalPlanner(const rclcpp::NodeOptions & node_options)
 : Node("navyu_global_planner", node_options)
 {
-  map_frame_id_ = declare_parameter<std::string>("map_frame_id");
+  map_frame_ = declare_parameter<std::string>("map_frame");
+  base_frame_ = declare_parameter<std::string>("base_frame");
+  declare_parameter<double>("lethal_cost_threshold");
 
-  declare_parameter<double>("lethal_cost_threshold", 45);
+  broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
 
   goal_pose_subscriber_ = create_subscription<geometry_msgs::msg::PoseStamped>(
     "goal_pose", 5,
@@ -31,6 +35,27 @@ void NavyuGlobalPlanner::wait_for_costmap()
   }
 }
 
+bool NavyuGlobalPlanner::get_robot_pose(geometry_msgs::msg::Pose & robot_pose)
+{
+  geometry_msgs::msg::TransformStamped robot_pose_frame;
+
+  try {
+    robot_pose_frame = tf_buffer_.lookupTransform(
+      map_frame_, base_frame_, tf2::TimePointZero, tf2::durationFromSec(0.5));
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR_STREAM(
+      get_logger(), "Can not get Transform " << map_frame_ << " to " << base_frame_);
+    return false;
+  }
+
+  robot_pose.position.x = robot_pose_frame.transform.translation.x;
+  robot_pose.position.y = robot_pose_frame.transform.translation.y;
+  robot_pose.position.z = robot_pose_frame.transform.translation.z;
+  robot_pose.orientation = robot_pose_frame.transform.rotation;
+
+  return true;
+}
+
 bool NavyuGlobalPlanner::plan(
   const geometry_msgs::msg::Pose start, const geometry_msgs::msg::Pose goal,
   std::vector<Node2D *> & path)
@@ -52,7 +77,7 @@ bool NavyuGlobalPlanner::plan(
 void NavyuGlobalPlanner::publish_path(std::vector<Node2D *> path)
 {
   nav_msgs::msg::Path path_msgs;
-  path_msgs.header.frame_id = map_frame_id_;
+  path_msgs.header.frame_id = map_frame_;
   path_msgs.header.stamp = now();
 
   for (auto & node : path) {
@@ -69,16 +94,20 @@ void NavyuGlobalPlanner::publish_path(std::vector<Node2D *> path)
 void NavyuGlobalPlanner::callback_goal_pose(const geometry_msgs::msg::PoseStamped & msg)
 {
   if (planner_ == nullptr) {
-    RCLCPP_ERROR_STREAM(get_logger(), "planner is not Initialized.");
+    RCLCPP_ERROR_STREAM(get_logger(), "Planner is not Initialized.");
     return;
   }
 
   geometry_msgs::msg::Pose start;
-  start.position.x = 0.0;
-  start.position.y = 0.0;
+  if (!get_robot_pose(start)) {
+    RCLCPP_ERROR_STREAM(get_logger(), "Can not get Robot Pose.");
+    return;
+  }
+
+  geometry_msgs::msg::Pose goal = msg.pose;
 
   std::vector<Node2D *> path;
-  if (plan(start, msg.pose, path)) {
+  if (plan(start, goal, path)) {
     publish_path(path);
   }
 }