horus-sdk/cpp/examples/print_occupancy_map.cpp at main · seoulrobotics/horus-sdk · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
/// @file
///
/// A command line executable which connects to a Horus notification service and prints all
/// received Occupancy Grid messages to the standard output.
///
/// Note that the executable will continuously try to silently reconnect to the notification
/// service, so even if it is not running no error message will be printed.

#include <algorithm>
#include <cstddef>
#include <cstdint>
#include <vector>

#include "examples/helpers.h"
#include "horus/pb/config/metadata_pb.h"
#include "horus/pb/cow.h"
#include "horus/pb/cow_repeated.h"
#include "horus/pb/preprocessing/messages_pb.h"
#include "horus/rpc/services.h"
#include "horus/sdk.h"
#include "horus/strings/chrono.h"  // IWYU pragma: keep
#include "horus/strings/stdio.h"
#include "horus/strings/stringify.h"
#include "horus/types/span.h"

int main(int argc, const char* argv[]) {
  const horus::Span<const char*> args{argv, static_cast<std::size_t>(argc)};

  horus::RpcServices::ServiceResolutionMap service_map;
  if (!horus::ParseArgs(service_map.notification, args)) {
    return 1;
  }
  horus::Sdk sdk{service_map};
  horus::SdkSubscription const subscription{
      sdk.SubscribeToOccupancyGrid(
             {/*on_occupancy_grid=*/[](const horus::pb::OccupancyGridListEvent& grids_event) {
               for (const horus::Cow<horus::pb::OccupancyGridEvent>& node :
                    grids_event.occupancy_grid_events()) {
                 const horus::pb::OccupancyGridEvent& event{node.Ref()};
                 horus::StringifyTo(horus::StdoutSink(), "Occupancy Grid received\n");
                 horus::StringifyTo(horus::StdoutSink(), "  node_id: ", event.node_id().Str(),
                                    "\n");
                 horus::StringifyTo(horus::StdoutSink(),
                                    "  detection_range_name: ", event.detection_range_name().Str(),
                                    "\n");
                 horus::StringifyTo(horus::StdoutSink(), "  resolution: ", event.resolution(),
                                    "\n");
                 horus::StringifyTo(horus::StdoutSink(), "  rows: ", event.grid().rows(), "\n");
                 horus::StringifyTo(horus::StdoutSink(), "  cols: ", event.grid().cols(), "\n");
                 horus::StringifyTo(horus::StdoutSink(), "  cells: ", event.grid().cells().size(),
                                    "\n");
                 horus::StringifyTo(horus::StdoutSink(), "  detection range x: ",
                                    event.detection_range().x_range().start(), " - ",
                                    event.detection_range().x_range().end(), "\n");
                 horus::StringifyTo(horus::StdoutSink(), "  detection range y: ",
                                    event.detection_range().y_range().start(), " - ",
                                    event.detection_range().y_range().end(), "\n");
                 horus::StringifyTo(horus::StdoutSink(), "  detection range z: ",
                                    event.detection_range().z_range().start(), " - ",
                                    event.detection_range().z_range().end(), "\n");
                 horus::StringifyTo(horus::StdoutSink(),
                                    "  timestamp: ", event.timestamp().seconds(), "s ",
                                    event.timestamp().nanos(), "ns\n");

                 std::vector<horus::pb::OccupancyClassification> classifications;

                 std::size_t const rows{event.grid().rows()};
                 std::size_t const cols{event.grid().cols()};
                 classifications.reserve(rows * cols);

                 constexpr std::uint32_t const kNumCountBits{29U};
                 constexpr std::uint32_t const kValueMask{(1U << kNumCountBits) - 1U};
                 for (const std::uint32_t cell : event.grid().cells()) {
                   std::uint32_t const value{cell >> kNumCountBits};
                   std::uint32_t const count{cell & kValueMask};
                   classifications.insert(classifications.end(), count,
                                          static_cast<horus::pb::OccupancyClassification>(value));
                 }

                 auto const num_occluded = std::count_if(
                     classifications.begin(), classifications.end(),
                     [](horus::pb::OccupancyClassification const& classification) {
                       return classification == horus::pb::OccupancyClassification::kOccluded;
                     });
                 auto const num_static_occupied =
                     std::count_if(classifications.begin(), classifications.end(),
                                   [](horus::pb::OccupancyClassification const& classification) {
                                     return classification ==
                                            horus::pb::OccupancyClassification::kStationaryOccupied;
                                   });
                 auto const num_free =
                     static_cast<std::uint32_t>(rows * cols) - num_occluded - num_static_occupied;
                 horus::StringifyTo(horus::StdoutSink(), "  num occluded: ", num_occluded,
                                    " num static occupied: ", num_static_occupied,
                                    " num free: ", num_free, "\n");
               }
             }})
          .Wait()};

  horus::WaitForTermination();
  return 0;
}