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)); }