state_estimation_node.hpp

Go to the documentation of this file.

// Copyright 2021 Apex.AI, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.



#ifndef STATE_ESTIMATION_NODES__STATE_ESTIMATION_NODE_HPP_
#define STATE_ESTIMATION_NODES__STATE_ESTIMATION_NODE_HPP_


#include <common/types.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/quaternion_stamped.hpp>
#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
#include <kalman_filter/esrcf.hpp>
#include <motion_model/constant_acceleration.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/subscription.hpp>
#include <state_estimation_nodes/kalman_filter_wrapper.hpp>
#include <state_estimation_nodes/measurement.hpp>
#include <state_estimation_nodes/measurement_conversion.hpp>
#include <state_estimation_nodes/visibility_control.hpp>

#include <tf2/buffer_core.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>

#include <Eigen/Geometry>

#include <chrono>
#include <memory>
#include <string>
#include <vector>

namespace autoware
{
namespace prediction
{
namespace state_estimation_nodes
{

class STATE_ESTIMATION_NODES_PUBLIC StateEstimationNode : public rclcpp::Node
{
public:
  explicit StateEstimationNode(
    const rclcpp::NodeOptions & node_options);

private:
  using OdomMsgT = nav_msgs::msg::Odometry;
  using PoseMsgT = geometry_msgs::msg::PoseWithCovarianceStamped;
  using TwistMsgT = geometry_msgs::msg::TwistWithCovarianceStamped;
  using TfMsgT = tf2_msgs::msg::TFMessage;

  template<std::int32_t kDim>
  using VectorT = Eigen::Matrix<float32_t, kDim, 1>;

  void STATE_ESTIMATION_NODES_LOCAL odom_callback(const OdomMsgT::SharedPtr msg);

  void STATE_ESTIMATION_NODES_LOCAL pose_callback(const PoseMsgT::SharedPtr msg);

  void STATE_ESTIMATION_NODES_LOCAL twist_callback(const TwistMsgT::SharedPtr msg);

  void STATE_ESTIMATION_NODES_LOCAL predict_and_publish_current_state();

  void STATE_ESTIMATION_NODES_LOCAL publish_current_state();

  void STATE_ESTIMATION_NODES_LOCAL update_latest_orientation_if_needed(
    const geometry_msgs::msg::QuaternionStamped & rotation);

  geometry_msgs::msg::TransformStamped STATE_ESTIMATION_NODES_LOCAL get_transform(
    const std_msgs::msg::Header & header);

  template<typename MessageT>
  using CallbackFnT = void (StateEstimationNode::*)(const typename MessageT::SharedPtr);

  template<typename MessageT>
  void create_subscriptions(
    const std::vector<std::string> & input_topics,
    std::vector<typename rclcpp::Subscription<MessageT>::SharedPtr> * subscribers,
    CallbackFnT<MessageT> callback);

  template<std::int32_t kNumOfStates, std::int32_t kProcessNoiseDim>
  static Eigen::Matrix<float32_t, kNumOfStates, kProcessNoiseDim> create_GQ_factor(
    const std::chrono::nanoseconds & expected_delta_t,
    const VectorT<kProcessNoiseDim> & process_noise_variances);

  std::vector<rclcpp::Subscription<OdomMsgT>::SharedPtr> m_odom_subscribers;
  std::vector<rclcpp::Subscription<PoseMsgT>::SharedPtr> m_pose_subscribers;
  std::vector<rclcpp::Subscription<TwistMsgT>::SharedPtr> m_twist_subscribers;

  std::shared_ptr<rclcpp::Publisher<OdomMsgT>> m_publisher{};
  std::shared_ptr<rclcpp::Publisher<TfMsgT>> m_tf_publisher{};

  std::chrono::system_clock::duration m_init_timeout{};
  std::chrono::system_clock::duration m_expected_time_between_publish_requests{};
  std::chrono::system_clock::duration m_history_length{};
  std::chrono::system_clock::time_point m_time_of_last_publish{};

  rclcpp::TimerBase::SharedPtr m_wall_timer{};

  common::types::bool8_t m_filter_initialized{};
  common::types::bool8_t m_publish_data_driven{};
  common::types::float64_t m_publish_frequency{};

  std::string m_frame_id{};
  std::string m_child_frame_id{};

  // TODO(igor): we can replace the unique_ptr here with std::variant or alike at a later time to
  // allow configuring which filter to use at runtime.
  std::unique_ptr<ConstantAccelerationFilter> m_ekf{};

  tf2::BufferCore m_tf_buffer;
  tf2_ros::TransformListener m_tf_listener;

  geometry_msgs::msg::QuaternionStamped m_latest_orientation{};
  common::types::float64_t m_min_speed_to_use_speed_orientation{};
};

}  // namespace state_estimation_nodes
}  // namespace prediction
}  // namespace autoware

#endif  // STATE_ESTIMATION_NODES__STATE_ESTIMATION_NODE_HPP_