Line data    Source code

       1             : #include "PointCloudNode.hpp"
       2             : #include <algorithm>
       3             : #include <iostream>
       4             : #include <ros/ros.h>
       5             : #include <ros/console.h>
       6             : #include <pcl/io/pcd_io.h>
       7             : #include <pcl/point_types.h>
       8             : #include <pcl/visualization/cloud_viewer.h>
       9             : 
      10             : namespace HLR
      11             : {
      12             : namespace PointCloud
      13             : {
      14           2 : PointCloudNode::PointCloudNode(ros::Rate update_rate)
      15           2 :     : update_rate{std::move(update_rate)}
      16             :     , nh{}
      17             :     , service{nh.advertiseService("world",
      18             :                                   &PointCloudNode::service_callback,
      19             :                                   this)}
      20             :     , publisher{nh.advertise<HLR::World>("world_updates", 1, true)}
      21             :     , sub{nh.subscribe<sensor_msgs::LaserScan>("/scan",
      22             :                                                1000,
      23             :                                                &PointCloudNode::scan_callback,
      24             :                                                this)}
      25             :     , timer{nh.createTimer(update_rate, &PointCloudNode::publish_world, this)}
      26             :     , interpreter{}
      27             :     , cup_detector{}
      28           2 :     , last_data{}
      29           2 : {}
      30             : 
      31           9 : void PointCloudNode::run()
      32             : {
      33             : /* Because this calls std::exit, it's not possible to use this version for
      34             :  * testing. */
      35             : #ifndef TESTING
      36             :     ros::spin();
      37             :     std::exit(EXIT_SUCCESS);
      38             : #else /* TESTING */
      39          16 :     while (!shutdown.load(std::memory_order_acquire))
      40             :     {
      41             :         // http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning
      42           7 :         ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.1));
      43             :     }
      44             : #endif
      45           2 : }
      46             : 
      47           4 : void PointCloudNode::publish_world(const ros::TimerEvent& /* event */)
      48             : {
      49             :     static std_msgs::Header::_seq_type sequence_number = 0;
      50             : 
      51             :     std::vector<Object> objects{interpreter.get_changed_objects(
      52           8 :         latest_cup_detection, latest_point_cloud)};
      53           8 :     HLR::World world;
      54           4 :     world.header.seq = ++sequence_number;
      55           4 :     world.header.stamp = last_data;
      56           4 :     std::transform(begin(objects),
      57             :                    end(objects),
      58             :                    std::back_inserter(world.objects),
      59           4 :                    [](const auto& e) { return static_cast<HLR::Object>(e); });
      60           4 :     publisher.publish(world);
      61           4 : }
      62             : 
      63           1 : bool PointCloudNode::service_callback(HLR::FullWorld::Request& /* request */,
      64             :                                       HLR::FullWorld::Response& response)
      65             : {
      66             :     static std_msgs::Header::_seq_type sequence_number = 0;
      67             : 
      68             :     std::vector<Object> objects{interpreter.get_current_objects(
      69           2 :         latest_cup_detection, latest_point_cloud)};
      70           1 :     response.world.header.seq = ++sequence_number;
      71           1 :     response.world.header.stamp = last_data;
      72           1 :     std::transform(begin(objects),
      73             :                    end(objects),
      74             :                    std::back_inserter(response.world.objects),
      75           1 :                    [](const auto& e) { return static_cast<HLR::Object>(e); });
      76           2 :     return true;
      77             : }
      78             : 
      79           1 : void PointCloudNode::scan_callback(const sensor_msgs::LaserScan::ConstPtr& scan)
      80             : {
      81           1 :     ROS_DEBUG("Incoming sensormessage");
      82           2 :     sensor_msgs::PointCloud2 cloud;
      83           2 :     laser_geometry::LaserProjection temp;
      84             : 
      85           2 :     temp.transformLaserScanToPointCloud(
      86           1 :         "laser_frame", *scan, cloud, transform_listener);
      87             : 
      88           1 :     pcl::fromROSMsg(cloud, latest_point_cloud);
      89             : 
      90             : #ifdef TESTING
      91             :     {
      92           2 :         std::lock_guard l{callback_mutex};
      93           1 :         callback_called = true;
      94             :     }
      95           1 :     callback_cv.notify_all();
      96             : #endif
      97           1 : }
      98             : 
      99             : } // namespace PointCloud
     100           3 : } // namespace HLR