Line data    Source code

       1             : 
       2             : #include "PointCloudNode.hpp"
       3             : #include <algorithm>
       4             : 
       5             : namespace HLR
       6             : {
       7             : namespace PointCloud
       8             : {
       9           0 : PointCloudNode::PointCloudNode(ros::Rate update_rate)
      10           0 :     : update_rate{std::move(update_rate)}
      11             :     , nh{}
      12             :     , service{nh.advertiseService("world",
      13             :                                   &PointCloudNode::service_callback,
      14             :                                   this)}
      15             :     , publisher{nh.advertise<HLR::World>("world_updates", 1, true)}
      16             :     , timer{nh.createTimer(update_rate, &PointCloudNode::publish_world, this)}
      17             :     , interpreter{}
      18             :     , cup_detector{}
      19           0 :     , last_data{}
      20           0 : {}
      21             : 
      22           0 : void PointCloudNode::run()
      23             : {
      24           0 :     ros::spin();
      25           0 :     std::exit(EXIT_SUCCESS);
      26             : }
      27             : 
      28           0 : void PointCloudNode::publish_world(const ros::TimerEvent& /* event */)
      29             : {
      30             :     static std_msgs::Header::_seq_type sequence_number = 0;
      31             : 
      32           0 :     std::vector<Object> objects{interpreter.get_changed_objects()};
      33           0 :     HLR::World world;
      34           0 :     world.header.seq = ++sequence_number;
      35           0 :     world.header.stamp = last_data;
      36           0 :     std::transform(begin(objects),
      37             :                    end(objects),
      38             :                    std::back_inserter(world.objects),
      39           0 :                    [](const auto& e) { return static_cast<HLR::Object>(e); });
      40           0 :     publisher.publish(world);
      41           0 : }
      42             : 
      43           0 : bool PointCloudNode::service_callback(HLR::FullWorld::Request& /* request */,
      44             :                                       HLR::FullWorld::Response& response)
      45             : {
      46             :     static std_msgs::Header::_seq_type sequence_number = 0;
      47             : 
      48           0 :     std::vector<Object> objects{interpreter.get_current_objects()};
      49           0 :     response.world.header.seq = ++sequence_number;
      50           0 :     response.world.header.stamp = last_data;
      51           0 :     std::transform(begin(objects),
      52             :                    end(objects),
      53             :                    std::back_inserter(response.world.objects),
      54           0 :                    [](const auto& e) { return static_cast<HLR::Object>(e); });
      55           0 :     return true;
      56             : }
      57             : 
      58             : } // namespace PointCloud
      59           3 : } // namespace HLR