HLR  0.0.1
PointCloudNode.hpp
1 #ifndef POINT_CLOUD_NODE_HPP
2 #define POINT_CLOUD_NODE_HPP
3 
4 #include <vector>
5 #include <ros/ros.h>
6 #include <HLR/FullWorld.h> //ROS srv
7 #include <HLR/World.h> //ROS msg
8 #include <sensor_msgs/LaserScan.h>
9 #include <std_msgs/Header.h>
10 #include <tf/transform_listener.h>
11 #include "Object.hpp"
12 #include "ObjectInterpreter.hpp"
13 #include <Vision/ICupDetector.hpp>
14 #include <laser_geometry/laser_geometry.h>
15 #include <pcl/PCLPointCloud2.h>
16 #include <pcl/conversions.h>
17 #include <pcl/point_types.h>
18 #include <pcl_conversions/pcl_conversions.h>
19 #include <pcl_ros/transforms.h>
20 
21 #ifdef TESTING
22 #include <atomic>
23 #include <condition_variable>
24 #include <mutex>
25 #endif
26 
27 namespace HLR
28 {
29 namespace PointCloud
30 {
40 {
41 public:
52  explicit PointCloudNode(ros::Rate update_rate);
61 #ifndef TESTING
62  [[noreturn]]
63 #endif
64  void
65  run();
66 
67 private:
68  const ros::Rate update_rate;
69 
70  ros::NodeHandle nh;
71  ros::ServiceServer service;
72  ros::Publisher publisher;
73  ros::Subscriber sub;
74  ros::Timer timer;
75  tf::TransformListener transform_listener;
76  ObjectInterpreter interpreter;
77  // TODO: how do we instantiate a CupDetector?
78  std::unique_ptr<HLR::Vision::ICupDetector> cup_detector;
79 
80  HLR::Vision::CupDetection latest_cup_detection;
81  pcl::PointCloud<pcl::PointXYZ> latest_point_cloud;
82 
83  void publish_world(const ros::TimerEvent& event);
84 
85  bool service_callback(HLR::FullWorld::Request& request,
86  HLR::FullWorld::Response& response);
87  void scan_callback(const sensor_msgs::LaserScan::ConstPtr& scan);
92  ros::Time last_data;
93 
94 #ifdef TESTING
95  std::atomic_bool shutdown{false};
96 
97  bool callback_called{false};
98  std::mutex callback_mutex;
99  std::condition_variable callback_cv;
100 #endif
101 };
102 
103 } // namespace PointCloud
104 } // namespace HLR
105 
106 #endif /* POINT_CLOUD_NODE_HPP */
Class responsible for merging the world data from the Kinect and the LiDAR.
Definition: ObjectInterpreter.hpp:20
void run()
Run the pipeline reading data from the Kinect and LiDAR, interpreting it and publishing it on a ROS t...
PointCloudNode(ros::Rate update_rate)
Initialize PointCloudNode.
Collection of detected cups Collection of detected cups where the location of the cups is described i...
Definition: ICupDetector.hpp:17
This class is the main class for the PointCloud namespaces.
Definition: PointCloudNode.hpp:39