Line data    Source code

       1             : 
       2             : #include "PointCloudNode.hpp"
       3             : #include <algorithm>
       4             : #include <iostream>
       5             : #include <ros/ros.h>
       6             : #include <ros/console.h>
       7             : #include <pcl/io/pcd_io.h>
       8             : #include <pcl/point_types.h>
       9             : #include <pcl/visualization/cloud_viewer.h>
      10             : 
      11             : namespace HLR
      12             : {
      13             : namespace PointCloud
      14             : {
      15           0 : PointCloudNode::PointCloudNode(ros::Rate update_rate)
      16           0 :     : update_rate{std::move(update_rate)}
      17             :     , nh{}
      18             :     , service{nh.advertiseService("world",
      19             :                                   &PointCloudNode::service_callback,
      20             :                                   this)}
      21             :     , publisher{nh.advertise<HLR::World>("world_updates", 1, true)}
      22             :     , sub{nh.subscribe<sensor_msgs::LaserScan>("/scan",
      23             :                                                1000,
      24             :                                                &PointCloudNode::scan_callback,
      25             :                                                this)}
      26             :     , timer{nh.createTimer(update_rate, &PointCloudNode::publish_world, this)}
      27             :     , interpreter{}
      28             :     , cup_detector{}
      29           0 :     , last_data{}
      30           0 : {}
      31             : 
      32           0 : void PointCloudNode::run()
      33             : {
      34           0 :     ros::spin();
      35           0 :     std::exit(EXIT_SUCCESS);
      36             : }
      37             : 
      38           0 : void PointCloudNode::publish_world(const ros::TimerEvent& /* event */)
      39             : {
      40             :     static std_msgs::Header::_seq_type sequence_number = 0;
      41             : 
      42             :     std::vector<Object> objects{interpreter.get_changed_objects(
      43           0 :         latest_cup_detection, latest_point_cloud)};
      44           0 :     HLR::World world;
      45           0 :     world.header.seq = ++sequence_number;
      46           0 :     world.header.stamp = last_data;
      47           0 :     std::transform(begin(objects),
      48             :                    end(objects),
      49             :                    std::back_inserter(world.objects),
      50           0 :                    [](const auto& e) { return static_cast<HLR::Object>(e); });
      51           0 :     publisher.publish(world);
      52           0 : }
      53             : 
      54           0 : bool PointCloudNode::service_callback(HLR::FullWorld::Request& /* request */,
      55             :                                       HLR::FullWorld::Response& response)
      56             : {
      57             :     static std_msgs::Header::_seq_type sequence_number = 0;
      58             : 
      59             :     std::vector<Object> objects{interpreter.get_current_objects(
      60           0 :         latest_cup_detection, latest_point_cloud)};
      61           0 :     response.world.header.seq = ++sequence_number;
      62           0 :     response.world.header.stamp = last_data;
      63           0 :     std::transform(begin(objects),
      64             :                    end(objects),
      65             :                    std::back_inserter(response.world.objects),
      66           0 :                    [](const auto& e) { return static_cast<HLR::Object>(e); });
      67           0 :     return true;
      68             : }
      69             : 
      70           0 : void PointCloudNode::scan_callback(const sensor_msgs::LaserScan::ConstPtr& scan)
      71             : {
      72           0 :     ROS_DEBUG("Incoming sensormessage");
      73           0 :     sensor_msgs::PointCloud2 cloud;
      74           0 :     laser_geometry::LaserProjection temp;
      75             : 
      76           0 :     temp.transformLaserScanToPointCloud(
      77           0 :         "laser_frame", *scan, cloud, transform_listener);
      78             : 
      79           0 :     pcl::fromROSMsg(cloud, latest_point_cloud);
      80           0 : }
      81             : 
      82             : } // namespace PointCloud
      83           3 : } // namespace HLR