35 #ifndef _STATE_MACHINE_CPP_
36 #define _STATE_MACHINE_CPP_
44 a_h = (((double)rand())/(
double)RAND_MAX);
78 printf(
"status.target_found=%d\n",status.target_found);
79 printf(
"status.last_id=%d\n",status.last_id);
82 status.target_found=
false;
83 int should_move=
false;
84 double dist_to_target=1000;
88 print_status(&status);
102 foveationcontrol_home();
111 status.previous_state=INITIALISE;
112 status.state=SEARCHING;
122 if (status.previous_state!=SEARCHING)
124 status.tic_searching=ros::Time::now();
129 status.target_found = search_for_new_target(&targets, &status,&search_area);
130 printf(
"status.target_found=%d\n",status.target_found);
131 status.last_id=status.target_id;
134 foveationcontrol_sweeping();
142 if(status.target_found && !is_safe_using_lasers())
144 status.previous_state=SEARCHING;
145 status.state=TRACKING_NOT_SAFE;
149 else if(status.target_found)
151 status.previous_state=SEARCHING;
152 status.state=TRACKING;
159 if (status.previous_state!=TRACKING)
163 status.tic_tracking=ros::Time::now();
167 foveationcontrol(&status);
168 status.target_found=get_new_target_information(&targets, &status);
176 status.direction = atan2(status.current_y,status.current_x);
181 if(status.reset_tracking==
true)
183 status.previous_state=TRACKING;
184 status.state=SEARCHING;
185 status.reset_tracking =
false;
189 else if(status.target_found==
false || status.current_x<3.0)
191 status.previous_state=TRACKING;
192 status.state=TARGET_LOST;
196 }
else if (!is_safe_using_lasers())
198 status.previous_state=TRACKING;
199 status.state=TRACKING_NOT_SAFE;
201 if (dist_to_target<6)
208 case TRACKING_NOT_SAFE:
209 if (status.previous_state!=TRACKING_NOT_SAFE)
213 status.tic_tracking_not_safe=ros::Time::now();
217 status.target_found = get_new_target_information(&targets, &status);
218 foveationcontrol(&status);
220 if (status.target_found)
222 dist_to_target = sqrt(pow(status.current_x-2.7,2) + pow(status.current_y,2));
223 printf(
"status.target_id=%d\n",status.target_id);
224 printf(
"dist_to_target=%f\n",dist_to_target);
225 if (dist_to_target<4)
233 status.direction = atan2(status.current_y,status.current_x);
238 if(status.reset_tracking==
true)
240 status.previous_state=TRACKING_NOT_SAFE;
241 status.state=SEARCHING;
242 status.reset_tracking =
false;
246 else if(status.target_found==
false || status.current_x<3.0)
248 status.previous_state=TRACKING_NOT_SAFE;
249 status.state=TARGET_LOST;
254 else if (is_safe_using_lasers())
256 int tmp = status.previous_state;
257 status.previous_state=TRACKING_NOT_SAFE;
258 status.state=TRACKING;
277 foveationcontrol_home();
285 status.previous_state=TARGET_LOST;
286 status.state=SEARCHING;
294 ROS_ERROR(
"WARNING DANGER unknown state in state machine abort demonstration");
#define DIRECTION_IN_FRONT