alb_reader.cpp

Go to the documentation of this file.

// ROS headers
#include "ros/ros.h"
 
// Osef headers
#include "osefTypes.h"
 
// Local headers
#include "alb_common.h"
#include "alb_reader.h"
 
// Constant definition.
constexpr const size_t k_buffer_size = 100000000;
 
namespace
{
// Check for Little/Big endian architecture.
// Returns true if BigEndian.
bool isBigEndianArchitecture()
{
        const uint32_t i = 0x01020304;
        return reinterpret_cast<const uint8_t *>(&i)[0] == 1;
}
}; // namespace
 
AlbReader::AlbReader(ros::NodeHandle &nodeHandle) : alb_publisher(nodeHandle)
{
}
 
bool AlbReader::init()
{
        if (!areInputArgumentsValid()) {
                return false;
        }
 
        parseInputArguments();
 
        if (connectToLiveStream() == -1) {
                ROS_ERROR("[AlbReader] Unable to connect to live stream with address %s.", ip_v4.c_str());
                return false;
        }
 
        // Initialize buffer for reading data.
        buffer = std::make_unique<uint8_t[]>(k_buffer_size);
 
        return true;
}
 
int AlbReader::parse()
{
        int ret = 0;
        ret = tcp_reader.getNextFrame(buffer.get(), k_buffer_size);
 
        if (ret == 0) {
                connectToLiveStream();
        } else if (ret == 1) {
                Tlv::tlv_s *frame = (Tlv::tlv_s *)buffer.get();
 
                if (!isTlvParsable(frame)) {
                        return -1;
                }
 
                alb_publisher.publish(frame);
        }
 
        return ret;
}
 
bool AlbReader::isTlvParsable(const Tlv::tlv_s *frame)
{
        // Check expected root TLV type.
        if (frame->getType() != OSEF_TYPE_TIMESTAMPED_DATA) {
                ROS_ERROR("[AlbReader] Error: wrong frame TLV type");
                return false;
        }
 
        // Value parsing on big endian architecture is not supported by this code.
        if (isBigEndianArchitecture()) {
                ROS_ERROR("[AlbReader] Value parsing on big endian architecture is not supported");
                return false;
        }
 
        return true;
}
 
bool AlbReader::areInputArgumentsValid()
{
        ros::NodeHandle private_node("~");
 
        if (!private_node.hasParam(ALBCommon::k_alb_ip) || !private_node.hasParam(ALBCommon::k_alb_port)) {
                ROS_ERROR("[AlbReader] IP and port are not defined in parameters.");
                return false;
        }
 
        return true;
}
 
void AlbReader::parseInputArguments()
{
        ros::NodeHandle private_node("~");
        private_node.getParam(ALBCommon::k_alb_ip, ip_v4);
        private_node.getParam(ALBCommon::k_alb_port, port);
}
 
int AlbReader::connectToLiveStream()
{
        if (ip_v4.empty()) {
                ROS_ERROR("[AlbReader] IP of the ALB not defined.");
                return -1;
        }
 
        int ret = 0;
        tcp_reader.disconnectfromALB();
        ret = tcp_reader.connectToALB(ip_v4.c_str(), port);
 
        if (ret == 0) {
                ROS_WARN("[AlbReader] ALB not available yet (processing not started?) actively waiting "
                         "for it...");
                while ((ret = tcp_reader.connectToALB(ip_v4.c_str(), port)) == 0)
                        ;
        }
 
        if (!alb_initialized) {
                alb_initialized = true;
        } else {
                ROS_WARN("[AlbReader] Processing restarts with the ALB already initialized. "
                         "Make sure that a reinitialization is not needed.");
        }
 
        return ret;
}