Autoware.Auto
recordreplay_planner_node.hpp
Go to the documentation of this file.
1 // Copyright 2020 Embotech AG, Zurich, Switzerland, inspired by Christopher Ho's mpc code
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 #ifndef RECORDREPLAY_PLANNER_NODES__RECORDREPLAY_PLANNER_NODE_HPP_
15 #define RECORDREPLAY_PLANNER_NODES__RECORDREPLAY_PLANNER_NODE_HPP_
16 
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>
23 
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>
30 #include <motion_common/config.hpp>
31 #include <visualization_msgs/msg/marker.hpp>
32 #include <visualization_msgs/msg/marker_array.hpp>
33 #include <common/types.hpp>
34 
35 #include <rclcpp_action/rclcpp_action.hpp>
36 #include <rclcpp/rclcpp.hpp>
37 
38 #include <string>
39 #include <memory>
40 
42 
43 namespace motion
44 {
45 namespace planning
46 {
47 namespace recordreplay_planner_nodes
48 {
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;
60 
62 class RECORDREPLAY_PLANNER_NODES_PUBLIC RecordReplayPlannerNode : public rclcpp::Node
63 {
64 public:
65  using GoalHandleRecordTrajectory = rclcpp_action::ServerGoalHandle<RecordTrajectory>;
66  using GoalHandleReplayTrajectory = rclcpp_action::ServerGoalHandle<ReplayTrajectory>;
67 
69  explicit RecordReplayPlannerNode(const rclcpp::NodeOptions & node_options);
70 
71 protected:
72  rclcpp_action::Server<RecordTrajectory>::SharedPtr m_recordserver;
73  rclcpp_action::Server<ReplayTrajectory>::SharedPtr m_replayserver;
74  // May be nullptr if disabled
75  rclcpp::Client<ModifyTrajectory>::SharedPtr m_modify_trajectory_client;
76  std::shared_ptr<GoalHandleRecordTrajectory> m_recordgoalhandle;
77  std::shared_ptr<GoalHandleReplayTrajectory> m_replaygoalhandle;
78 
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{};
82  PlannerPtr m_planner{nullptr};
83 
84 private:
91  RECORDREPLAY_PLANNER_NODES_LOCAL Marker to_marker(
92  const TrajectoryPoint & traj_point,
93  const std::string & frame_id,
94  int32_t index,
95  const std::string & ns);
96 
101  RECORDREPLAY_PLANNER_NODES_LOCAL MarkerArray to_markers(
102  const Trajectory & traj, const std::string & ns);
103 
105  RECORDREPLAY_PLANNER_NODES_LOCAL void clear_recorded_markers();
106 
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);
110 
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);
118 
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);
126 
127  std::string m_odom_frame_id{};
128  MarkerArray m_recorded_markers{};
129  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
130  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
131 
132  float64_t m_goal_distance_threshold_m = {};
133  float64_t m_goal_angle_threshold_rad;
134  std::string m_recording_frame = "map";
135 
136  bool8_t m_enable_loop = false;
137  float64_t m_max_loop_gap_m = 0.0;
138 }; // class RecordReplayPlannerNode
139 } // namespace recordreplay_planner_nodes
140 } // namespace planning
141 } // namespace motion
142 
143 #endif // RECORDREPLAY_PLANNER_NODES__RECORDREPLAY_PLANNER_NODE_HPP_
motion::planning::recordreplay_planner_nodes::RecordReplayPlannerNode
ROS Node for recording and replaying trajectories.
Definition: recordreplay_planner_node.hpp:62
motion::motion_testing_nodes::TrajectoryPoint
autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint
Definition: motion_testing_publisher.hpp:36
motion::motion_common::Real
decltype(autoware_auto_planning_msgs::msg::TrajectoryPoint::longitudinal_velocity_mps) Real
Definition: motion_common.hpp:37
autoware::rviz_plugins::object_detection::detail::Marker
visualization_msgs::msg::Marker Marker
Definition: object_polygon_detail.cpp:31
visibility_control.hpp
autoware::off_map_obstacles_filter_nodes::MarkerArray
visualization_msgs::msg::MarkerArray MarkerArray
Definition: off_map_obstacles_filter_node.cpp:39
motion::planning::recordreplay_planner_nodes::RecordReplayPlannerNode::m_recordserver
rclcpp_action::Server< RecordTrajectory >::SharedPtr m_recordserver
Definition: recordreplay_planner_node.hpp:72
types.hpp
This file includes common type definition.
motion::planning::recordreplay_planner_nodes::RecordReplayPlannerNode::m_replaygoalhandle
std::shared_ptr< GoalHandleReplayTrajectory > m_replaygoalhandle
Definition: recordreplay_planner_node.hpp:77
motion::planning::recordreplay_planner_nodes::RecordReplayPlannerNode::m_modify_trajectory_client
rclcpp::Client< ModifyTrajectory >::SharedPtr m_modify_trajectory_client
Definition: recordreplay_planner_node.hpp:75
motion::planning::recordreplay_planner_nodes::RecordReplayPlannerNode::m_replayserver
rclcpp_action::Server< ReplayTrajectory >::SharedPtr m_replayserver
Definition: recordreplay_planner_node.hpp:73
autoware::common::types::bool8_t
bool bool8_t
Definition: types.hpp:39
config.hpp
motion::planning::recordreplay_planner_nodes::RecordReplayPlannerNode::m_recordgoalhandle
std::shared_ptr< GoalHandleRecordTrajectory > m_recordgoalhandle
Definition: recordreplay_planner_node.hpp:76
motion::planning::recordreplay_planner_nodes::Transform
geometry_msgs::msg::TransformStamped Transform
Definition: recordreplay_planner_node.hpp:56
motion::motion_testing_nodes::TransformStamped
geometry_msgs::msg::TransformStamped TransformStamped
Definition: motion_testing_publisher.hpp:37
motion_common.hpp
motion
Definition: controller_base.hpp:31
motion::planning::recordreplay_planner_nodes::PlannerPtr
std::unique_ptr< motion::planning::recordreplay_planner::RecordReplayPlanner > PlannerPtr
Definition: recordreplay_planner_node.hpp:49
motion::motion_common::Trajectory
autoware_auto_planning_msgs::msg::Trajectory Trajectory
Definition: motion_common.hpp:41
motion::planning::recordreplay_planner_nodes::RecordReplayPlannerNode::GoalHandleRecordTrajectory
rclcpp_action::ServerGoalHandle< RecordTrajectory > GoalHandleRecordTrajectory
Definition: recordreplay_planner_node.hpp:65
autoware::common::types::float64_t
double float64_t
Definition: types.hpp:47
motion::planning::recordreplay_planner_nodes::State
autoware_auto_vehicle_msgs::msg::VehicleKinematicState State
Definition: recordreplay_planner_node.hpp:55
autoware::trajectory_spoofer::VehicleKinematicState
autoware_auto_vehicle_msgs::msg::VehicleKinematicState VehicleKinematicState
Definition: trajectory_spoofer.hpp:42
recordreplay_planner.hpp
motion::planning::recordreplay_planner_nodes::RecordReplayPlannerNode::GoalHandleReplayTrajectory
rclcpp_action::ServerGoalHandle< ReplayTrajectory > GoalHandleReplayTrajectory
Definition: recordreplay_planner_node.hpp:66