Introduction


In this guide, we will walk through the implementation of the Global Waypoint List Driver and requires you to understand  How to create a Component and How to add Services to a Component.


Starting in SDK v6.0.0-dev.12, the OjListManager class was introduced. This class partially implements the ListManager service to handle most of the list management needs and simplify the implementation of all derived List Driver services.


.h file

#include <openjaus/core_v1_1/Managed.h>
#include <openjaus/mobility_v1_0/Services/impl/OjListManager.h>
#include <openjaus/mobility_v1_0/Services/GlobalWaypointListDriver.h>

class MyComponent :
    public virtual openjaus::mobility_v1_0::services::impl::OjListManager,
    public virtual openjaus::mobility_v1_0::services::GlobalWaypointListDriver,
    public openjaus::core_v1_1::Managed // [1]
{
public:
    MyComponent();
    virtual ~MyComponent();

protected:
    // Required OjListManager methods
    virtual bool isElementDataSupported(const openjaus::mobility_v1_0::ElementDataBlob& value); // [2]

    // Required GlobalWaypointListDriver methods // [3]
    virtual bool modifyTravelSpeed(openjaus::mobility_v1_0::ExecuteList *executeList);
    virtual bool executeWaypointList(openjaus::mobility_v1_0::ExecuteList *executeList);

    virtual bool waypointExists(openjaus::mobility_v1_0::ExecuteList *executeList);
    virtual bool elementSpecified(openjaus::mobility_v1_0::ExecuteList *executeList);

    virtual openjaus::mobility_v1_0::ReportActiveElement getReportActiveElement(openjaus::mobility_v1_0::QueryActiveElement *queryActiveElement);
};


Notes:

[1] The Managed base component is used since the Management service is required in the inheritance chain. For IOP components the IopManaged base component should be used.

[2] The OjListManager class requires this method to be implemented.

[3] Only the methods relevant to the List management functionality is shown. There may be additional methods that need to be implemented for a fully functional Global Waypoint List Driver service.



.cpp file

#include <openjaus/mobility_v1_0/Triggers/SetGlobalWaypoint.h>

bool MyComponent::isElementDataSupported(const openjaus::mobility_v1_0::ElementDataBlob& value)
{
    // OJINFO: Check that the contents of the ElementDataBlob is a supported type
    uint16 messageId;

    openjaus::system::Buffer& payload = const_cast<openjaus::system::Buffer&>(value.getPayload());
    openjaus::system::BufferReader& reader = payload.getReader();
    reader.peek(messageId);

    return (messageId == openjaus::mobility_v1_0::SetGlobalWaypoint::ID);
}

bool MyComponent::modifyTravelSpeed(openjaus::mobility_v1_0::ExecuteList *executeList)
{
    double speed_mps = executeList->getSpeed_mps();

    // OJTASK: Update the travel speed of the vehicle to match the specified speed

    return true;
}

bool MyComponent::executeWaypointList(openjaus::mobility_v1_0::ExecuteList *executeList)
{
    // OJINFO: Get the list from the OjListManager
    openjaus::mobility_v1_0::OjManagedList& managedList = openjaus::mobility_v1_0::services::impl::OjListManager::getManagedList();

    uint16 uid = executeList->getElementUID();
    if (uid == 0)
    {
        uid = managedList.getStartUid();
    }

    openjaus::mobility_v1_0::ElementRecord record = managedList.get(uid);
    openjaus::mobility_v1_0::ElementDataBlob& dataBlob = record.getElementData();


    // OJINFO: Convert the ElementDatBlob to the specific message type
    openjaus::system::Buffer& payload = const_cast<openjaus::system::Buffer&>(dataBlob.getPayload());
    openjaus::system::BufferReader& reader = payload.getReader();

    openjaus::mobility_v1_0::SetGlobalWaypoint setGlobalWaypoint;
    setGlobalWaypoint->from(reader);

    double speed_mps = executeList->getSpeed_mps();

    // OJTASK: Navigate to the specified waypoint at the specified speed
    // **OJNOTE**: This method should not block while moving between the various commanded waypoints.
    // It is left to the implementer to properly handle this requirement.

    return true;
}

bool MyComponent::waypointExists(openjaus::mobility_v1_0::ExecuteList *executeList)
{
    //OJINFO: True if the Element UID specified in the message that triggered the transition exists in the list.

    uint16 elementUid = 0;
    if ((executeList->isElementUIDEnabled()))
    {
        elementUid = executeList->getElementUID();
    }

    openjaus::mobility_v1_0::OjManagedList& managedList = openjaus::mobility_v1_0::services::impl::OjListManager::getManagedList();

    if (elementUid == 0)
    {
        return (managedList.size() > 0);
    }
    else
    {
        return managedList.contains(elementUid);
    }
}

bool MyComponent::elementSpecified(openjaus::mobility_v1_0::ExecuteList *executeList)
{
    //OJINFO: True if the ExecuteList message that triggered the transition contains an Element UID
    return executeList->isElementUIDEnabled();
}

openjaus::mobility_v1_0::ReportActiveElement MyComponent::getReportActiveElement(openjaus::mobility_v1_0::QueryActiveElement *queryActiveElement)
{
    openjaus::mobility_v1_0::ReportActiveElement message;

    // OJTASK: Populate the ReportActiveElement message with the element UID of the waypoint being executed.

    return message;
}