00001
00002
00003
00004
00005 #include "darwin_autocharge_alg_node.h"
00006
00007 DarwinAutochargeAlgNode::DarwinAutochargeAlgNode(void) :
00008 algorithm_base::IriBaseAlgorithm<DarwinAutochargeAlgorithm>(),
00009 move_left_arm_client_("move_left_arm", true),
00010 move_right_arm_client_("move_right_arm", true),
00011 tracking_head_client_("tracking_head", true),
00012 move_joints_client_("move_joints", true)
00013 {
00014
00015 this->loop_rate_ = 10;
00016
00017
00018 this->execute_action_publisher_ = this->public_node_handle_.advertise<std_msgs::Int32>("execute_action", 100);
00019 this->cmd_vel_publisher_ = this->public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 100);
00020 this->head_target_publisher_ = this->public_node_handle_.advertise<std_msgs::Float64MultiArray>("head_target", 100);
00021
00022
00023
00024 this->adc_channels_subscriber_ = this->public_node_handle_.subscribe("adc_channels", 100, &DarwinAutochargeAlgNode::adc_channels_callback, this);
00025 this->joint_states_subscriber_ = this->public_node_handle_.subscribe("joint_states", 100, &DarwinAutochargeAlgNode::joint_states_callback, this);
00026 this->marker_subscriber_ = this->public_node_handle_.subscribe("marker", 100, &DarwinAutochargeAlgNode::marker_callback, this);
00027
00028
00029
00030
00031 get_ik_client_ = this->public_node_handle_.serviceClient<kinematics_msgs::GetPositionIK>("get_ik");
00032
00033
00034
00035
00036 sleep(10);
00037 this->n=0;
00038 this->k=-1;
00039 this->m=0;
00040 this->p=0;
00041 this->current_pan=0.0;
00042 this->current_tilt=0.0;
00043 this->marker_found=false;
00044 this->marker_really_founded=false;
00045 this->ready=false;
00046 this->close_to_station=false;
00047 this->joints_moving=false;
00048 this->right_arm_moving=false;
00049 this->left_arm_moving=false;
00050 this->head_is_tracking=false;
00051 this->ready_for_check=false;
00052 this->left_ok=false;
00053 this->right_ok=false;
00054 this->prev_left_ok=false;
00055 this->prev_right_ok=false;
00056 this->left_done=false;
00057 this->left_marker_found=false;
00058 this->right_marker_found=false;
00059 this->first_goal=true;
00060 this->left_data_ready=false;
00061 this->right_data_ready=false;
00062 this->left_ready_tf=false;
00063 this->right_ready_tf=false;
00064 this->success=true;
00065
00066 this->state=searching;
00067
00068 }
00069
00070 DarwinAutochargeAlgNode::~DarwinAutochargeAlgNode(void)
00071 {
00072
00073 }
00074
00075 void DarwinAutochargeAlgNode::mainNodeThread(void)
00076 {
00077 int i=0;
00078
00079
00080
00081
00082
00083
00084
00085 ROS_INFO("Main Thread");
00086 try{
00087 switch(this->state)
00088 {
00089 case searching: ROS_INFO("Current state: Searching marker");
00090 if(this->marker_found){
00091 move_joints_client_.cancelGoal();
00092 ROS_WARN("Marker founded");
00093 this->state=new_goal;
00094 }else{
00095 if((this->joints_moving)||(this->head_is_tracking))
00096 this->state=searching;
00097 else{
00098 this->move_joints_goal_.trajectory.header.seq=0;
00099 this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
00100 this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
00101 this->move_joints_goal_.trajectory.joint_names.resize(2);
00102 this->move_joints_goal_.trajectory.joint_names[0]="j_pan";
00103 this->move_joints_goal_.trajectory.joint_names[1]="j_tilt";
00104 this->move_joints_goal_.trajectory.points.resize(1);
00105 this->move_joints_goal_.trajectory.points[0].positions.resize(2);
00106 this->move_joints_goal_.trajectory.points[0].velocities.resize(2);
00107 this->move_joints_goal_.trajectory.points[0].accelerations.resize(2);
00108 if(this->n==0)
00109 this->move_joints_goal_.trajectory.points[0].positions[0]=60.0;
00110 else
00111 this->move_joints_goal_.trajectory.points[0].positions[0]=this->current_state.position[18]*180.0/3.14159;
00112 this->move_joints_goal_.trajectory.points[0].positions[1]=60.0-15.0*this->n;
00113 for(i=0;i<2;i++)
00114 {
00115 this->move_joints_goal_.trajectory.points[0].velocities[i]=30.0;
00116 this->move_joints_goal_.trajectory.points[0].accelerations[i]=60.0;
00117 }
00118 this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
00119 move_jointsMakeActionRequest();
00120 if(this->n>3){
00121
00122 if((this->conv_msg_.pose.position.z>0.0)&&(this->conv_msg_.pose.position.z<=0.2))
00123 this->state=move_back;
00124 else
00125 this->state=rotate;
00126 }else
00127 this->state=searching2;
00128 }
00129 }
00130 break;
00131
00132 case searching2: ROS_INFO("Current state: Searching marker");
00133 if(this->marker_found){
00134 move_joints_client_.cancelGoal();
00135 ROS_WARN("Marker founded");
00136 this->state=new_goal;
00137 }else{
00138 if(this->joints_moving)
00139 this->state=searching2;
00140 else{
00141 this->ready=true;
00142 this->move_joints_goal_.trajectory.header.seq=0;
00143 this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
00144 this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
00145 this->move_joints_goal_.trajectory.joint_names.resize(1);
00146 this->move_joints_goal_.trajectory.joint_names[0]="j_pan";
00147 this->move_joints_goal_.trajectory.points.resize(1);
00148 this->move_joints_goal_.trajectory.points[0].positions.resize(1);
00149 this->move_joints_goal_.trajectory.points[0].velocities.resize(1);
00150 this->move_joints_goal_.trajectory.points[0].accelerations.resize(1);
00151 this->move_joints_goal_.trajectory.points[0].velocities[0]=30.0;
00152 this->move_joints_goal_.trajectory.points[0].accelerations[0]=60.0;
00153 if(this->k==-1){
00154 this->move_joints_goal_.trajectory.points[0].positions[0]=-60.0;
00155 this->k=1;
00156 }else{
00157 this->move_joints_goal_.trajectory.points[0].positions[0]=60.0;
00158 this->k=-1;
00159 }
00160 this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
00161 move_jointsMakeActionRequest();
00162 this->n++;
00163 this->state=searching;
00164 }
00165 }
00166 break;
00167
00168 case rotate: ROS_INFO("Current state: Rotating");
00169 if(this->joints_moving)
00170 this->state=rotate;
00171 else{
00172 this->Twist_msg_.linear.x=0.0;
00173 this->Twist_msg_.linear.y=0.0;
00174 this->Twist_msg_.linear.z=0.0;
00175 this->Twist_msg_.angular.x=0.0;
00176 this->Twist_msg_.angular.y=0.0;
00177 this->Twist_msg_.angular.z=1.05;
00178 this->cmd_vel_publisher_.publish(this->Twist_msg_);
00179 sleep(5);
00180 this->Twist_msg_.angular.z=0.0;
00181 this->cmd_vel_publisher_.publish(this->Twist_msg_);
00182
00183 this->n=0;
00184 this->k=-1;
00185 this->state=searching;
00186 }
00187 break;
00188
00189 case new_goal: ROS_INFO("Current state: New goal");
00190 if(this->joints_moving)
00191 this->state=new_goal;
00192 else{
00193
00194 this->Float64MultiArray_msg_.data.resize(2);
00195 this->Float64MultiArray_msg_.data[0]=(this->current_state.position[18]+atan(this->msg_.markers[0].pose.pose.position.x/this->msg_.markers[0].pose.pose.position.z))*180.0/3.14159;
00196 this->Float64MultiArray_msg_.data[1]=(this->current_state.position[19]+atan(this->msg_.markers[0].pose.pose.position.y/this->msg_.markers[0].pose.pose.position.z))*180.0/3.14159; if((fabs(this->Float64MultiArray_msg_.data[0])<=60.0)&&(fabs(this->Float64MultiArray_msg_.data[1])<=40.0))
00197 ROS_WARN("target pan and tilt: %f, %f", Float64MultiArray_msg_.data[0], Float64MultiArray_msg_.data[1]);
00198 else{
00199 this->Float64MultiArray_msg_.data[0]=this->current_state.position[18]*180.0/3.14159;
00200 this->Float64MultiArray_msg_.data[1]=this->current_state.position[19]*180.0/3.14159;
00201 }
00202 this->tracking_head_goal_.pan_goal=this->Float64MultiArray_msg_.data[0];
00203 this->tracking_head_goal_.tilt_goal=this->Float64MultiArray_msg_.data[1];
00204 this->head_target_publisher_.publish(this->Float64MultiArray_msg_);
00205
00206 if(this->first_goal)
00207 this->state=tracking;
00208 else{
00209
00210 if(this->close_to_station){
00211
00212 if(fabs(this->current_pan)>=35.0){
00213 if(this->current_pan>0.0)
00214 this->state=go_left;
00215 else
00216 this->state=go_right;
00217 }else{
00218 if((this->left_pose_.pose.position.x>=0.11)&&(this->right_pose_.pose.position.x>=0.11))
00219 this->state=go_forward;
00220 else if((this->left_pose_.pose.position.x<0.11)&&(this->right_pose_.pose.position.x<0.11))
00221 this->state=go_back;
00222 else if((this->left_pose_.pose.position.x>=0.11)&&(this->right_pose_.pose.position.x<0.11))
00223 this->state=go_left;
00224 else if((this->right_pose_.pose.position.x>=0.11)&&(this->left_pose_.pose.position.x<0.11))
00225 this->state=go_right;
00226 }
00227 }else
00228 this->state=walking;
00229 }
00230 }
00231 break;
00232
00233 case tracking: ROS_INFO("Current state: Start tracking");
00234
00235 tracking_headMakeActionRequest();
00236 this->first_goal=false;
00237 sleep(1);
00238 this->state=new_goal;
00239 break;
00240
00241 case walking: ROS_INFO("Current state: Walking");
00242
00243 if((this->conv_msg_.pose.position.z<=0.15)&&(this->conv_msg_.pose.position.z>0.0)){
00244 tracking_head_client_.cancelGoal();
00245 this->first_goal=true;
00246 this->state=initial_pose;
00247 }else{
00248 this->state=new_goal;
00249 this->Twist_msg_.linear.x=0.03;
00250 this->Twist_msg_.linear.y=0.0;
00251 this->Twist_msg_.linear.z=0.0;
00252 this->Twist_msg_.angular.x=0.0;
00253 this->Twist_msg_.angular.y=0.0;
00254 this->Twist_msg_.angular.z=0.8*this->current_state.position[18];
00255 this->cmd_vel_publisher_.publish(this->Twist_msg_);
00256
00257 if(this->msg_.markers[0].pose.pose.position.z==0.0){
00258 tracking_head_client_.cancelGoal();
00259 this->marker_found=false;
00260 this->marker_really_founded=false;
00261 this->first_goal=true;
00262
00263 this->n=0;
00264 this->k=-1;
00265 this->Twist_msg_.linear.x=0.0;
00266 this->Twist_msg_.angular.z=0.0;
00267 this->cmd_vel_publisher_.publish(this->Twist_msg_);
00268 this->state=searching;
00269 }
00270 }
00271 break;
00272
00273 case initial_pose: ROS_INFO("Current state: Initial pose");
00274 if((this->joints_moving)||(this->head_is_tracking))
00275 this->state=initial_pose;
00276 else{
00277
00278 this->success=true;
00279 this->ready=false;
00280 this->left_done=false;
00281 this->left_marker_found=false;
00282 this->right_marker_found=false;
00283 this->close_to_station=true;
00284 this->n=0;
00285 this->k=-1;
00286
00287 this->Int32_msg_.data=15;
00288 this->execute_action_publisher_.publish(this->Int32_msg_);
00289
00290 this->move_joints_goal_.trajectory.header.seq=0;
00291 this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
00292 this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
00293 this->move_joints_goal_.trajectory.joint_names.resize(4);
00294 this->move_joints_goal_.trajectory.joint_names[0]="j_shoulder_r";
00295 this->move_joints_goal_.trajectory.joint_names[1]="j_shoulder_l";
00296 this->move_joints_goal_.trajectory.joint_names[2]="j_low_arm_r";
00297 this->move_joints_goal_.trajectory.joint_names[3]="j_low_arm_l";
00298 this->move_joints_goal_.trajectory.points.resize(1);
00299 this->move_joints_goal_.trajectory.points[0].positions.resize(4);
00300 this->move_joints_goal_.trajectory.points[0].velocities.resize(4);
00301 this->move_joints_goal_.trajectory.points[0].accelerations.resize(4);
00302 this->move_joints_goal_.trajectory.points[0].positions[0]=-30.0;
00303 this->move_joints_goal_.trajectory.points[0].positions[1]=35.0;
00304 this->move_joints_goal_.trajectory.points[0].positions[2]=20.0;
00305 this->move_joints_goal_.trajectory.points[0].positions[3]=-20.0;
00306 for(i=0;i<4;i++){
00307 this->move_joints_goal_.trajectory.points[0].velocities[i]=60.0;
00308 this->move_joints_goal_.trajectory.points[0].accelerations[i]=100.0;
00309 }
00310 this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
00311 move_jointsMakeActionRequest();
00312 this->state=search_left_marker;
00313 }
00314 break;
00315
00316 case search_left_marker: ROS_INFO("Current state: Searching left marker");
00317 if(this->left_marker_found){
00318 ROS_WARN("Left marker founded");
00319 move_joints_client_.cancelGoal();
00320 this->state=transform_left_pose;
00321 }else{
00322 if(this->joints_moving)
00323 this->state=search_left_marker;
00324 else{
00325 this->move_joints_goal_.trajectory.header.seq=0;
00326 this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
00327 this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
00328 this->move_joints_goal_.trajectory.joint_names.resize(2);
00329 this->move_joints_goal_.trajectory.joint_names[0]="j_tilt";
00330 this->move_joints_goal_.trajectory.joint_names[1]="j_pan";
00331 this->move_joints_goal_.trajectory.points.resize(1);
00332 this->move_joints_goal_.trajectory.points[0].positions.resize(2);
00333 this->move_joints_goal_.trajectory.points[0].velocities.resize(2);
00334 this->move_joints_goal_.trajectory.points[0].accelerations.resize(2);
00335 this->move_joints_goal_.trajectory.points[0].positions[0]=40.0-5.0*this->n;
00336 if(this->n==0)
00337 this->move_joints_goal_.trajectory.points[0].positions[1]=60.0;
00338 else
00339 this->move_joints_goal_.trajectory.points[0].positions[1]=this->current_state.position[18]*180.0/3.14159;
00340 for(i=0;i<2;i++){
00341 this->move_joints_goal_.trajectory.points[0].velocities[i]=20.0;
00342 this->move_joints_goal_.trajectory.points[0].accelerations[i]=40.0;
00343 }
00344 this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
00345 move_jointsMakeActionRequest();
00346 if(this->n>3){
00347 move_joints_client_.cancelGoal();
00348 this->state=move_back;
00349 }else
00350 this->state=search_left_marker2;
00351 }
00352 }
00353 break;
00354
00355 case search_left_marker2: ROS_INFO("Current state: Searching left marker");
00356 if(this->left_marker_found){
00357 ROS_WARN("Left marker founded");
00358 move_joints_client_.cancelGoal();
00359 this->state=transform_left_pose;
00360 }else{
00361 if(this->joints_moving)
00362 this->state=search_left_marker2;
00363 else{
00364 this->ready=true;
00365 this->move_joints_goal_.trajectory.header.seq=0;
00366 this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
00367 this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
00368 this->move_joints_goal_.trajectory.joint_names.resize(1);
00369 this->move_joints_goal_.trajectory.joint_names[0]="j_pan";
00370 this->move_joints_goal_.trajectory.points.resize(1);
00371 this->move_joints_goal_.trajectory.points[0].positions.resize(1);
00372 this->move_joints_goal_.trajectory.points[0].velocities.resize(1);
00373 this->move_joints_goal_.trajectory.points[0].accelerations.resize(1);
00374 this->move_joints_goal_.trajectory.points[0].velocities[0]=20.0;
00375 this->move_joints_goal_.trajectory.points[0].accelerations[0]=40.0;
00376 if(this->k==-1){
00377 this->move_joints_goal_.trajectory.points[0].positions[0]=-60.0;
00378 this->k=1;
00379 }else{
00380 this->move_joints_goal_.trajectory.points[0].positions[0]=60.0;
00381 this->k=-1;
00382 }
00383 this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
00384 move_jointsMakeActionRequest();
00385 this->n++;
00386 this->state=search_left_marker;
00387 }
00388 }
00389 break;
00390
00391 case search_right_marker: ROS_INFO("Current state: Searching right marker");
00392 if(this->right_marker_found){
00393 ROS_WARN("Right marker founded");
00394 move_joints_client_.cancelGoal();
00395 this->state=transform_right_pose;
00396 }else{
00397 if(this->joints_moving)
00398 this->state=search_right_marker;
00399 else{
00400 this->move_joints_goal_.trajectory.header.seq=0;
00401 this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
00402 this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
00403 this->move_joints_goal_.trajectory.joint_names.resize(2);
00404 this->move_joints_goal_.trajectory.joint_names[0]="j_tilt";
00405 this->move_joints_goal_.trajectory.joint_names[1]="j_pan";
00406 this->move_joints_goal_.trajectory.points.resize(1);
00407 this->move_joints_goal_.trajectory.points[0].positions.resize(2);
00408 this->move_joints_goal_.trajectory.points[0].velocities.resize(2);
00409 this->move_joints_goal_.trajectory.points[0].accelerations.resize(2);
00410 this->move_joints_goal_.trajectory.points[0].positions[0]=40.0-5.0*this->n;
00411 if(this->n==0)
00412 this->move_joints_goal_.trajectory.points[0].positions[1]=-60.0;
00413 else
00414 this->move_joints_goal_.trajectory.points[0].positions[1]=this->current_state.position[18]*180.0/3.14159;
00415 for(i=0;i<2;i++){
00416 this->move_joints_goal_.trajectory.points[0].velocities[i]=20.0;
00417 this->move_joints_goal_.trajectory.points[0].accelerations[i]=40.0;
00418 }
00419 this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
00420 move_jointsMakeActionRequest();
00421 if(this->n>3){
00422 move_joints_client_.cancelGoal();
00423 this->state=move_back;
00424 }else
00425 this->state=search_right_marker2;
00426 }
00427 }
00428 break;
00429
00430 case search_right_marker2: ROS_INFO("Current state: Searching right marker");
00431 if(this->right_marker_found){
00432 ROS_WARN("Right marker founded");
00433 move_joints_client_.cancelGoal();
00434 this->state=transform_right_pose;
00435 }else{
00436 if(this->joints_moving)
00437 this->state=search_right_marker2;
00438 else{
00439 this->move_joints_goal_.trajectory.header.seq=0;
00440 this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
00441 this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
00442 this->move_joints_goal_.trajectory.joint_names.resize(1);
00443 this->move_joints_goal_.trajectory.joint_names[0]="j_pan";
00444 this->move_joints_goal_.trajectory.points.resize(1);
00445 this->move_joints_goal_.trajectory.points[0].positions.resize(1);
00446 this->move_joints_goal_.trajectory.points[0].velocities.resize(1);
00447 this->move_joints_goal_.trajectory.points[0].accelerations.resize(1);
00448 if(this->k==-1){
00449 this->move_joints_goal_.trajectory.points[0].positions[0]=-60.0;
00450 this->k=1;
00451 }else{
00452 this->move_joints_goal_.trajectory.points[0].positions[0]=60.0;
00453 this->k=-1;
00454 }
00455 this->move_joints_goal_.trajectory.points[0].velocities[0]=20.0;
00456 this->move_joints_goal_.trajectory.points[0].accelerations[0]=40.0;
00457 this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
00458 move_jointsMakeActionRequest();
00459 this->n++;
00460 this->state=search_right_marker;
00461 }
00462 }
00463 break;
00464
00465 case move_back: ROS_INFO("Current state: Going back");
00466
00467 this->Int32_msg_.data=9;
00468 this->execute_action_publisher_.publish(this->Int32_msg_);
00469 sleep(2);
00470
00471 this->Twist_msg_.linear.x=-0.03;
00472 this->Twist_msg_.linear.y=0.0;
00473 this->Twist_msg_.linear.z=0.0;
00474 this->Twist_msg_.angular.x=0.0;
00475 this->Twist_msg_.angular.y=0.0;
00476 this->Twist_msg_.angular.z=0.0;
00477 this->cmd_vel_publisher_.publish(this->Twist_msg_);
00478 usleep(500000);
00479 this->Twist_msg_.linear.x=0.0;
00480 this->cmd_vel_publisher_.publish(this->Twist_msg_);
00481 this->state=initial_pose;
00482 break;
00483
00484 case transform_left_pose: ROS_INFO("Current state: Transforming left pose");
00485 if(this->joints_moving)
00486 this->state=transform_left_pose;
00487 else{
00488
00489 this->move_joints_goal_.trajectory.header.seq=0;
00490 this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
00491 this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
00492 this->move_joints_goal_.trajectory.joint_names.resize(2);
00493 this->move_joints_goal_.trajectory.joint_names[0]="j_pan";
00494 this->move_joints_goal_.trajectory.joint_names[1]="j_tilt";
00495 this->move_joints_goal_.trajectory.points.resize(1);
00496 this->move_joints_goal_.trajectory.points[0].positions.resize(2);
00497 this->move_joints_goal_.trajectory.points[0].velocities.resize(2);
00498 this->move_joints_goal_.trajectory.points[0].accelerations.resize(2);
00499 for(i=0;i<2;i++){
00500 this->move_joints_goal_.trajectory.points[0].velocities[i]=40.0;
00501 this->move_joints_goal_.trajectory.points[0].accelerations[i]=60.0;
00502 }
00503 this->move_joints_goal_.trajectory.points[0].positions[0]=this->current_pan;
00504 this->move_joints_goal_.trajectory.points[0].positions[1]=this->current_tilt;
00505 this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
00506 move_jointsMakeActionRequest();
00507
00508 if(this->left_data_ready){
00509 ROS_WARN("Left pose transformed");
00510 this->n=0;
00511 this->k=1;
00512 this->state=search_right_marker;
00513 }else{
00514 if(this->p>2){
00515 this->p=0;
00516
00517 this->n=0;
00518 this->k=-1;
00519 this->state=search_left_marker;
00520 }else
00521 this->state=transform_left_pose;
00522 }
00523 this->left_ready_tf=true;
00524 this->p++;
00525 }
00526 break;
00527
00528 case transform_right_pose: ROS_INFO("Current state: Transforming right pose");
00529 if(this->joints_moving)
00530 this->state=transform_right_pose;
00531 else{
00532
00533 this->move_joints_goal_.trajectory.header.seq=0;
00534 this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
00535 this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
00536 this->move_joints_goal_.trajectory.joint_names.resize(2);
00537 this->move_joints_goal_.trajectory.joint_names[0]="j_pan";
00538 this->move_joints_goal_.trajectory.joint_names[1]="j_tilt";
00539 this->move_joints_goal_.trajectory.points.resize(1);
00540 this->move_joints_goal_.trajectory.points[0].positions.resize(2);
00541 this->move_joints_goal_.trajectory.points[0].velocities.resize(2);
00542 this->move_joints_goal_.trajectory.points[0].accelerations.resize(2);
00543 for(i=0;i<2;i++){
00544 this->move_joints_goal_.trajectory.points[0].velocities[i]=40.0;
00545 this->move_joints_goal_.trajectory.points[0].accelerations[i]=60.0;
00546 }
00547 this->move_joints_goal_.trajectory.points[0].positions[0]=this->current_pan;
00548 this->move_joints_goal_.trajectory.points[0].positions[1]=this->current_tilt;
00549 this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
00550 move_jointsMakeActionRequest();
00551
00552 if(this->right_data_ready){
00553 ROS_WARN("Right pose transformed");
00554 this->ready_for_check=true;
00555 this->state=make_decision;
00556 }else{
00557 if(this->p>2){
00558 this->p=0;
00559
00560 this->n=0;
00561 this->k=1;
00562 this->state=search_right_marker;
00563 }else
00564 this->state=transform_right_pose;
00565 }
00566 this->right_ready_tf=true;
00567 this->p++;
00568 }
00569 break;
00570
00571 case make_decision: ROS_INFO("Current state: Looking if the final pose has inverse kinematics solution");
00572 if(this->joints_moving)
00573 this->state=make_decision;
00574 else{
00575
00576 this->get_ik_srv_.request.ik_request.ik_link_name="MP_ARM_GRIPPER_FIX_L";
00577
00578 this->left_pose_.header.stamp=ros::Time::now();
00579 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name.resize(4);
00580 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[0]="j_shoulder_l";
00581 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[1]="j_high_arm_l";
00582 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[2]="j_low_arm_l";
00583 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[3]="j_wrist_l";
00584 this->joint_states_mutex_.enter();
00585 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position.resize(4);
00586 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[0]=this->current_state.position[2];
00587 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[1]=this->current_state.position[4];
00588 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[2]=this->current_state.position[6];
00589 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[3]=this->current_state.position[23];
00590 this->joint_states_mutex_.exit();
00591 this->get_ik_srv_.request.ik_request.pose_stamped=left_pose_;
00592 this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.x=left_pose_.pose.position.x+0.02;
00593 this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.y=left_pose_.pose.position.y;
00594 this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.z=left_pose_.pose.position.z-0.18;
00595
00596 if(get_ik_client_.call(get_ik_srv_)){
00597 if(this->get_ik_srv_.response.error_code.val==this->get_ik_srv_.response.error_code.SUCCESS)
00598 this->left_ok=true;
00599 else this->left_ok=false;
00600 }else
00601 ROS_INFO("ConvertPoseAlgNode:: Failed to Call Server on topic get_ik ");
00602
00603 this->get_ik_srv_.request.ik_request.ik_link_name="MP_ARM_GRIPPER_FIX_R";
00604
00605 this->right_pose_.header.stamp=ros::Time::now();
00606 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name.resize(4);
00607 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[0]="j_shoulder_r";
00608 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[1]="j_high_arm_r";
00609 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[2]="j_low_arm_r";
00610 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[3]="j_wrist_r";
00611 this->joint_states_mutex_.enter();
00612 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position.resize(4);
00613 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[0]=this->current_state.position[1];
00614 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[1]=this->current_state.position[3];
00615 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[2]=this->current_state.position[5];
00616 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[3]=this->current_state.position[21];
00617 this->joint_states_mutex_.exit();
00618 this->get_ik_srv_.request.ik_request.pose_stamped=right_pose_;
00619 this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.x=right_pose_.pose.position.x+0.03;
00620 this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.y=right_pose_.pose.position.y+0.02;
00621 this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.z=right_pose_.pose.position.z-0.18;
00622
00623 if(get_ik_client_.call(get_ik_srv_)){
00624 if(this->get_ik_srv_.response.error_code.val==this->get_ik_srv_.response.error_code.SUCCESS)
00625 this->right_ok=true;
00626 else this->right_ok=false;
00627 }else
00628 ROS_INFO("ConvertPoseAlgNode:: Failed to Call Server on topic get_ik ");
00629
00630 if((this->left_ok)&&(this->right_ok)){
00631 ROS_WARN("Darwin can take the connectors");
00632 this->state=make_decision2;
00633 }else{
00634 ROS_WARN("Darwin need to correct its position");
00635 this->state=correct_position;
00636 }
00637 }
00638 break;
00639
00640 case make_decision2: ROS_INFO("Current state: Looking if the previous pose has inverse kinematics solution");
00641
00642 this->get_ik_srv_.request.ik_request.ik_link_name="MP_ARM_GRIPPER_FIX_L";
00643
00644 this->left_pose_.header.stamp=ros::Time::now();
00645 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name.resize(4);
00646 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[0]="j_shoulder_l";
00647 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[1]="j_high_arm_l";
00648 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[2]="j_low_arm_l";
00649 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[3]="j_wrist_l";
00650 this->joint_states_mutex_.enter();
00651 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position.resize(4);
00652 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[0]=this->current_state.position[2];
00653 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[1]=this->current_state.position[4];
00654 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[2]=this->current_state.position[6];
00655 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[3]=this->current_state.position[23];
00656 this->joint_states_mutex_.exit();
00657 this->get_ik_srv_.request.ik_request.pose_stamped=left_pose_;
00658 this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.x=left_pose_.pose.position.x-0.01;
00659 this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.y=left_pose_.pose.position.y+0.04;
00660 this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.z=left_pose_.pose.position.z-0.20;
00661
00662 if(get_ik_client_.call(get_ik_srv_)){
00663 if(this->get_ik_srv_.response.error_code.val==this->get_ik_srv_.response.error_code.SUCCESS)
00664 this->prev_left_ok=true;
00665 else this->prev_left_ok=false;
00666 }else
00667 ROS_INFO("ConvertPoseAlgNode:: Failed to Call Server on topic get_ik ");
00668
00669 this->get_ik_srv_.request.ik_request.ik_link_name="MP_ARM_GRIPPER_FIX_R";
00670
00671 this->right_pose_.header.stamp=ros::Time::now();
00672 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name.resize(4);
00673 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[0]="j_shoulder_r";
00674 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[1]="j_high_arm_r";
00675 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[2]="j_low_arm_r";
00676 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[3]="j_wrist_r";
00677 this->joint_states_mutex_.enter();
00678 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position.resize(4);
00679 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[0]=this->current_state.position[1];
00680 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[1]=this->current_state.position[3];
00681 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[2]=this->current_state.position[5];
00682 this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[3]=this->current_state.position[21];
00683 this->joint_states_mutex_.exit();
00684 this->get_ik_srv_.request.ik_request.pose_stamped=right_pose_;
00685 this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.x=right_pose_.pose.position.x;
00686 this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.y=right_pose_.pose.position.y+0.03;
00687 this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.z=right_pose_.pose.position.z-0.20;
00688
00689 if(get_ik_client_.call(get_ik_srv_)){
00690 if(this->get_ik_srv_.response.error_code.val==this->get_ik_srv_.response.error_code.SUCCESS)
00691 this->prev_right_ok=true;
00692 else this->prev_right_ok=false;
00693 }else
00694 ROS_INFO("ConvertPoseAlgNode:: Failed to Call Server on topic get_ik ");
00695 this->state=open_gripper;
00696 break;
00697
00698 case open_gripper: ROS_INFO("Current state: Open gripper");
00699 this->move_joints_goal_.trajectory.header.seq=0;
00700 this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
00701 this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
00702 this->move_joints_goal_.trajectory.joint_names.resize(2);
00703 this->move_joints_goal_.trajectory.joint_names[0]="j_gripper_l";
00704 this->move_joints_goal_.trajectory.joint_names[1]="j_gripper_r";
00705 this->move_joints_goal_.trajectory.points.resize(1);
00706 this->move_joints_goal_.trajectory.points[0].positions.resize(2);
00707 this->move_joints_goal_.trajectory.points[0].velocities.resize(2);
00708 this->move_joints_goal_.trajectory.points[0].accelerations.resize(2);
00709 this->move_joints_goal_.trajectory.points[0].positions[0] = -30.0;
00710 this->move_joints_goal_.trajectory.points[0].positions[1] = 30.0;
00711 for(i=0;i<2;i++){
00712 this->move_joints_goal_.trajectory.points[0].velocities[i] = 60.0;
00713 this->move_joints_goal_.trajectory.points[0].accelerations[i] = 100.0;
00714 }
00715 this->move_joints_goal_.trajectory.points[0].time_from_start = ros::Duration(1.0);
00716 move_jointsMakeActionRequest();
00717
00718 if(this->success)
00719 this->state=open_gripper2;
00720 else
00721 this->state=correct_position;
00722 break;
00723
00724 case open_gripper2: ROS_INFO("Current state: Open gripper");
00725 if(this->joints_moving)
00726 this->state=open_gripper2;
00727 else{
00728
00729 if(this->prev_left_ok){
00730 this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints.resize(1);
00731 this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.seq=0;
00732 this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.stamp=ros::Time::now();
00733 this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id="MP_BODY";
00734
00735 this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.x=this->left_pose_.pose.position.x-0.01;
00736 this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.y=this->left_pose_.pose.position.y+0.04;
00737 this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.z=this->left_pose_.pose.position.z-0.20;
00738
00739 move_left_armMakeActionRequest();
00740 }
00741
00742 if(this->prev_right_ok){
00743 this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints.resize(1);
00744 this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.seq=0;
00745 this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.stamp=ros::Time::now();
00746 this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id="MP_BODY";
00747
00748 this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.x=this->right_pose_.pose.position.x;
00749 this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.y=this->right_pose_.pose.position.y+0.03;
00750 this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.z=this->right_pose_.pose.position.z-0.20;
00751
00752 move_right_armMakeActionRequest();
00753 }
00754 this->state=take_connectors;
00755 }
00756 break;
00757
00758 case take_connectors: ROS_INFO("Current state: Moving arms to a previous pose");
00759 if((this->right_arm_moving)||(this->left_arm_moving))
00760 this->state=take_connectors;
00761 else{
00762
00763 this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints.resize(1);
00764 this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.seq=0;
00765 this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.stamp=ros::Time::now();
00766 this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id="MP_BODY";
00767
00768 this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.x=this->left_pose_.pose.position.x+0.02;
00769 this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.y=this->left_pose_.pose.position.y;
00770 this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.z=this->left_pose_.pose.position.z-0.18;
00771
00772 this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints.resize(1);
00773 this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.seq=0;
00774 this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.stamp=ros::Time::now();
00775 this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id="MP_BODY";
00776
00777 this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.x=this->right_pose_.pose.position.x+0.03;
00778 this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.y=this->right_pose_.pose.position.y+0.02;
00779 this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.z=this->right_pose_.pose.position.z-0.18;
00780
00781 move_left_armMakeActionRequest();
00782 move_right_armMakeActionRequest();
00783 this->state=take_connectors2;
00784 }
00785 break;
00786
00787 case take_connectors2: ROS_INFO("Current state: Moving arms to the final pose");
00788 if((this->left_arm_moving)||(this->right_arm_moving))
00789 this->state=take_connectors2;
00790 else{
00791
00792 this->move_joints_goal_.trajectory.header.seq=0;
00793 this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
00794 this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
00795 this->move_joints_goal_.trajectory.joint_names.resize(2);
00796 this->move_joints_goal_.trajectory.joint_names[0]="j_gripper_l";
00797 this->move_joints_goal_.trajectory.joint_names[1]="j_gripper_r";
00798 this->move_joints_goal_.trajectory.points.resize(1);
00799 this->move_joints_goal_.trajectory.points[0].positions.resize(2);
00800 this->move_joints_goal_.trajectory.points[0].velocities.resize(2);
00801 this->move_joints_goal_.trajectory.points[0].accelerations.resize(2);
00802 this->move_joints_goal_.trajectory.points[0].positions[0]=45.0;
00803 this->move_joints_goal_.trajectory.points[0].positions[1]=-45.0;
00804 for(i=0;i<2;i++){
00805 this->move_joints_goal_.trajectory.points[0].velocities[i]=60.0;
00806 this->move_joints_goal_.trajectory.points[0].accelerations[i]=100.0;
00807 }
00808 this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
00809 move_jointsMakeActionRequest();
00810 this->state=close_gripper;
00811 }
00812 break;
00813
00814 case close_gripper: ROS_INFO("Current state: Close gripper");
00815 if(this->joints_moving)
00816 this->state=close_gripper;
00817 else{
00818 ROS_WARN("Current voltage: %f", this->current_voltage);
00819 sleep(2);
00820
00821 if(this->current_voltage>0.0)
00822 ROS_WARN("Darwin has caught the connectors");
00823 else{
00824 ROS_WARN("Darwin hasn't caught the connectors");
00825
00826 this->state=open_gripper;
00827 this->success=false;
00828 }
00829 }
00830 break;
00831
00832 case correct_position: ROS_INFO("Current state: Correct position");
00833 if((this->joints_moving)||(this->head_is_tracking))
00834 this->state=correct_position;
00835 else{
00836
00837 this->Int32_msg_.data=9;
00838 this->execute_action_publisher_.publish(this->Int32_msg_);
00839 sleep(1);
00840
00841 this->ready_for_check=false;
00842 this->left_done=false;
00843 this->first_goal=true;
00844 this->left_data_ready=false;
00845 this->right_data_ready=false;
00846 this->left_ready_tf=false;
00847 this->right_ready_tf=false;
00848 this->left_marker_found=false;
00849 this->right_marker_found=false;
00850 this->n=0;
00851
00852 if(this->right_pose_.pose.position.x>this->left_pose_.pose.position.x){
00853 this->left_done=true;
00854 this->k=1;
00855 this->state=tracking_right;
00856 }else{
00857 this->k=-1;
00858 this->state=tracking_left;
00859 }
00860 }
00861 break;
00862
00863 case tracking_left: ROS_INFO("Current state: Tracking left marker");
00864 if(this->left_marker_found){
00865 ROS_WARN("Left marker found");
00866 move_joints_client_.cancelGoal();
00867 this->state=new_goal;
00868 }else{
00869 if(this->joints_moving){
00870 this->state=tracking_left;
00871 }else{
00872 this->move_joints_goal_.trajectory.header.seq=0;
00873 this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
00874 this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
00875 this->move_joints_goal_.trajectory.joint_names.resize(2);
00876 this->move_joints_goal_.trajectory.joint_names[0]="j_tilt";
00877 this->move_joints_goal_.trajectory.joint_names[1]="j_pan";
00878 this->move_joints_goal_.trajectory.points.resize(1);
00879 this->move_joints_goal_.trajectory.points[0].positions.resize(2);
00880 this->move_joints_goal_.trajectory.points[0].velocities.resize(2);
00881 this->move_joints_goal_.trajectory.points[0].accelerations.resize(2);
00882 this->move_joints_goal_.trajectory.points[0].positions[0]=15.0-10.0*this->n;
00883 if(this->n==0)
00884 this->move_joints_goal_.trajectory.points[0].positions[1]=60.0;
00885 else
00886 this->move_joints_goal_.trajectory.points[0].positions[1]=this->current_state.position[18]*180.0/3.14159;
00887 for(i=0;i<2;i++){
00888 this->move_joints_goal_.trajectory.points[0].velocities[i]=20.0;
00889 this->move_joints_goal_.trajectory.points[0].accelerations[i]=40.0;
00890 }
00891 this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
00892 move_jointsMakeActionRequest();
00893 if(this->n>2)
00894 this->state=move_back;
00895 else
00896 this->state=tracking_left2;
00897 }
00898 }
00899 break;
00900
00901 case tracking_left2: ROS_INFO("Current state: Tracking left marker");
00902 if(this->left_marker_found){
00903 ROS_WARN("Left marker found");
00904 move_joints_client_.cancelGoal();
00905 this->state=new_goal;
00906 }else{
00907 if(this->joints_moving)
00908 this->state=tracking_left2;
00909 else{
00910 this->ready=true;
00911 this->move_joints_goal_.trajectory.header.seq=0;
00912 this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
00913 this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
00914 this->move_joints_goal_.trajectory.joint_names.resize(1);
00915 this->move_joints_goal_.trajectory.joint_names[0]="j_pan";
00916 this->move_joints_goal_.trajectory.points.resize(1);
00917 this->move_joints_goal_.trajectory.points[0].positions.resize(1);
00918 this->move_joints_goal_.trajectory.points[0].velocities.resize(1);
00919 this->move_joints_goal_.trajectory.points[0].accelerations.resize(1);
00920 this->move_joints_goal_.trajectory.points[0].velocities[0]=20.0;
00921 this->move_joints_goal_.trajectory.points[0].accelerations[0]=40.0;
00922 if(this->k==-1){
00923 this->move_joints_goal_.trajectory.points[0].positions[0]=-60.0;
00924 this->k=1;
00925 }else{
00926 this->move_joints_goal_.trajectory.points[0].positions[0]=60.0;
00927 this->k=-1;
00928 }
00929 this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
00930 move_jointsMakeActionRequest();
00931 this->n++;
00932 this->state=tracking_left;
00933 }
00934 }
00935 break;
00936
00937 case tracking_right: ROS_INFO("Current state: Tracking right marker");
00938 if(this->right_marker_found){
00939 ROS_WARN("Right marker found");
00940 move_joints_client_.cancelGoal();
00941 this->state=new_goal;
00942 }else{
00943 if(this->joints_moving){
00944 this->state=tracking_right;
00945 }else{
00946 this->move_joints_goal_.trajectory.header.seq=0;
00947 this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
00948 this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
00949 this->move_joints_goal_.trajectory.joint_names.resize(2);
00950 this->move_joints_goal_.trajectory.joint_names[0]="j_tilt";
00951 this->move_joints_goal_.trajectory.joint_names[1]="j_pan";
00952 this->move_joints_goal_.trajectory.points.resize(1);
00953 this->move_joints_goal_.trajectory.points[0].positions.resize(2);
00954 this->move_joints_goal_.trajectory.points[0].velocities.resize(2);
00955 this->move_joints_goal_.trajectory.points[0].accelerations.resize(2);
00956 this->move_joints_goal_.trajectory.points[0].positions[0]=15.0-10.0*this->n;
00957 if(this->n==0)
00958 this->move_joints_goal_.trajectory.points[0].positions[1]=-60.0;
00959 else
00960 this->move_joints_goal_.trajectory.points[0].positions[1]=this->current_state.position[18]*180.0/3.14159;
00961 for(i=0;i<2;i++){
00962 this->move_joints_goal_.trajectory.points[0].velocities[i]=20.0;
00963 this->move_joints_goal_.trajectory.points[0].accelerations[i]=40.0;
00964 }
00965 this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
00966 move_jointsMakeActionRequest();
00967 if(this->n>2)
00968 this->state=move_back;
00969 else
00970 this->state=tracking_right2;
00971 }
00972 }
00973 break;
00974
00975 case tracking_right2: ROS_INFO("Current state: Tracking right marker");
00976 if(this->right_marker_found){
00977 ROS_WARN("Right marker found");
00978 move_joints_client_.cancelGoal();
00979 this->state=new_goal;
00980 }else{
00981 if(this->joints_moving)
00982 this->state=tracking_right2;
00983 else{
00984 this->ready=true;
00985 this->move_joints_goal_.trajectory.header.seq=0;
00986 this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
00987 this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
00988 this->move_joints_goal_.trajectory.joint_names.resize(1);
00989 this->move_joints_goal_.trajectory.joint_names[0]="j_pan";
00990 this->move_joints_goal_.trajectory.points.resize(1);
00991 this->move_joints_goal_.trajectory.points[0].positions.resize(1);
00992 this->move_joints_goal_.trajectory.points[0].velocities.resize(1);
00993 this->move_joints_goal_.trajectory.points[0].accelerations.resize(1);
00994 this->move_joints_goal_.trajectory.points[0].velocities[0]=20.0;
00995 this->move_joints_goal_.trajectory.points[0].accelerations[0]=40.0;
00996 if(this->k==-1){
00997 this->move_joints_goal_.trajectory.points[0].positions[0]=-60.0;
00998 this->k=1;
00999 }else{
01000 this->move_joints_goal_.trajectory.points[0].positions[0]=60.0;
01001 this->k=-1;
01002 }
01003 this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
01004 move_jointsMakeActionRequest();
01005 this->n++;
01006 this->state=tracking_right;
01007 }
01008 }
01009 break;
01010
01011 case go_forward: ROS_INFO("Current state: Go forward");
01012 this->Twist_msg_.linear.x=0.03;
01013 this->Twist_msg_.linear.y=0.0;
01014 this->Twist_msg_.linear.z=0.0;
01015 this->Twist_msg_.angular.x=0.0;
01016 this->Twist_msg_.angular.y=0.0;
01017 this->Twist_msg_.angular.z=0.4*this->current_state.position[18];
01018 this->cmd_vel_publisher_.publish(this->Twist_msg_);
01019 usleep(100000);
01020
01021 if((this->msg_.markers[0].pose.pose.position.z>0.0)&&(this->msg_.markers[0].pose.pose.position.z<=0.12)){
01022 tracking_head_client_.cancelGoal();
01023 this->first_goal=true;
01024 this->state=initial_pose;
01025 }else{
01026
01027 if(this->msg_.markers[0].pose.pose.position.z==0.0){
01028 tracking_head_client_.cancelGoal();
01029 this->first_goal=true;
01030 this->Twist_msg_.linear.x=0.0;
01031 this->Twist_msg_.angular.z=0.0;
01032 this->cmd_vel_publisher_.publish(this->Twist_msg_);
01033 this->state=correct_position;
01034 }else
01035 this->state=new_goal;
01036 }
01037 break;
01038
01039 case go_back: ROS_INFO("Current state: Go back");
01040 this->Twist_msg_.linear.x=-0.03;
01041 this->Twist_msg_.linear.y=0.0;
01042 this->Twist_msg_.linear.z=0.0;
01043 this->Twist_msg_.angular.x=0.0;
01044 this->Twist_msg_.angular.y=0.0;
01045 this->Twist_msg_.angular.z=0.4*this->current_state.position[18];
01046 this->cmd_vel_publisher_.publish(this->Twist_msg_);
01047 usleep(100000);
01048
01049 if(this->msg_.markers[0].pose.pose.position.z>0.09){
01050 tracking_head_client_.cancelGoal();
01051 this->first_goal=true;
01052 this->state=initial_pose;
01053 }else{
01054
01055 if(this->msg_.markers[0].pose.pose.position.z==0.0){
01056 tracking_head_client_.cancelGoal();
01057 this->first_goal=true;
01058 this->Twist_msg_.linear.x=0.0;
01059 this->Twist_msg_.angular.z=0.0;
01060 this->cmd_vel_publisher_.publish(this->Twist_msg_);
01061 this->state=correct_position;
01062 }else
01063 this->state=new_goal;
01064 }
01065 break;
01066
01067 case turn_right: ROS_INFO("Current state: Turn to the right");
01068 this->Twist_msg_.linear.x=0.0;
01069 this->Twist_msg_.linear.y=0.0;
01070 this->Twist_msg_.linear.z=0.0;
01071 this->Twist_msg_.angular.x=0.0;
01072 this->Twist_msg_.angular.y=0.0;
01073 this->Twist_msg_.angular.z=-0.65;
01074 this->cmd_vel_publisher_.publish(this->Twist_msg_);
01075 usleep(100000);
01076
01077 if((this->msg_.markers[0].pose.pose.position.z>0.0)&&(this->msg_.markers[0].pose.pose.position.z<=0.10)){
01078 tracking_head_client_.cancelGoal();
01079 this->first_goal=true;
01080 this->state=initial_pose;
01081 }else{
01082
01083 if(this->msg_.markers[0].pose.pose.position.z==0.0){
01084 tracking_head_client_.cancelGoal();
01085 this->first_goal=true;
01086 this->Twist_msg_.angular.z=0.0;
01087 this->cmd_vel_publisher_.publish(this->Twist_msg_);
01088 this->state=correct_position;
01089 }else
01090 this->state=new_goal;
01091 }
01092 break;
01093
01094 case turn_left: ROS_INFO("Current state: Turn to the left");
01095 this->Twist_msg_.linear.x=0.0;
01096 this->Twist_msg_.linear.y=0.0;
01097 this->Twist_msg_.linear.z=0.0;
01098 this->Twist_msg_.angular.x=0.0;
01099 this->Twist_msg_.angular.y=0.0;
01100 this->Twist_msg_.angular.z=0.65;
01101 this->cmd_vel_publisher_.publish(this->Twist_msg_);
01102 usleep(100000);
01103
01104 if((this->msg_.markers[0].pose.pose.position.z>0.0)&&(this->msg_.markers[0].pose.pose.position.z<=0.10)){
01105 tracking_head_client_.cancelGoal();
01106 this->first_goal=true;
01107 this->state=initial_pose;
01108 }else{
01109
01110 if(this->msg_.markers[0].pose.pose.position.z==0.0){
01111 tracking_head_client_.cancelGoal();
01112 this->first_goal=true;
01113 this->Twist_msg_.angular.z=0.0;
01114 this->cmd_vel_publisher_.publish(this->Twist_msg_);
01115 this->state=correct_position;
01116 }else
01117 this->state=new_goal;
01118 }
01119 break;
01120
01121 case go_right: ROS_INFO("Current state: Go to the right");
01122 this->Twist_msg_.linear.x=0.0;
01123 this->Twist_msg_.linear.y=-0.04;
01124 this->Twist_msg_.linear.z=0.0;
01125 this->Twist_msg_.angular.x=0.0;
01126 this->Twist_msg_.angular.y=0.0;
01127 this->Twist_msg_.angular.z=0.0;
01128 this->cmd_vel_publisher_.publish(this->Twist_msg_);
01129 usleep(400000);
01130
01131 if(fabs(this->current_state.position[18])<=0.52)
01132 this->state=turn_left;
01133 else{
01134
01135 if(this->msg_.markers[0].pose.pose.position.z==0.0){
01136 tracking_head_client_.cancelGoal();
01137 this->first_goal=true;
01138 this->Twist_msg_.linear.y=0.0;
01139 this->cmd_vel_publisher_.publish(this->Twist_msg_);
01140 this->state=correct_position;
01141 }else
01142 this->state=new_goal;
01143 }
01144 break;
01145
01146 case go_left: ROS_INFO("Current state: Go to the left");
01147 this->Twist_msg_.linear.x=0.0;
01148 this->Twist_msg_.linear.y=0.04;
01149 this->Twist_msg_.linear.z=0.0;
01150 this->Twist_msg_.angular.x=0.0;
01151 this->Twist_msg_.angular.y=0.0;
01152 this->Twist_msg_.angular.z=0.0;
01153 this->cmd_vel_publisher_.publish(this->Twist_msg_);
01154 usleep(400000);
01155
01156 if(fabs(this->current_state.position[18])<=0.52)
01157 this->state=turn_right;
01158 else{
01159
01160 if(this->msg_.markers[0].pose.pose.position.z==0.0){
01161 tracking_head_client_.cancelGoal();
01162 this->first_goal=true;
01163 this->Twist_msg_.linear.y=0.0;
01164 this->cmd_vel_publisher_.publish(this->Twist_msg_);
01165 this->state=correct_position;
01166 }else
01167 this->state=new_goal;
01168 }
01169 break;
01170 }
01171 }catch(...){
01172 ROS_INFO("exception!!!!!!!!!!!!!!!!!!!!!1");
01173 }
01174 }
01175
01176
01177 void DarwinAutochargeAlgNode::adc_channels_callback(const std_msgs::Float64MultiArray::ConstPtr& msg)
01178 {
01179
01180
01181
01182
01183 this->adc_channels_mutex_.enter();
01184 this->current_voltage=msg->data[2];
01185
01186
01187
01188
01189 this->adc_channels_mutex_.exit();
01190 }
01191 void DarwinAutochargeAlgNode::joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg)
01192 {
01193
01194
01195
01196
01197 this->joint_states_mutex_.enter();
01198 this->current_state=(*msg);
01199
01200
01201
01202 this->joint_states_mutex_.exit();
01203 }
01204 void DarwinAutochargeAlgNode::marker_callback(const ar_pose::ARMarkers::ConstPtr& msg)
01205 {
01206 bool tf_exists;
01207 int x,y,z;
01208
01209
01210
01211
01212
01213 if((this->ready)&&(!this->ready_for_check)){
01214 this->marker_mutex_.enter();
01215 this->msg_ = *msg;
01216 this->msg_.markers.resize(1);
01217
01218 if(this->msg_.markers[0].pose.pose.position.z<0.0){
01219 this->msg_.markers[0].pose.pose.position.x=-this->msg_.markers[0].pose.pose.position.x;
01220 this->msg_.markers[0].pose.pose.position.y=-this->msg_.markers[0].pose.pose.position.y;
01221 this->msg_.markers[0].pose.pose.position.z=-this->msg_.markers[0].pose.pose.position.z;
01222 }
01223 ROS_INFO("marker dist x, y, z; id: %f, %f, %f, %d", this->msg_.markers[0].pose.pose.position.x, this->msg_.markers[0].pose.pose.position.y, this->msg_.markers[0].pose.pose.position.z, this->msg_.markers[0].id);
01224
01225 if(this->msg_.markers[0].pose.pose.position.z>0.0){
01226 this->marker_found=true;
01227
01228 if(this->m>2){
01229 this->marker_really_founded=true;
01230 this->conv_msg_.header=this->msg_.markers[0].header;
01231 this->conv_msg_.pose.position=this->msg_.markers[0].pose.pose.position;
01232 this->conv_msg_.pose.orientation=this->msg_.markers[0].pose.pose.orientation;
01233 try{
01234 this->conv_msg_.header.stamp=ros::Time::now();
01235
01236 tf_exists=tf_listener.waitForTransform(this->conv_msg_.header.frame_id,std::string("/MP_HEAD"),this->conv_msg_.header.stamp, ros::Duration(10), ros::Duration(0.01));
01237 tf_listener.transformPose(std::string("/MP_HEAD"),this->conv_msg_,this->marker_);
01238 }catch(tf::TransformException ex){
01239 ROS_ERROR("NoCollisionAlgNode:: %s",ex.what());
01240 }catch(...){
01241 ROS_INFO("exception!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
01242 }
01243
01244 if((this->msg_.markers[0].id==0)&&(!this->left_done)){
01245 this->left_marker_found=true;
01246 this->current_pan=this->current_state.position[18]*180.0/3.14159;
01247 this->current_tilt=this->current_state.position[19]*180.0/3.14159;
01248 if(this->left_ready_tf){
01249 try{
01250
01251 this->marker_.header.stamp=ros::Time::now();
01252 tf_exists=tf_listener.waitForTransform(this->marker_.header.frame_id,std::string("/MP_BODY"),this->marker_.header.stamp, ros::Duration(10), ros::Duration(0.01));
01253 tf_listener.transformPose(std::string("/MP_BODY"),this->marker_,this->left_pose_);
01254
01255 x=1000*this->left_pose_.pose.position.x;
01256 y=1000*this->left_pose_.pose.position.y;
01257 z=1000*this->left_pose_.pose.position.z;
01258 this->left_pose_.pose.position.x=x/1000.0;
01259 this->left_pose_.pose.position.y=y/1000.0;
01260 this->left_pose_.pose.position.z=z/1000.0;
01261
01262 ROS_WARN_STREAM(this->left_pose_);
01263
01264 this->left_done=true;
01265 this->left_data_ready=true;
01266 this->left_marker_found=false;
01267 }catch(tf::TransformException ex){
01268 ROS_ERROR("NoCollisionAlgNode:: %s",ex.what());
01269 }catch(...){
01270 ROS_INFO("exception!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
01271 }
01272 }
01273 }
01274
01275 if((this->msg_.markers[0].id==1)&&(this->left_done)){
01276 this->right_marker_found=true;
01277 this->current_pan=this->current_state.position[18]*180.0/3.14159;
01278 this->current_tilt=this->current_state.position[19]*180.0/3.14159;
01279 if(this->right_ready_tf){
01280 try{
01281
01282 this->marker_.header.stamp=ros::Time::now();
01283 tf_exists=tf_listener.waitForTransform(this->marker_.header.frame_id,std::string("/MP_BODY"),this->marker_.header.stamp, ros::Duration(10), ros::Duration(0.01));
01284 tf_listener.transformPose(std::string("/MP_BODY"),this->marker_,this->right_pose_);
01285
01286 x=1000*this->right_pose_.pose.position.x;
01287 y=1000*this->right_pose_.pose.position.y;
01288 z=1000*this->right_pose_.pose.position.z;
01289 this->right_pose_.pose.position.x=x/1000.0;
01290 this->right_pose_.pose.position.y=y/1000.0;
01291 this->right_pose_.pose.position.z=z/1000.0;
01292
01293 ROS_WARN_STREAM(this->right_pose_);
01294
01295 this->left_done=false;
01296 this->right_data_ready=true;
01297 this->right_marker_found=false;
01298 }catch(tf::TransformException ex){
01299 ROS_ERROR("NoCollisionAlgNode:: %s",ex.what());
01300 }catch(...){
01301 ROS_INFO("exception!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
01302 }
01303 }
01304 }
01305 this->m=0;
01306 }
01307 this->m++;
01308 }
01309
01310
01311 this->marker_mutex_.exit();
01312 }
01313 }
01314
01315
01316
01317
01318 void DarwinAutochargeAlgNode::move_left_armDone(const actionlib::SimpleClientGoalState& state, const arm_navigation_msgs::MoveArmResultConstPtr& result)
01319 {
01320 if( state.toString().compare("SUCCEEDED") == 0 )
01321 ROS_INFO("DarwinAutochargeAlgNode::move_left_armDone: Goal Achieved!");
01322 else
01323 ROS_INFO("DarwinAutochargeAlgNode::move_left_armDone: %s", state.toString().c_str());
01324
01325
01326 this->left_arm_moving=false;
01327 }
01328
01329 void DarwinAutochargeAlgNode::move_left_armActive()
01330 {
01331
01332 }
01333
01334 void DarwinAutochargeAlgNode::move_left_armFeedback(const arm_navigation_msgs::MoveArmFeedbackConstPtr& feedback)
01335 {
01336
01337
01338 bool feedback_is_ok = true;
01339
01340
01341
01342
01343
01344 if( !feedback_is_ok )
01345 {
01346 move_left_arm_client_.cancelGoal();
01347
01348 }
01349 }
01350 void DarwinAutochargeAlgNode::move_right_armDone(const actionlib::SimpleClientGoalState& state, const arm_navigation_msgs::MoveArmResultConstPtr& result)
01351 {
01352 if( state.toString().compare("SUCCEEDED") == 0 )
01353 ROS_INFO("DarwinAutochargeAlgNode::move_right_armDone: Goal Achieved!");
01354 else
01355 ROS_INFO("DarwinAutochargeAlgNode::move_right_armDone: %s", state.toString().c_str());
01356
01357
01358 this->right_arm_moving=false;
01359 }
01360
01361 void DarwinAutochargeAlgNode::move_right_armActive()
01362 {
01363
01364 }
01365
01366 void DarwinAutochargeAlgNode::move_right_armFeedback(const arm_navigation_msgs::MoveArmFeedbackConstPtr& feedback)
01367 {
01368
01369
01370 bool feedback_is_ok = true;
01371
01372
01373
01374
01375
01376 if( !feedback_is_ok )
01377 {
01378 move_right_arm_client_.cancelGoal();
01379
01380 }
01381 }
01382 void DarwinAutochargeAlgNode::tracking_headDone(const actionlib::SimpleClientGoalState& state, const iri_darwin_robot::tracking_headResultConstPtr& result)
01383 {
01384 if( state.toString().compare("SUCCEEDED") == 0 )
01385 ROS_INFO("DarwinAutochargeAlgNode::tracking_headDone: Goal Achieved!");
01386 else
01387 ROS_INFO("DarwinAutochargeAlgNode::tracking_headDone: %s", state.toString().c_str());
01388 this->head_is_tracking=false;
01389
01390 }
01391
01392 void DarwinAutochargeAlgNode::tracking_headActive()
01393 {
01394
01395 }
01396
01397 void DarwinAutochargeAlgNode::tracking_headFeedback(const iri_darwin_robot::tracking_headFeedbackConstPtr& feedback)
01398 {
01399
01400
01401 bool feedback_is_ok = true;
01402
01403
01404
01405 if( !feedback_is_ok )
01406 {
01407 tracking_head_client_.cancelGoal();
01408
01409 }
01410 }
01411 void DarwinAutochargeAlgNode::move_jointsDone(const actionlib::SimpleClientGoalState& state, const control_msgs::FollowJointTrajectoryResultConstPtr& result)
01412 {
01413 if( state.toString().compare("SUCCEEDED") == 0 )
01414 ROS_INFO("DarwinAutochargeAlgNode::move_jointsDone: Goal Achieved!");
01415 else
01416 ROS_INFO("DarwinAutochargeAlgNode::move_jointsDone: %s", state.toString().c_str());
01417
01418
01419 this->joints_moving=false;
01420 }
01421
01422 void DarwinAutochargeAlgNode::move_jointsActive()
01423 {
01424
01425 }
01426
01427 void DarwinAutochargeAlgNode::move_jointsFeedback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr& feedback)
01428 {
01429
01430
01431 bool feedback_is_ok = true;
01432
01433
01434
01435
01436
01437 if( !feedback_is_ok )
01438 {
01439 move_joints_client_.cancelGoal();
01440
01441 }
01442 }
01443
01444
01445 void DarwinAutochargeAlgNode::move_left_armMakeActionRequest()
01446 {
01447 ROS_INFO("DarwinAutochargeAlgNode::move_left_armMakeActionRequest: Starting New Request!");
01448
01449
01450
01451 move_left_arm_client_.waitForServer();
01452 ROS_INFO("DarwinAutochargeAlgNode::move_left_armMakeActionRequest: Server is Available!");
01453
01454
01455
01456 move_left_arm_client_.sendGoal(move_left_arm_goal_,
01457 boost::bind(&DarwinAutochargeAlgNode::move_left_armDone, this, _1, _2),
01458 boost::bind(&DarwinAutochargeAlgNode::move_left_armActive, this),
01459 boost::bind(&DarwinAutochargeAlgNode::move_left_armFeedback, this, _1));
01460 ROS_INFO("DarwinAutochargeAlgNode::move_left_armMakeActionRequest: Goal Sent. Wait for Result!");
01461 this->left_arm_moving=true;
01462 }
01463 void DarwinAutochargeAlgNode::move_right_armMakeActionRequest()
01464 {
01465 ROS_INFO("DarwinAutochargeAlgNode::move_right_armMakeActionRequest: Starting New Request!");
01466
01467
01468
01469 move_right_arm_client_.waitForServer();
01470 ROS_INFO("DarwinAutochargeAlgNode::move_right_armMakeActionRequest: Server is Available!");
01471
01472
01473
01474 move_right_arm_client_.sendGoal(move_right_arm_goal_,
01475 boost::bind(&DarwinAutochargeAlgNode::move_right_armDone, this, _1, _2),
01476 boost::bind(&DarwinAutochargeAlgNode::move_right_armActive, this),
01477 boost::bind(&DarwinAutochargeAlgNode::move_right_armFeedback, this, _1));
01478 ROS_INFO("DarwinAutochargeAlgNode::move_right_armMakeActionRequest: Goal Sent. Wait for Result!");
01479 this->right_arm_moving=true;
01480 }
01481 void DarwinAutochargeAlgNode::tracking_headMakeActionRequest()
01482 {
01483 ROS_INFO("DarwinAutochargeAlgNode::tracking_headMakeActionRequest: Starting New Request!");
01484
01485
01486
01487 tracking_head_client_.waitForServer();
01488 ROS_INFO("DarwinAutochargeAlgNode::tracking_headMakeActionRequest: Server is Available!");
01489
01490
01491
01492 tracking_head_client_.sendGoal(tracking_head_goal_,
01493 boost::bind(&DarwinAutochargeAlgNode::tracking_headDone, this, _1, _2),
01494 boost::bind(&DarwinAutochargeAlgNode::tracking_headActive, this),
01495 boost::bind(&DarwinAutochargeAlgNode::tracking_headFeedback, this, _1));
01496 ROS_INFO("DarwinAutochargeAlgNode::tracking_headMakeActionRequest: Goal Sent. Wait for Result!");
01497 this->head_is_tracking=true;
01498 }
01499 void DarwinAutochargeAlgNode::move_jointsMakeActionRequest()
01500 {
01501 ROS_INFO("DarwinAutochargeAlgNode::move_jointsMakeActionRequest: Starting New Request!");
01502
01503
01504
01505 move_joints_client_.waitForServer();
01506 ROS_INFO("DarwinAutochargeAlgNode::move_jointsMakeActionRequest: Server is Available!");
01507
01508
01509
01510 move_joints_client_.sendGoal(move_joints_goal_,
01511 boost::bind(&DarwinAutochargeAlgNode::move_jointsDone, this, _1, _2),
01512 boost::bind(&DarwinAutochargeAlgNode::move_jointsActive, this),
01513 boost::bind(&DarwinAutochargeAlgNode::move_jointsFeedback, this, _1));
01514 ROS_INFO("DarwinAutochargeAlgNode::move_jointsMakeActionRequest: Goal Sent. Wait for Result!");
01515 this->joints_moving=true;
01516 }
01517
01518 void DarwinAutochargeAlgNode::node_config_update(Config &config, uint32_t level)
01519 {
01520 this->alg_.lock();
01521
01522 this->alg_.unlock();
01523 }
01524
01525 void DarwinAutochargeAlgNode::addNodeDiagnostics(void)
01526 {
01527 }
01528
01529
01530 int main(int argc,char *argv[])
01531 {
01532 return algorithm_base::main<DarwinAutochargeAlgNode>(argc, argv, "darwin_autocharge_alg_node");
01533 }