Autoware.Auto
motion_testing_publisher.hpp
Go to the documentation of this file.
1 // Copyright 2019 Christopher Ho
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 MOTION_TESTING_NODES__MOTION_TESTING_PUBLISHER_HPP_
15 #define MOTION_TESTING_NODES__MOTION_TESTING_PUBLISHER_HPP_
16 
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>
21 
22 #include <rclcpp/rclcpp.hpp>
23 
24 #include <chrono>
25 #include <list>
26 #include <string>
27 #include <vector>
28 
29 namespace motion
30 {
31 namespace motion_testing_nodes
32 {
38 
42 struct MOTION_TESTING_NODES_PUBLIC TrajectoryProfile
43 {
45  std::chrono::nanoseconds active_time;
46  std::chrono::nanoseconds trajectory_sample_period;
47  std::chrono::nanoseconds state_update_period;
48  std::string trajectory_frame;
49  std::string ego_frame;
50 }; // struct TrajectoryProfile
51 
52 using TrajectoryProfilesVec = std::vector<TrajectoryProfile>;
53 
58 class MOTION_TESTING_NODES_PUBLIC MotionTestingPublisher : public rclcpp::Node
59 {
60 public:
62  const std::string & node_name,
63  const std::string & traj_topic,
64  const std::string & state_topic,
65  const std::string & tf_topic,
66  const TrajectoryProfilesVec & profiles);
67 
69  bool done() const noexcept;
70 
72  void match(
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;
76 
77 private:
78  using Header = std_msgs::msg::Header;
80  MOTION_TESTING_NODES_LOCAL void on_timer();
81  // Initialization helper functions
82  MOTION_TESTING_NODES_LOCAL
83  std::chrono::nanoseconds min_period(const TrajectoryProfilesVec & profiles) const noexcept;
84  MOTION_TESTING_NODES_LOCAL
85  std::size_t end_index(const TrajectoryProfile & prof, const Trajectory & traj) const noexcept;
86  MOTION_TESTING_NODES_LOCAL void add_state_and_tf(
87  const Header & header,
88  const TrajectoryProfile & prof,
89  const TrajectoryPoint & pt) noexcept;
90 
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;
102 }; // class MotionTestingPublisher
103 } // namespace motion_testing_nodes
104 } // namespace motion
105 
106 #endif // MOTION_TESTING_NODES__MOTION_TESTING_PUBLISHER_HPP_
motion::motion_testing_nodes::TrajectoryPoint
autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint
Definition: motion_testing_publisher.hpp:36
motion::motion_testing_nodes::TFMessage
tf2_msgs::msg::TFMessage TFMessage
Definition: motion_testing_publisher.hpp:34
motion::motion_testing_nodes::TrajectoryProfilesVec
std::vector< TrajectoryProfile > TrajectoryProfilesVec
Definition: motion_testing_publisher.hpp:52
motion::motion_testing_nodes::Trajectory
autoware_auto_planning_msgs::msg::Trajectory Trajectory
Definition: motion_testing_publisher.hpp:35
motion::motion_testing_nodes::TrajectoryProfile::trajectory_sample_period
std::chrono::nanoseconds trajectory_sample_period
Definition: motion_testing_publisher.hpp:46
motion::motion_testing_nodes::TrajectoryProfile::start_state
State start_state
Definition: motion_testing_publisher.hpp:44
motion::motion_testing_nodes::State
autoware_auto_vehicle_msgs::msg::VehicleKinematicState State
Definition: motion_testing_publisher.hpp:33
motion::motion_testing_nodes::TrajectoryProfile
Definition: motion_testing_publisher.hpp:42
motion::motion_testing_nodes::TrajectoryProfile::state_update_period
std::chrono::nanoseconds state_update_period
Definition: motion_testing_publisher.hpp:47
motion::motion_testing_nodes::TrajectoryProfile::active_time
std::chrono::nanoseconds active_time
Definition: motion_testing_publisher.hpp:45
motion::motion_testing_nodes::TransformStamped
geometry_msgs::msg::TransformStamped TransformStamped
Definition: motion_testing_publisher.hpp:37
motion::motion_testing_nodes::MotionTestingPublisher
Definition: motion_testing_publisher.hpp:58
visibility_control.hpp
motion
Definition: controller_base.hpp:31
motion::motion_testing_nodes::TrajectoryProfile::trajectory_frame
std::string trajectory_frame
Definition: motion_testing_publisher.hpp:48
autoware::trajectory_spoofer::VehicleKinematicState
autoware_auto_vehicle_msgs::msg::VehicleKinematicState VehicleKinematicState
Definition: trajectory_spoofer.hpp:42
motion::motion_testing_nodes::TrajectoryProfile::ego_frame
std::string ego_frame
Definition: motion_testing_publisher.hpp:49