Autoware.Auto
state_estimation_node.hpp
Go to the documentation of this file.
1 // Copyright 2021 Apex.AI, Inc.
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 //
15 // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
16 
19 
20 
21 #ifndef STATE_ESTIMATION_NODES__STATE_ESTIMATION_NODE_HPP_
22 #define STATE_ESTIMATION_NODES__STATE_ESTIMATION_NODE_HPP_
23 
24 
25 #include <autoware_auto_geometry_msgs/msg/relative_position_with_covariance_stamped.hpp>
26 #include <common/types.hpp>
27 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
28 #include <geometry_msgs/msg/quaternion_stamped.hpp>
29 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
31 #include <nav_msgs/msg/odometry.hpp>
32 #include <rclcpp/publisher.hpp>
33 #include <rclcpp/rclcpp.hpp>
34 #include <rclcpp/subscription.hpp>
37 
38 #include <tf2/buffer_core.h>
39 #include <tf2_ros/transform_broadcaster.h>
40 #include <tf2_ros/transform_listener.h>
41 
42 #include <Eigen/Geometry>
43 
44 #include <chrono>
45 #include <memory>
46 #include <string>
47 #include <vector>
48 
49 namespace autoware
50 {
51 namespace common
52 {
53 namespace state_estimation
54 {
55 
59 class STATE_ESTIMATION_NODES_PUBLIC StateEstimationNode : public rclcpp::Node
60 {
61 public:
67  explicit StateEstimationNode(
68  const rclcpp::NodeOptions & node_options);
69 
71  tf2::BufferCore & buffer() {return m_tf_buffer;}
72 
73 private:
74  using OdomMsgT = nav_msgs::msg::Odometry;
75  using PoseMsgT = geometry_msgs::msg::PoseWithCovarianceStamped;
76  using TwistMsgT = geometry_msgs::msg::TwistWithCovarianceStamped;
77  using RelativePosMsgT = autoware_auto_geometry_msgs::msg::RelativePositionWithCovarianceStamped;
78  using TfMsgT = tf2_msgs::msg::TFMessage;
79  using FilterWrapperT = ConstantAccelerationFilterWrapperXYZRPY;
80 
81  template<std::int32_t kDim>
82  using VectorT = Eigen::Matrix<autoware::common::types::float32_t, kDim, 1>;
83 
88  void STATE_ESTIMATION_NODES_LOCAL pose_callback(const PoseMsgT::SharedPtr msg);
89 
94  void STATE_ESTIMATION_NODES_LOCAL relative_pos_callback(const RelativePosMsgT::SharedPtr msg);
95 
97  void STATE_ESTIMATION_NODES_LOCAL predict_and_publish_current_state();
98 
100  void STATE_ESTIMATION_NODES_LOCAL publish_current_state();
101 
102  template<typename MessageT>
103  using CallbackFnT = void (StateEstimationNode::*)(const typename MessageT::SharedPtr);
104 
105  template<typename MessageT>
106  void create_subscriptions(
107  const std::vector<std::string> & input_topics,
108  std::vector<typename rclcpp::Subscription<MessageT>::SharedPtr> * subscribers,
109  CallbackFnT<MessageT> callback);
110 
111  std::vector<rclcpp::Subscription<PoseMsgT>::SharedPtr> m_pose_subscribers;
112  std::vector<rclcpp::Subscription<RelativePosMsgT>::SharedPtr> m_relative_pos_subscribers;
113 
114  std::shared_ptr<rclcpp::Publisher<OdomMsgT>> m_publisher{};
115  std::shared_ptr<rclcpp::Publisher<TfMsgT>> m_tf_publisher{};
116 
117  std::chrono::system_clock::time_point m_time_of_last_publish{};
118 
119  rclcpp::TimerBase::SharedPtr m_wall_timer{};
120 
121  common::types::bool8_t m_publish_data_driven{};
122  common::types::float64_t m_publish_frequency{};
123 
124  std::string m_frame_id{};
125  std::string m_child_frame_id{};
126 
127  // TODO(igor): we can replace the unique_ptr here with std::variant or alike at a later time to
128  // allow configuring which filter to use at runtime.
129  std::unique_ptr<FilterWrapperT> m_ekf{};
130 
131  tf2::BufferCore m_tf_buffer;
132  tf2_ros::TransformListener m_tf_listener;
133 
134  double noise_add_to_variance;
135 };
136 
137 } // namespace state_estimation
138 } // namespace common
139 } // namespace autoware
140 
141 #endif // STATE_ESTIMATION_NODES__STATE_ESTIMATION_NODE_HPP_
motion::motion_testing_nodes::TFMessage
tf2_msgs::msg::TFMessage TFMessage
Definition: motion_testing_publisher.hpp:34
types.hpp
This file includes common type definition.
measurement_transformation.hpp
autoware::common::types::bool8_t
bool bool8_t
Definition: types.hpp:39
kalman_filter_wrapper.hpp
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
autoware::common::state_estimation::StateEstimationNode::buffer
tf2::BufferCore & buffer()
Get the tf2::buffer used by the node.
Definition: state_estimation_node.hpp:71
autoware::common::state_estimation::StateEstimationNode
Definition: state_estimation_node.hpp:59
autoware::drivers::vehicle_interface::Odometry
autoware_auto_vehicle_msgs::msg::VehicleOdometry Odometry
Definition: safety_state_machine.hpp:50
autoware::common::state_estimation::ConstantAccelerationFilterWrapperXYZRPY
KalmanFilterWrapper< ConstAccelerationKalmanFilterXYZRPY > ConstantAccelerationFilterWrapperXYZRPY
Definition: kalman_filter_wrapper.hpp:206
visibility_control.hpp
autoware::common::types::float64_t
double float64_t
Definition: types.hpp:47