Program Listing for File sensor.hpp

↰ Return to documentation for file (/tmp/ws/src/aerostack2/as2_core/include/as2_core/sensor.hpp)

// Copyright 2023 Universidad Politécnica de Madrid
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright
//      notice, this list of conditions and the following disclaimer.
//
//    * Redistributions in binary form must reproduce the above copyright
//      notice, this list of conditions and the following disclaimer in the
//      documentation and/or other materials provided with the distribution.
//
//    * Neither the name of the Universidad Politécnica de Madrid nor the names of its
//      contributors may be used to endorse or promote products derived from
//      this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

/*!*******************************************************************************************
 *  \file       sensor.hpp
 *  \brief      Sensor class for AS2 header file
 *  \authors    Rafael Pérez Seguí
 *              David Pérez Saura
 *              Miguel Fernández Cortizas
 *              Pedro Arias Pérez
 ********************************************************************************/

#ifndef AS2_CORE__SENSOR_HPP_
#define AS2_CORE__SENSOR_HPP_

#include <cv_bridge/cv_bridge.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <opencv2/opencv.hpp>
#include <image_transport/image_transport.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/publisher_options.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/battery_state.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/fluid_pressure.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/magnetic_field.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <sensor_msgs/msg/range.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/quaternion_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include "as2_core/custom/tf2_geometry_msgs.hpp"
#include "as2_core/names/topics.hpp"
#include "as2_core/node.hpp"
#include "as2_core/utils/tf_utils.hpp"

namespace as2
{
namespace sensors
{

class TFStatic
{
public:
  explicit TFStatic(rclcpp::Node * node_ptr);

  virtual ~TFStatic();

  virtual void setStaticTransform(
    const geometry_msgs::msg::TransformStamped & transformStamped);

  virtual void setStaticTransform(
    const std::string & frame_id, const std::string & parent_frame_id, float x, float y, float z,
    float qx, float qy, float qz, float qw);

  virtual void setStaticTransform(
    const std::string & frame_id, const std::string & parent_frame_id, float x, float y, float z,
    float roll, float pitch, float yaw);

  const rclcpp::Node * getNode() const;

private:
  rclcpp::Node * node_ptr_ = nullptr;

  virtual void setStaticTransform_(const geometry_msgs::msg::TransformStamped & transformStamped);
};  // class TFStatic

class TFDynamic
{
public:
  explicit TFDynamic(rclcpp::Node * node_ptr);

  virtual ~TFDynamic();

  virtual void setDynamicTransform(const geometry_msgs::msg::TransformStamped & transform);

  virtual void setDynamicTransform(
    const std::string & frame_id, const std::string & parent_frame_id, float x, float y, float z,
    float qx, float qy, float qz, float qw);

  virtual void setDynamicTransform(
    const std::string & frame_id, const std::string & parent_frame_id, float x, float y, float z,
    float roll, float pitch, float yaw);

  std::shared_ptr<tf2_ros::TransformBroadcaster> getTransformBroadcaster() const;

  const rclcpp::Node * getNode() const;

protected:
  rclcpp::Node * node_ptr_ = nullptr;
  std::shared_ptr<tf2_ros::TransformBroadcaster> dynamic_tf_broadcaster_ptr_;
};  // class TFDynamic

template<typename T>
class SensorData
{
public:
  explicit SensorData(
    const std::string & topic_name, rclcpp::Node * node_ptr,
    bool add_sensor_measurements_base = true)
  {
    // check if topic already has "sensor_measurements "in the name
    // if not, add it if flag add_sensor_measurements_base is true
    topic_name_ = processTopicName(topic_name, add_sensor_measurements_base);

    sensor_publisher_ = node_ptr->create_publisher<T>(
      topic_name_, as2_names::topics::sensor_measurements::qos);
  }

  virtual ~SensorData() {}

  static std::string processTopicName(
    const std::string & topic_name,
    bool add_sensor_measurements_base = true)
  {
    std::string topic = topic_name;
    if (add_sensor_measurements_base &&
      topic_name.find(as2_names::topics::sensor_measurements::base) == std::string::npos)
    {
      topic = as2_names::topics::sensor_measurements::base + topic_name;
    }
    return topic;
  }

  void setData(const T & msg)
  {
    msg_data_ = msg;
  }

  void publish()
  {
    sensor_publisher_->publish(msg_data_);
  }

  void updateAndPublish(const T & msg)
  {
    msg_data_ = msg;
    publish();
  }

  const std::string & getTopicName() const
  {
    return topic_name_;
  }

  const T & getData() const
  {
    return msg_data_;
  }

private:
  typename rclcpp::Publisher<T>::SharedPtr sensor_publisher_;
  T msg_data_;
  std::string topic_name_;
};  // class SensorData

class GenericSensor
{
public:
  explicit GenericSensor(as2::Node * node_ptr, const float pub_freq = -1.0f);

  virtual ~GenericSensor();

  void dataUpdated();

  virtual void publishData() = 0;

private:
  rclcpp::TimerBase::SharedPtr timer_;
  float pub_freq_;

  void timerCallback();
};

template<typename T>
class Sensor : public TFStatic, protected GenericSensor, protected SensorData<T>
{
public:
  Sensor(
    const std::string & id, as2::Node * node_ptr, float pub_freq = -1.0f,
    bool add_sensor_measurements_base = true)
  : TFStatic(node_ptr),
    GenericSensor(node_ptr, pub_freq),
    SensorData<T>(id, node_ptr, add_sensor_measurements_base)
  {}

  Sensor(
    const std::string & id, as2::Node * node_ptr, int pub_freq,
    bool add_sensor_measurements_base = true)
  : Sensor(id, node_ptr, static_cast<float>(pub_freq), add_sensor_measurements_base)
  {}

  Sensor(
    const std::string & id, as2::Node * node_ptr, double pub_freq,
    bool add_sensor_measurements_base = true)
  : Sensor(id, node_ptr, static_cast<float>(pub_freq), add_sensor_measurements_base)
  {}

  virtual ~Sensor() {}

  void updateData(const T & msg)
  {
    SensorData<T>::setData(msg);
    dataUpdated();
  }

protected:
  void publishData() override
  {
    SensorData<T>::publish();
  }
};  // class Sensor

class Camera : public TFStatic, protected GenericSensor
{
public:
  Camera(
    const std::string & id, as2::Node * node_ptr, const float pub_freq = -1.0f,
    bool add_sensor_measurements_base = true,
    const std::string & info_name = "camera_info",
    const std::string & camera_link = "camera_link");

  virtual ~Camera();

  void updateData(const sensor_msgs::msg::Image & img);

  void updateData(const cv::Mat & img);

  void setParameters(
    const sensor_msgs::msg::CameraInfo & camera_info, const std::string & encoding,
    const std::string & camera_model = "pinhole");

  void setStaticTransform(
    const std::string & frame_id, const std::string & parent_frame_id, float x, float y, float z,
    float qx, float qy, float qz, float qw);  // Shadowing the TFStatic function

  void setStaticTransform(
    const std::string & frame_id, const std::string & parent_frame_id, float x, float y, float z,
    float roll, float pitch, float yaw);  // Shadowing the TFStatic function

  // TODO(perezsaura-david): Implement the following functions
  // void loadParameters(const std::string & file)
  // void publishRectifiedImage(const sensor_msgs::msg::Image & msg)
  // void publishCompressedImage(const sensor_msgs::msg::Image & msg)

private:
  as2::Node * node_ptr_ = nullptr;
  std::string camera_link_;

  std::string camera_base_topic_;
  std::string camera_frame_;

  // Camera info
  std::shared_ptr<SensorData<sensor_msgs::msg::CameraInfo>> camera_info_sensor_;
  bool setup_ = false;
  bool camera_info_available_ = false;
  std::string encoding_;
  std::string camera_model_;
  std::string camera_name_;

  // Camera image
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_;
  std::shared_ptr<image_transport::ImageTransport> image_transport_ptr_ = nullptr;
  image_transport::Publisher it_publisher_;
  sensor_msgs::msg::Image image_data_;

  std::shared_ptr<rclcpp::Node> getSelfPtr();

  void setup();

  void publishData();
};  // class CameraSensor

class GroundTruth : protected GenericSensor
{
public:
  explicit GroundTruth(
    as2::Node * node_ptr, const float pub_freq = -1,
    const std::string & topic_name_base = "");

  virtual ~GroundTruth();

  void updateData(const geometry_msgs::msg::PoseStamped & pose_msg);

  void updateData(const geometry_msgs::msg::TwistStamped & twist_msg);

  void updateData(
    const geometry_msgs::msg::PoseStamped & pose_msg,
    const geometry_msgs::msg::TwistStamped & twist_msg);

protected:
  std::shared_ptr<SensorData<geometry_msgs::msg::PoseStamped>> pose_sensor_;
  std::shared_ptr<SensorData<geometry_msgs::msg::TwistStamped>> twist_sensor_;

  void publishData() override;
};  // class GroundTruth

class Gimbal : public TFStatic, protected TFDynamic, protected GenericSensor,
  protected SensorData<geometry_msgs::msg::PoseStamped>
{
public:
  explicit Gimbal(
    const std::string & gimbal_id,
    const std::string & gimbal_base_id,
    as2::Node * node_ptr,
    const float pub_freq = -1.0f,
    bool add_sensor_measurements_base = true);

  virtual ~Gimbal();

  void setGimbalBaseTransform(
    const geometry_msgs::msg::Transform & gimbal_base_transform,
    const std::string & gimbal_parent_frame_id = "base_link");

  void updateData(const geometry_msgs::msg::PoseStamped & pose_msg);

  void updateData(const geometry_msgs::msg::QuaternionStamped & orientation_msg);

  const std::string & getGimbalFrameId() const;

  const std::string & getGimbalBaseFrameId() const;

protected:
  std::string gimbal_frame_id_;
  std::string gimbal_base_frame_id_;
  geometry_msgs::msg::TransformStamped gimbal_transform_;

  void publishData();
};  // class Gimbal

using Odometry = Sensor<nav_msgs::msg::Odometry>;
using Imu = Sensor<sensor_msgs::msg::Imu>;
using GPS = Sensor<sensor_msgs::msg::NavSatFix>;
using Lidar = Sensor<sensor_msgs::msg::LaserScan>;
using Battery = Sensor<sensor_msgs::msg::BatteryState>;
using Barometer = Sensor<sensor_msgs::msg::FluidPressure>;
using Compass = Sensor<sensor_msgs::msg::MagneticField>;
using RangeFinder = Sensor<sensor_msgs::msg::Range>;

}  // namespace sensors
}  // namespace as2

#endif  // AS2_CORE__SENSOR_HPP_