Line data    Source code

       1             : #include "PointCloudHandler.hpp"
       2             : #include <exception>
       3             : 
       4             : #include "HLR/FullWorld.h"
       5             : 
       6             : namespace HLR
       7             : {
       8             : namespace Kinematics
       9             : {
      10           0 : PointCloudHandler::PointCloudHandler(RobotController& controller)
      11           0 :     : controller(controller)
      12             : {
      13           0 :     this->world_changes = this->nh.subscribe(
      14           0 :         "world_updates", 100, &PointCloudHandler::handle_object, this);
      15             : 
      16           0 :     auto world = this->nh.serviceClient<HLR::FullWorld>("world");
      17           0 :     HLR::FullWorld srv;
      18           0 :     if (!world.call(srv))
      19             :     {
      20           0 :         ROS_ERROR("Failed to get full world from world service.");
      21           0 :         throw std::runtime_error("Failed to call the world service.");
      22             :     }
      23             : 
      24           0 :     for (const auto& obj : srv.response.world.objects)
      25             :     {
      26           0 :         if (!static_cast<bool>((obj.flags & HLR::Object::IS_DELETED) > 0))
      27             :         {
      28           0 :             std::istringstream is(obj.polyhedron);
      29             :             CGAL::Surface_mesh<CGAL::Simple_cartesian<double>::Point_3>
      30           0 :                 polyhedron;
      31           0 :             is >> polyhedron;
      32           0 :             this->controller.add_object(
      33           0 :                 {obj.id,
      34           0 :                  {{obj.position.x}, {obj.position.y}, {obj.position.z}},
      35           0 :                  {{obj.velocity.x}, {obj.velocity.y}, {obj.velocity.z}},
      36           0 :                  static_cast<bool>((obj.flags & HLR::Object::IS_CUP) > 0),
      37           0 :                  static_cast<bool>((obj.flags & HLR::Object::HAS_SPEED) > 0),
      38             :                  polyhedron});
      39             :         }
      40             :     }
      41           0 : }
      42             : 
      43           0 : void PointCloudHandler::handle_object(const HLR::Object::ConstPtr& obj)
      44             : {
      45           0 :     std::istringstream is(obj->polyhedron);
      46           0 :     CGAL::Surface_mesh<CGAL::Simple_cartesian<double>::Point_3> polyhedron;
      47           0 :     is >> polyhedron;
      48             :     EnvironmentObject parsed = {
      49           0 :         obj->id,
      50           0 :         {{obj->position.x}, {obj->position.y}, {obj->position.z}},
      51           0 :         {{obj->velocity.x}, {obj->velocity.y}, {obj->velocity.z}},
      52           0 :         static_cast<bool>((obj->flags & HLR::Object::IS_CUP) > 0),
      53           0 :         static_cast<bool>((obj->flags & HLR::Object::HAS_SPEED) > 0),
      54           0 :         polyhedron};
      55             : 
      56           0 :     if (static_cast<bool>((obj->flags & HLR::Object::IS_DELETED) > 0))
      57             :     {
      58           0 :         this->controller.remove_object(parsed);
      59             :     }
      60             :     else
      61             :     {
      62           0 :         this->controller.add_object(std::move(parsed));
      63             :     }
      64           0 : }
      65             : 
      66             : } // namespace Kinematics
      67           3 : } // namespace HLR