Program Listing for File primary_parser.h
↰ Return to documentation for file (/tmp/ws/src/ur_client_library/include/ur_client_library/primary/primary_parser.h
)
/*
* Copyright 2019, FZI Forschungszentrum Informatik (refactor)
*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#pragma once
#include <vector>
#include "ur_client_library/comm/bin_parser.h"
#include "ur_client_library/comm/pipeline.h"
#include "ur_client_library/comm/parser.h"
#include "ur_client_library/primary/package_header.h"
#include "ur_client_library/primary/robot_state.h"
#include "ur_client_library/primary/robot_message.h"
#include "ur_client_library/primary/robot_state/kinematics_info.h"
#include "ur_client_library/primary/robot_message/version_message.h"
namespace urcl
{
namespace primary_interface
{
class PrimaryParser : public comm::Parser<PrimaryPackage>
{
public:
PrimaryParser() = default;
virtual ~PrimaryParser() = default;
bool parse(comm::BinParser& bp, std::vector<std::unique_ptr<PrimaryPackage>>& results)
{
int32_t packet_size;
RobotPackageType type;
bp.parse(packet_size);
bp.parse(type);
switch (type)
{
case RobotPackageType::ROBOT_STATE:
{
while (!bp.empty())
{
if (!bp.checkSize(sizeof(uint32_t)))
{
URCL_LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
return false;
}
uint32_t sub_size = bp.peek<uint32_t>();
if (!bp.checkSize(static_cast<size_t>(sub_size)))
{
URCL_LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size);
return false;
}
// deconstruction of a sub parser will increment the position of the parent parser
comm::BinParser sbp(bp, sub_size);
sbp.consume(sizeof(sub_size));
RobotStateType type;
sbp.parse(type);
std::unique_ptr<PrimaryPackage> packet(stateFromType(type));
if (packet == nullptr)
{
sbp.consume();
// TODO: create robot state type here
continue;
}
if (!packet->parseWith(sbp))
{
URCL_LOG_ERROR("Sub-package parsing of type %d failed!", static_cast<int>(type));
return false;
}
results.push_back(std::move(packet));
if (!sbp.empty())
{
URCL_LOG_ERROR("Sub-package of type %d was not parsed completely!", static_cast<int>(type));
sbp.debug();
return false;
}
}
break;
}
case RobotPackageType::ROBOT_MESSAGE:
{
uint64_t timestamp;
uint8_t source;
RobotMessagePackageType message_type;
bp.parse(timestamp);
bp.parse(source);
bp.parse(message_type);
std::unique_ptr<PrimaryPackage> packet(messageFromType(message_type, timestamp, source));
if (!packet->parseWith(bp))
{
URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast<int>(message_type));
return false;
}
results.push_back(std::move(packet));
return true;
break;
}
default:
{
URCL_LOG_DEBUG("Invalid robot package type recieved: %u", static_cast<uint8_t>(type));
bp.consume();
return true;
}
}
return true;
}
private:
RobotState* stateFromType(RobotStateType type)
{
switch (type)
{
/*case robot_state_type::ROBOT_MODE_DATA:
// SharedRobotModeData* rmd = new SharedRobotModeData();
//return new rmd;
case robot_state_type::MASTERBOARD_DATA:
return new MBD;*/
case RobotStateType::KINEMATICS_INFO:
return new KinematicsInfo(type);
default:
return new RobotState(type);
}
}
RobotMessage* messageFromType(RobotMessagePackageType type, uint64_t timestamp, uint8_t source)
{
switch (type)
{
/*case robot_state_type::ROBOT_MODE_DATA:
// SharedRobotModeData* rmd = new SharedRobotModeData();
//return new rmd;
case robot_state_type::MASTERBOARD_DATA:
return new MBD;*/
case RobotMessagePackageType::ROBOT_MESSAGE_VERSION:
return new VersionMessage(timestamp, source);
default:
return new RobotMessage(timestamp, source);
}
}
};
} // namespace primary_interface
} // namespace urcl