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>
 
#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());
}
 
uint8_t getEnableByte(uint8_t nr_ubscribers)
{
  uint8_t base{ 0b00001000 };
  for (int i = 0; i < nr_ubscribers; i++)
  {
    base = base | (base >> 1);
  }
  return base;
}
 
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_);
 
  uint8_t nr_ubscribers = static_cast<uint8_t>(msg.master_device_settings_.nrSubscribers());
 
  const uint8_t device_enabled{ getEnableByte(nr_ubscribers) };  // hard coded for one subscriber
  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{ getEnableByte(nr_ubscribers) };
  const uint8_t io_pin_data_enabled{ 0b00001000 };
  const uint8_t scan_counter_enabled{ getEnableByte(nr_ubscribers) };  // hard coded for one subscriber
  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 (int i = 0; i < nr_ubscribers; i++)  // Note: This refers to the scanner type subscriber, *not* a ros subscriber
  {
    raw_processing::write(os, start);
    raw_processing::write(os, end);
    raw_processing::write(os, resolution);
  }
 
  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