int main(int argc, char* argv[] )
{
// Copy to char pointers
char outputChanChar[1024];
strcpy(outputChanChar, PERCEPTION_CHANNEL.c_str());
char debugChanChar[1024];
strcpy(debugChanChar, DEBUG_CHANNEL.c_str());
// open the channels
int r = ach_open( &channel, outputChanChar, NULL );
int rdebug = ach_open( &debug_channel, debugChanChar, NULL );
assert(ACH_OK == r && ACH_OK == rdebug);
r = ach_flush(&channel);
r = ach_flush(&debug_channel);
// test receive
double rtraj[NUM_OBJECTS][NDIM];
memset(rtraj, 0, NUM_OBJECTS*NDIM*sizeof(double));
size_t frame_size;
while( true ) {
r = ach_get( &channel, &rtraj,
sizeof(rtraj),
&frame_size,
NULL,
ACH_O_WAIT );
std::cout << "Received traj (visible, x, y, angle): " << std::endl;
print_arr_2d( rtraj, NUM_OBJECTS );
// DEBUG CHANNEL
r = ach_get( &debug_channel, &rtraj,
sizeof(rtraj),
&frame_size,
NULL,
ACH_O_WAIT );
std::cout << "[DEBUG] Received traj (visible, x, y, angle): " << std::endl;
print_arr_2d( rtraj, NUM_OBJECTS );
// Read every second
usleep(1.0*1e6);
}
}
static PyObject *
flush_channel( PyObject *self, PyObject *args ) {
(void)self;
PyObject *py_chan;
// get arg objects
if( !PyArg_ParseTuple(args, "O", &py_chan) ) {
return NULL;
}
// parse channel
ach_channel_t *c = parse_channel_pointer(py_chan);
if( NULL == c ) {
return NULL;
}
// make the damn call
ach_status_t r = ach_flush( c );
// check the result
if( ACH_OK != r ) {
PyErr_SetString( ach_py_error, ach_result_to_string(r) );
return NULL;
}
// cleanup
Py_RETURN_NONE;
}
bool Aggregator::open_channels()
{
if(_channels_opened)
return true;
_channels_opened = true;
ach_status_t result = ach_open(&_cmd_chan, HUBO_CMD_CHANNEL, NULL);
if( ACH_OK != result )
{
fprintf(stderr, "Error opening command channel: %s (%d)\n",
ach_result_to_string(result), (int)result);
_channels_opened = false;
return false;
}
result = ach_open(&_agg_chan, HUBO_AGG_CHANNEL, NULL);
if( ACH_OK != result )
{
fprintf(stderr, "Error opening aggregated channel: %s (%d)\n",
ach_result_to_string(result), (int)result);
_channels_opened = false;
report_ach_errors(ach_close(&_cmd_chan), "Aggregator::open_channels",
"ach_close", HUBO_CMD_CHANNEL);
return false;
}
report_ach_errors(ach_flush(&_cmd_chan), "Aggregator::open_channels",
"ach_flush", HUBO_CMD_CHANNEL);
report_ach_errors(ach_flush(&_agg_chan), "Aggregator::open_channels",
"ach_flush", HUBO_AGG_CHANNEL);
return true;
}
int ach_move_init() {
// Channels
int r = 0;
printf("ACH: Creating Channel\n");
ach_create_attr_t attr;
ach_create_attr_init(&attr);
r = ach_create((char*)craftyo_chan_name, 100, IBOARDSTATE_SIZE+IMOVESTRING_SIZE, &attr);
printf("ACH: %s\n", ach_result_to_string((ach_status_t)r));
printf("ACH: Opening Channel\n");
craftyo_chan = (ach_channel_t*) malloc (sizeof(ach_channel_t));
if(craftyo_chan == NULL) return 0;
r = ach_open(craftyo_chan, craftyo_chan_name, NULL);
if(r != ACH_OK) {
printf("ACH: %s\n", ach_result_to_string((ach_status_t)r));
return 0;
}
ach_chmod(craftyo_chan, SOMATIC_CHANNEL_MODE );
r = ach_flush(craftyo_chan);
if(r != ACH_OK) {
printf("ACH: %s\n", ach_result_to_string((ach_status_t)r));
return 0;
}
// Data
craftyo_message = (Somatic__Crafty*)malloc(sizeof(Somatic__Crafty));
if(craftyo_message == NULL) return 0;
somatic__crafty__init(craftyo_message);
//craftyo_message->boardstate.data = (uint8_t*) malloc(128);
//craftyo_message->has_boardstate = 1;
//craftyo_message->boardstate.len = 128;
craftyo_message->move = (char*)malloc(10);
printf("ACH: Info\n"
"ACH: Name: %s\n"
"ACH: Size: %d\n", craftyo_chan_name, somatic__crafty__get_packed_size(craftyo_message));
return 1;
}
static long ach_ch_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
/* TODO: Validate argument */
int ret = 0;
struct ach_ch_file *ch_file = (struct ach_ch_file *)file->private_data;
KDEBUG("ach: In ach_ch_ioctl\n");
switch (cmd) {
case ACH_CH_SET_MODE: {
struct achk_opt opt;
if (copy_from_user(&opt, (void*)arg, sizeof(opt)) ) {
ret = -EFAULT;
} else {
/* This is not threadsafe */
ch_file->mode = opt;
/* if (ch_file->mode.reltime.tv_sec != 0 */
/* || ch_file->mode.reltime.tv_nsec != 0) */
/* KDEBUG("ach: Setting wait time to %ld.%09ld\n", */
/* ch_file->mode.reltime.tv_sec, */
/* ch_file->mode.reltime.tv_nsec); */
/* KDEBUG("ach: Got cmd ACH_CH_SET_MODE: \n"); */
/* KDEBUG1(" ACH_O_WAIT=%d\n", */
/* ch_file->mode.mode & ACH_O_WAIT); */
/* KDEBUG1(" ACH_O_LAST=%d\n", */
/* ch_file->mode.mode & ACH_O_LAST); */
/* KDEBUG1(" ACH_O_COPY=%d\n", */
/* ch_file->mode.mode & ACH_O_COPY); */
ret = 0;
break;
}
}
case ACH_CH_GET_MODE: {
KDEBUG("ach: Got cmd ACH_CH_GET_MODE: %ld\n", arg);
if( copy_to_user((void*)arg, &ch_file->mode, sizeof(ch_file->mode)) )
ret = -EFAULT;
else
ret = 0;
break;
}
case ACH_CH_GET_STATUS: {
KDEBUG("ach: Got cmd ACH_CH_GET_STATUS\n");
if (rt_mutex_lock_interruptible(&ch_file->shm->sync.mutex)) {
ret = -ERESTARTSYS;
break;
}
{
struct ach_ch_status stat;
struct ach_header *shm = ch_file->shm;
ach_index_t *index_ar = ACH_SHM_INDEX(shm);
size_t oldest_index = oldest_index_i(shm);
uint64_t oldest_seq =
index_ar[oldest_index].seq_num;
stat.mode = ch_file->mode.options;
stat.size = shm->len;
stat.count = shm->index_cnt - shm->index_free;
if (oldest_seq > ch_file->seq_num) {
stat.new_msgs = shm->last_seq - oldest_seq;
} else {
stat.new_msgs =
shm->last_seq - ch_file->seq_num;
}
stat.last_seq = shm->last_seq;
stat.last_seq_read = ch_file->seq_num;
printk(KERN_INFO "ach: Status:\n");
printk(KERN_INFO "ach: mode : %02x\n",
stat.mode);
printk(KERN_INFO "ach: size : %zu\n",
stat.size);
printk(KERN_INFO "ach: count: %zu\n",
stat.count);
printk(KERN_INFO "ach: new : %zu\n",
stat.new_msgs);
printk(KERN_INFO "ach: last_seq : %lu\n",
stat.last_seq);
printk(KERN_INFO "ach: last_seq_read : %lu\n",
stat.last_seq_read);
if (copy_to_user((void *)arg, &stat, sizeof(stat))) {
ret = -EFAULT;
}
}
rt_mutex_unlock(&ch_file->shm->sync.mutex);
}
case ACH_CH_FLUSH:
KDEBUG("ach: Got cmd ACH_CH_FLUSH\n");
ret = -get_errno( ach_flush(ch_file) );
break;
case ACH_CH_CANCEL: {
unsigned int unsafe = (unsigned int)arg;
KDEBUG("ach: Got cmd ACH_CH_CANCEL\n");
ret = -get_errno(ach_cancel(ch_file, unsafe));
break;
}
case ACH_CH_GET_OPTIONS: {
struct ach_ch_options retval;
retval.mode = ch_file->mode;
retval.clock = ch_file->shm->clock;
if( copy_to_user( (void*)arg, &retval, sizeof(retval) ) )
ret = -EFAULT;
else
ret = 0;
break;
}
default:
printk(KERN_ERR "ach: Unknown ioctl option: %d\n", cmd);
ret = -ENOSYS;
break;
}
return ret;
}
int main(int argc, char* argv[] )
{
atexit( closeFiles );
// Copy to char pointers
char outputChanChar[1024];
strcpy(outputChanChar, PERCEPTION_CHANNEL.c_str());
char debugChanChar[1024];
strcpy(debugChanChar, DEBUG_CHANNEL.c_str());
// open the channels
int r = ach_open( &channel, outputChanChar, NULL );
int rdebug = ach_open( &debug_channel, debugChanChar, NULL );
assert(ACH_OK == r && ACH_OK == rdebug);
r = ach_flush(&channel);
r = ach_flush(&debug_channel);
// test receive
double percep[NUM_OBJECTS][NDIM];
memset(percep, 0, NUM_OBJECTS*NDIM*sizeof(double));
double debug[NUM_OBJECTS][NDIM];
memset(debug, 0, NUM_OBJECTS*NDIM*sizeof(double));
size_t frame_size;
// Open files for debug
for( int i = 0; i < NUM_OBJECTS; ++i ) {
char name[50];
sprintf( name, "logging_est%d.txt", i );
pF.push_back(fopen( name, "w+"));
}
int counter = 0;
while( true ) {
if( counter > 60 ) {
std::cout << "Getting out of loop"<< std::endl;
break;
}
// PERCEP CHANNEL
r = ach_get( &channel, &percep,
sizeof(percep),
&frame_size,
NULL,
ACH_O_WAIT );
std::cout << "Received traj (visible, x, y, angle): " << std::endl;
print_arr_2d( percep, NUM_OBJECTS );
// DEBUG CHANNEL
r = ach_get( &debug_channel, &debug,
sizeof(debug),
&frame_size,
NULL,
ACH_O_WAIT );
std::cout << "[DEBUG] Received traj (visible, x, y, angle): " << std::endl;
print_arr_2d( debug, NUM_OBJECTS );
// Store them (to cm and to degrees)
for( int i = 0; i < NUM_OBJECTS; ++i ) {
fprintf( pF[i], " %f %f %f %f %f %f \n", percep[i][0]*100.0, percep[i][1]*100.0, percep[i][2]*57.3, debug[i][0]*100.0, debug[i][1]*100.0, debug[i][2]*57.3 );
}
// Read every second
usleep(0.25*1e6);
counter++;
}
return 0;
}
