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
}
//========================================================================
}
