ego_sensor_canon.hpp

Go to the documentation of this file.

/*
 * Copyright 2020 Robert Bosch GmbH
 *
 * 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.
 *
 * SPDX-License-Identifier: Apache-2.0
 */
#pragma once
#ifndef CLOE_COMPONENT_UTILITY_EGO_SENSOR_CANON_HPP_
#define CLOE_COMPONENT_UTILITY_EGO_SENSOR_CANON_HPP_

#include <math.h>  // for fabs
#include <memory>  // for shared_ptr<>

#include <cloe/component/ego_sensor.hpp>  // for EgoSensor, Object

namespace cloe {
namespace utility {

class EgoSensorCanon : public EgoSensor {
 public:
  explicit EgoSensorCanon(std::shared_ptr<const EgoSensor> ego) : ego_(ego) {}
  const Object& sensed_state() const override { return ego_->sensed_state(); }
  double wheel_steering_angle() const override { return ego_->wheel_steering_angle(); }
  double steering_wheel_speed() const override { return ego_->steering_wheel_speed(); }
  double vehicle_length() const { return sensed_state().dimensions.x(); }
  double vehicle_width() const { return sensed_state().dimensions.y(); }
  double vehicle_height() const { return sensed_state().dimensions.z(); }

  double velocity_as_mps() const { return sensed_state().velocity.norm(); }

  double velocity_as_kmph() const { return velocity_as_mps() * (60 * 60 / 1000.0); }

  double acceleration_as_mpss() const { return sensed_state().acceleration.norm(); }

 protected:
  std::shared_ptr<const EgoSensor> ego_;
};

inline double distance_forward(const Object& o) { return o.pose.translation().x(); }

inline double distance_starboard(const Object& o) { return -o.pose.translation().y(); }

inline bool is_object_fore(const Object& o) { return distance_forward(o) > 1.0E-9; }

inline bool is_object_aft(const Object& o) { return distance_forward(o) < 1.0E-9; }

inline bool is_same_lane(const Object& o) { return fabs(distance_starboard(o)) < 2.5; }

std::shared_ptr<Object> closest_forward(const Objects objects);

}  // namespace utility
}  // namespace cloe

#endif  // CLOE_COMPONENT_UTILITY_EGO_SENSOR_CANON_HPP_