Creating a Primitive Driver

Table of Contents

  1. Introduction
  2. JAUS Primitive Driver Description
  3. PD Demo Application
  4. PdClientDemo
  5. Simple Demo Walkthrough

Introduction

The purpose of this tutorial is to demonstrate how to use the OpenJAUS SDK to create a JAUS Primitive Driver (PD). Additionally a client of that driver will be developed to demonstrate the behavior and capabilities of the PD service.

This tutorial can also be used as a starting point for implementing other standard driver services (VelocityStateDriver, GlobalWaypointDriver, etc) or for implementing any services that inherit from the Management service.

When combined with the Global Positioning Sensor Tutorial , you should be able to get started with about 95% of all JAUS services.


JAUS Primitive Driver (PD)

The PD is a common service in JAUS systems. Its purpose is to provide basic platform mobility without implying any particular platform type, such as tracked or wheeled ground vehicle. It describes mobility in six degrees of freedom using a percent of available effort in each direction. Additionally, no power plant (gasoline, diesel, or battery) is implied and the service functions strictly in an open loop manner, i.e., a velocity is not commanded since that requires a speed sensor. Note that the specific actuator commands are not defined by JAUS.

The message set of the PrimitiveDriver service is defined in the JAUS Mobility Service Set (AS6009) and is outlined below:

Despite its small message set, the PD is a fairly complex service due its service inheritance chain. It inherits from the Management service, which is part of the JAUS Core Service Set (AS5710a)

The JAUS platform coordinate system is defined in AS6009 and is shown in Figure 1.

platform_coordinate

Figure 1. The JAUS coordinate system as defined in AS6009.


PdDemo

The PdDemo is a simple implementation of the PD service. It consists of two pieces in a single *.cpp file.

PdDemo.cpp source code


PdComponent Class

Services are implemented through the use of components. A component is a software element in a JAUS system which either provides or uses one or more services. More details on Components can be found in AS5710a. We implement our PdComponent by extending the skeleton component in openjaus::mobility::PrimitiveDriver.

class PdComponent : public mobility::PrimitiveDriver
{
public:
	PdComponent(std::string name) :
		PrimitiveDriver(),
		reportWrenchEffort()
	{
		this->name = name;

		// Initialize the ReportWrenchEffort message.
		// This message would be populated by the actual platform.
		// Setting values for all fields to show the fields being populated,
		// though they probably wouldn't make sense on a real platform.
		reportWrenchEffort.setPropulsiveLinearEffortX_percent(1.0);
		reportWrenchEffort.setPropulsiveLinearEffortY_percent(2.0);
		reportWrenchEffort.setPropulsiveLinearEffortZ_percent(3.0);
		reportWrenchEffort.setPropulsiveRotationalEffortX_percent(14.0);
		reportWrenchEffort.setPropulsiveRotationalEffortY_percent(15.0);
		reportWrenchEffort.setPropulsiveRotationalEffortZ_percent(16.0);
		reportWrenchEffort.setResistiveLinearEffortX_percent(27.0);
		reportWrenchEffort.setResistiveLinearEffortY_percent(28.0);
		reportWrenchEffort.setResistiveLinearEffortZ_percent(29.0);
		reportWrenchEffort.setResistiveRotationalEffortX_percent(30.0);
		reportWrenchEffort.setResistiveRotationalEffortY_percent(31.0);
		reportWrenchEffort.setResistiveRotationalEffortZ_percent(32.0);

		publish(mobility::ReportWrenchEffort::ID);
	}

In the code above, we first set the name of the component, and then initialize the fields of the ReportWrenchEffort message. In a real-world PD component, a thread or some other run-time mechanism would populate the message fields, but for this example we use the constructor. Finally, we need to register the ID number of any messages we want to be handled by the underlying Events service. Calling publish(mobility::ReportWrenchEffort::ID) lets the Event service properly respond to Create Event requests for the ReportWrenchEffort message. If we don’t register this ID then client components will not be able to create events for that message.

The next part of the PdComponent is the implementation of the virtual functions from the PrimitiveDriver base class. Message callbacks are implemented as “get” accessors where the query message is passed in as a parameter. This allows the component to respond appropriately.

The JAUS standards define state and guard conditions that are automatically handled by the OpenJAUS library so the message callbacks will not be executed unless it is appropriate to do so. The reader is encouraged to read the JAUS standard documents for a better understanding of state and guard conditions. Here is a description of each implemented method:

  • getReportWrenchEffort(QueryWrenchEffort *queryWrenchEffort)
    • Executed when a QueryWrenchEffort message is received.
    • In a real-world application the values of the ReportWrenchEffort message would come from some other thread or run-time process that queries the actual platform state.
  • setWrenchEffort(SetWrenchEffort *setWrenchEffort)
    • Executed when a SetWrenchEffort message is received. This callback will only be executed if the PD is in the READY State as defined in the Primitive Driver state machine.
    • In a real-world application the values of the SetWrenchEffort message would be used to control the platform. It would be the programmer’s responsibility to interface the appropriate message data to drive the vehicle actuators.
  • getReportWrench()
    • Accessor method so that our demo application can print the values of the message’s fields. This may not be necessary in your real-world application.
  • changeWrenchEffort(SetWrenchEffort *setWrenchEffort)
    • Helper method to update the ReportWrenchEffort message from the SetWrenchEffort message. This is only necessary for our demo application and wouldn’t be needed in your real-world application.

Main Application

The main application is a simple text-based console application that allows you to interact with the PdComponent and simulate different behaviors.

We have designed the main application to highlight a few critical and interesting methods which allow you to:

  • Initialize the PD service
  • Dismiss the controlling client
  • Notify subscribers of on-change events.

Initialization of the PD Service

One critical method that is used in the main application is component.initialized(). This method moves the PD from the INIT state to the STANDBY state. The INIT state allows the PD to initialize command and control over the vehicle’s actuators before accepting external JAUS commands. Any service that inherits from Management must use the initialized() method to transition out of the INIT state.

After the PD service has moved out of the INIT state it can be controlled, via the Access Control service messages, and moved to the READY state, using the Management service messages. The SetWrenchEffort message will have no effect until the PD service is both controlled and in the READY state.

It is important to review and understand the interactions between the state machines of the Primitive Driver, Management, and Access Control services as they are complex and can be confusing. The Primitive Driver itself is pretty simple but a lot of its behavior is handled by the Management and Access Control state machines.

// Initialize the Primitive Driver.
// Normally this would happen automatically and would do whatever
// work is necessary to initialize the platform. For example,
// you may need to run a calibration sequence.
// Since we don't have an actual platform we use a manual trigger.
std::cout << "Initializing the Primitive Driver" << std::endl;

// After the platform has been initialized, trigger the component
// to move out of the Init state. This is required since the Primitive
// Driver implements the Management interface.
component.initialized();

We provide a command in the main application to initialize the PD so that you can experiment with the behavior of the client if the PD remains in the INIT state. In a real-world application, the PD would move out of the INIT state after the platform has been initialized. For example, after performing a calibration routine or connecting to the actuators.


Dismissing the Controlling Client

As explained above, the PD service must be controlled before it can be transitioned to the READY state and the SetWrenchEffort message will have no effect until then. Usually, transitions between the Controlled and Not Controlled states are initiated from an Access Control client. However, sometimes it may be necessary for a service to dismiss the controller due to some internal event. A service can do this by calling the dismissController() method. When this is done the service will send a RejectControl message to the controlling client (if necessary) with the CONTROL_RELEASED response code.

// Dismiss the controlling client.
// The controlling client can be dismissed internally if necessary.
std::cout << "Dismissing controller" << std::endl;
component.dismissController();

Notifying On-Change Event Subscribers

If you want your service to support on-change events then you must alert the Event service when a change has occurred. This is done by calling the notifyChanged(unit16 messageId) method. In our example, we support on-change events for the ReportWrenchEffort message so we call the notifyChanged method using the ReportWrenchEffort message ID. When this method is called the Event service will send out a ReportWrenchEffort message to all the clients that subscribed to that on-change event.

// Notify the Event service that the ReportWrenchEffort message has
// changed. This will cause a ReportWrenchEffort message to be sent
// to all the on-change event subscribers.
std::cout << "Trigger On-Changed Event for Report Wrench Effort" << std::endl;
component.notifyChanged(mobility::ReportWrenchEffort::ID);

PdClientDemo

PdClientDemo.cpp source code

The PdClientDemo is designed to give an example of how to build a component which would be a client to the PD component. This simple example shows the user how to do common activities within the OpenJAUS framework such as:

  • Finding a PD service by its uniform resource identifier (URI) (Discovery service).
  • Creating and stopping periodic and on-change events (Events service).
  • Taking control of the PD service (Access Control service).
  • Changing the PD state (Management service).
  • Querying and setting the wrench effort (Primitive Driver service).

Finding a Service

The Discovery service automatically exchanges system information with other JAUS components and maintains a System Tree of components and services. Finding a PD service on the System Tree is easily done using the lookupService(string uri) method of the SystemTree class. This returns a vector of JausAddress objects where each JausAddress is for a matching component that implements the PD service. This method can be used to find any service by providing the correct URI.

pdList = component.getSystemTree()->lookupService(mobility::PrimitiveDriverInterface::PrimitiveDriverUri());

std::cout << "PD Services (" << pdList.size() << "):\n";
for(size_t i = 0; i < pdList.size(); i++)
{
	std::cout << "\t" << pdList.at(i).toString() << std::endl;
}

Creating and Stopping Periodic Events

Creating a periodic event is something done very often in the JAUS architecture. This allows a component to create a standing query to another service’s information at a particular update rate. Creation and management of the periodic event is handled by the Event service which is implemented in the OpenJAUS library. Creation is done using the subscribePeriodic(address, message, rateHz) function as shown below. This returns a unique Event ID which is used for further interaction with the Event service such as unsubscribing to an event using the unsubscribe(id) method.

mobility::QueryWrenchEffort *query = new mobility::QueryWrenchEffort();
query->setQueryPresenceVector(mobility::ReportWrenchEffort::PV_ALL_FIELDS);

// Magic Number [10]: 10 is the requested rate (in Hz) of the periodic event
periodicSubscriptionId = component.subscribePeriodic(pdList.at(0), query, 10.0);
if(component.unsubscribe(periodicSubscriptionId))
{
	periodicSubscriptionId = 0;
}

Creating and Stopping On-Change Events

Another type of event that can be created are on-change events. Unlike periodic events, on-change events only send updates when there has been a change in the system. This reduces the number of redundant messages received from the client; but care should be taken when using this type of event since update messages may be lost, depending on the communication channel being used (for example, UDP).

If you are using the OpenJAUS SDK to create the server side of the service, ensure that you are correctly notifying subscribers of the changes as described in the Notifying On-Change Event Subscribers section above. If the server side of the service does not correctly notify its subscribers then you will never receive an on-change event update.

Create on-change events by using the subscribeOnChange (address, message) method as shown below. Just like periodic events, you will get a unique Event ID which can be used to manage the created event. Stopping an on-change event is also done using the unsubscribe(id) method.

mobility::QueryWrenchEffort *query = new mobility::QueryWrenchEffort();
query->setQueryPresenceVector(mobility::ReportWrenchEffort::PV_ALL_FIELDS);
onChangeSubscriptionId = component.subscribeOnChange(pdList.at(0), query);

Requesting and Releasing Control of a Service

If a service inherits from Access Control then usually you will need to have control of the service before you can do anything useful. In order to control a service you must control the entire component it belongs to.

Requesting control of a component is done through the use of the requestControl method. When you call this method you register a callback function that will be executed when a control response message (ConfirmControl or RejectControl) is received. In your callback, you should ensure that you actually did get control of the component since control may not be available, or the component may already have a controller and reject your request.

component.requestControl(pdList.at(0), processControlResponse);
void processControlResponse(const model::ControlResponse &response)
{
	std::cout << "Recv Control Request Response from: " << response.getAddress() << std::endl;
	std::cout << "Response code: " << response.getResponseType() << std::endl;
}

Releasing control of component is done using the releaseControl method. Unlike the requestControl method you do not register a callback; but when a control response message is returned the callback you originally registered will be executed. Again, it is a good idea to check the type of the response to ensure control was actually released.

component.releaseControl(pdList.at(0));

Sending a Message

Sending a message is very simple. Create the message to be sent, populate it with the required data and send the message using the sendMessage function. Remember to set the destination address and presence vector (if necessary) message fields before you send the message otherwise you may experience problems. The source address will automatically be set for you.

mobility::QueryWrenchEffort *query = new mobility::QueryWrenchEffort();
query->setQueryPresenceVector(mobility::ReportWrenchEffort::PV_ALL_FIELDS);
sendMessage(component, pdList, query);
void sendMessage(core::Base& component, const std::vector<transport::address>& pdList, model::Message* message)
{
	if (pdList.size() > 0)
	{
		message->setDestination(pdList.at(0));
		component.sendMessage(message);
	}
	else
	{
		std::cout << "No known Primitive Drivers!. You need to find one first." << std::endl;
	}
}

Simple Demo Walkthrough

We provide a simple walkthrough to highlight the sequence of events necessary to actually set the wrench effort of the Primitive Driver.

  1. Start the PdDemo and PdClientDemo applications.
    • Note that the default networking configuration components allows both demo applications to run on the same machine and they will discover each other. If you wish to run components across a network on separate machines, then you will have to configure them differently using an application configuration (.conf) file.
  2. Switch to the PdClientDemo screen.
  3. Press ‘t’ to show the System Tree.
    • You should see both the PdDemo and PdClientDemo components listed.
  4. Press ‘q’ to query the status of the PdDemo component.
    • Notice that you can’t query the PD. You need to find it in the System Tree first.
  5. Press ‘f’ to find a PD.
    • The component address of the PD service is listed.
  6. Press ‘q’ to query the status of the PD.
    • The PD is in the INIT state.
  7. Press ‘5’ to request control of the PD.
    • Note that control is NOT_AVAILABLE. This is because the PD is in the INIT state.
  8. Switch to the PdDemo screen.
  9. Press ‘i’ to initialize the PD.
    • If the client queries the PD status now it will be in the STANDBY state.
  10. Switch back to the PdClientDemo screen.
  11. Press ‘7’ to query the wrench effort.
    • Take note of the wrench effort values.
  12. Press ‘8’ to set the wrench effort.
    • Check the values by querying the wrench effort again. The values will not have changed. The PD needs to be in the READY state before it will accept SetWrenchEffort messages.
  13. Press ‘5’ to take control of the PD.
    • The PD cannot move to READY state without a controller. The PD will remain in the STANDBY state.
  14. Press ‘s’ to resume the PD and move it to the READY state.
    • You can always query the PD status by pressing ‘q’.
  15. Press ‘8’ to set the wrench effort.
    • Querying the wrench effort again shows that this time the values have changed.

Leave a Reply

Your email address will not be published. Required fields are marked *