static bool caerInputDVS128Init(caerModuleData moduleData) {
caerLog(CAER_LOG_DEBUG, moduleData->moduleSubSystemString, "Initializing module ...");
// USB port/bus/SN settings/restrictions.
// These can be used to force connection to one specific device at startup.
sshsNodePutShortIfAbsent(moduleData->moduleNode, "BusNumber", 0);
sshsNodePutShortIfAbsent(moduleData->moduleNode, "DevAddress", 0);
sshsNodePutStringIfAbsent(moduleData->moduleNode, "SerialNumber", "");
// Add auto-restart setting.
sshsNodePutBoolIfAbsent(moduleData->moduleNode, "Auto-Restart", true);
// Start data acquisition, and correctly notify mainloop of new data and module of exceptional
// shutdown cases (device pulled, ...).
char *serialNumber = sshsNodeGetString(moduleData->moduleNode, "SerialNumber");
moduleData->moduleState = caerDeviceOpen(moduleData->moduleID, CAER_DEVICE_DVS128,
U8T(sshsNodeGetShort(moduleData->moduleNode, "BusNumber")),
U8T(sshsNodeGetShort(moduleData->moduleNode, "DevAddress")), serialNumber);
free(serialNumber);
if (moduleData->moduleState == NULL) {
// Failed to open device.
return (false);
}
// Put global source information into SSHS.
struct caer_dvs128_info devInfo = caerDVS128InfoGet(moduleData->moduleState);
sshsNode sourceInfoNode = sshsGetRelativeNode(moduleData->moduleNode, "sourceInfo/");
sshsNodePutShort(sourceInfoNode, "logicVersion", devInfo.logicVersion);
sshsNodePutBool(sourceInfoNode, "deviceIsMaster", devInfo.deviceIsMaster);
sshsNodePutShort(sourceInfoNode, "dvsSizeX", devInfo.dvsSizeX);
sshsNodePutShort(sourceInfoNode, "dvsSizeY", devInfo.dvsSizeY);
// Put source information for "virtual" APS frame that can be used to display and debug filter information.
sshsNodePutShort(sourceInfoNode, "apsSizeX", devInfo.dvsSizeX);
sshsNodePutShort(sourceInfoNode, "apsSizeY", devInfo.dvsSizeY);
caerModuleSetSubSystemString(moduleData, devInfo.deviceString);
// Ensure good defaults for data acquisition settings.
// No blocking behavior due to mainloop notification, and no auto-start of
// all producers to ensure cAER settings are respected.
caerDeviceConfigSet(moduleData->moduleState, CAER_HOST_CONFIG_DATAEXCHANGE,
CAER_HOST_CONFIG_DATAEXCHANGE_BLOCKING, false);
caerDeviceConfigSet(moduleData->moduleState, CAER_HOST_CONFIG_DATAEXCHANGE,
CAER_HOST_CONFIG_DATAEXCHANGE_START_PRODUCERS, false);
caerDeviceConfigSet(moduleData->moduleState, CAER_HOST_CONFIG_DATAEXCHANGE,
CAER_HOST_CONFIG_DATAEXCHANGE_STOP_PRODUCERS, true);
// Create default settings and send them to the device.
createDefaultConfiguration(moduleData);
sendDefaultConfiguration(moduleData);
// Start data acquisition.
bool ret = caerDeviceDataStart(moduleData->moduleState, &mainloopDataNotifyIncrease, &mainloopDataNotifyDecrease,
caerMainloopGetReference(), &moduleShutdownNotify, moduleData->moduleNode);
if (!ret) {
// Failed to start data acquisition, close device and exit.
caerDeviceClose((caerDeviceHandle *) &moduleData->moduleState);
return (false);
}
return (true);
}
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);
}
DvsRosDriver::DvsRosDriver(ros::NodeHandle & nh, ros::NodeHandle nh_private) :
nh_(nh), parameter_update_required_(false)
{
// load parameters
nh_private.param<std::string>("serial_number", device_id_, "");
nh_private.param<bool>("master", master_, true);
double reset_timestamps_delay;
nh_private.param<double>("reset_timestamps_delay", reset_timestamps_delay, -1.0);
// start driver
bool device_is_running = false;
while (!device_is_running)
{
const char* serial_number_restrict = (device_id_ == "") ? NULL : device_id_.c_str();
dvs128_handle = caerDeviceOpen(1, CAER_DEVICE_DVS128, 0, 0, serial_number_restrict);
//dvs_running = driver_->isDeviceRunning();
device_is_running = !(dvs128_handle == NULL);
if (!device_is_running)
{
ROS_WARN("Could not find DVS. Will retry every second.");
ros::Duration(1.0).sleep();
ros::spinOnce();
}
else
{
// configure as master or slave
caerDeviceConfigSet(dvs128_handle, DVS128_CONFIG_DVS, DVS128_CONFIG_DVS_TS_MASTER, master_);
}
if (!ros::ok())
{
return;
}
}
dvs128_info_ = caerDVS128InfoGet(dvs128_handle);
device_id_ = "DVS128-V1-" + std::string(dvs128_info_.deviceString).substr(15, 4);
ROS_INFO("%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);
current_config_.streaming_rate = 30;
delta_ = boost::posix_time::microseconds(1e6/current_config_.streaming_rate);
// set namespace
std::string ns = ros::this_node::getNamespace();
if (ns == "/")
ns = "/dvs";
event_array_pub_ = nh_.advertise<dvs_msgs::EventArray>(ns + "/events", 1);
camera_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(ns + "/camera_info", 1);
// camera info handling
ros::NodeHandle nh_ns(ns);
camera_info_manager_ = new camera_info_manager::CameraInfoManager(nh_ns, device_id_);
// reset timestamps is publisher as master, subscriber as slave
if (master_)
{
reset_pub_ = nh_.advertise<std_msgs::Time>((ns + "/reset_timestamps").c_str(), 1);
}
else
{
reset_sub_ = nh_.subscribe((ns + "/reset_timestamps").c_str(), 1, &DvsRosDriver::resetTimestampsCallback, this);
}
// initialize timestamps
resetTimestamps();
// spawn threads
running_ = true;
parameter_thread_ = boost::shared_ptr< boost::thread >(new boost::thread(boost::bind(&DvsRosDriver::changeDvsParameters, this)));
readout_thread_ = boost::shared_ptr< boost::thread >(new boost::thread(boost::bind(&DvsRosDriver::readout, this)));
// Dynamic reconfigure
dynamic_reconfigure_callback_ = boost::bind(&DvsRosDriver::callback, this, _1, _2);
server_.reset(new dynamic_reconfigure::Server<dvs_ros_driver::DVS_ROS_DriverConfig>(nh_private));
server_->setCallback(dynamic_reconfigure_callback_);
// start timer to reset timestamps for synchronization
if (reset_timestamps_delay > 0.0 && master_)
{
timestamp_reset_timer_ = nh_.createTimer(ros::Duration(reset_timestamps_delay), &DvsRosDriver::resetTimerCallback, this);
ROS_INFO("Started timer to reset timestamps on master DVS for synchronization (delay=%3.2fs).", reset_timestamps_delay);
}
}
