ParseTCPPacket.cpp

Go to the documentation of this file.

// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-

// -- BEGIN LICENSE BLOCK ----------------------------------------------

// -- END LICENSE BLOCK ------------------------------------------------

//----------------------------------------------------------------------
//----------------------------------------------------------------------

#include <sick_safetyscanners/data_processing/ParseTCPPacket.h>

#include <sick_safetyscanners/cola2/Command.h>

namespace sick {
namespace data_processing {

ParseTCPPacket::ParseTCPPacket() {}

uint32_t ParseTCPPacket::getExpectedPacketLength(const datastructure::PacketBuffer& buffer)
{
  // Keep our own copy of the shared_ptr to keep the iterators valid
  const std::shared_ptr<std::vector<uint8_t> const> vec_ptr = buffer.getBuffer();
  std::vector<uint8_t>::const_iterator data_ptr             = vec_ptr->begin();
  return readLength(data_ptr) + 8; // for STX and Length which is not included in length datafield
}

uint16_t ParseTCPPacket::getRequestID(const datastructure::PacketBuffer& buffer) const
{
  // Keep our own copy of the shared_ptr to keep the iterators valid
  const std::shared_ptr<std::vector<uint8_t> const> vec_ptr = buffer.getBuffer();
  std::vector<uint8_t>::const_iterator data_ptr             = vec_ptr->begin();
  return readRequestID(data_ptr);
}


bool ParseTCPPacket::parseTCPSequence(const datastructure::PacketBuffer& buffer,
                                      sick::cola2::Command& command) const
{
  setCommandValuesFromPacket(buffer, command);

  std::vector<uint8_t> byte_vector = readData(buffer);
  command.setDataVector(byte_vector);

  return true;
}

void ParseTCPPacket::setCommandValuesFromPacket(const sick::datastructure::PacketBuffer& buffer,
                                                sick::cola2::Command& command) const
{
  // Keep our own copy of the shared_ptr to keep the iterators valid
  const std::shared_ptr<std::vector<uint8_t> const> vec_ptr = buffer.getBuffer();
  std::vector<uint8_t>::const_iterator data_ptr             = vec_ptr->begin();
  command.setSessionID(readSessionID(data_ptr));
  command.setRequestID(readRequestID(data_ptr));
  command.setCommandType(readCommandType(data_ptr));
  command.setCommandMode(readCommandMode(data_ptr));
}

uint32_t ParseTCPPacket::readSTx(std::vector<uint8_t>::const_iterator data_ptr) const
{
  return read_write_helper::readUint32BigEndian(data_ptr + 0);
}

uint32_t ParseTCPPacket::readLength(std::vector<uint8_t>::const_iterator data_ptr) const
{
  return read_write_helper::readUint32BigEndian(data_ptr + 4);
}

uint8_t ParseTCPPacket::readHubCntr(std::vector<uint8_t>::const_iterator data_ptr) const
{
  return read_write_helper::readUint8BigEndian(data_ptr + 8);
}
uint8_t ParseTCPPacket::readNoC(std::vector<uint8_t>::const_iterator data_ptr) const
{
  return read_write_helper::readUint8BigEndian(data_ptr + 9);
}
uint32_t ParseTCPPacket::readSessionID(std::vector<uint8_t>::const_iterator data_ptr) const
{
  return read_write_helper::readUint32BigEndian(data_ptr + 10);
}

uint16_t ParseTCPPacket::readRequestID(std::vector<uint8_t>::const_iterator data_ptr) const
{
  return read_write_helper::readUint16BigEndian(data_ptr + 14);
}

uint8_t ParseTCPPacket::readCommandType(std::vector<uint8_t>::const_iterator data_ptr) const
{
  return read_write_helper::readUint8BigEndian(data_ptr + 16);
}
uint8_t ParseTCPPacket::readCommandMode(std::vector<uint8_t>::const_iterator data_ptr) const
{
  return read_write_helper::readUint8BigEndian(data_ptr + 17);
}
uint16_t ParseTCPPacket::readErrorCode(std::vector<uint8_t>::const_iterator data_ptr) const
{
  return read_write_helper::readUint16BigEndian(data_ptr + 18);
}

std::vector<uint8_t> ParseTCPPacket::readData(const datastructure::PacketBuffer& buffer) const
{
  if (buffer.getLength() < 20)
  {
    return std::vector<uint8_t>();
  }
  // Keep our own copy of the shared_ptr to keep the iterators valid
  const std::shared_ptr<std::vector<uint8_t> const> vec_ptr = buffer.getBuffer();
  return std::vector<uint8_t>(vec_ptr->begin() + 20, vec_ptr->end());
}

} // namespace data_processing
} // namespace sick