int main( int argc, char* argv[] ) {
// Open trajectory channel
sns_chan_open( &traj_chan, "traj-left", NULL );
// Set path
std::list<Eigen::VectorXd> path;
Eigen::VectorXd po(num_joints);
Eigen::VectorXd pf(num_joints);
po(0) = 0.162630;
po(1) = 0.098454;
po(2) = 0.13533;
po(3) = -0.002723;
pf(0) = po(0) - 0.16;
pf(1) = po(1) - 0.1;
pf(2) = po(2) - 0.135;
pf(3) = po(3) + 0.1;
path.push_back( po );
path.push_back( pf );
// Build a message
struct sns_msg_path_dense* msg = sns_msg_path_dense_alloc( path.size(),
(*(path.begin())).size() );
msg->n_dof = (*(path.begin())).size();
msg->n_steps = path.size();
sns_msg_header_fill( &msg->header );
msg->header.n = (*(path.begin())).size();
// Fill path
int counter = 0;
std::list<Eigen::VectorXd>::iterator it;
for( it = path.begin(); it != path.end(); ++it ) {
for( int j = 0; j < (*it).size(); ++j ) {
msg->x[counter] = (*it)(j);
counter++;
}
}
// Set message duration
double tsec = 0.05;
int64_t dt_nsec = tsec*1e9;
struct timespec now;
if( clock_gettime( ACH_DEFAULT_CLOCK, &now ) ) {
SNS_LOG( LOG_ERR, "Clock_gettime failed: %s \n", strerror(errno) );
}
sns_msg_set_time( &msg->header, &now, dt_nsec );
// Send message
ach_status_t r;
r = ach_put( &traj_chan, msg, sns_msg_path_dense_size(msg) );
if( r!= ACH_OK ) { printf("\t * [ERROR] Error sending path \n"); }
else { printf("\t * [INFO] Path was sent all right\n"); }
}
/**
* @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");
}
/**************************
* @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;
}
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) );
}
}
}
/**
* @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;
}
