/**
* Copy event packets to the ring buffer for transfer to the output handler thread.
*
* @param state output module state.
* @param packetsListSize the length of the variable-length argument list of event packets.
* @param packetsList a variable-length argument list of event packets.
*/
static void copyPacketsToTransferRing(outputCommonState state, size_t packetsListSize, va_list packetsList) {
caerEventPacketHeader packets[packetsListSize];
size_t packetsSize = 0;
// Count how many packets are really there, skipping empty event packets.
for (size_t i = 0; i < packetsListSize; i++) {
caerEventPacketHeader packetHeader = va_arg(packetsList, caerEventPacketHeader);
// Found non-empty event packet.
if (packetHeader != NULL) {
// Get source information from the event packet.
int16_t eventSource = caerEventPacketHeaderGetEventSource(packetHeader);
// Check that source is unique.
int16_t sourceID = I16T(atomic_load_explicit(&state->sourceID, memory_order_relaxed));
if (sourceID == -1) {
state->sourceInfoNode = caerMainloopGetSourceInfo(U16T(eventSource));
if (state->sourceInfoNode == NULL) {
// This should never happen, but we handle it gracefully.
caerLog(CAER_LOG_ERROR, state->parentModule->moduleSubSystemString,
"Failed to get source info to setup output module.");
return;
}
atomic_store(&state->sourceID, eventSource); // Remember this!
}
else if (sourceID != eventSource) {
caerLog(CAER_LOG_ERROR, state->parentModule->moduleSubSystemString,
"An output module can only handle packets from the same source! "
"A packet with source %" PRIi16 " was sent, but this output module expects only packets from source %" PRIi16 ".",
eventSource, sourceID);
continue;
}
// Source ID is correct, packet is not empty, we got it!
packets[packetsSize++] = packetHeader;
}
}
// There was nothing in this mainloop run!
if (packetsSize == 0) {
return;
}
// Allocate memory for event packet array structure that will get passed to output handler thread.
caerEventPacketContainer eventPackets = caerEventPacketContainerAllocate((int32_t) packetsSize);
if (eventPackets == NULL) {
return;
}
// Handle the valid only flag here, that way we don't have to do another copy and
// process it in the output handling thread. We get the value once here, so we do
// the same for all packets from the same mainloop run, avoiding mid-way changes.
bool validOnly = atomic_load_explicit(&state->validOnly, memory_order_relaxed);
// Now copy each event packet and send the array out. Track how many packets there are.
size_t idx = 0;
for (size_t i = 0; i < packetsSize; i++) {
if (validOnly) {
caerEventPacketContainerSetEventPacket(eventPackets, (int32_t) idx,
caerCopyEventPacketOnlyValidEvents(packets[i]));
}
else {
caerEventPacketContainerSetEventPacket(eventPackets, (int32_t) idx,
caerCopyEventPacketOnlyEvents(packets[i]));
}
if (caerEventPacketContainerGetEventPacket(eventPackets, (int32_t) idx) == NULL) {
// Failed to copy packet. Signal but try to continue anyway.
if ((validOnly && (caerEventPacketHeaderGetEventValid(packets[i]) == 0))
|| (!validOnly && (caerEventPacketHeaderGetEventNumber(packets[i]) == 0))) {
caerLog(CAER_LOG_NOTICE, state->parentModule->moduleSubSystemString,
"Submitted empty event packet to output. Ignoring empty event packet.");
}
else {
caerLog(CAER_LOG_ERROR, state->parentModule->moduleSubSystemString,
"Failed to copy event packet to output.");
}
}
else {
idx++;
}
}
// We might have failed to copy all packets (unlikely).
if (idx == 0) {
caerEventPacketContainerFree(eventPackets);
return;
}
// Reset packet container size so we only consider the packets we managed
// to successfully copy.
caerEventPacketContainerSetEventPacketsNumber(eventPackets, (int32_t) idx);
retry: if (!ringBufferPut(state->transferRing, eventPackets)) {
if (atomic_load_explicit(&state->keepPackets, memory_order_relaxed)) {
// Retry forever if requested.
goto retry;
}
caerEventPacketContainerFree(eventPackets);
caerLog(CAER_LOG_INFO, state->parentModule->moduleSubSystemString,
"Failed to put packet's array copy on transfer ring-buffer: full.");
return;
}
}
int main(void) {
// Install signal handler for global shutdown.
#if defined(_WIN32)
if (signal(SIGTERM, &globalShutdownSignalHandler) == SIG_ERR) {
caerLog(CAER_LOG_CRITICAL, "ShutdownAction", "Failed to set signal handler for SIGTERM. Error: %d.", errno);
return (EXIT_FAILURE);
}
if (signal(SIGINT, &globalShutdownSignalHandler) == SIG_ERR) {
caerLog(CAER_LOG_CRITICAL, "ShutdownAction", "Failed to set signal handler for SIGINT. Error: %d.", errno);
return (EXIT_FAILURE);
}
#else
struct sigaction shutdownAction;
shutdownAction.sa_handler = &globalShutdownSignalHandler;
shutdownAction.sa_flags = 0;
sigemptyset(&shutdownAction.sa_mask);
sigaddset(&shutdownAction.sa_mask, SIGTERM);
sigaddset(&shutdownAction.sa_mask, SIGINT);
if (sigaction(SIGTERM, &shutdownAction, NULL) == -1) {
caerLog(CAER_LOG_CRITICAL, "ShutdownAction", "Failed to set signal handler for SIGTERM. Error: %d.", errno);
return (EXIT_FAILURE);
}
if (sigaction(SIGINT, &shutdownAction, NULL) == -1) {
caerLog(CAER_LOG_CRITICAL, "ShutdownAction", "Failed to set signal handler for SIGINT. Error: %d.", errno);
return (EXIT_FAILURE);
}
#endif
// Open a DYNAPSE, give it a device ID of 1, and don't care about USB bus or SN restrictions.
caerDeviceHandle dynapse_handle = caerDeviceOpen(1, CAER_DEVICE_DYNAPSE, 0, 0, NULL);
if (dynapse_handle == NULL) {
return (EXIT_FAILURE);
}
// Let's take a look at the information we have on the device.
struct caer_dynapse_info dynapse_info = caerDynapseInfoGet(dynapse_handle);
printf("%s --- ID: %d, Master: %d, Logic: %d.\n", dynapse_info.deviceString, dynapse_info.deviceID,
dynapse_info.deviceIsMaster, dynapse_info.logicVersion);
// Send the default configuration before using the device.
// No configuration is sent automatically!
caerDeviceSendDefaultConfig(dynapse_handle);
// Now let's get start getting some data from the device. We just loop in blocking mode,
// no notification needed regarding new events. The shutdown notification, for example if
// the device is disconnected, should be listened to.
// This automatically turns on the AER and CHIP state machines.
caerDeviceDataStart(dynapse_handle, NULL, NULL, NULL, &usbShutdownHandler, NULL);
// Let's turn on blocking data-get mode to avoid wasting resources.
caerDeviceConfigSet(dynapse_handle, CAER_HOST_CONFIG_DATAEXCHANGE, CAER_HOST_CONFIG_DATAEXCHANGE_BLOCKING, true);
while (!atomic_load_explicit(&globalShutdown, memory_order_relaxed)) {
caerEventPacketContainer packetContainer = caerDeviceDataGet(dynapse_handle);
if (packetContainer == NULL) {
continue; // Skip if nothing there.
}
int32_t packetNum = caerEventPacketContainerGetEventPacketsNumber(packetContainer);
printf("\nGot event container with %d packets (allocated).\n", packetNum);
for (int32_t i = 0; i < packetNum; i++) {
caerEventPacketHeader packetHeader = caerEventPacketContainerGetEventPacket(packetContainer, i);
if (packetHeader == NULL) {
printf("Packet %d is empty (not present).\n", i);
continue; // Skip if nothing there.
}
printf("Packet %d of type %d -> size is %d.\n", i, caerEventPacketHeaderGetEventType(packetHeader),
caerEventPacketHeaderGetEventNumber(packetHeader));
// Spike Events
if (i == SPIKE_EVENT) {
caerSpikeEventPacket spike = (caerSpikeEventPacket) packetHeader;
// Get full timestamp and addresses of first event.
caerSpikeEventConst firstEvent = caerSpikeEventPacketGetEventConst(spike, 0);
int32_t ts = caerSpikeEventGetTimestamp(firstEvent);
uint16_t neuid = caerSpikeEventGetNeuronID(firstEvent);
uint16_t coreid = caerSpikeEventGetSourceCoreID(firstEvent);
printf("First spike event - ts: %d, neu: %d, core: %d\n", ts, neuid, coreid);
}
}
caerEventPacketContainerFree(packetContainer);
}
caerDeviceDataStop(dynapse_handle);
caerDeviceClose(&dynapse_handle);
printf("Shutdown successful.\n");
return (EXIT_SUCCESS);
}
int main(void) {
// Install signal handler for global shutdown.
#if defined(_WIN32)
if (signal(SIGTERM, &globalShutdownSignalHandler) == SIG_ERR) {
caerLog(CAER_LOG_CRITICAL, "ShutdownAction", "Failed to set signal handler for SIGTERM. Error: %d.", errno);
return (EXIT_FAILURE);
}
if (signal(SIGINT, &globalShutdownSignalHandler) == SIG_ERR) {
caerLog(CAER_LOG_CRITICAL, "ShutdownAction", "Failed to set signal handler for SIGINT. Error: %d.", errno);
return (EXIT_FAILURE);
}
#else
struct sigaction shutdownAction;
shutdownAction.sa_handler = &globalShutdownSignalHandler;
shutdownAction.sa_flags = 0;
sigemptyset(&shutdownAction.sa_mask);
sigaddset(&shutdownAction.sa_mask, SIGTERM);
sigaddset(&shutdownAction.sa_mask, SIGINT);
if (sigaction(SIGTERM, &shutdownAction, NULL) == -1) {
caerLog(CAER_LOG_CRITICAL, "ShutdownAction", "Failed to set signal handler for SIGTERM. Error: %d.", errno);
return (EXIT_FAILURE);
}
if (sigaction(SIGINT, &shutdownAction, NULL) == -1) {
caerLog(CAER_LOG_CRITICAL, "ShutdownAction", "Failed to set signal handler for SIGINT. Error: %d.", errno);
return (EXIT_FAILURE);
}
#endif
// Open a DVS128, give it a device ID of 1, and don't care about USB bus or SN restrictions.
caerDeviceHandle dvs128_handle = caerDeviceOpen(1, CAER_DEVICE_DVS128, 0, 0, NULL);
if (dvs128_handle == NULL) {
return (EXIT_FAILURE);
}
// Let's take a look at the information we have on the device.
struct caer_dvs128_info dvs128_info = caerDVS128InfoGet(dvs128_handle);
printf("%s --- ID: %d, Master: %d, DVS X: %d, DVS Y: %d, Logic: %d.\n", dvs128_info.deviceString,
dvs128_info.deviceID, dvs128_info.deviceIsMaster, dvs128_info.dvsSizeX, dvs128_info.dvsSizeY,
dvs128_info.logicVersion);
// Send the default configuration before using the device.
// No configuration is sent automatically!
caerDeviceSendDefaultConfig(dvs128_handle);
// Tweak some biases, to increase bandwidth in this case.
caerDeviceConfigSet(dvs128_handle, DVS128_CONFIG_BIAS, DVS128_CONFIG_BIAS_PR, 695);
caerDeviceConfigSet(dvs128_handle, DVS128_CONFIG_BIAS, DVS128_CONFIG_BIAS_FOLL, 867);
// Let's verify they really changed!
uint32_t prBias, follBias;
caerDeviceConfigGet(dvs128_handle, DVS128_CONFIG_BIAS, DVS128_CONFIG_BIAS_PR, &prBias);
caerDeviceConfigGet(dvs128_handle, DVS128_CONFIG_BIAS, DVS128_CONFIG_BIAS_FOLL, &follBias);
printf("New bias values --- PR: %d, FOLL: %d.\n", prBias, follBias);
// Now let's get start getting some data from the device. We just loop in blocking mode,
// no notification needed regarding new events. The shutdown notification, for example if
// the device is disconnected, should be listened to.
caerDeviceDataStart(dvs128_handle, NULL, NULL, NULL, &usbShutdownHandler, NULL);
// Let's turn on blocking data-get mode to avoid wasting resources.
caerDeviceConfigSet(dvs128_handle, CAER_HOST_CONFIG_DATAEXCHANGE, CAER_HOST_CONFIG_DATAEXCHANGE_BLOCKING, true);
while (!atomic_load_explicit(&globalShutdown, memory_order_relaxed)) {
caerEventPacketContainer packetContainer = caerDeviceDataGet(dvs128_handle);
if (packetContainer == NULL) {
continue; // Skip if nothing there.
}
int32_t packetNum = caerEventPacketContainerGetEventPacketsNumber(packetContainer);
printf("\nGot event container with %d packets (allocated).\n", packetNum);
for (int32_t i = 0; i < packetNum; i++) {
caerEventPacketHeader packetHeader = caerEventPacketContainerGetEventPacket(packetContainer, i);
if (packetHeader == NULL) {
printf("Packet %d is empty (not present).\n", i);
continue; // Skip if nothing there.
}
printf("Packet %d of type %d -> size is %d.\n", i, caerEventPacketHeaderGetEventType(packetHeader),
caerEventPacketHeaderGetEventNumber(packetHeader));
// Packet 0 is always the special events packet for DVS128, while packet is the polarity events packet.
if (i == POLARITY_EVENT) {
caerPolarityEventPacket polarity = (caerPolarityEventPacket) packetHeader;
// Get full timestamp and addresses of first event.
caerPolarityEvent firstEvent = caerPolarityEventPacketGetEvent(polarity, 0);
int32_t ts = caerPolarityEventGetTimestamp(firstEvent);
uint16_t x = caerPolarityEventGetX(firstEvent);
uint16_t y = caerPolarityEventGetY(firstEvent);
bool pol = caerPolarityEventGetPolarity(firstEvent);
printf("First polarity event - ts: %d, x: %d, y: %d, pol: %d.\n", ts, x, y, pol);
}
}
caerEventPacketContainerFree(packetContainer);
}
caerDeviceDataStop(dvs128_handle);
caerDeviceClose(&dvs128_handle);
printf("Shutdown successful.\n");
return (EXIT_SUCCESS);
}
void DvsRosDriver::readout()
{
caerDeviceDataStart(dvs128_handle, NULL, NULL, NULL, NULL, NULL);
caerDeviceConfigSet(dvs128_handle, CAER_HOST_CONFIG_DATAEXCHANGE, CAER_HOST_CONFIG_DATAEXCHANGE_BLOCKING, true);
boost::posix_time::ptime next_send_time = boost::posix_time::microsec_clock::local_time();
dvs_msgs::EventArrayPtr event_array_msg(new dvs_msgs::EventArray());
event_array_msg->height = dvs128_info_.dvsSizeY;
event_array_msg->width = dvs128_info_.dvsSizeX;
while (running_)
{
try
{
caerEventPacketContainer packetContainer = caerDeviceDataGet(dvs128_handle);
if (packetContainer == NULL)
{
continue; // Skip if nothing there.
}
int32_t packetNum = caerEventPacketContainerGetEventPacketsNumber(packetContainer);
for (int32_t i = 0; i < packetNum; i++)
{
caerEventPacketHeader packetHeader = caerEventPacketContainerGetEventPacket(packetContainer, i);
if (packetHeader == NULL)
{
continue; // Skip if nothing there.
}
// Packet 0 is always the special events packet for DVS128, while packet is the polarity events packet.
if (i == POLARITY_EVENT)
{
caerPolarityEventPacket polarity = (caerPolarityEventPacket) packetHeader;
const int numEvents = caerEventPacketHeaderGetEventNumber(packetHeader);
for (int j = 0; j < numEvents; j++)
{
// Get full timestamp and addresses of first event.
caerPolarityEvent event = caerPolarityEventPacketGetEvent(polarity, j);
dvs_msgs::Event e;
e.x = caerPolarityEventGetX(event);
e.y = caerPolarityEventGetY(event);
e.ts = reset_time_ +
ros::Duration().fromNSec(caerPolarityEventGetTimestamp64(event, polarity) * 1000);
e.polarity = caerPolarityEventGetPolarity(event);
event_array_msg->events.push_back(e);
}
// throttle event messages
if (boost::posix_time::microsec_clock::local_time() > next_send_time || current_config_.streaming_rate == 0)
{
event_array_pub_.publish(event_array_msg);
event_array_msg->events.clear();
if (current_config_.streaming_rate > 0)
{
next_send_time += delta_;
}
}
if (camera_info_manager_->isCalibrated())
{
sensor_msgs::CameraInfoPtr camera_info_msg(new sensor_msgs::CameraInfo(camera_info_manager_->getCameraInfo()));
camera_info_pub_.publish(camera_info_msg);
}
}
}
caerEventPacketContainerFree(packetContainer);
ros::spinOnce();
}
catch (boost::thread_interrupted&)
{
return;
}
}
caerDeviceDataStop(dvs128_handle);
}
