motion_computation_worker.hpp

Go to the documentation of this file.

// Copyright 2019-2023 Leidos
//
// 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 MOTION_COMPUTATION__MOTION_COMPUTATION_WORKER_HPP_
#define MOTION_COMPUTATION__MOTION_COMPUTATION_WORKER_HPP_
 
#include <gtest/gtest_prod.h>
#include <lanelet2_extension/projection/local_frame_projector.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
#include <functional>
#include <memory>
#include <string>
#include <tuple>
#include <unordered_map>
 
#include <motion_predict/motion_predict.hpp>
#include <motion_predict/predict_ctrv.hpp>
#include <rclcpp/rclcpp.hpp>
 
#include <carma_perception_msgs/msg/external_object.hpp>
#include <carma_perception_msgs/msg/external_object_list.hpp>
#include <carma_v2x_msgs/msg/bsm.hpp>
#include <carma_v2x_msgs/msg/mobility_path.hpp>
#include <carma_v2x_msgs/msg/psm.hpp>
#include <std_msgs/msg/string.hpp>
 
namespace motion_computation
{
class MotionComputationWorker
{
public:
  using PublishObjectCallback =
    std::function<void(const carma_perception_msgs::msg::ExternalObjectList &)>;
  using LookUpTransform = std::function<void()>;
 
  MotionComputationWorker(
    const PublishObjectCallback & obj_pub,
    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger,
    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock);
  void predictionLogic(carma_perception_msgs::msg::ExternalObjectList::UniquePtr obj_list);
 
  // Setters for the prediction parameters
  void setPredictionTimeStep(double time_step);
  void setPredictionPeriod(double period);
  void setXAccelerationNoise(double noise);
  void setYAccelerationNoise(double noise);
  void setProcessNoiseMax(double noise_max);
  void setConfidenceDropRate(double drop_rate);
  void setDetectionInputFlags(
    bool enable_sensor_processing, bool enable_bsm_processing, bool enable_psm_processing,
    bool enable_mobility_path_processing);
  void setDetectionMotionModelFlags(
    bool enable_ctrv_for_unknown_obj, bool enable_ctrv_for_motorcycle_obj,
    bool enable_ctrv_for_small_vehicle_obj, bool enable_ctrv_for_large_vehicle_obj,
    bool enable_ctrv_for_pedestrian_obj);
 
  // callbacks
  void mobilityPathCallback(const carma_v2x_msgs::msg::MobilityPath::UniquePtr msg);
 
  void bsmCallback(const carma_v2x_msgs::msg::BSM::UniquePtr msg);
 
  void psmCallback(const carma_v2x_msgs::msg::PSM::UniquePtr msg);
 
  void georeferenceCallback(const std_msgs::msg::String::UniquePtr msg);
 
  carma_perception_msgs::msg::ExternalObject mobilityPathToExternalObject(
    const carma_v2x_msgs::msg::MobilityPath::UniquePtr & msg) const;
 
  carma_perception_msgs::msg::ExternalObjectList synchronizeAndAppend(
    const carma_perception_msgs::msg::ExternalObjectList & base_objects,
    carma_perception_msgs::msg::ExternalObjectList new_objects) const;
 
  carma_perception_msgs::msg::ExternalObject matchAndInterpolateTimeStamp(
    carma_perception_msgs::msg::ExternalObject path, const rclcpp::Time & time_to_match) const;
 
private:
  // Local copy of external object publisher
  PublishObjectCallback obj_pub_;
 
  // Prediction parameters
 
  double prediction_time_step_ = 0.1;  // Seconds
  double prediction_period_ = 2.0;     // Seconds
  double cv_x_accel_noise_ = 9.0;
  double cv_y_accel_noise_ = 9.0;
  double prediction_process_noise_max_ = 1000.0;
  double prediction_confidence_drop_rate_ = 0.9;
 
  // Flags for the different possible detection inputs
  bool enable_sensor_processing_ = true;
  bool enable_bsm_processing_ = false;
  bool enable_psm_processing_ = false;
  bool enable_mobility_path_processing_ = false;
 
  // Flags for the different possible motion models for detections
  bool enable_ctrv_for_unknown_obj_ = true;
  bool enable_ctrv_for_motorcycle_obj_ = true;
  bool enable_ctrv_for_small_vehicle_obj_ = true;
  bool enable_ctrv_for_large_vehicle_obj_ = true;
  bool enable_ctrv_for_pedestrian_obj_ = false;
 
  // Map frame
  std::string map_frame_id_ = "map";
 
  // Logger interface
  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_;
  // Clock interface - gets the ros simulated clock from Node
  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
 
  // Queue for v2x msgs to synchronize them with sensor msgs
  carma_perception_msgs::msg::ExternalObjectList mobility_path_list_;
  carma_perception_msgs::msg::ExternalObjectList bsm_list_;
  carma_perception_msgs::msg::ExternalObjectList psm_list_;
 
  // Maps of external object id to index in synchronization queues
  std::unordered_map<uint32_t, size_t> mobility_path_obj_id_map_;
  std::unordered_map<uint32_t, size_t> bsm_obj_id_map_;
  std::unordered_map<uint32_t, size_t> psm_obj_id_map_;
 
  std::string georeference_{""};
  std::shared_ptr<lanelet::projection::LocalFrameProjector> map_projector_;
 
  // Rotation of a North East Down frame located on the map origin described in the map frame
  tf2::Quaternion ned_in_map_rotation_;
 
  // Unit Test Accessors
  FRIEND_TEST(MotionComputationWorker, MobilityPathToExternalObject);
  FRIEND_TEST(MotionComputationWorker, PsmToExternalObject);
  FRIEND_TEST(MotionComputationWorker, BSMtoExternalObject);
};
 
}  // namespace motion_computation
 
#endif  // MOTION_COMPUTATION__MOTION_COMPUTATION_WORKER_HPP_