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"); }
}
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 update
* @brief TODO
*/
bool BaseDualControl::update() {
if( clock_gettime( ACH_DEFAULT_CLOCK, &mNow ) ) {
SNS_LOG( LOG_ERR, "clock_gettime failed: '%s' \n", strerror(errno) );
return false;
}
return mBc[0].update() && mBc[1].update();
}
/**
* @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;
}
/**
* @function update
*/
static int update(void) {
if( clock_gettime( ACH_DEFAULT_CLOCK, &now ) ) {
SNS_LOG( LOG_ERR, "clock_gettime failed: '%s' \n", strerror(errno) );
return 0;
}
struct timespec timeout = sns_time_add_ns( now, 1000*1000*1 );
// Arm Right
int u_r = update_n( ARM_AXES, qr, dqr, &chan_state_right, &timeout );
printState( qr, dqr, ARM_AXES, "Right arm" );
// Both are updated, return 1, otherwise return 0
return u_r;
}
/**
* @function followTrajectory
*/
bool Arm_Interface::followTrajectory( const std::list<Eigen::VectorXd> &_path ) {
// 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 = msg->n_dof;
// Fill data in message
int counter = 0;
std::list<Eigen::VectorXd>::const_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;
if( clock_gettime( ACH_DEFAULT_CLOCK, &mNow ) ) {
SNS_LOG( LOG_ERR, "Clock_gettime failed: %s \n", strerror(errno) );
}
sns_msg_set_time( &msg->header, &mNow, dt_nsec );
// Send message
ach_status_t r;
r = ach_put( mChan_output, msg, sns_msg_path_dense_size(msg) );
if( r!= ACH_OK ) { printf("\t * [ERROR] Error sending arm path \n"); }
else { printf("\t * [INFO] Arm path was sent all right\n"); }
return 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;
}
int main( int argc, char* argv[] ) {
int v;
while( (v=getopt(argc,argv,"c:ho:")) != -1 ) {
switch(v) {
case 'c': {
chan_opt.assign(optarg);
} break;
case 'o': {
output_file.assign(optarg);
} break;
case 'h': {
printf("Syntax: %s -c CHANNEL_NAME -o OUTPUT_FILE.txt\n", argv[0]);
return 1;
} break;
}
}
// Open channel
if( ach_open( &chan, chan_opt.c_str(), NULL ) != ACH_OK ) {
printf("Error opening channel %s \n", chan_opt.c_str() );
return -1;
}
// Loop
std::ofstream output( output_file.c_str(), std::ofstream::out );
while(true) {
// Read channel
if( clock_gettime( ACH_DEFAULT_CLOCK, &t_now ) ) {
SNS_LOG( LOG_ERR, "clock_gettime failed: '%s' \n", strerror(errno) );
break;
}
t_timeout = sns_time_add_ns( t_now, 1000*1000*1 );
size_t frame_size;
void* buf = NULL;
ach_status_t r;
r = sns_msg_local_get( &chan, &buf, &frame_size, &t_timeout,
ACH_O_LAST | ACH_O_WAIT );
switch(r) {
case ACH_OK:
case ACH_MISSED_FRAME: {
struct sns_msg_tracker* msg = (struct sns_msg_tracker*)buf;
sns_msg_tracker_dump( stdout, msg );
for( int i = 0; i < msg->header.n; ++i ) {
printf("Writing \n");
output << msg->u[i].x << " " << msg->u[i].y << " " << msg->u[i].z << " ";
}
output <<std::endl;
} break;
}
usleep(0.25e6);
} // end while
output.close();
}
/**
* @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
}
