Data Socket for communication with Data Scope

From Neuromeka Wiki
Jump to: navigation, search


NRMKDataSocket is intended to work with NRMK Data Scope.

Definition of Custom Data Socket

  • Users have to define their own custom data sockets derived from NRMKHelper::NRMKDataSocket. The general rule is very simple as shown in the following code, e.g. STEPDataSocket.h.
#pragma once
#include "NRMKDataSocket.h"

/// TO DO: Define the proper number of (float) data (transmitted via data socket) in the template arguments.
enum
{
	NUM_DATASCOPE_SIGNALS = NUM_DATA_CH1 + NUM_DATA_CH2,
};

typedef NRMKHelper::NRMKDataSocket<NUM_DATASCOPE_SIGNALS> _STEP_DATA_SOCKET;

class STEPDataSocket : public _STEP_DATA_SOCKET
{
public:
	enum
	{
		NUM_DATA = _STEP_DATA_SOCKET::NUM_DATA,
	};

public:
	inline STEPDataSocket()
		: _STEP_DATA_SOCKET(_dataChannelArray, _sizeDataChannelArray)
	{
	}

private:
	/// TO DO: Implement the data array.
	// The following is the default behavior sending the joint angles followed by the joint velocities.
	// Note that only float data is allowed for valid plot in Data Scope.
	virtual int _updatePacketData(float * const data)
	{
                // Prepare datas 
		for (int k = 0; k < NUM_DATA; k++)
			data[k] = ... ;

		// Always return the number of bytes of the updated data packet
		return NUM_DATA;
	}

private:
	/// TO DO: Define the proper data channel format array in NRMKFrameworkSR5.cpp.
	static const unsigned int _dataChannelArray[];

	/// TO DO: Define the size of actual _dataChannelArray in NRMKFrameworkSR5.cpp.
	static const unsigned int _sizeDataChannelArray;
};
  • The parameter NUM_DATASCOPE_SIGNALS is the number of data (of type float) to transmit to Data Scope.
  • Users have to prepare the data in _updatePacketData() .
  • For example, the following code generates two channels of plot in Data Scope, where the first channel displays two signals and the second three signals. This information is implemented in the C++ source file, e.g. STEPDataSocket.cpp. Note that NRMKFoundation does not rely on dynamic memory allocation and every data should be provided statically.


#include "STEPDataSocket.h"

/// TO DO: Fill in the number of data and the data channel index array
const unsigned int STEPDataSocket::_dataChannelArray[]
// The following code generates two (or three) channels charts (all of time chart style),
// where each chart plots the signals (as many as the joint dof).
//
// This is the example to plot the joint angles in the first channel, the joint velocities in the second channel,
// the joint torque (in controlled simulation) in the third channel for a seven dof manipulator.
	= {
		NRMK_SCOKET_PACKET_DATA_CH(0), NRMK_SOCKET_PACKET_DATA_STYLE_TIME_CHART, 2, 0, 1,
		NRMK_SCOKET_PACKET_DATA_CH(1), NRMK_SOCKET_PACKET_DATA_STYLE_TIME_CHART, 3, 2, 3, 4,
	};

// TO DO: Set the actual size of the data channel format array defined above
const unsigned int STEPDataSocket::_sizeDataChannelArray = sizeof(_dataChannelArray)/sizeof(unsigned int);


  • The data channel format array or _dataChannelArray[] specifies the format of each channel: channel id, plot style, the number of signals, the signal id.
  • The complete data socket definition file, STEPDataSocket.h is shown below.


#pragma once
#include <math.h>
#include "NRMKDataSocket.h"

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

/// TO DO: Define the proper number of (float) data (transmitted via data socket) in the template arguments.
enum
{
	NUM_DATA_CH1 = 2,
	NUM_DATA_CH2 = 3,
	NUM_DATASCOPE_SIGNALS = NUM_DATA_CH1 + NUM_DATA_CH2,
};

typedef NRMKHelper::NRMKDataSocket<NUM_DATASCOPE_SIGNALS> _STEP_DATA_SOCKET;

class STEPDataSocket : public _STEP_DATA_SOCKET
{
public:
	enum
	{
		NUM_DATA = _STEP_DATA_SOCKET::NUM_DATA,
	};

public:
	inline STEPDataSocket()
		: _STEP_DATA_SOCKET(_dataChannelArray, _sizeDataChannelArray)
		, _tick(0), _delT(0.001)
	{
		_base_freq = 3.0;
	}

	void setPeriod(float delT)
	{
		_delT = delT;
	}

private:
	/// TO DO: Implement the data array.
	// The following is the default behavior sending the joint angles followed by the joint velocities.
	// Note that only float data is allowed for valid plot in Data Scope.
	virtual int _updatePacketData(float * const data)
	{
		for (int k = 0; k < NUM_DATA_CH1; k++)
			data[k] = static_cast<float>(sin(2*M_PI*_base_freq*(k+1)*_tick*_delT));

		for (int k = 0; k < NUM_DATA_CH2; k++)
			data[NUM_DATA_CH1 + k] = static_cast<float>(cos(2*M_PI*_base_freq*(k+1)*_tick*_delT));

		_tick++;

		// Always return the number of bytes of the updated data packet
		return NUM_DATA;
	}

private:
	/// TO DO: Define the proper data channel format array in NRMKFrameworkSR5.cpp.
	static const unsigned int _dataChannelArray[];

	/// TO DO: Define the size of actual _dataChannelArray in NRMKFrameworkSR5.cpp.
	static const unsigned int _sizeDataChannelArray;

	float _base_freq;
	float _delT;
	long _tick;
};

Implementation of Data Socket Server

  • One has to start a data socket server in applications, so that Data Scope can connect to the server and receives the signals.

Creating the data socket server

#include "STEPDataSocket.h"

STEPDataSocket datasocket;
static int step_data = 10;
volatile int tick_data = 0;
  • "step_data" is used to specify how often the data is transmitted to Data Scope. If you want to send the data every time set this to 1.

Starting the data socket server

/// TO DO: Create data socket server
if (datasocket.startServer(SOCK_TCP, '''NRMK_PORT_DATA'''))
	printf("Data server started at IP of : %s on Port: %d\n", datasocket.getAddress(), NRMK_PORT_DATA);

printf("Waiting for Data Scope to connect...\n");
datasocket.waitForConnection();
//datasocket.waitForConnection(0);
  • The default port number is NRMK_PORT_DATA which is set to 3001. You may change if you want.
  • waitForConnection() is used to make the server wait for a client to connect. If you do not specify the waiting time, it will wait indefinitely till a client to connect. If you specify a time, it will wait only for a specified time. Later, a client can still connect.

Transmitting the signals via the data socket

if (datasocket.hasConnection() && tick_data-- == 0)
{
	tick_data = step_data - 1; // 'minus one' is necessary for intended operation

	datasocket.update(t);
}
  • update() updates the signals and send them to the Data Scope. It will call _updatePacketData() implemented in the custom data socket, i.e. STEPDataSocket.h.

Example

  • The following is an example of HelloDataScope.cpp. It sends the data periodically.


#include <stdio.h>
#include <signal.h>
#include <unistd.h>
#include <sys/mman.h>

#include <native/task.h>
#include <native/timer.h>

#include "STEPDataSocket.h"

RT_TASK plot_task;

// Data is sent once every 10 times.
volatile float t;

STEPDataSocket datasocket;

static int step_data = 10;
volatile int tick_data = 0;

/* NOTE: error handling omitted. */

void plot(void *arg)
{
	/*
	 * Arguments: &task (NULL=self),
	 *            start time,
	 *            period (here: 1 s)
	 */
	rt_task_set_periodic(NULL, TM_NOW, 1e6);	// period = 1 (msec)

	t = 0;

	while (t <= 10.0)
	{
		/// TO DO: You have to prepare data for NRMKDataSocket
		if (datasocket.hasConnection() && tick_data-- == 0)
		{
			tick_data = step_data - 1; // 'minus one' is necessary for intended operation

			datasocket.update(t);
		}

		t += 0.001;

		rt_task_wait_period(NULL);
	}
}

void catch_signal(int sig)
{
	rt_task_delete(&plot_task);

	printf("Now exiting...\n");
	exit(1);
}

int main(int argc, char* argv[])
{
	signal(SIGTERM, catch_signal);
	signal(SIGINT, catch_signal);

	/* Avoids memory swapping for this program */
	mlockall(MCL_CURRENT|MCL_FUTURE);

	/// TO DO: Create data socket server
	datasocket.setPeriod(0.001);

	if (datasocket.startServer(SOCK_TCP, NRMK_PORT_DATA))
		printf("Data server started at IP of : %s on Port: %d\n", datasocket.getAddress(), NRMK_PORT_DATA);

	printf("Waiting for Data Scope to connect...\n");
	datasocket.waitForConnection();
	//datasocket.waitForConnection(0);

	/*
	 * Arguments: &task,
	 *            name,
	 *            stack size (0=default),
	 *            priority,
	 *            mode (FPU, start suspended, ...)
	 */
	rt_task_create(&plot_task, "trivial", 0, 99, 0);

	/*
	 * Arguments: &task,
	 *            task function,
	 *            function argument
	 */
	rt_task_start(&plot_task, &plot, NULL);

	pause();

	rt_task_delete(&plot_task);

	printf("Now terminating...\n");

	return 0;
}

Project Setting

  • In order to build the project you have to set proper a project setting. The easiest way is to copy the preconfigured Eclipse project file to the current one.
STEP download
STEP/PC .cproject
STEP/BBB .cproject

Running Data Scope

  • Open Data Scope. While the data socket server is running, click Connect button and enter the IP address of the server and port number.


Connecting Data Scope

Connecting Data Scope

  • Then the channels are initialized and the signals are drawn as they arrives.
  • Note that depending on socket bandwidth some data may be lost. If you want to plot every signals you are to send you'd better save a bundle of the data packet and send it. For detailed explanation see Data Cope.