void hubo_cb(const hubo_ros::HuboCommand &msg)
{
printf("Received command message\n");
//Send the commands from the HuboCommand message onto ACH to the robot
//Make the necessary hubo struct for ACH
struct hubo_ref H_ref_filter;
memset( &H_ref_filter, 0, sizeof(H_ref_filter));
size_t fs;
//First, get the current state of the Hubo from ACH
int r = ach_get(&chan_hubo_ref_filter, &H_ref_filter, sizeof(H_ref_filter), &fs, NULL, ACH_O_LAST);
if(ACH_OK != r)
{
if(hubo_debug)
{
printf("State ini r = %i\n",r);
}
}
else
{
assert(sizeof(H_ref_filter) == fs);
}
printf("Converting ROS message containing [%d] joint commands to hubo-ach\n", msg.num_joints);
//Add the joint values one at a time into the hubo struct
//for each joint command, we lookup the best matching
//joint in the header to set the index
for (int i = 0; i < msg.num_joints; i++)
{
int index = IndexLookup(msg.joints[i].name);
if (index != -1)
{
printf("Mapped URDF joint name [%s] to hubo joint index [%d]\n", msg.joints[i].name.c_str(), index);
H_ref_filter.ref[index] = msg.joints[i].position;
}
}
//If there are any joint values not assigned in the message, don't change them in the struct!
printf("Sending a new state on ACH\n");
//Put the new message into the ACH channel
ach_put(&chan_hubo_ref_filter, &H_ref_filter, sizeof(H_ref_filter));
}
/**
* Home a joint
* @param joint The joint to home
* @return True on success
*/
bool CommandChannel::home(string joint) {
BoardCommand command;
memset(&command, 0, sizeof(command));
int jointNum = 0;
if (strcmp(joint.c_str(), "all") == 0) {
command.type = D_GOTO_HOME_ALL;
} else if ((jointNum = indexLookup(joint)) == -1) {
return false;
} else {
command.type = D_GOTO_HOME;
command.joint = jointNum;
}
int r = ach_put(&huboBoardCommandChannel, &command, sizeof(command));
if (ACH_OK != r) {
cerr << "Error! Command home failed with state " << r << endl;
return false;
}
return true;
}
void run(cx_t *cx) {
// somatic_d_event( &cx->d, SOMATIC__EVENT__PRIORITIES__NOTICE,
// SOMATIC__EVENT__CODES__PROC_RUNNING,
// NULL, NULL );
fastrak_c_t fastrak;
//printf("About to enter loop\n");
while(!daemon_sig_quit) {
// get tfs
//printf("About to read\n");
int r = fastrak_read(&cx->fk, &fastrak );
//printf("Something was read\n");
// somatic_verbprintf( 2, "Got reading\n");
if( 0 == r ) { //TODO: Use c-struct
//printf("About to put\n");
ach_put( &chan, &fastrak, sizeof(fastrak) );
dump( &fastrak );
// fill protobuf with tfs
/* for( size_t i = 0; i < 4; i ++ ) {
somatic_transform_set_vec( cx->msg->tf[i], cx->fk.tf_vq[i] );
somatic_transform_set_quat( cx->msg->tf[i], cx->fk.tf_vq[i] + 3 );
}
if( somatic_opt_verbosity >= 3 ) {
dump(cx);
}
*/
// set time
// somatic_metadata_set_time_now( cx->msg->meta );
// send
// r = SOMATIC_PACK_SEND( &cx->chan, somatic__multi_transform, cx->msg );
}
}
// somatic_d_event( &cx->d, SOMATIC__EVENT__PRIORITIES__NOTICE,
// SOMATIC__EVENT__CODES__PROC_STOPPING,
// NULL, NULL );
}
/**
* Sends CAN packet to desired channel
*
* @param $first
* "@param" is the socket you want to send to
* @param $second
* CAN frame to send
*/
int sendCan(hubo_can_t skt, struct can_frame *f) {
errno = 0;
int bytes_sent = write( skt, f, sizeof(*f) );
if (bytes_sent != sizeof(*f)) {
perror("write");
}
if (have_iotrace_chan) {
io_trace_t trace;
trace.timestamp = iotrace_gettime();
trace.is_read = 0;
trace.fd = skt;
trace.result_errno = errno;
trace.transmitted = bytes_sent;
trace.frame = *f;
ach_put(&iotrace_chan, &trace, sizeof(trace));
}
if (bytes_sent < 0) { bytes_sent = 0; }
return bytes_sent;
}
/**
* Enable a joint
* @param joint The joint to enable
* @return True on success
*/
bool CommandChannel::enable(string joint) {
BoardCommand command;
memset(&command, 0, sizeof(command));
int jointNum = 0;
if (strcmp(joint.c_str(), "all") == 0) {
command.type = D_CTRL_ON_OFF_ALL;
command.param[0] = D_ENABLE;
} else if ((jointNum = indexLookup(joint)) == -1) {
return false;
} else {
command.type = D_CTRL_ON_OFF;
command.joint = jointNum;
command.param[0] = D_ENABLE;
}
int r = ach_put(&huboBoardCommandChannel, &command, sizeof(command));
if (ACH_OK != r) {
cerr << "Error! Command enable failed with state " << r << endl;
return false;
}
return true;
}
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;
}
int runTraj(char* s, int mode, struct hubo_ref *r, struct timespec *t, struct hubo_state* H_state, bool compliance_mode, bool pause_feature) {
int i = 0;
// int interval = 10000000; // 100 hz (0.01 sec)
hubo_virtual_t H_virtual;
memset( &H_virtual, 0, sizeof(H_virtual));
size_t fs;
int rr = 0;
char str[1000];
FILE *fp; // file pointer
fp = fopen(s,"r");
if(!fp) {
printf("No Trajectory File!!!\n");
return 1; // exit if not file
}
char c;
bool paused=false;
// tweak_init();
int line_counter=0;
double T = (double)interval/(double)NSEC_PER_SEC;
// printf("Reading %s\n",s);
double id = 0.0;
while(fgets(str,sizeof(str),fp) != NULL) {
// printf("i = %d\n",i);
// i = i+1;
// wait until next shot
clock_nanosleep(0,TIMER_ABSTIME,t, NULL);
if( HUBO_VIRTUAL_MODE_OPENHUBO == mode ){
for( id = 0 ; id < T; id = id + HUBO_LOOP_PERIOD ){
rr = ach_get( &chan_hubo_from_sim, &H_virtual, sizeof(H_virtual), &fs, NULL, ACH_O_WAIT );
}
}
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT AVBOE THIS LINE]---------------------------------
// ------------------------------------------------------------------------------
line_counter++;
//printf("line is %d \n", line_counter);
// if ( read(STDIN_FILENO, &c, 1) == 1) {
// if (c=='p' && pause_feature==true) {
// paused=!paused;
// printf("paused is now %s \n", paused ? "true" : "false");
// }
// }
// while (paused==true){
// usleep(1000000);//1 second
// t->tv_sec+=1; // for the 1 sec delay in line above
// if ( read(STDIN_FILENO, &c, 1) == 1) {
// if (c=='p'&& pause_feature==true) {
// paused=!paused;
// printf("paused is now %s \n", paused ? "true" : "false");
// }
// }
// }
int len = strlen(str)-1;
if(str[len] == '\n') {
str[len] = 0;
}
// Read motion from the text file
getArg(str, r);
// Compliance Mode
int joint;
if (compliance_mode==true){
for (joint=4; joint<18; joint++){//hard coded for arms only
r->comply[joint]=1;
}
}
else{
for (joint=4; joint<18; joint++){//hard coded for arms only
r->comply[joint]=0;
}
}
if (goto_init_flag) {
struct hubo_ref tmpref;
int i;
for (i=0; i<200; ++i) {
double u = (double)(i+1)/200;
for (joint=0; joint<HUBO_JOINT_COUNT; ++joint) {
tmpref.ref[joint] = u * r->ref[joint] + (1-u) * H_state->joint[joint].ref;
}
ach_put( &chan_hubo_ref, &tmpref, sizeof(tmpref));
usleep(0.01 * 1e6);
}
goto_init_flag = 0;
clock_gettime(0, t);
// reset clock
}
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT BELOW THIS LINE]---------------------------------
// ------------------------------------------------------------------------------
// Cheeting No more RAP or LAP
/*
r->ref[RHP] = 0.0;
r->ref[LHP] = 0.0;
r->ref[RAP] = 0.0;
r->ref[LAP] = 0.0;
r->ref[RKN] = 0.0;
r->ref[LKN] = 0.0;
r->ref[RAR] = 0.0;
r->ref[LAR] = 0.0;
r->ref[RHR] = 0.0;
r->ref[LHR] = 0.0;
*/
/*
for( i = 0 ; i < HUBO_JOINT_COUNT; i++){
r->mode[i] = HUBO_REF_MODE_REF;
}
*/
ach_put( &chan_hubo_ref, r, sizeof(*r));
//printf("Ref r = %s\n",ach_result_to_string(r));
t->tv_nsec+=interval;
tsnorm(t);
}
}
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 );
}
void Walker::sendState()
{
ach_put( &bal_state_chan, &bal_state, sizeof(bal_state) );
}
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);
}
}
static int testsig(void)
{
enum ach_status r;
/* Fork 0 */
pid_t pid_p = fork();
check_errno( "Fork 0", pid_p );
/* GP: wait */
if( pid_p ) {
return testsig_gp(pid_p);
}
/* Parent */
/* Create Kernel Channel */
{
ach_create_attr_t attr;
ach_create_attr_init(&attr);
attr.map = ACH_MAP_KERNEL;
r = ach_unlink(OPT_CHAN);
if( ! ach_status_match(r, ACH_MASK_OK | ACH_MASK_ENOENT) ) {
fail_ach( "unlink before create", r );
}
r = ach_unlink(OPT_CHAN);
if( ACH_ENOENT != r ) fail_ach( "unlink noent", r );
check_ach( "ach_create",
ach_create( OPT_CHAN, opt_msg_cnt, opt_msg_size, &attr ) );
}
/* Open Kernel Channel */
struct ach_channel chan;
{
for(;;) {
usleep(1000); /* Race against udev */
r = ach_open( &chan, OPT_CHAN, NULL );
if( ACH_EACCES == r ) continue;
else if (ACH_OK == r) break;
else fail_ach("ach_open", r);
}
}
/* Install Parent sighandler */
sighandler_install();
/* fork 1 */
pid_t pid_c = fork();
check_errno( "fork 1", pid_c );
if( pid_c ) {
/* Parent: */
for(;;) {
usleep(10000); /* Racy... */
check_errno( "kill child", kill( pid_c, SIGUSR1) );
usleep(10000);
int i = 42;
check_ach( "put to child", ach_put( &chan, &i, sizeof(i) ) );
int status;
pid_t wp = waitpid( pid_c, &status, WNOHANG );
if( wp ) {
if( wp != pid_c ) {
fail_errno("Wait 1");
} else if( WIFEXITED(status) && (0 == WEXITSTATUS(status)) ) {
exit(EXIT_SUCCESS);
} else {
fprintf(stderr, "Child 1 failed\n");
exit(EXIT_FAILURE);
}
}
}
} else {
/* child */
sig_atomic_t s0, s1;
int i;
do {
size_t frame_size;
s0 = count_sigusr1;
r = ach_get(&chan, &i, sizeof(i), &frame_size, NULL, ACH_O_WAIT );
s1 = count_sigusr1;
check_ach("child sig get", r);
} while( s1 == s0 || s1 < 10 ); /* This is racy... */
printf("done: %s, %d,%d,%d\n", ach_result_to_string(r), s0, s1, i);
exit(EXIT_SUCCESS);
}
return 0;
}
void CrichtonView<PointT>::send( int state, void* userData ) {
mMutex.lock(); // Don't process anything while we are sending
printf("Sending \n");
for( int i = 0; i < 4; ++i ) { printf("**Table coeffs [%d]: %f \n", i, mTts.mTableCoeffs[i]);}
printf("Clusters size: %d \n", mTts.getNumClusters() );
if( mSelectedCluster < 0 || mSelectedCluster >= mTts.getNumClusters() ) {
printf("--> ERROR: Did not select a cloud? \n");
return;
}
// 0. Set message alloc
sns_msg_tabletop_sq* msg = sns_msg_tabletop_sq_alloc( mTts.getNumClusters() );
sns_msg_header_fill( &msg->header );
msg->header.n = (uint32_t)mTts.getNumClusters();
msg->n = (uint32_t) mTts.getNumClusters();
msg->i = (uint32_t) mSelectedCluster;
// Variables
pcl::PolygonMesh mesh;
std::vector<SQ_parameters> ps;
// 1. Save SQs
for( int i = 0; i < mTts.getNumClusters(); ++i ) {
// Recognize
int xmin, ymin, xmax, ymax; int index; std::string label;
mTts.getClusterBB( i, xmin, ymin, xmax, ymax );
mOd.classify( cv::Mat( mRgbImg, cv::Rect(xmin,ymin, xmax-xmin, ymax-ymin) ),
index, label );
// Send this info for fitting
//index = 5; // BOTTLE BY NOW
printf("*** Label!!: %s index: %d \n", label.c_str(), index );
ObjectEntry entry; entry = mOd.getEntry( index );
fit_SQ( mTts.getCluster(i), i, ps, entry );
copy_SQparam_msg( ps[0], msg->u[i] );
std::cout << "Dim ps 0: "<< ps[0].dim[0]<<", "<< ps[0].dim[1]<<", "<< ps[0].dim[2]<<std::endl;
//////////////////////////////
// Debug Store mesh
char debug_name[100];
if( mMirror ) { sprintf( debug_name, "%s_mirrored_%d.ply", (char*)label.c_str(), i ); }
else { sprintf( debug_name, "%s_no_mirror_%d.ply", (char*)label.c_str(), i ); }
SQ_utils::create_SQ_mesh( ps[0], 25, debug_name, false );
/////////////////////////
char mesh_name[50]; sprintf( mesh_name, "%s/mesh_%d.ply", gPicturesPath.c_str(), i );
if( ps.size() == 1 ) {
SQ_utils::create_SQ_mesh( ps[0], 25, mesh_name, false );
} else {
SQ_utils::convertMeshes( ps, mesh_name );
std::cout << "Mesh name: "<< mesh_name << std::endl;
}
msg->u[i].mesh_generated = true;
strcpy( msg->u[i].mesh_filename, mesh_name );
}
// 2. Get the table
char tableName[50]; sprintf( tableName, "%s/tableMesh.ply", gPicturesPath.c_str() );
create_table_mesh( mesh, tableName );
pcl::io::savePLYFile( tableName, mesh );
for( int i = 0; i < 4; ++i ) { msg->table_coeffs[i] = mTts.mTableCoeffs[i]; }
sprintf( msg->table_meshfile, "%s", tableName );
ach_status_t r = ach_put( &mObj_param_chan, msg, sns_msg_tabletop_sq_size(msg) );
if( r != ACH_OK ) {
printf("\t [BAD] Error sending message \n");
} else {
printf("\t [GOOD] Message sent all right \n ");
}
msg = 0;
mMutex.unlock(); // Let the processing go on with its life
}
int readCan(hubo_can_t skt, struct can_frame *f, double timeoD) {
fd_set read_fds;
int result;
// note timeo is the time out in nanoseconds
int64_t timeo = (int64_t)(timeoD*1e9);
struct timespec timeout;
int bytes_read = 0;
int read_errno = 0;
FD_ZERO(&read_fds);
FD_SET(skt, &read_fds);
timeout.tv_sec = timeo / (int64_t)NSEC_PER_SEC;
timeout.tv_nsec = timeo % (int64_t)NSEC_PER_SEC;
errno = 0;
result = pselect(skt+1, &read_fds, 0, 0, &timeout, NULL);
if (result < 0) {
if (errno != EINTR) {
perror("select");
}
} else if (result && FD_ISSET(skt, &read_fds)) {
errno = 0;
bytes_read = read( skt, f, sizeof(*f) );
read_errno = errno;
if (bytes_read < 0) {
perror("read");
}
}
if (have_iotrace_chan) {
io_trace_t trace;
trace.timestamp = iotrace_gettime();
trace.is_read = 1;
trace.fd = skt;
trace.result_errno = read_errno;
trace.transmitted = bytes_read;
trace.frame = *f;
ach_put(&iotrace_chan, &trace, sizeof(trace));
}
// just turn errors into no read
if (bytes_read < 0) { bytes_read = 0; }
return bytes_read;
}
static void send_time(float t) {
int r = ach_put(&time_chan, &t, sizeof(t));
assert(ACH_OK == r);
if( ACH_OK != r ) exit(EXIT_FAILURE);
}
void Aggregator::_send_output()
{
ach_put(&_agg_chan, _output_data, hubo_cmd_data_get_size(_output_data));
}
/**
* Update the reference channel
*/
void ReferenceChannel::update(){
if (errored) return;
ach_put(&huboReferenceChannel, ¤tReference, sizeof(currentReference));
}
void com_cb(const hubo_ros::AchCommand &msg){
printf("Received Ach Command Message!\n");
hubo_board_cmd_t H_cmd;
memset(&H_cmd, 0, sizeof(H_cmd));
int r = 0;
if (msg.commandName.compare("enableJoint") == 0){
int index = IndexLookup(msg.jointName);
if (index == -1){
printf("Error! Unable to convert joint name [%s] to hubo joint index! Aborting...", msg.jointName.c_str());
return;
}
printf("Mapped URDF joint name [%s] to hubo joint index [%d]\n", msg.jointName.c_str(), index);
H_cmd.type = D_CTRL_ON_OFF;
H_cmd.joint = IndexLookup(msg.jointName);
H_cmd.param[0] = D_ENABLE;
} else if (msg.commandName.compare("disableJoint") == 0){
int index = IndexLookup(msg.jointName);
if (index == -1){
printf("Error! Unable to convert joint name [%s] to hubo joint index! Aborting...", msg.jointName.c_str());
return;
}
printf("Mapped URDF joint name [%s] to hubo joint index [%d]\n", msg.jointName.c_str(), index);
H_cmd.type = D_CTRL_ON_OFF;
H_cmd.joint = IndexLookup(msg.jointName);
H_cmd.param[0] = D_DISABLE;
} else if (msg.commandName.compare("enableAll") == 0){
H_cmd.type = D_CTRL_ON_OFF_ALL;
H_cmd.param[0] = D_ENABLE;
} else if (msg.commandName.compare("disableAll") == 0){
H_cmd.type = D_CTRL_ON_OFF_ALL;
H_cmd.param[0] = D_DISABLE;
} else if (msg.commandName.compare("homeJoint") == 0){
int index = IndexLookup(msg.jointName);
if (index == -1){
printf("Error! Unable to convert joint name [%s] to hubo joint index! Aborting...", msg.jointName.c_str());
return;
}
printf("Mapped URDF joint name [%s] to hubo joint index [%d]\n", msg.jointName.c_str(), index);
H_cmd.type = D_GOTO_HOME;
H_cmd.joint = index;
} else if (msg.commandName.compare("homeAll") == 0){
H_cmd.type = D_GOTO_HOME_ALL;
} else {
printf("Unable to identify Ach Command. Discarding message.\n");
return;
}
printf("Sending a new command out on ACH.");
r = ach_put(&chan_hubo_board_cmd, &H_cmd, sizeof(H_cmd));
}
