Skip to content

feat: ignore RTPS datagrams #264

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
jmonticelli wants to merge 1 commit into
base: master
Choose a base branch
from
Open

feat: ignore RTPS datagrams #264

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions source/Legacy/details/dispatch.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -883,14 +883,25 @@ void impl::handle()
const wire::Header& header = *(reinterpret_cast<const wire::Header*>(inP));

if (wire::HEADER_MAGIC != header.magic)
{
if (wire::ignoredUdpProtocol(m_incomingBuffer))
{
continue;
}

CRL_EXCEPTION("bad protocol magic: 0x%x, expecting 0x%x",
header.magic, wire::HEADER_MAGIC);
}
else if (wire::HEADER_VERSION != header.version)
{
CRL_EXCEPTION("bad protocol version: 0x%x, expecting 0x%x",
header.version, wire::HEADER_VERSION);
}
else if (wire::HEADER_GROUP != header.group)
{
CRL_EXCEPTION("bad protocol group: 0x%x, expecting 0x%x",
header.group, wire::HEADER_GROUP);
}

//
// Unwrap the sequence identifier
Expand Down
5 changes: 5 additions & 0 deletions source/LibMultiSense/details/legacy/message.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,6 +109,11 @@ bool header_valid(const std::vector<uint8_t> &raw_data)

if (wire::HEADER_MAGIC != header.magic)
{
if (wire::ignoredUdpProtocol(m_incomingBuffer))
{
return false;
}

CRL_DEBUG("bad protocol magic: 0x%x, expecting 0x%x\n", header.magic, wire::HEADER_MAGIC);
return false;
}
Expand Down
18 changes: 18 additions & 0 deletions source/Wire/include/wire/Protocol.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#define LibMultiSense_details_wire_protocol

#include <stdint.h>
#include <string.h>

#include "../utility/Portability.hh"

Expand Down Expand Up @@ -362,6 +363,23 @@ static CRL_CONSTEXPR float WIRE_IMAGER_GAIN_MAX = 1000.0f;
for(uint32_t j_=0; j_<(m_); j_++) \
(d_)[i_][j_] = (s_)[i_][j_]; \

//
// Some helper functions

inline bool ignoredUdpProtocol(const std::vector<uint8_t>& dgramPayload)
{
// Ignore RTPS headers
if (dgramPayload.size() < 20) // minimum RTPS header size, but only read 4 bytes
{
static uint8_t rtps_magic[4] = {0x52, 0x54, 0x50, 0x53}; // RTPS
if (memcmp(dgramPayload.data(), rtps_magic, sizeof(rtps_magic)) == 0)
{
return true;
}
}
return false;
}

}}}} // namespaces

#endif
Loading