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;
}
}
Walker::Walker(double maxInitTime, double jointSpaceTolerance, double jointVelContinuityTolerance) :
m_maxInitTime(maxInitTime),
m_jointSpaceTolerance( jointSpaceTolerance ),
m_jointVelContTol( jointVelContinuityTolerance ),
keepWalking(true),
hubo()
{
ach_status_t r = ach_open( &zmp_chan, HUBO_CHAN_ZMP_TRAJ_NAME, NULL );
if( r != ACH_OK )
fprintf( stderr, "Problem with channel %s: %s (%d)\n",
HUBO_CHAN_ZMP_TRAJ_NAME, ach_result_to_string(r), (int)r );
r = ach_open( ¶m_chan, BALANCE_PARAM_CHAN, NULL );
if( r != ACH_OK )
fprintf( stderr, "Problem with channel %s: %s (%d)\n",
BALANCE_PARAM_CHAN, ach_result_to_string(r), (int)r );
r = ach_open( &bal_cmd_chan, BALANCE_CMD_CHAN, NULL );
if( r != ACH_OK )
fprintf( stderr, "Problem with channel %s: %s (%d)\n",
BALANCE_CMD_CHAN, ach_result_to_string(r), (int)r );
r = ach_open( &bal_state_chan, BALANCE_STATE_CHAN, NULL );
if( r != ACH_OK )
fprintf( stderr, "Problem with channel %s: %s (%d)\n",
BALANCE_STATE_CHAN, ach_result_to_string(r), (int)r );
memset( &cmd, 0, sizeof(cmd) );
memset( &bal_state, 0, sizeof(bal_state) );
}
void openAllCAN(int vCan) {
int skt1;
int skt0;
if(HUBO_VIRTUAL_MODE_VIRTUAL == vCan | HUBO_VIRTUAL_MODE_OPENHUBO == vCan){
skt1 = openCAN("vcan1");
skt0 = openCAN("vcan0");
}
else {
skt1 = openCAN("can1");
skt0 = openCAN("can0");
}
//H.socket[0] = skt0;
hubo_socket[0] = skt0;
//H.socket[1] = skt1;
hubo_socket[1] = skt1;
ach_status_t result = ach_open(&iotrace_chan,
IO_TRACE_CHAN_NAME,
NULL);
have_iotrace_chan = (result == ACH_OK);
fprintf(stderr, "open iotrace channel: %s\n",
ach_result_to_string(result));
}
static PyObject *
chmod_channel( PyObject *self, PyObject *args ) {
(void)self;
PyObject *py_chan;
int mode;
// get arg objects
if( !PyArg_ParseTuple(args, "Oi", &py_chan, &mode) ) {
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_chmod( c, (mode_t)mode );
// check the result
if( ACH_OK != r ) {
PyErr_SetString( ach_py_error, ach_result_to_string(r) );
return NULL;
}
// cleanup
Py_RETURN_NONE;
}
static void check_status(ach_status_t r, const char fmt[], ...) {
if( ACH_OK != r ) {
va_list ap;
va_start(ap, fmt);
vfprintf(stderr, fmt, ap);
va_end( ap );
if( errno ) {
fprintf( stderr, ": %s, errno (%d) %s\n",
ach_result_to_string(r), errno, strerror(errno) );
} else {
fprintf( stderr, ": %s\n", ach_result_to_string(r) );
}
exit( EXIT_FAILURE );
}
}
int cmd_search(void)
{
if( NULL == opt_chan_name ) {
fprintf(stderr, "Must specify channel\n");
exit(EXIT_FAILURE);
}
if( NULL == opt_domain ) {
fprintf(stderr, "Must specify domain\n");
exit(EXIT_FAILURE);
}
char host[512];
int port;
enum ach_status r = ach_srv_search( opt_chan_name, opt_domain, host, 512, &port );
if( ACH_OK == r ) {
printf("channel: %s\n"
"host: %s\n"
"port: %d\n"
".\n",
opt_chan_name, host, port );
} else {
fprintf( stderr, "Could not lookup channel `%s' %s, %s\n",
opt_chan_name, ach_result_to_string(r), ach_errstr() );
}
return 0;
}
static void test(ach_status_t r, const char *thing) {
if( r != ACH_OK ) {
fprintf(stderr, "%s: %s\n",
thing, ach_result_to_string(r));
exit(-1);
}
}
static void test(int t, enum ach_status r, const char *thing) {
if( !t ) {
fprintf(stderr, "%s failed: %s\n",
thing, ach_result_to_string(r));
exit(-1);
}
}
int main( int argc, char **argv ) {
ach_channel_t chan;
sns_start();
// open channel
sns_chan_open( &chan, "pir-state", NULL );
{
ach_channel_t *chans[] = {&chan, NULL};
sns_sigcancel( chans, sns_sig_term_default );
}
// state
/* -- RUN -- */
while (!sns_cx.shutdown) {
struct pir_state state;
size_t frame_size;
ach_status_t r = ach_get( &chan, &state, sizeof(state), &frame_size,
NULL, ACH_O_LAST );
switch(r) {
case ACH_OK:
case ACH_MISSED_FRAME:
dump(&state);
case ACH_CANCELED:
case ACH_TIMEOUT:
case ACH_STALE_FRAMES:
break;
default:
SNS_LOG(LOG_ERR, "Failed to get frame: %s\n", ach_result_to_string(r) );
}
}
}
static PyObject *
result_string( PyObject *self, PyObject *args ) {
(void)self;
int r;
if( !PyArg_ParseTuple(args, "i", &r ) ) {
return NULL;
}
return PyString_FromString( ach_result_to_string((enum ach_status)r) );
}
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);
}
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;
}
void HuboInitWidget::achCreateCHandle()
{
ach_status_t r = ach_open(&cmdChan, HUBO_CHAN_BOARD_CMD_NAME, NULL);
if( r == ACH_OK )
cmdOpen = true;
else
{
ROS_INFO("Command Channel failed to open");
fprintf(stderr, "Command Channel failed to open: %s", ach_result_to_string(r));
}
}
void HuboInitWidget::achCreateSHandle()
{
ach_status_t r = ach_open(&stateChan, HUBO_CHAN_STATE_NAME, NULL);
if( r == ACH_OK )
stateOpen = true;
else
{
ROS_INFO("State Channel failed to open");
fprintf(stderr, "State Channel failed to open: %s", ach_result_to_string(r));
}
}
int huboLoop() {
double newRef[2] = {1.0, 0.0};
// get initial values for hubo
struct hubo_ref H_ref;
memset( &H_ref, 0, sizeof(H_ref));
size_t fs;
int r = ach_get( &chan_hubo_ref, &H_ref, sizeof(H_ref), &fs, NULL, ACH_O_COPY );
if(ACH_OK != r) {
if(hubo_debug) {
printf("Ref ini r = %s\n",ach_result_to_string(r));}
}
else{ assert( sizeof(H_ref) == fs ); }
// time info
struct timespec t;
/* Sampling Period */
double T = (double)interval/(double)NSEC_PER_SEC; // (sec)
clock_gettime( 0,&t);
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT AVBOE THIS LINE]---------------------------------
// ------------------------------------------------------------------------------
// char* fileName = "valve0.traj";
runTraj(fileName, &H_ref, &t);
// runTraj("ybTest1.traj",&H_ref_filter, &t);
// runTraj("valve0.traj", &H_ref_filter, &t);
// runTraj("valve1.traj", &H_ref_filter, &t);
// runTraj("valve2.traj", &H_ref_filter, &t);
// runTraj("valve1.traj", &H_ref_filter, &t);
// runTraj("valve2.traj", &H_ref_filter, &t);
// runTraj("valve1.traj", &H_ref_filter, &t);
// runTraj("valve2.traj", &H_ref_filter, &t);
// runTraj("valve1.traj", &H_ref_filter, &t);
// runTraj("valve2.traj", &H_ref_filter, &t);
// runTraj("valve1.traj", &H_ref_filter, &t);
// runTraj("valve2.traj", &H_ref_filter, &t);
// runTraj("valve3.traj", &H_ref_filter, &t);
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT BELOW THIS LINE]---------------------------------
// ------------------------------------------------------------------------------
printf("Trajectory Finished\n");
return 0;
}
HuboCan::error_result_t receive_data(double timeout_seconds = 0)
{
if(!_check_initialized("receive_data"))
return HuboCan::ACH_ERROR;
size_t fs = 0;
struct timespec wait_time;
clock_gettime( ACH_DEFAULT_CLOCK, &wait_time );
long long nano_wait = wait_time.tv_nsec + (long long)(timeout_seconds*1E9);
wait_time.tv_sec += (int)(nano_wait/1E9);
wait_time.tv_nsec = (int)(nano_wait%((int)1E9));
ach_status_t r = ach_get(&_channel, _raw_data,
get_data_size<DataClass>(_raw_data),
&fs, &wait_time, ACH_O_LAST | ACH_O_WAIT);
if( ACH_TIMEOUT == r )
{
if(verbose)
{
std::cout << "[HuboData::receive_data] Ach channel '" << _channel_name
<< "' timed out!" << std::endl;
}
return HuboCan::TIMEOUT;
}
if(fs != get_data_size<DataClass>(_raw_data))
{
std::cout << "[HuboData::receive_data] Framesize mismatch for '" << _channel_name
<< "': " << fs << " received, " << get_data_size<DataClass>(_raw_data)
<< " expected!" << std::endl;
}
if( ACH_OK == r || ACH_STALE_FRAMES == r || ACH_MISSED_FRAME == r )
{
if(verbose)
{
std::cout << "[HuboData::receive_data] Ach result for channel '"
<< _channel_name << "': " << ach_result_to_string(r) << std::endl;
}
return HuboCan::OKAY;
}
else
{
std::cout << "[HuboData::receive_data] Unexpected ach result for channel '"
<< _channel_name << "'. Check error log for more info" << std::endl;
report_ach_errors(r, "HuboData::receive_data", "ach_get", _channel_name.c_str());
return HuboCan::ACH_ERROR;
}
return HuboCan::UNDEFINED_ERROR;
}
void Aggregator::_aggregator_loop()
{
size_t max_expected_size = hubo_cmd_data_get_size(_input_data);
while(_rt.good())
{
size_t fs;
double quit_check = 1;
struct timespec wait_time;
clock_gettime( ACH_DEFAULT_CLOCK, &wait_time);
long nano_wait = wait_time.tv_nsec + (long)(quit_check*1E9);
wait_time.tv_sec += (long)(nano_wait/1E9);
wait_time.tv_nsec = (long)(nano_wait%((long)1E9));
ach_status_t result = ach_get(&_cmd_chan, _input_data, max_expected_size,
&fs, &wait_time, ACH_O_WAIT);
if(ACH_TIMEOUT == result)
{
continue;
}
if( ACH_OK != result )
{
std::cout << "Ach error: (" << (int)result << ")" << ach_result_to_string(result) << std::endl;
// TODO: Broadcast the fact that we had an ach error?
continue;
}
if(hubo_cmd_header_check(_input_data) != HUBO_DATA_OKAY)
{
std::cout << "Malformed command header!" << std::endl;
// TODO: Broadcast the fact that the header was malformed?
continue;
}
if( hubo_cmd_data_get_size(_input_data) != fs )
{
std::cout << "Data size error! Expected size:" << hubo_cmd_data_get_size(_input_data)
<< ", Frame size:" << fs <<", Max size:" << max_expected_size << std::endl;
// TODO: Broadcast the fact that we had a sizing error?
continue;
}
_check_hubocan_state();
_collate_input();
_send_output();
}
}
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;
}
void receiver_ach(int rt) {
fprintf(stderr,"receiver\n");
make_realtime(99);
/* flush some initial delayed messages */
size_t i;
for( i = 0; i < 5; i ++ ) {
ticks_t ticks;
size_t fs;
enum ach_status r = ach_get(&chan, &ticks, sizeof(ticks), &fs, NULL,
ACH_O_LAST | ACH_O_WAIT);
switch (r) {
case ACH_OK: case ACH_MISSED_FRAME: break;
default:
fprintf(stderr, "bad ach_get result: %s\n", ach_result_to_string(r));
exit(EXIT_FAILURE);
}
}
/* now the good stuff */
ticks_t ticks = get_ticks();
while(1) {
size_t fs;
ticks_t then;
if (KERNDEV) {
then.tv_sec = 1;
then.tv_nsec = 0;
} else {
then = ticks;
then.tv_sec += 1;
}
int r = ach_get(&chan, &ticks, sizeof(ticks), &fs, &then,
ACH_O_LAST | ACH_O_WAIT);
ticks_t now = get_ticks();
if( ACH_TIMEOUT == r ) break;
assert(ACH_OK == r || sizeof(ticks) == fs);
/* only print real-time latencies */
if (rt) {
send_time((float)ticks_delta(ticks,now));
}
}
}
static PyObject *
put_buf( PyObject *self, PyObject *args ) {
(void)self;
PyObject *py_chan, *b;
// get arg objects
if( !PyArg_ParseTuple(args, "OO", &py_chan, &b) ) {
return NULL;
}
// parse channel
ach_channel_t *c = parse_channel_pointer(py_chan);
if( NULL == c ) {
return NULL;
}
// parse buffer
if( ! PyObject_CheckBuffer(b) ) {
PyErr_SetString( PyExc_TypeError, "invalid buffer" );
return NULL;
}
// view buffer
Py_buffer buf;
if( PyObject_GetBuffer( b, &buf, PyBUF_SIMPLE ) ) {
PyErr_SetString( PyExc_BufferError, "couldn't view buffer" );
return NULL;
}
// make the damn call
ach_status_t r = ach_put( c, buf.buf, (size_t)buf.len );
// check the result
if( ACH_OK != r ) {
PyErr_SetString( ach_py_error, ach_result_to_string(r) );
return NULL;
}
// cleanup
PyBuffer_Release(&buf);
Py_RETURN_NONE;
}
const JointCmdArray& Aggregator::update()
{
if(!_memory_set)
{
std::cout << "Trying to update commands before a valid HuboDescription has been loaded! Don't do that!!" << std::endl;
return _aggregated_cmds;
}
size_t fs;
ach_status_t result = ach_get(&_agg_chan, _final_data, hubo_cmd_data_get_size(_final_data), &fs, NULL, ACH_O_LAST);
if( ACH_OK != result && ACH_STALE_FRAMES != result && ACH_MISSED_FRAME != result )
{ // TODO: Is ACH_STALE_FRAMES really okay? It might imply that the upstream pipeline has been frozen
std::cout << "Unexpected Ach result: " << ach_result_to_string(result) << " (" << (int)result << ")" << std::endl;
return _aggregated_cmds;
}
_copy_final_data_to_array();
return _aggregated_cmds;
}
bool initialize(const std::vector<std::string>& names, const std::string& channel_name)
{
free(_raw_data);
_raw_data = initialize_data<DataClass>(names.size());
_mapping.clear();
_names.clear();
for(size_t i=0; i<names.size(); ++i)
{
StringMap::iterator it = _mapping.find(names[i]);
if(it == _mapping.end())
{
_mapping[names[i]] = i;
}
else
{
std::cout << "[HuboData::initialize] You have a repeated data member name ('"
<< names[i] << "')\n"
<< " -- Already exists as entry #" << it->second << "\n"
<< " -- Attempted to assign it to entry #" << i << std::endl;
_mapping.clear();
return false;
}
}
_names = names;
_channel_name = channel_name;
ach_status_t r = ach_open(&_channel, channel_name.c_str(), NULL);
if( ACH_OK == r )
{
_initialized = true;
return true;
}
_initialized = false;
std::cout << "[HuboData::initialize] Failed to open channel '" << _channel_name << "': "
<< ach_result_to_string(r) << std::endl;
return false;
}
int test_multi() {
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\n: %s",
ach_result_to_string(r));
return -1;
}
r = ach_create(opt_channel_name, 32ul, 64ul, NULL );
/* pid_t sub_pid[opt_n_sub]; */
/* pid_t pub_pid[opt_n_pub]; */
int i;
/* create subscribers */
for( i = 0; i < opt_n_sub; i++ ) {
pid_t p = fork();
if( p < 0 ) exit(-1);
else if( 0 == p ) return subscriber(i);
/* else sub_pid[i] = p; */
}
/* create publishers */
for( i = 0; i < opt_n_pub; i++ ) {
pid_t p = fork();
if( p < 0 ) exit(-1);
else if( 0 == p ) return publisher(i);
/* else pub_pid[i] = p; */
}
/* wait */
for( i = 0; i < opt_n_sub+opt_n_pub; i++ ) {
int s;
pid_t pid = wait(&s);
(void)pid;
if( 0 != s ) return -1;
}
return 0;
}
/**
* @function update_n
*/
static int update_n( size_t n,
double *q,
double *dq,
ach_channel_t *chan,
struct timespec *ts ) {
size_t frame_size;
void *buf = NULL;
ach_status_t r = sns_msg_local_get( chan, &buf,
&frame_size,
ts, ACH_O_LAST | (ts ? ACH_O_WAIT : 0 ) );
switch(r) {
case ACH_OK:
case ACH_MISSED_FRAME: {
struct sns_msg_motor_state *msg = (struct sns_msg_motor_state*)buf;
if( n == msg->header.n &&
frame_size == sns_msg_motor_state_size_n((uint32_t)n) ) {
for( size_t j = 0; j < n; ++j ) {
q[j] = msg->X[j].pos;
dq[j] = msg->X[j].vel;
}
return 1;
} else {
SNS_LOG( LOG_ERR, "Invalid motor_state message \n" );
}
break;
}
case ACH_TIMEOUT:
case ACH_STALE_FRAMES:
case ACH_CANCELED:
break;
default:
SNS_LOG( LOG_ERR, "Failed ach_get: %s \n", ach_result_to_string(r) );
} // end switch
return 0;
}
static PyObject *
unlink_channel( PyObject *self, PyObject *args ) {
(void)self;
const char *name;
// get arg objects
if( !PyArg_ParseTuple(args, "s", &name) ) {
return NULL;
}
// make the damn call
ach_status_t r = ach_unlink( name );
// check the result
if( ACH_OK != r ) {
PyErr_SetString( ach_py_error, ach_result_to_string(r) );
return NULL;
}
// cleanup
Py_RETURN_NONE;
}
int main( int argc, char **argv ) {
/* Check if we're running under achcop */
if( getenv("ACHCOP") ) {
ach_pid_notify = getppid();
}
int c;
while( (c = getopt( argc, argv, "zlnh?V")) != -1 ) {
switch(c) {
case 'v':
ach_verbosity ++;
break;
case 'l':
opt_last = 1;
break;
case 'n':
opt_last = 0;
break;
case 'z':
opt_gzip = 1;
break;
/* case 'f': */
/* opt_freq = atof(optarg); */
/* break; */
case 'V': /* version */
ach_print_version("achpipe.bin");
exit(EXIT_SUCCESS);
case '?':
case 'h':
case 'H':
puts( "Usage: achlog [OPTIONS] channels...\n"
"Log ach channels to files"
"\n"
"Options:\n"
" -?, Show help\n"
" -z, Filter output through gzip\n"
"\n"
"Examples:\n"
" achlog foo bar Log channels foo and bar\n"
"\n"
"Report bugs to <*****@*****.**>"
);
exit(EXIT_SUCCESS);
default:
posarg(optarg);
}
}
while( optind < argc ) {
posarg(argv[optind++]);
}
if( 0 == n_log ) ACH_DIE("No channels to log\n");
/* Block Signals */
/* Have to block these before forking so ctrl-C doesn't kill the
* gzip */
int sigs[] = {SIGTERM, SIGINT, 0};
ach_sig_block_dummy( sigs );
/* Open Channels */
size_t i;
for( i = 0; i < n_log; i ++ ) {
ach_status_t r = ach_open(&log_desc[i].chan, log_desc[i].name, NULL);
if( ACH_OK != r ) {
ACH_DIE( "Could not open channel %s: %s\n",
log_desc[i].name, ach_result_to_string(r) );
}
/* Open log file */
if( opt_gzip ) {
log_desc[i].fout = filter( "gzip -c", log_desc[i].name, ".gz" );
} else {
log_desc[i].fout = fopen(log_desc[i].name, "w");
}
if( NULL == log_desc[i].fout ) {
ACH_DIE( "Could not open log file for %s: %s\n",
log_desc[i].name, strerror(errno) );
}
}
/* get some data */
if( clock_gettime(ACH_DEFAULT_CLOCK, &now_ach ) ||
clock_gettime(CLOCK_REALTIME, &now_real ) )
{
ACH_DIE( "Could not get time: %s\n", strerror(errno) );
}
if( gethostname( host, sizeof(host) ) ) {
ACH_LOG(LOG_ERR, "Could not get host name: %s\n", strerror(errno));
}
host[sizeof(host)-1] = '\0';
passwd = getpwuid(getuid());
if( passwd ) {
strtok(passwd->pw_gecos, ",");
}
now_real_str = ctime( &now_real.tv_sec );
/* Create Workers */
pthread_t thread[n_log];
for( i = 0; i < n_log; i ++ ) {
int r = pthread_create( thread+i, NULL, worker, (void*)(log_desc+i) );
if( r ) ACH_DIE( "Couldn't start worker thread: %s\n", strerror(r) );
}
ach_notify(ACH_SIG_OK);
/* Wait for Signal */
ach_sig_wait( sigs );
/* Cancel workers */
ach_cancel_attr_t cattr;
ach_cancel_attr_init( &cattr );
cattr.async_unsafe = 1;
for( i = 0; i < n_log; i ++ ) {
ach_cancel( &log_desc[i].chan, &cattr );
}
/* Join worker threads */
for( i = 0; i < n_log; i ++ ) {
int r = pthread_join( thread[i], NULL );
if( r ) ACH_DIE( "Couldn't join worker thread: %s\n", strerror(r) );
if( opt_gzip ) {
if( pclose(log_desc[i].fout) < 0 ) {
ACH_LOG( LOG_ERR, "Could not pclose output for %s: %s\n",
log_desc[i].name, strerror(errno) );
}
} else {
fclose(log_desc[i].fout);
}
}
return 0;
}
static PyObject *raise_error( ach_status_t r ) {
PyErr_SetString( ach_py_error, ach_result_to_string(r) );
return NULL;
}
void huboLoop(struct hubo_param *H_param) {
// get initial values for hubo
struct hubo_ref H_ref;
struct hubo_state H_state;
memset( &H_ref, 0, sizeof(H_ref));
memset( &H_state, 0, sizeof(H_state));
size_t fs;
//int r = ach_get( &chan_hubo_ref, &H, sizeof(H), &fs, NULL, ACH_O_LAST );
//assert( sizeof(H) == fs );
int r = ach_get( &chan_hubo_ref, &H_ref, sizeof(H_ref), &fs, NULL, ACH_O_LAST );
if(ACH_OK != r) {
if(hubo_debug) {
printf("Ref ini r = %s\n",ach_result_to_string(r));}
}
else{ assert( sizeof(H_ref) == fs ); }
r = ach_get( &chan_hubo_state, &H_state, sizeof(H_state), &fs, NULL, ACH_O_LAST );
if(ACH_OK != r) {
if(hubo_debug) {
printf("State ini r = %s\n",ach_result_to_string(r));}
}
else{
assert( sizeof(H_state) == fs );
}
// time info
struct timespec t;
//int interval = 500000000; // 2hz (0.5 sec)
int interval = 10000000; // 100 hz (0.01 sec)
//int interval = 5000000; // 200 hz (0.005 sec)
//int interval = 2000000; // 500 hz (0.002 sec)
/* Sampling Period */
double T = (double)interval/(double)NSEC_PER_SEC; // (sec)
// get current time
//clock_gettime( CLOCK_MONOTONIC,&t);
clock_gettime( 0,&t);
while(1) {
// wait until next shot
clock_nanosleep(0,TIMER_ABSTIME,&t, NULL);
/* Get latest ACH message */
r = ach_get( &chan_hubo_ref, &H_ref, sizeof(H_ref), &fs, NULL, ACH_O_LAST );
if(ACH_OK != r) {
if(hubo_debug) {
printf("Ref r = %s\n",ach_result_to_string(r));}
}
else{ assert( sizeof(H_ref) == fs ); }
r = ach_get( &chan_hubo_state, &H_state, sizeof(H_state), &fs, NULL, ACH_O_LAST );
if(ACH_OK != r) {
if(hubo_debug) {
printf("State r = %s\n",ach_result_to_string(r));}
}
else{ assert( sizeof(H_state) == fs ); }
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT AVBOE THIS LINE]---------------------------------
// ------------------------------------------------------------------------------
H_ref.ref[RHY] = 0.3;
H_ref.ref[LEB] = -0.4;
H_ref.ref[RSP] = 0.3;
double encRSP = H_state.joint[RSP].pos;
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT BELOW THIS LINE]---------------------------------
// ------------------------------------------------------------------------------
ach_put( &chan_hubo_ref, &H_ref, sizeof(H_ref));
t.tv_nsec+=interval;
tsnorm(&t);
}
}
/**
* @function check_traj_chan
*/
bool check_traj_chan(struct timespec *ts) {
std::list<Eigen::VectorXd> path;
for( int c = 0; c < 2; ++c ) {
size_t frame_size;
void *buf = NULL;
ach_status_t r = sns_msg_local_get( traj_chan[c],
&buf,
&frame_size,
ts,
ACH_O_LAST | (ts ? ACH_O_WAIT : 0 ) );
switch(r) {
case ACH_OK:
case ACH_MISSED_FRAME: {
struct sns_msg_path_dense *msg = (struct sns_msg_path_dense*)buf;
if( frame_size == sns_msg_path_dense_size(msg) ) {
// Save traj
int n_dofs; int n_steps;
n_dofs = msg->n_dof;
n_steps = msg->n_steps;
printf("Received trajectory with %d points and %d dofs \n", n_steps, n_dofs );
Eigen::VectorXd maxVel; maxVel = mMaxVel*Eigen::VectorXd::Ones( n_dofs );
Eigen::VectorXd maxAccel; maxAccel = mMaxAccel*Eigen::VectorXd::Ones( n_dofs );
std::cout << "Max accel: "<< maxAccel.transpose() << std::endl;
std::cout << "Max vel: "<< maxVel.transpose() << std::endl;
int counter = 0;
for( int i = 0; i < n_steps; ++i ) {
Eigen::VectorXd p(n_dofs);
for( int j = 0; j < n_dofs; ++j ) {
p(j) = msg->x[counter];
counter++;
}
path.push_back( p );
}
// Send trajectory
printf("Received trajectory for arm %d. Executing \n", c);
std::list<Eigen::VectorXd>::iterator it;
int m = 0;
for( it = path.begin(); it != path.end(); ++it ) {
std::cout << "P["<<m<<"]: "<< (*it).transpose() << std::endl;
m++;
}
std::cout << "Max accel: "<< maxAccel.transpose() << std::endl;
std::cout << "Max vel: "<< maxVel.transpose() << std::endl;
return true;
} else {
SNS_LOG( LOG_ERR, "Invalid motor_state message \n" );
return false;
}
} break;
case ACH_TIMEOUT:
case ACH_STALE_FRAMES:
case ACH_CANCELED:
break;
default:
SNS_LOG( LOG_ERR, "Failed ach_get: %s \n", ach_result_to_string(r) );
} // end switch
return false;
} // end for
}
int main(int argc, char** argv) {
if (argc < 2) {
std::cerr << "usage: " << argv[0] << " OUTPUTFILE.data\n";
return 1;
}
ach_channel_t chan_hubo_state;
ach_status_t r;
r = ach_open(&chan_hubo_state, HUBO_CHAN_STATE_NAME, NULL);
if (r != ACH_OK) {
std::cerr << "error opening state channel: " << ach_result_to_string(r) << "\n";
return 1;
}
LogWriter writer(argv[1], 200);
struct hubo_state H_state;
JointInfoArray jinfo;
buildJointTable(jinfo);
writer.add(&H_state.time, "state.time", "s");
for (size_t i=0; i<jinfo.size(); ++i) {
const std::string& name = jinfo[i].second;
const size_t index = jinfo[i].first;
writer.add(&H_state.joint[index].ref, "state.joint." + name + ".ref", "rad");
writer.add(&H_state.joint[index].pos, "state.joint." + name + ".pos", "rad");
writer.add(&H_state.joint[index].cur, "state.joint." + name + ".cur", "amp");
writer.add(&H_state.joint[index].vel, "state.joint." + name + ".vel", "rad/s");
writer.add(&H_state.joint[index].duty, "state.joint." + name + ".duty");
writer.add(&H_state.joint[index].heat, "state.joint." + name + ".heat", "J");
writer.add(&H_state.joint[index].active, "state.joint." + name + ".active");
writer.add(&H_state.joint[index].zeroed, "state.joint." + name + ".zeroed");
}
for (int i=0; i<4; ++i) {
std::ostringstream ostr;
ostr << "state.imu" << i;
std::string imustr = ostr.str();
writer.add(&H_state.imu[i].a_x, imustr+".a_x");
writer.add(&H_state.imu[i].a_y, imustr+".a_y");
writer.add(&H_state.imu[i].a_z, imustr+".a_z");
writer.add(&H_state.imu[i].w_x, imustr+".w_x");
writer.add(&H_state.imu[i].w_y, imustr+".w_y");
writer.add(&H_state.imu[i].w_z, imustr+".w_z");
}
for (int i=0; i<4; ++i) {
std::ostringstream ostr;
ostr << "state.ft" << i;
std::string ftstr = ostr.str();
writer.add(&H_state.ft[i].m_x, ftstr+".m_x");
writer.add(&H_state.ft[i].m_y, ftstr+".m_y");
writer.add(&H_state.ft[i].f_z, ftstr+".f_z");
}
writer.sortChannels();
writer.writeHeader();
std::cout << "Opened " << argv[1] << " for output, Ctrl+C to halt logging.\n";
signal(SIGINT, interruptHandler);
while (!interrupted) {
bool write = false;
size_t fs;
r = ach_get( &chan_hubo_state, &H_state, sizeof(H_state), &fs, NULL, ACH_O_WAIT );
if (useable(r)) {
write = true;
} else {
std::cout << "warning: ach returned " << ach_result_to_string(r) << " for state\n";
}
if (write) {
writer.writeSample();
}
}
std::cout << "Successfully wrote " << argv[1] << "\n";
return 0;
}
