PointCloudNode.hpp

#ifndef POINT_CLOUD_NODE_HPP
#define POINT_CLOUD_NODE_HPP

#include <vector>
#include <ros/ros.h>
#include <HLR/FullWorld.h> //ROS srv
#include <HLR/World.h>     //ROS msg
#include <sensor_msgs/LaserScan.h>
#include <std_msgs/Header.h>
#include <tf/transform_listener.h>
#include "Object.hpp"
#include "ObjectInterpreter.hpp"
#include <Vision/ICupDetector.hpp>
#include <laser_geometry/laser_geometry.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>

#ifdef TESTING
#include <atomic>
#include <condition_variable>
#include <mutex>
#endif

namespace HLR
{
namespace PointCloud
{
class PointCloudNode
{
public:
    explicit PointCloudNode(ros::Rate update_rate);
#ifndef TESTING
    [[noreturn]]
#endif
        void
        run();

private:
    const ros::Rate update_rate;

    ros::NodeHandle nh;
    ros::ServiceServer service;
    ros::Publisher publisher;
    ros::Subscriber sub;
    ros::Timer timer;
    tf::TransformListener transform_listener;
    ObjectInterpreter interpreter;
    // TODO: how do we instantiate a CupDetector?
    std::unique_ptr<HLR::Vision::ICupDetector> cup_detector;

    HLR::Vision::CupDetection latest_cup_detection;
    pcl::PointCloud<pcl::PointXYZ> latest_point_cloud;

    void publish_world(const ros::TimerEvent& event);

    bool service_callback(HLR::FullWorld::Request& request,
                          HLR::FullWorld::Response& response);
    void scan_callback(const sensor_msgs::LaserScan::ConstPtr& scan);
    ros::Time last_data;

#ifdef TESTING
    std::atomic_bool shutdown{false};

    bool callback_called{false};
    std::mutex callback_mutex;
    std::condition_variable callback_cv;
#endif
};

} // namespace PointCloud
} // namespace HLR

#endif /* POINT_CLOUD_NODE_HPP */