15 #include <autoware_auto_planning_msgs/srv/modify_trajectory.hpp>
16 #include <autoware_auto_perception_msgs/msg/bounding_box_array.hpp>
17 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
18 #include <visualization_msgs/msg/marker_array.hpp>
19 #include <visualization_msgs/msg/marker.hpp>
20 #include <tf2_ros/transform_listener.h>
21 #include <tf2_ros/buffer.h>
22 #include <rclcpp/rclcpp.hpp>
29 #ifndef OBJECT_COLLISION_ESTIMATOR_NODES__OBJECT_COLLISION_ESTIMATOR_NODE_HPP_
30 #define OBJECT_COLLISION_ESTIMATOR_NODES__OBJECT_COLLISION_ESTIMATOR_NODE_HPP_
36 namespace object_collision_estimator_nodes
41 using autoware_auto_perception_msgs::msg::PredictedObjects;
63 void estimate_collision(
64 const std::shared_ptr<autoware_auto_planning_msgs::srv::ModifyTrajectory::Request> request,
65 std::shared_ptr<autoware_auto_planning_msgs::srv::ModifyTrajectory::Response> response);
68 rclcpp::Subscription<BoundingBoxArray>::SharedPtr m_obstacles_sub{
nullptr};
71 rclcpp::Subscription<PredictedObjects>::SharedPtr m_predicted_objects_sub{
nullptr};
74 rclcpp::Publisher<MarkerArray>::SharedPtr m_trajectory_bbox_pub{
nullptr};
79 void on_predicted_object(
const PredictedObjects::SharedPtr & msg);
83 std::unique_ptr<ObjectCollisionEstimator> m_estimator{
nullptr};
88 std::string m_target_frame_id{};
91 rclcpp::Service<autoware_auto_planning_msgs::srv::ModifyTrajectory>::SharedPtr
92 m_service_interface{
nullptr};
95 std::shared_ptr<tf2_ros::Buffer> m_tf_buffer;
99 std::shared_ptr<tf2_ros::TransformListener> m_tf_listener;
103 rclcpp::TimerBase::SharedPtr m_wall_timer{
nullptr};
106 rclcpp::Time m_last_obstacle_msg_time {0, 0, RCL_ROS_TIME};
109 std::chrono::milliseconds m_staleness_threshold_ms{};
112 static constexpr
const char * OBJECT_COLLISION_ESTIMATOR_NODE_NAME =
113 "object_collision_estimator_node";
116 static constexpr
const char * OBSTACLE_TOPIC =
"obstacle_topic";
123 #endif // OBJECT_COLLISION_ESTIMATOR_NODES__OBJECT_COLLISION_ESTIMATOR_NODE_HPP_