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