start_request_serialization.cpp

Go to the documentation of this file.

// Copyright (c) 2020-2021 Pilz GmbH & Co. KG
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.

#include <string>
#include <cassert>
#include <iostream>

#ifdef _WIN32
#include <Windows.h>
#endif

#include <boost/crc.hpp>

#include "psen_scan_v2_standalone/data_conversion_layer/start_request.h"
#include "psen_scan_v2_standalone/data_conversion_layer/start_request_serialization.h"
#include "psen_scan_v2_standalone/data_conversion_layer/raw_processing.h"
#include "psen_scan_v2_standalone/data_conversion_layer/raw_scanner_data.h"
#include "psen_scan_v2_standalone/util/logging.h"

using namespace std;

namespace psen_scan_v2_standalone
{
namespace data_conversion_layer
{
namespace start_request
{
static constexpr uint64_t RESERVED{ 0 };

static const uint32_t OPCODE{ 0x35 };
}  // namespace start_request

uint32_t calculateCRC(const data_conversion_layer::RawData& data)
{
  boost::crc_32_type crc;
  crc.process_bytes(&data.at(0), data.size());
  return static_cast<uint32_t>(crc.checksum());
}

RawData data_conversion_layer::start_request::serialize(const data_conversion_layer::start_request::Message& msg,
                                                        const uint32_t& seq_number)
{
  std::ostringstream os;

  raw_processing::write(os, seq_number);
  raw_processing::write(os, data_conversion_layer::start_request::RESERVED);
  raw_processing::write(os, data_conversion_layer::start_request::OPCODE);

#ifdef __linux__
  const uint32_t host_ip_big_endian = htobe32(msg.host_ip_);
#endif

#ifdef _WIN32
  const uint32_t host_ip_big_endian = _byteswap_ulong(msg.host_ip_);
#endif

  raw_processing::write(os, host_ip_big_endian);

  raw_processing::write(os, msg.host_udp_port_data_);

  const uint8_t device_enabled{ 0b00001000 };
  const uint8_t intensity_enabled{ static_cast<uint8_t>(
      msg.master_device_settings_.intensitiesEnabled() ? 0b00001000 : 0b00000000) };
  const uint8_t point_in_safety_enabled{ 0 };
  const uint8_t active_zone_set_enabled{ 0b00001000 };
  const uint8_t io_pin_data_enabled{ 0b00001000 };
  const uint8_t scan_counter_enabled{ 0b00001000 };
  const uint8_t speed_encoder_enabled{ 0 }; 
  const uint8_t diagnostics_enabled{ static_cast<uint8_t>(
      msg.master_device_settings_.diagnosticsEnabled() ? 0b00001000 : 0b00000000) };

  raw_processing::write(os, device_enabled);
  raw_processing::write(os, intensity_enabled);
  raw_processing::write(os, point_in_safety_enabled);
  raw_processing::write(os, active_zone_set_enabled);
  raw_processing::write(os, io_pin_data_enabled);
  raw_processing::write(os, scan_counter_enabled);
  raw_processing::write(os, speed_encoder_enabled);
  raw_processing::write(os, diagnostics_enabled);

  const auto start = msg.master_.scanRange().start().value();
  auto end = msg.master_.scanRange().end().value();
  const auto resolution = msg.master_.resolution().value();

  /* In order to get all the data points we want, the scanner needs a value
     that is strictly greater than the end point */
  if ((end - start) % resolution == 0)
  {
    end++;
  }
  raw_processing::write(os, start);
  raw_processing::write(os, end);
  raw_processing::write(os, resolution);

  PSENSCAN_DEBUG("StartRequestSerialization",
                 "Serializing start request with angle_start={} angle_end={} resolution={} tenths of degree.",
                 start,
                 end,
                 resolution);

  for (const auto& subscriber :
       msg.subscribers_)  // Note: This refers to the scanner type subscriber, *not* a ros subscriber
  {
    raw_processing::write(os, subscriber.scanRange().start().value());
    raw_processing::write(os, subscriber.scanRange().end().value());
    raw_processing::write(os, subscriber.resolution().value());
  }

  const std::string raw_data_as_str{ os.str() };
  const data_conversion_layer::RawData raw_data(raw_data_as_str.cbegin(), raw_data_as_str.cend());

  std::ostringstream os_crc;
  raw_processing::write(os_crc, calculateCRC(raw_data));

  std::string raw_data_with_crc_str(os_crc.str() + os.str());
  data_conversion_layer::RawData raw_data_with_crc{ raw_data_with_crc_str.cbegin(), raw_data_with_crc_str.cend() };

  assert(raw_data_with_crc.size() == 58 && "Message data of start request has not the size expceted by protocol");

  return raw_data_with_crc;
}

}  // namespace data_conversion_layer
}  // namespace psen_scan_v2_standalone