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;
}
}
