17 #ifndef LANELET2_GLOBAL_PLANNER_NODES__LANELET2_GLOBAL_PLANNER_NODE_HPP_
18 #define LANELET2_GLOBAL_PLANNER_NODES__LANELET2_GLOBAL_PLANNER_NODE_HPP_
20 #include <rclcpp/rclcpp.hpp>
21 #include <rclcpp_action/rclcpp_action.hpp>
22 #include <std_msgs/msg/string.hpp>
23 #include <geometry_msgs/msg/pose_stamped.hpp>
24 #include <tf2/buffer_core.h>
25 #include <tf2_ros/transform_listener.h>
31 #include <lanelet2_core/LaneletMap.h>
33 #include <autoware_auto_mapping_msgs/srv/had_map_service.hpp>
34 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
35 #include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
36 #include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>
47 using namespace std::chrono_literals;
56 namespace lanelet2_global_planner_nodes
63 void request_osm_binary_map();
64 void goal_pose_cb(
const geometry_msgs::msg::PoseStamped::SharedPtr msg);
65 void current_pose_cb(
const autoware_auto_vehicle_msgs::msg::VehicleKinematicState::SharedPtr msg);
66 void send_global_path(
67 const std::vector<lanelet::Id> & had_map_route,
70 const std_msgs::msg::Header & header);
72 const geometry_msgs::msg::PoseStamped & pose_in, geometry_msgs::msg::PoseStamped & pose_out);
76 rclcpp::Client<autoware_auto_mapping_msgs::srv::HADMapService>::SharedPtr map_client;
77 rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_pose_sub_ptr;
78 rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::VehicleKinematicState>::SharedPtr
80 rclcpp::Publisher<autoware_auto_planning_msgs::msg::HADMapRoute>::SharedPtr global_path_pub_ptr;
81 geometry_msgs::msg::PoseStamped start_pose;
82 geometry_msgs::msg::PoseStamped goal_pose;
84 tf2::BufferCore tf_buffer;
85 tf2_ros::TransformListener tf_listener;
91 #endif // LANELET2_GLOBAL_PLANNER_NODES__LANELET2_GLOBAL_PLANNER_NODE_HPP_