static int publisher( int32_t i ) { ach_channel_t chan; ach_status_t r = ach_open( &chan, opt_channel_name, NULL ); if( r != ACH_OK ) { fprintf(stderr, "publisher %d couldn't ach_open: %s\n", i, ach_result_to_string(r) ); return -1; } int32_t data[2] = {i, 0}; size_t j; for( j = 0; j < (unsigned int)opt_n_msgs; j++,data[1]++ ) { r = ach_put( &chan, data, sizeof(data) ); if( r != ACH_OK ) { fprintf(stderr, "publisher %d couldn't ach_put: %s\n", i, ach_result_to_string(r) ); return -1; } if( opt_pub_sleep_us <= 0 ) { sched_yield(); } else { usleep((unsigned int)opt_pub_sleep_us); } } r = ach_close(&chan); if( ACH_OK != r ) { fprintf(stderr, "publisher %d couldn't ach_close: %s\n", i, ach_result_to_string(r) ); return -1; } else { fprintf(stderr, "publisher %d ok\n", i); return 0; } }
void ach_move_close() { printf("ACH: Closing ACH\n"); int r = ach_close(craftyo_chan); if(r != ACH_OK) printf("ACH: %s\n", ach_result_to_string((ach_status_t)r)); //free(craftyo_message->boardstate.data); free(craftyo_message->move); free(craftyo_message); }
/** Close the channel. */ ach_status_t close(int allow_mask=ACH_MASK_OK, int warn_mask=ACH_MASK_NONE) { ach_status_t r = ach_close(&channel); switch(check_status(r, allow_mask, warn_mask)) { case STATUS_WARN: warn_close(r); break; case STATUS_ERR: error_close(r); break; case STATUS_OK: break; } return r; }
int cmd_dump(void) { if( opt_verbosity > 0 ) { fprintf(stderr, "Dumping Channel %s\n", opt_chan_name); } ach_channel_t chan; ach_status_t r = ach_open( &chan, opt_chan_name, NULL ); check_status( r, "Error opening ach channel '%s'", opt_chan_name ); ach_dump( chan.shm ); r = ach_close( &chan ); check_status( r, "Error closing ach channel '%s'", opt_chan_name ); return r; }
static PyObject * close_channel( PyObject *self, PyObject *args ) { (void)self; PyObject *py_chan; if( !PyArg_ParseTuple(args, "O", &py_chan) ) { return NULL; } ach_channel_t *c = parse_channel_pointer(py_chan); if( NULL == c ) { return NULL; } ach_status_t r = ach_close(c); if( ACH_OK != r ) { return raise_error(r); } free(c); Py_RETURN_NONE; }
int cmd_chmod(void) { assert(opt_mode >=0 ); if( opt_verbosity > 0 ) { fprintf( stderr, "Changing mode of %s to %o\n", opt_chan_name, (unsigned)opt_mode ); } /* open */ ach_channel_t chan; errno = 0; ach_status_t r = ach_open( &chan, opt_chan_name, NULL ); check_status( r, "Error opening channel '%s'", opt_chan_name ); /* chmod */ r = ach_chmod( &chan, (mode_t)opt_mode ); check_status( r, "Error chmodding channel '%s'", opt_chan_name ); /* close */ r = ach_close( &chan ); check_status( r, "Error closing channel '%s'", opt_chan_name ); return r; }
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 main( int argc, char **argv ) { Hubo_Control hubo("proto-manip-daemon"); ach_channel_t chan_manip_cmd; int r = ach_open( &chan_manip_cmd, CHAN_HUBO_MANIP, NULL ); daemon_assert( r==ACH_OK, __LINE__ ); hubo_manip_cmd manip; memset( &manip, 0, sizeof(manip) ); hubo.update(); Eigen::Isometry3d Br, Bl; Vector6d right, left, zeros; zeros.setZero(); Vector3d rtrans, ltrans, langles, rangles; hubo.getRightArmAngles(right); hubo.getLeftArmAngles(left); hubo.huboArmFK( Br, right, RIGHT ); hubo.huboArmFK( Bl, left, LEFT ); std::cout << "Performed initial FK" << std::endl; for(int i=0; i<3; i++) { manip.translation[RIGHT][i] = Br(i,3); manip.translation[LEFT][i] = Bl(i,3); } std::cout << "Putting first transformation" << std::endl; ach_put( &chan_manip_cmd, &manip, sizeof(manip) ); size_t fs; std::cout << "About to start loop" << std::endl; while( !daemon_sig_quit ) { hubo.update(); ach_get( &chan_manip_cmd, &manip, sizeof(manip), &fs, NULL, ACH_O_LAST ); for(int i=0; i<3; i++) { rtrans(i) = manip.translation[RIGHT][i]; ltrans(i) = manip.translation[LEFT][i]; rangles(i) = manip.eulerAngles[RIGHT][i]; langles(i) = manip.eulerAngles[LEFT][i]; } // Handle the right arm Br = Eigen::Matrix4d::Identity(); Br.translate(rtrans); Br.rotate( Eigen::AngleAxisd(rangles(0), Vector3d(1,0,0)) ); Br.rotate( Eigen::AngleAxisd(rangles(1), Vector3d(0,1,0)) ); Br.rotate( Eigen::AngleAxisd(rangles(2), Vector3d(0,0,1)) ); hubo.huboArmIK( right, Br, zeros, RIGHT ); hubo.setRightArmAngles( right ); // Handle the left arm Bl = Eigen::Matrix4d::Identity(); Bl.translate(ltrans); Bl.rotate( Eigen::AngleAxisd(langles(0), Vector3d(1,0,0)) ); Bl.rotate( Eigen::AngleAxisd(langles(1), Vector3d(0,1,0)) ); Bl.rotate( Eigen::AngleAxisd(langles(2), Vector3d(0,0,1)) ); hubo.huboArmIK( left, Bl, zeros, LEFT ); hubo.setLeftArmAngles( left ); // Send commands off to the control daemon hubo.sendControls(); } ach_close( &chan_manip_cmd ); }
int test_basic() { /* unlink */ ach_status_t r = ach_unlink(opt_channel_name); if( ! ach_status_match(r, ACH_MASK_OK | ACH_MASK_ENOENT) ) { fprintf(stderr, "ach_unlink failed: %s\n", ach_result_to_string(r)); return -1; } /* create */ r = ach_create(opt_channel_name, 32ul, 64ul, NULL ); test(r, "ach_create"); ach_channel_t chan; int s, p; size_t frame_size; struct timespec ts; /* open */ r = ach_open(&chan, opt_channel_name, NULL); /* empty channel means stale */ r = ach_get( &chan, &s, sizeof(s), &frame_size, NULL, ACH_O_LAST); if( ACH_STALE_FRAMES != r ) { printf("get stale failed: %s\n", ach_result_to_string(r)); exit(-1); } r = ach_get( &chan, &s, sizeof(s), &frame_size, NULL, 0); if( ACH_STALE_FRAMES != r ) { printf("get stale failed: %s\n", ach_result_to_string(r)); exit(-1); } /* put */ p = 42; r = ach_put( &chan, &p, sizeof(p) ); test(r, "ach_put"); /* get */ r = ach_get( &chan, &s, sizeof(s), &frame_size, NULL, 0); test(r, "first ach_get"); if(frame_size!= sizeof(s) || s != 42 ) exit(-1); /* put 2 */ p = 43; r = ach_put( &chan, &p, sizeof(p) ); test(r, "ach_put"); p = 44; r = ach_put( &chan, &p, sizeof(p) ); test(r, "ach_put"); /* get last */ r = ach_get( &chan, &s, sizeof(s), &frame_size, NULL, ACH_O_LAST); if( ACH_MISSED_FRAME != r ) { printf("get last failed: %s\n", ach_result_to_string(r)); exit(-1); } if(frame_size != sizeof(s) || s != 44 ) exit(-1); /* wait last */ p = 45; r = ach_put( &chan, &p, sizeof(p) ); test(r, "ach_put"); clock_gettime(ACH_DEFAULT_CLOCK, &ts); ts.tv_sec += 30; /* don't yield too long now */ r = ach_get( &chan, &s, sizeof(s), &frame_size, &ts, ACH_O_LAST | ACH_O_WAIT ); if( ACH_OK != r ) { printf("get wait failed: %s\n", ach_result_to_string(r)); exit(-1); } if(frame_size != sizeof(s) || s != 45 ) exit(-1); /* get last stale */ r = ach_get( &chan, &s, sizeof(s), &frame_size, NULL, ACH_O_LAST); if( ACH_STALE_FRAMES != r ) { printf("get stale failed: %s\n", ach_result_to_string(r)); exit(-1); } /* timeout */ clock_gettime(ACH_DEFAULT_CLOCK, &ts); ts.tv_sec -= 10; r = ach_get( &chan, &s, sizeof(s), &frame_size, &ts, ACH_O_LAST | ACH_O_WAIT ); if( ACH_TIMEOUT != r ) { printf("get timeout failed: %s\n", ach_result_to_string(r)); exit(-1); } /* copy last */ printf("> copy start\n"); r = ach_get( &chan, &s, sizeof(s), &frame_size, NULL, ACH_O_LAST | ACH_O_COPY); printf("> copy done\n"); if( ACH_OK != r ) { printf("copy_last failed: %s\n", ach_result_to_string(r)); exit(-1); } if( p != s ) { printf("wrong copy last : %d\n", s); exit(-1); } /* get copy */ /*ach_dump(chan.shm);*/ /*printf("chan seq_num: %"PRIu64"\n", chan.seq_num);*/ r = ach_get( &chan, &s, sizeof(s), &frame_size, NULL, ACH_O_COPY); if( ACH_OK != r ) { printf("copy_last failed: %s\n", ach_result_to_string(r)); exit(-1); } /* missed frames */ size_t i; for( i = 0; i < 100; i ++ ) { r = ach_put( &chan, &p, sizeof(p) ); test(r, "ach_put"); } r = ach_get( &chan, &s, sizeof(s), &frame_size, NULL, ACH_O_LAST); if( ACH_MISSED_FRAME != r ) { printf("get missed failed: %s\n", ach_result_to_string(r)); exit(-1); } /* close */ r = ach_close(&chan); test(r, "ach_close"); /* unlink */ r = ach_unlink(opt_channel_name); test(r, "ach_unlink"); fprintf(stderr, "basic ok\n"); return 0; }
static int subscriber( int i ) { ach_channel_t chan; int32_t ctr[opt_n_pub]; memset(ctr,0,sizeof(ctr)); ach_status_t r = ach_open( &chan, opt_channel_name, NULL ); if( r != ACH_OK ) { fprintf(stderr, "subscriber %d couldn't ach_open: %s", i, ach_result_to_string(r) ); return -1; } int32_t data[2]; int seen_last = 0; int j; for( j = 0; j < opt_n_pub*opt_n_msgs; j++ ) { struct timespec abstime = {0,0}; /* Wait at least 1 sec for msg. Adding 2 sec due to nanosecond * truncation of time() call */ abstime.tv_sec = time(NULL) + 2; size_t frame_size; r = ach_get( &chan, data, sizeof(data), &frame_size, &abstime, ACH_O_WAIT ); if( seen_last && ACH_TIMEOUT == r ) { break; } else if( ACH_OK != r && ACH_MISSED_FRAME != r) { fprintf(stderr, "subscriber %d couldn't ach_get: %s\n", i, ach_result_to_string(r) ); return -1; } else if( sizeof(data) != frame_size ) { fprintf(stderr, "subscriber %d bad frame size: %"PRIuPTR"\n", i, frame_size); return -1; } else if( 0 > data[0] || opt_n_pub <= data[0] ) { fprintf(stderr, "subscriber %d bad pub id: %d\n", i, data[0]); return -1; } else if( ctr[ data[0] ] > data[1] ) { fprintf(stderr, "subscriber %d, count %d, got [%d, %d]\n", i, ctr[data[0]], data[1], data[0] ); return -1; } else { if( ctr[ data[0] ] != data[1] ) { fprintf(stderr, "subscriber %d missed %d frames from %d, (it's probably ok)\n", i, data[1] - ctr[data[0]], data[0]); } j += data[1] - ctr[ data[0] ] ; ctr[ data[0] ] = data[1]+1; if( data[1]+1 == opt_n_msgs ) seen_last = 1; } } r = ach_close(&chan); if( ACH_OK != r ) { fprintf(stderr, "subscriber %d couldn't ach_close: %s", i, ach_result_to_string(r) ); return -1; } else { fprintf(stderr, "subscriber %d ok, last (%d)\n", i, seen_last); return 0; } }