14 #ifndef RECORDREPLAY_PLANNER_NODES__RECORDREPLAY_PLANNER_NODE_HPP_
15 #define RECORDREPLAY_PLANNER_NODES__RECORDREPLAY_PLANNER_NODE_HPP_
17 #include <tf2_ros/transform_listener.h>
18 #include <tf2_ros/buffer.h>
21 #include <autoware_auto_planning_msgs/action/record_trajectory.hpp>
22 #include <autoware_auto_planning_msgs/action/replay_trajectory.hpp>
24 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
25 #include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
26 #include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>
27 #include <autoware_auto_planning_msgs/srv/modify_trajectory.hpp>
28 #include <geometry_msgs/msg/transform_stamped.hpp>
31 #include <visualization_msgs/msg/marker.hpp>
32 #include <visualization_msgs/msg/marker_array.hpp>
35 #include <rclcpp_action/rclcpp_action.hpp>
36 #include <rclcpp/rclcpp.hpp>
47 namespace recordreplay_planner_nodes
49 using PlannerPtr = std::unique_ptr<motion::planning::recordreplay_planner::RecordReplayPlanner>;
52 using autoware_auto_planning_msgs::srv::ModifyTrajectory;
53 using autoware_auto_planning_msgs::action::RecordTrajectory;
54 using autoware_auto_planning_msgs::action::ReplayTrajectory;
79 rclcpp::Subscription<State>::SharedPtr m_ego_sub{};
80 rclcpp::Publisher<Trajectory>::SharedPtr m_trajectory_pub{};
81 rclcpp::Publisher<MarkerArray>::SharedPtr m_trajectory_viz_pub{};
91 RECORDREPLAY_PLANNER_NODES_LOCAL
Marker to_marker(
93 const std::string & frame_id,
95 const std::string & ns);
101 RECORDREPLAY_PLANNER_NODES_LOCAL
MarkerArray to_markers(
102 const Trajectory & traj,
const std::string & ns);
105 RECORDREPLAY_PLANNER_NODES_LOCAL
void clear_recorded_markers();
107 RECORDREPLAY_PLANNER_NODES_LOCAL
void on_ego(
const State::SharedPtr & msg);
108 RECORDREPLAY_PLANNER_NODES_LOCAL
void modify_trajectory_response(
109 rclcpp::Client<ModifyTrajectory>::SharedFuture future);
111 RECORDREPLAY_PLANNER_NODES_LOCAL rclcpp_action::GoalResponse record_handle_goal(
112 const rclcpp_action::GoalUUID & uuid,
113 const std::shared_ptr<const RecordTrajectory::Goal> goal);
114 RECORDREPLAY_PLANNER_NODES_LOCAL rclcpp_action::CancelResponse record_handle_cancel(
115 const std::shared_ptr<GoalHandleRecordTrajectory> goal_handle);
116 RECORDREPLAY_PLANNER_NODES_LOCAL
void record_handle_accepted(
117 const std::shared_ptr<GoalHandleRecordTrajectory> goal_handle);
119 RECORDREPLAY_PLANNER_NODES_LOCAL rclcpp_action::GoalResponse replay_handle_goal(
120 const rclcpp_action::GoalUUID & uuid,
121 const std::shared_ptr<const ReplayTrajectory::Goal> goal);
122 RECORDREPLAY_PLANNER_NODES_LOCAL rclcpp_action::CancelResponse replay_handle_cancel(
123 const std::shared_ptr<GoalHandleReplayTrajectory> goal_handle);
124 RECORDREPLAY_PLANNER_NODES_LOCAL
void replay_handle_accepted(
125 const std::shared_ptr<GoalHandleReplayTrajectory> goal_handle);
127 std::string m_odom_frame_id{};
129 std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
130 std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
132 float64_t m_goal_distance_threshold_m = {};
134 std::string m_recording_frame =
"map";
143 #endif // RECORDREPLAY_PLANNER_NODES__RECORDREPLAY_PLANNER_NODE_HPP_