/**************************
* @function main
***************************/
int main( int argc, char* argv[] ) {
// Call sns
sns_init();
sns_start();
// 2. Open channels
sns_chan_open( &chan_state_right, "state-right", NULL );
{
ach_channel_t *chans[] = {&chan_state_right, NULL};
sns_sigcancel( chans, sns_sig_term_default );
}
// 3. Run
while( !sns_cx.shutdown ) {
update();
aa_mem_region_local_release();
usleep(0.5*1e6);
}
// 4. Leave
sns_end();
return 0;
}
/**
* @function openComm
*/
void openComm() {
// Start sns
sns_init();
sns_start();
// Open channels to read motor states
ach_channel_t* arm_state_chan[2];
ach_channel_t* hand_state_chan[2];
for( int i = 0; i < 2; ++i ) {
arm_state_chan[i] = new ach_channel_t();
hand_state_chan[i] = new ach_channel_t();
}
for( int i = 0; i < 2; ++i ) {
sns_chan_open( arm_state_chan[i], mS.limb[i].arm_state_chan.c_str(), NULL );
sns_chan_open( hand_state_chan[i], mS.limb[i].hand_state_chan.c_str(), NULL );
}
// BIMANUAL
sns_chan_open( &mBiTraj_chan, "bimanual_chan", NULL );
sns_chan_open( &mBiHand_chan, "bimanual_hand_chan", NULL );
mDli.set_numJoints(7,7);
mDli.set_hand_channels( hand_state_chan[0], hand_state_chan[1], &mBiHand_chan );
mDli.set_arm_channels( arm_state_chan[0], arm_state_chan[1], &mBiTraj_chan );
// Ready
printf("\t * [SET_COMM] We should be ready to go \n");
}
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) );
}
}
}
void CrichtonView<PointT>::startComm( int state, void* userdata ) {
sns_init();
sns_start();
ach_status_t r;
r = ach_open( &mObj_param_chan, gObj_param_chan_name.c_str(), NULL );
if( r != ACH_OK ) { printf("[ERROR] COULD NOT OPEN OBJ PARAM CHAN"); return; }
r = ach_open( &mServer2Module_chan, gServer2Module_chan_name.c_str(), NULL );
if( r != ACH_OK ) { printf("[ERROR] COULD NOT OPEN SERVER-2-SEE CHAN"); return; }
r = ach_open( &mModule2Server_chan, gModule2Server_chan_name.c_str(), NULL );
if( r != ACH_OK ) { printf("[ERROR] COULD NOT SEE-2-SERVER CHAN"); return; }
printf("\t [OK] Communication stablished and ready to go \n");
mChanReady = true;
}
/**
* @function main
*/
int main( int argc, char* argv[] ) {
sns_init();
sns_start();
traj_name[0] = "test-left";
traj_name[1] = "test-right";
// Create channels
for( int i = 0; i < 2; ++i ) {
traj_chan[i] = new ach_channel_t();
}
// Open & set channels
for( int i = 0; i < 2; ++i ) {
sns_chan_open( traj_chan[i], traj_name[i].c_str(), NULL );
}
// Loop until you hear a trajectory coming
while(true) {
if( clock_gettime( ACH_DEFAULT_CLOCK, &now ) ) {
SNS_LOG( LOG_ERR, "clock_gettime failed: '%s' \n", strerror(errno) );
printf("Did not get time right \n");
return 0;
}
ts= sns_time_add_ns( now, 1000*1000*1 );
check_traj_chan( &ts );
usleep((1e6*mDt) );
}
return 0;
}
