void test() { std::unique_lock<std::mutex> lk(m); get_robot_state(); is_ready = true; cv.notify_one(); }
void TrajActionServer::home() { /* """This method will provide power to the robot as will as home the robot. This method requries the robot name."""*/ //rospy.loginfo(rospy.get_caller_id() + ' -> start homing') std_msgs::String state; state.data = "Home"; if(robot_state.compare(state.data)==0) { return true; } dvrk_set_state(state); int counter = 10; // up to 10 transitions to get ready cout << "robot_state: " << get_robot_state() << endl; while (counter > 0){ cout << "robot_state: " << get_robot_state() << endl; cv.wait_for(lk, 20); // give up to 20 secs for each transition if (robot_state.compare("DVRK_READY") != 0){ counter = counter - 1; ROS_INFO("waiting for state to be DVRK_READY"); } //rospy.loginfo(rospy.get_caller_id() + ' -> waiting for state to be DVRK_READY') else{ counter = -1; } } if (robot_state.compare("DVRK_READY") != 0) { ROS_INFO("failed to reach state DVRK_READY"); //rospy.logfatal(rospy.get_caller_id() + ' -> failed to reach state DVRK_READY') } ROS_INFO("homing complete"); //rospy.loginfo(rospy.get_caller_id() + ' <- homing complete') }
//========================================================================================================= void* tf_main(void* thread_arg) { float angle; float v_ref, w_ref; int i; long int counter=0; #ifdef EKF_LOC int data[NUM_SENS]; // provided by ekf.h #endif #ifdef HOKUYO int maximum_laser_data=get_data_max(); #endif setup_termination(); angle = 0.0; gyro_integral = 0.0; gyro_sup_integral = 0.0; float a,b; #ifdef CARTESIAN_REGULATOR float v_return, w_return; int get_goal=0; fuzzy_horizon_result fh; #endif a=4; b=0; // set_robot_speed(&a,&b); #define RAD2DEG(x) ((float) x*180/M_PI ) struct timeval tvb, tva; #ifdef LOG_FROM_MAIN_THREAD int cycle; char debug_buf[24]; log_fd=open("timing_main.txt",O_CREAT | O_WRONLY | O_TRUNC , S_IWUSR | S_IRUSR | S_IRGRP | S_IROTH); #endif #ifdef HOKUYO_SENSOR #endif // The timing of the first cycle is not allined due to the // fact that the two threads are not setup exactly at the // same time pthread_cond_wait(&cond, &mutex); pthread_mutex_unlock(&mutex); #ifdef CARTESIAN_REGULATOR goal_state = (float *) malloc(sizeof(float)*3); curr_state = (float *) malloc(sizeof(float)*3); goal_state[0]=viapoints[curr_via][0]; goal_state[1]=viapoints[curr_via][1]; goal_state[2]=0; #endif while(1){ counter++; pthread_cond_wait(&cond, &mutex); printf("Alive! [%d]\n",counter); pthread_mutex_unlock(&mutex); gettimeofday(&tvb,NULL); // get_robot_state(&robot_state); #ifdef CARTESIAN_REGULATOR // Cartesian Regulator // get_goal=cartesian_controller(robot_state, goal_state, .1, .1, &v_return, &w_return); if (!via_done) { get_goal=cartesian_controller(curr_state, goal_state, 0.3, 0.25, &v_return, &w_return); printf("v:%f \tw:%f\n",v_return,w_return); if (get_goal==1){ curr_via++; goal_state[0]=viapoints[curr_via][0]; goal_state[1]=viapoints[curr_via][1]; get_goal=0; } if (curr_via==N_VIA) { via_done=1; v_return=0; w_return=0; } } #ifdef OB_AV apply_fuzzy_horizon_algorithm(&v_return,&w_return,&fh ); set_robot_speed(&fh.v_ref_hor_fuz, &fh.w_ref_hor_fuz); #else set_robot_speed(&v_return, &w_return); #endif #endif #ifdef EKF_LOC #ifdef HOKUYO_SENSOR // To select the subset of laser beams of interests // These are defined in ekf.c by ANGLE_H for (i=0; i<NUM_SENS; i++) { obsv[i]=data_laser[ANGLE_IDX[i]]/10; } #endif #ifdef IR_SENSOR for (i=0; i<NUM_SENS; i++){ obsv[i]= *(ir->range+i); // printf("%f\n ", obsv[i]); } #endif // Get the latest control input u[0]=last_v_ref; u[1]=last_w_ref; // Prediction EKF_Prediction(xpost, &xpred, u); // Ekf Correction EKF_Correction(xpred, &xpost, obsv); printf("%f\t%f\t%f\n",xpred.x, xpred.y, RAD2DEG(xpred.th)); printf("%f\t%f\t%f\n",xpost.x, xpost.y, RAD2DEG(xpost.th)); #ifdef CARTESIAN_REGULATOR curr_state[0]=xpost.x; curr_state[1]=xpost.y; curr_state[2]=xpost.th; #endif #endif printf("%f\t%f\t%f\n",state[0],state[1],RAD2DEG(state[2])); printf("lu: %f \tlw: %f\n",last_v_ref,last_w_ref); gettimeofday(&tva,NULL); if (tva.tv_sec==tvb.tv_sec){ printf("%ld\n", tva.tv_usec-tvb.tv_usec); #ifdef LOG_FROM_MAIN_THREAD cycle=tva.tv_usec-tvb.tv_usec; #endif } else { int delta; delta = 1000000-tvb.tv_usec; printf("%ld\n",tva.tv_usec+delta); #ifdef LOG_FROM_MAIN_THREAD cycle=tva.tv_usec+delta; #endif } #ifdef LOG_FROM_MAIN_THREAD sprintf(debug_buf,"%ld\n",cycle); write(log_fd, debug_buf,strlen(debug_buf)); #endif } //======================================================================== while (0) { // packet_type = analizza_pacchetto(); if (packet_type == LOAD_PACKET_ANALYZED) { //============================================ pthread_mutex_lock(&mutex_fp); fflush(fp_log); pthread_mutex_unlock(&mutex_fp); flag = 0; } #ifdef EKF_LOC #ifdef HOKUYO_SENSOR // To select the subset of laser beams of interests // These are defined in ekf.c by ANGLE_H for (i=0; i<NUM_SENS; i++) { obsv[i]=data_laser[ANGLE_IDX[i]]; } #endif #ifdef IR_SENSOR for (i=0; i<NUM_SENS; i++){ obsv[i]= *(ir->range+i); // printf("%f\n ", obsv[i]); } #endif // Get the latest control input u[0]=last_v_ref; u[1]=last_w_ref; // EKF Prediction // EKF_Prediction(xpost, &xpred, u); // Ekf Correction // EKF_Correction(xpred, &xpost, obsv); // printf("%f\t%f\t%f\n",xpred.x, xpred.y, RAD2DEG(xpred.th)); // printf("%f\t%f\t%f\n",xpost.x, xpost.y, RAD2DEG(xpost.th)); printf("%f\t%f\t%f\n",state[0],state[1],RAD2DEG(state[2])); printf("u: %f \tw: %f\n",u[0],u[1]); printf("lu: %f \tlw: %f\n",last_v_ref,last_w_ref); #endif } //======================================================================== }