int cbpy_get_trial_config(int nInstance, cbSdkConfigParam * pcfg_param) { cbSdkResult sdkres = cbSdkGetTrialConfig(nInstance, &pcfg_param->bActive, &pcfg_param->Begchan,&pcfg_param->Begmask, &pcfg_param->Begval, &pcfg_param->Endchan, &pcfg_param->Endmask, &pcfg_param->Endval, &pcfg_param->bDouble, &pcfg_param->uWaveforms, &pcfg_param->uConts, &pcfg_param->uEvents, &pcfg_param->uComments, &pcfg_param->uTrackings, &pcfg_param->bAbsolute); return sdkres; }
int main(int argc, char **argv) { ros::Time::init(); cbSdkResult res = testOpen(); if (res < 0) ROS_INFO("testOpen failed (%d)!\n", res); else ROS_INFO("testOpen succeeded\n"); UINT16 uBegChan = 0; UINT32 uBegMask = 0; UINT32 uBegVal = 0; UINT16 uEndChan = 0; UINT32 uEndMask = 0; UINT32 uEndVal = 0; bool bDouble = false; bool bAbsolute = false; UINT32 uWaveforms = 0; UINT32 uConts = cbSdk_CONTINUOUS_DATA_SAMPLES; UINT32 uEvents = cbSdk_EVENT_DATA_SAMPLES; UINT32 uComments = 0; UINT32 uTrackings = 0; UINT32 bWithinTrial = false; // test cbSdkGet/Set Trial Config UINT32 bActive = 1; res = cbSdkSetTrialConfig(INST, bActive, uBegChan, uBegMask, uBegVal, uEndChan, uEndMask, uEndVal, bDouble, uWaveforms, uConts, uEvents, uComments, uTrackings, bAbsolute); ros::Duration(.1).sleep(); UINT32 bValid = 1; cbSdkTrialCont trialcont = cbSdkTrialCont(); cbPKT_CHANINFO chan_info = cbPKT_CHANINFO(); res = cbSdkInitTrialData(INST, NULL, &trialcont, NULL, NULL); int num_channels = trialcont.count; blackrock_interface::blackrock_data br_data_msg; br_data_msg.num_samples.resize(trialcont.count); br_data_msg.data.resize(trialcont.count); br_data_msg.channel_labels.resize(trialcont.count); for (int ii=0; ii<num_channels; ii++) { cbSdkGetChannelConfig(INST, trialcont.chan[ii], &chan_info); // Get a full channel configuration packet br_data_msg.channel_labels[ii].assign(chan_info.label); } ros::init(argc, argv, "br_data_stream"); ros::NodeHandle n; ros::Publisher chatter_pub = n.advertise<blackrock_interface::blackrock_data>("blackrock_data", 1); ros::Rate loop_rate(STREAM_FREQ); while (ros::ok()) { // 1 - Get how many samples are waiting bool bTrialDouble = false; res = cbSdkGetTrialConfig(INST, NULL, NULL, NULL, NULL, NULL, NULL, NULL, &bTrialDouble, NULL, &uConts, &uEvents); res = cbSdkInitTrialData(INST, NULL, &trialcont, NULL, NULL); // 2 - Allocate buffer for (UINT32 channel = 0; channel < trialcont.count; channel++) { trialcont.samples[channel] = NULL; UINT32 num_samples = trialcont.num_samples[channel]; if (bTrialDouble) trialcont.samples[channel] = calloc(num_samples,sizeof(double)); else trialcont.samples[channel] = calloc(num_samples,sizeof(UINT16)); } // 3 - get the data int bFlushBuffer = 1; INT16 **data = (INT16 **)trialcont.samples; res = cbSdkGetTrialData(INST, bFlushBuffer, NULL, &trialcont, NULL, NULL); data = (INT16 **)trialcont.samples; // 4 - forward the data for (UINT32 channel = 0; channel < trialcont.count; channel++) { blackrock_interface::blackrock_channel_data br_channel_data; // holds single chan data br_channel_data.channel_data.insert( br_channel_data.channel_data.begin(), data[channel], data[channel]+trialcont.num_samples[channel]); br_data_msg.data[channel] = br_channel_data; br_data_msg.num_samples[channel] = trialcont.num_samples[channel]; } chatter_pub.publish(br_data_msg); loop_rate.sleep(); } return 0; }