14 #ifndef MOTION_TESTING_NODES__MOTION_TESTING_PUBLISHER_HPP_
15 #define MOTION_TESTING_NODES__MOTION_TESTING_PUBLISHER_HPP_
18 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
19 #include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>
20 #include <tf2_msgs/msg/tf_message.hpp>
22 #include <rclcpp/rclcpp.hpp>
31 namespace motion_testing_nodes
62 const std::string & node_name,
63 const std::string & traj_topic,
64 const std::string & state_topic,
65 const std::string & tf_topic,
69 bool done()
const noexcept;
73 const std::size_t tf_subs = 1U,
74 const std::size_t state_subs = 1U,
75 const std::size_t traj_subs = 1U)
const;
78 using Header = std_msgs::msg::Header;
80 MOTION_TESTING_NODES_LOCAL
void on_timer();
82 MOTION_TESTING_NODES_LOCAL
84 MOTION_TESTING_NODES_LOCAL
86 MOTION_TESTING_NODES_LOCAL
void add_state_and_tf(
87 const Header & header,
91 rclcpp::Publisher<TFMessage>::SharedPtr m_tf_pub;
92 rclcpp::Publisher<State>::SharedPtr m_state_pub;
93 rclcpp::Publisher<Trajectory>::SharedPtr m_traj_pub;
94 rclcpp::TimerBase::SharedPtr m_timer;
95 std::list<State> m_states;
96 std::list<Trajectory> m_trajectories;
97 std::list<TFMessage> m_tfs;
98 std::chrono::system_clock::time_point m_last_traj_time;
99 std::chrono::system_clock::time_point m_last_state_time;
100 std::list<std::chrono::nanoseconds> m_state_period;
101 std::list<std::chrono::nanoseconds> m_traj_period;
106 #endif // MOTION_TESTING_NODES__MOTION_TESTING_PUBLISHER_HPP_