arm_manip_services.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id:$
00005  *
00006  * Copyright (C) Brno University of Technology
00007  *
00008  * This file is part of software developed by dcgm-robotics@FIT group.
00009  *
00010  * Author: Zdenek Materna (imaterna@fit.vutbr.cz)
00011  * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
00012  * Date: dd/mm/2012
00013  *
00014  * This file is free software: you can redistribute it and/or modify
00015  * it under the terms of the GNU Lesser General Public License as published by
00016  * the Free Software Foundation, either version 3 of the License, or
00017  * (at your option) any later version.
00018  *
00019  * This file is distributed in the hope that it will be useful,
00020  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00021  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00022  * GNU Lesser General Public License for more details.
00023  *
00024  * You should have received a copy of the GNU Lesser General Public License
00025  * along with this file.  If not, see <http://www.gnu.org/licenses/>.
00026  */
00027 
00028 #include <srs_assisted_arm_navigation/assisted_arm_navigation/arm_manip_node.h>
00029 
00030 using namespace planning_scene_utils;
00031 using namespace srs_assisted_arm_navigation;
00032 using namespace srs_assisted_arm_navigation_msgs;
00033 
00034 
00035 bool CArmManipulationEditor::ArmNavNew(ArmNavNew::Request &req, ArmNavNew::Response &res) {
00036 
00037 
00038         if (planning_scene_id == "") {
00039 
00040                 planning_scene_id = createNewPlanningScene();
00041 
00042                 setCurrentPlanningScene(planning_scene_id,true,true);
00043 
00044                 ROS_INFO("Created a new planning scene: %s", planning_scene_id.c_str());
00045 
00046         }
00047 
00048     createMotionPlanRequest(*getRobotState(), // start state
00049                            *getRobotState(), // end state
00050                            params_.left_arm_group_, // group_name
00051                            params_.left_ik_link_, // end effector name
00052                            planning_scene_utils::getPlanningSceneIdFromName(planning_scene_id), // planning_scene_name
00053                            true, // start from robot state
00054                            mpr_id); // motion plan id out
00055 
00056 
00057    ROS_INFO("Created a new MPR: %d", mpr_id);
00058 
00059    MotionPlanRequestData& data = motion_plan_map_[getMotionPlanRequestNameFromId(mpr_id)];
00060 
00061    if (data.getEndEffectorLink() != end_eff_link_) {
00062 
00063            ROS_ERROR("End effector configuration mismatch. Setting end_eff_link to be %s",data.getEndEffectorLink().c_str());
00064 
00065            end_eff_link_ = data.getEndEffectorLink();
00066 
00067    }
00068 
00069    planning_models::KinematicState *robot_state = getRobotState();
00070    planning_models::KinematicState::JointStateGroup* jsg = robot_state->getJointStateGroup(data.getGroupName());
00071    if ( !(robot_state->areJointsWithinBounds(jsg->getJointNames())) ) {
00072 
00073            // TODO print which one !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
00074            //ROS_FATAL("Some joint is out of limits! Can't start planning.");
00075 
00076            arm_nav_state_.out_of_limits = true;
00077 
00078            std::vector<std::string> joints = jsg->getJointNames();
00079 
00080            for (unsigned int i=0; i < joints.size(); i++) {
00081 
00082                    if ( !robot_state->isJointWithinBounds(joints[i]) ) {
00083 
00084                            ROS_FATAL("Joint %s is out of limits! Can't start planning.",joints[i].c_str());
00085 
00086                    }
00087 
00088            }
00089 
00090            reset();
00091 
00092            res.error = "Some joint is out of limits.";
00093            res.completed = false;
00094            return true;
00095 
00096    } else arm_nav_state_.out_of_limits = false;
00097 
00098 
00099    ROS_INFO("Performing planning for %s end effector link.",end_eff_link_.c_str());
00100 
00101    data.setStartVisible(false);
00102    data.setEndVisible(true);
00103    data.setJointControlsVisible(joint_controls_,this);
00104 
00105    std_msgs::ColorRGBA goal_color;
00106 
00107    goal_color.r = 0.0;
00108    goal_color.g = 1.0;
00109    goal_color.b = 0.0;
00110    goal_color.a = 0.6;
00111 
00112    data.setGoalColor(goal_color);
00113 
00114    #define TR 0.1
00115 
00116    if (aco_) {
00117 
00118      ROS_INFO("Adding attached collision object to planning scene");
00119      coll_obj_attached_id.push_back(add_coll_obj_attached(0,0,0.25,0.2,0.15));
00120 
00121    }
00122 
00124    /*coll_obj_id.push_back(add_coll_obj(TR,TR,0.15,0.05,0.25));
00125    coll_obj_id.push_back(add_coll_obj(TR,-TR,0.15,0.05,0.25));
00126    coll_obj_id.push_back(add_coll_obj(-TR,TR,0.2));
00127    coll_obj_id.push_back(add_coll_obj(-TR,-TR,0.2));*/
00128 
00130    std::vector<t_det_obj>::iterator tmp;
00131 
00132     for(tmp = coll_obj_det.begin(); tmp != coll_obj_det.end(); tmp++) {
00133 
00134       (*tmp).id = add_coll_obj_bb(*tmp);
00135 
00136       }
00137 
00138    res.completed = true;
00139    res.error = "";
00140 
00142    if (action_server_ptr_!=NULL)
00143      action_server_ptr_->srv_set_state(ManualArmManipActionServer::S_NEW);
00144 
00145 
00146    GripperPosesClean();
00147 
00148    if (ros::service::exists("sn_teleop_srv_dis",true)) {
00149 
00150            std_srvs::Empty srv;
00151            ros::service::call("sn_teleop_srv_dis",srv);
00152 
00153    }
00154 
00155    inited = true;
00156    disable_gripper_poses_ = false;
00157 
00158   return true;
00159 }
00160 
00161 bool CArmManipulationEditor::ArmNavPlan(ArmNavPlan::Request &req, ArmNavPlan::Response &res) {
00162 
00163   planned_ = false;
00164 
00165   MotionPlanRequestData& data = motion_plan_map_[getMotionPlanRequestNameFromId(mpr_id)];
00166 
00167   visualization_msgs::MarkerArray arr = data.getCollisionMarkers();
00168 
00169   if (arr.markers.size() != 0) {
00170 
00171           ROS_ERROR("Goal position is in collision. Can't plan.");
00172           res.completed = false;
00173           res.error = "Goal position is in collision.";
00174           return true;
00175 
00176   }
00177 
00178 
00179   if (!data.hasGoodIKSolution(planning_scene_utils::GoalPosition)) {
00180 
00181           ROS_ERROR("There is no IK solution for goal position. Can't plan.");
00182           res.completed = false;
00183           res.error = "Goal position is not reachable.";
00184           return true;
00185 
00186   }
00187 
00188 
00189   ROS_DEBUG("Planning trajectory...");
00190 
00191   //MotionPlanRequestData& data = motion_plan_map_[getMotionPlanRequestNameFromId(mpr_id)];
00192 
00193   if (planToRequest(data,traj_id)) {
00194 
00195         TrajectoryData& trajectory = trajectory_map_[getMotionPlanRequestNameFromId(mpr_id)][getTrajectoryNameFromId(traj_id)];
00196 
00197         if (trajectory.getTrajectorySize() < 3) {
00198 
00199                 ROS_ERROR("Strange trajectory with only %d points.",(int)trajectory.getTrajectorySize());
00200                 res.completed = false;
00201             res.error = "Planning failed.";
00202             return true;
00203 
00204         }
00205 
00206     ROS_INFO("Trajectory successfully planned");
00207 
00208     unsigned int filterID;
00209 
00210 
00211     if (filterTrajectory(data,trajectory,filterID)) {
00212 
00213       ROS_INFO("Successfully filtered, playing started");
00214 
00215       filt_traj_id = filterID;
00216 
00217       TrajectoryData& f_trajectory = trajectory_map_[getMotionPlanRequestNameFromId(mpr_id)][getTrajectoryNameFromId(filt_traj_id)];
00218       playTrajectory(data,f_trajectory);
00219 
00220       res.completed = true;
00221 
00222       //boost::mutex::scoped_lock(im_server_mutex_);
00223       lockScene();
00224 
00225       planned_ = true;
00226       disable_gripper_poses_ = true;
00227 
00228       //data.setGoalEditable(false);
00229       //data.hideGoal();
00230 
00231       if (!interactive_marker_server_->erase("MPR 0_end_control")) {
00232 
00233         ROS_WARN("Cannot remove gripper IM.");
00234 
00235       } else interactive_marker_server_->applyChanges();
00236 
00237       unlockScene();
00238 
00239     } else {
00240 
00241 
00242       res.completed = false;
00243       res.error = "Error on filtering";
00244       return true;
00245 
00246 
00247     }
00248 
00249 
00250   } else {
00251 
00252 
00253     res.completed = false;
00254     res.error = "Error on planning";
00255     return false;
00256 
00257   }
00258 
00259   if (action_server_ptr_!=NULL)
00260      action_server_ptr_->srv_set_state(ManualArmManipActionServer::S_PLAN);
00261 
00262   return true;
00263 }
00264 
00265 bool CArmManipulationEditor::ArmNavPlay(ArmNavPlay::Request &req, ArmNavPlay::Response &res) {
00266 
00267   if (!planned_) {
00268 
00269     ROS_ERROR("Nothing to play!");
00270     res.completed = false;
00271     return false;
00272 
00273   }
00274 
00275   ROS_INFO("Playing trajectory...");
00276 
00277   MotionPlanRequestData& data = motion_plan_map_[getMotionPlanRequestNameFromId(mpr_id)];
00278   TrajectoryData& f_trajectory = trajectory_map_[getMotionPlanRequestNameFromId(mpr_id)][getTrajectoryNameFromId(filt_traj_id)];
00279 
00280   playTrajectory(data,f_trajectory);
00281 
00282   res.completed = true;
00283 
00284   return true;
00285 }
00286 
00287 bool CArmManipulationEditor::ArmNavExecute(ArmNavExecute::Request &req, ArmNavExecute::Response &res) {
00288 
00289   if (!planned_) {
00290 
00291     ROS_ERROR("Nothing to execute!");
00292     res.completed = false;
00293     reset();
00294     return false;
00295 
00296   }
00297 
00298   ROS_INFO("Executing trajectory...");
00299 
00300   executeTrajectory(getMotionPlanRequestNameFromId(mpr_id),getTrajectoryNameFromId(filt_traj_id));
00301   
00302   ROS_INFO("Trajectory was sent...");
00303 
00304   reset();
00305   
00306   ROS_INFO("Reset of stuff after executing trajectory.");
00307 
00308   ros::Rate r(10);
00309 
00310   // TODO put some timeout there !!!!
00311   while (monitor_status_ == Executing) r.sleep();
00312 
00313   ROS_INFO("Trajectory should be executed.");
00314 
00315   res.completed = true;
00316 
00318 
00319   if (action_server_ptr_!=NULL)
00320     action_server_ptr_->srv_set_state(ManualArmManipActionServer::S_EXECUTE);
00321 
00322   return true;
00323 }
00324 
00325 bool CArmManipulationEditor::ArmNavReset(ArmNavReset::Request &req, ArmNavReset::Response &res) {
00326 
00327   reset();
00328 
00329   res.completed = true;
00330 
00331   if (action_server_ptr_!=NULL)
00332       action_server_ptr_->srv_set_state(ManualArmManipActionServer::S_RESET);
00333 
00334   return true;
00335 }
00336 
00337 bool CArmManipulationEditor::ArmNavRemoveCollObjects(ArmNavRemoveCollObjects::Request &req, ArmNavRemoveCollObjects::Response &res) {
00338 
00339         std::vector<CollisionObject> objects;
00340 
00341         for (unsigned int i=0; i < coll_obj_det.size(); i++) {
00342 
00343                 CollisionObject tmp;
00344 
00345                 tmp.allow_collision = coll_obj_det[i].allow_collision;
00346                 tmp.allow_pregrasps = coll_obj_det[i].allow_pregrasps;
00347                 tmp.attached = coll_obj_det[i].attached;
00348                 tmp.bb_lwh = coll_obj_det[i].bb_lwh;
00349                 tmp.id = coll_obj_det[i].id;
00350                 tmp.name = coll_obj_det[i].name;
00351                 tmp.pose = coll_obj_det[i].pose;
00352 
00353                 objects.push_back(tmp);
00354 
00355         }
00356 
00357         res.objects = objects;
00358 
00359         remove_coll_objects();
00360         return true;
00361 
00362 }
00363 
00364 bool CArmManipulationEditor::ArmNavSuccess(ArmNavSuccess::Request &req, ArmNavSuccess::Response &res) {
00365 
00366   reset();
00367 
00368   //remove_coll_objects();
00369 
00370   if (action_server_ptr_!=NULL)
00371       action_server_ptr_->srv_set_state(ManualArmManipActionServer::S_SUCCESS);
00372 
00373   return true;
00374 }
00375 
00376 bool CArmManipulationEditor::ArmNavFailed(ArmNavFailed::Request &req, ArmNavFailed::Response &res) {
00377 
00378   reset();
00379 
00380   //remove_coll_objects();
00381 
00382   if (action_server_ptr_!=NULL)
00383       action_server_ptr_->srv_set_state(ManualArmManipActionServer::S_FAILED);
00384 
00385   return true;
00386 }
00387 
00388 bool CArmManipulationEditor::ArmNavRepeat(ArmNavRepeat::Request &req, ArmNavRepeat::Response &res) {
00389 
00390   reset();
00391 
00392   //remove_coll_objects();
00393 
00394   if (action_server_ptr_!=NULL)
00395       action_server_ptr_->srv_set_state(ManualArmManipActionServer::S_REPEAT);
00396 
00397   return true;
00398 }
00399 
00400 bool CArmManipulationEditor::ArmNavRefresh(ArmNavRefresh::Request &req, ArmNavRefresh::Response &res) {
00401 
00402   if (refresh()) {
00403 
00404     res.completed = true;
00405     ROS_INFO("Refreshing planning scene");
00406     return true;
00407 
00408   } else {
00409 
00410     res.completed = false;
00411     ROS_ERROR("Error on refreshing planning scene");
00412     return false;
00413 
00414   }
00415 
00416 }
00417 
00418 bool CArmManipulationEditor::ArmNavSetAttached(ArmNavSetAttached::Request &req, ArmNavSetAttached::Response &res) {
00419 
00420 
00421         unsigned int idx = 0;
00422         bool f = false;
00423 
00424         for (unsigned int i=0; i < coll_obj_det.size(); i++) {
00425 
00426                 if (coll_obj_det[i].name == req.object_name) {
00427 
00428                         idx = i;
00429                         f = true;
00430                         break;
00431                 }
00432 
00433 
00434         }
00435 
00436         if (!f) {
00437 
00438                 ROS_ERROR("Collision object %s does not exist.",req.object_name.c_str());
00439                 res.completed = false;
00440                 return true;
00441 
00442         } else {
00443 
00444                 if (req.attached) ROS_INFO("Setting %s object to be attached.",req.object_name.c_str());
00445                 else ROS_INFO("Setting %s object to NOT be attached.",req.object_name.c_str());
00446 
00447                 //MotionPlanRequestData& data = motion_plan_map_[getMotionPlanRequestNameFromId(mpr_id)];
00448                 std::string target = end_eff_link_;
00449 
00450                 if (req.attached) {
00451 
00452                         // attaching -> we are going to transform object to end effector link
00453 
00454                         coll_obj_det[idx].pose.header.stamp = ros::Time::now();
00455 
00456                         if (!transf(target,coll_obj_det[idx].pose)) {
00457 
00458                                 ROS_ERROR("Failed to attach collision object (TF error).");
00459                                 res.completed = false;
00460                                 return true;
00461 
00462                         }
00463                         
00464                         // once attached, we will remove pregrasps
00465                         coll_obj_det[idx].allow_pregrasps = false;
00466 
00467                 } else {
00468 
00469                         coll_obj_det[idx].pose.header.stamp = ros::Time::now();
00470 
00471                         // dis-attaching -> transform it back to world frame id
00472                         if (coll_obj_det[idx].pose.header.frame_id != world_frame_) {
00473 
00474                                 if (!transf(world_frame_,coll_obj_det[idx].pose)) {
00475 
00476                                         ROS_ERROR("Failed to dis-attach collision object (TF error).");
00477                                         res.completed = false;
00478                                         return true;
00479 
00480                                 }
00481 
00482 
00483                         }
00484 
00485                 }
00486 
00487                 coll_obj_det[idx].attached = req.attached;
00488 
00489                 res.completed = true;
00490                 return true;
00491 
00492         }
00493 
00494 
00495 }
00496 
00497 
00504 bool CArmManipulationEditor::ArmNavCollObj(ArmNavCollObj::Request &req, ArmNavCollObj::Response &res) {
00505 
00506   ROS_INFO("Trying to add collision object name: %s",req.object_name.c_str());
00507 
00508   geometry_msgs::PoseStamped opose = req.pose;
00509 
00510   if (checkPose(opose,"/map")) {
00511 
00512           ROS_INFO("Ok, object pose is in /map coord. system. Lets store it.");
00513 
00514 
00515   } else {
00516 
00517         ROS_INFO("Object is not in map frame, lets transform it first.");
00518 
00519         
00520         if (!transf("/map",opose)) {
00521 
00522                 ROS_ERROR("Error on transforming collision object.");
00523                 return false;
00524 
00525         };
00526 
00527 }
00528 
00529   t_det_obj obj;
00530 
00531   obj.name = req.object_name;
00532   obj.bb_lwh = req.bb_lwh;
00533   obj.pose = opose;
00534   obj.attached = false;
00535 
00536   obj.allow_collision = req.allow_collision;
00537   obj.allow_pregrasps = req.allow_pregrasps;
00538 
00539   coll_obj_det.push_back(obj);
00540 
00541   return true;
00542 
00543 }
00544 
00545 bool CArmManipulationEditor::ArmNavMovePalmLink(ArmNavMovePalmLink::Request &req, ArmNavMovePalmLink::Response &res) {
00546 
00547 
00548   if (inited) {
00549 
00550         geometry_msgs::PoseStamped ps(req.sdh_palm_link_pose);
00551 
00552         //if (ps.header.frame_id != collision_objects_frame_id_) ROS_INFO("Setting position of end eff. to x: %f, y: %f, z: %f (in %s frame)",ps.pose.position.x,ps.pose.position.y,ps.pose.position.z,collision_objects_frame_id_.c_str());
00553 
00554         if (!checkPose(ps,collision_objects_frame_id_)) return false;
00555 
00556         ROS_INFO("Setting position of end eff. to x: %f, y: %f, z: %f (in %s frame)",ps.pose.position.x,ps.pose.position.y,ps.pose.position.z,ps.header.frame_id.c_str());
00557 
00558     //boost::mutex::scoped_lock(im_server_mutex_);
00559 
00560 
00561     /*std_msgs::Header h;
00562 
00563     h.frame_id = "map";
00564     h.stamp = ros::Time::now();*/
00565 
00566     lockScene();
00567 
00568     if (interactive_marker_server_->setPose("MPR 0_end_control",ps.pose/*,h*/)) {
00569 
00570       interactive_marker_server_->applyChanges();
00571 
00572       unlockScene();
00573 
00574       if (!findIK(ps.pose)) {
00575 
00576           ros::Duration(0.25).sleep();
00577 
00578           // try it for second time... just for sure...
00579           findIK(ps.pose);
00580 
00581       }
00582 
00583       res.completed = true;
00584       return true;
00585 
00586     } else unlockScene();
00587 
00588 
00589     res.completed = false;
00590     return true;
00591 
00592   } else {
00593 
00594     std::string str = SRV_NEW;
00595 
00596     ROS_ERROR("Cannot move arm to specified position. Call %s service first.",str.c_str());
00597     res.completed = false;
00598     reset();
00599     return false;
00600 
00601   }
00602 
00603 }
00604 
00605 bool CArmManipulationEditor::ArmNavMovePalmLinkRel(ArmNavMovePalmLinkRel::Request &req, ArmNavMovePalmLinkRel::Response &res) {
00606 
00607   //ROS_INFO("Lets try to move arm IMs little bit... :) Relatively...");
00608 
00609   if (!inited) {
00610 
00611     std::string str = SRV_NEW;
00612 
00613     ROS_ERROR("Cannot move arm to specified position. Call %s service first.",str.c_str());
00614     res.completed = false;
00615     reset();
00616     return false;
00617 
00618   }
00619 
00620   //boost::mutex::scoped_lock(im_server_mutex_);
00621 
00622   lockScene();
00623 
00624   visualization_msgs::InteractiveMarker marker;
00625   geometry_msgs::Pose new_pose;
00626 
00627   if (!(interactive_marker_server_->get("MPR 0_end_control",marker))) {
00628 
00629         unlockScene();
00630 
00631     ROS_ERROR("CanĀ“t get gripper IM pose.");
00632     res.completed = false;
00633     reset();
00634     return false;
00635   }
00636 
00637   new_pose = marker.pose;
00638 
00639   //new_pose.position = req.relative_movement.position;
00640   new_pose.position.x += req.relative_movement.position.x;
00641   new_pose.position.y += req.relative_movement.position.y;
00642   new_pose.position.z += req.relative_movement.position.z;
00643 
00644   tf::Quaternion o;
00645   tf::quaternionMsgToTF(req.relative_movement.orientation,o);
00646 
00647   tf::Quaternion g;
00648   tf::quaternionMsgToTF(new_pose.orientation,g);
00649 
00650   tf::quaternionTFToMsg(o*g,new_pose.orientation);
00651 
00652 
00653 
00654   if (!(interactive_marker_server_->setPose("MPR 0_end_control",new_pose))) {
00655 
00656         ROS_ERROR("Can't set new pose for MPR 0_end_control");
00657 
00658         unlockScene();
00659 
00660     res.completed = false;
00661     reset();
00662     return false;
00663 
00664   }
00665 
00666   interactive_marker_server_->applyChanges();
00667 
00668   unlockScene();
00669 
00670   if (!findIK(new_pose)) {
00671 
00672           ros::Duration(0.25).sleep();
00673 
00674           // try it for second time... just for sure
00675           findIK(new_pose);
00676 
00677   }
00678 
00679   res.completed = true;
00680   return true;
00681 
00682 }
00683 
00684 bool CArmManipulationEditor::ArmNavStep(ArmNavStep::Request &req, ArmNavStep::Response &res) {
00685 
00686   if (!inited) {
00687 
00688     res.completed = false;
00689     res.msg = "Start planning first!";
00690     reset();
00691     return true;
00692 
00693   }
00694 
00695     if (req.undo) {
00696 
00697       //boost::mutex::scoped_lock(im_server_mutex_);
00698       lockScene();
00699 
00700       if (gripper_poses_->size()<=1) {
00701 
00702         res.completed = false;
00703         res.msg = "There is no stored position.";
00704         res.b_steps_left = 0;
00705         res.f_steps_left = 0;
00706         return true;
00707 
00708       }
00709 
00710       gripper_poses_->pop_back(); // discard current position
00711       geometry_msgs::Pose p = gripper_poses_->back();
00712 
00713       res.b_steps_left = gripper_poses_->size()-1;
00714 
00715       ROS_INFO("Undoing change in gripper IM (%d steps left)",res.b_steps_left);
00716 
00717       if (!(interactive_marker_server_->setPose("MPR 0_end_control",p))) {
00718 
00719          unlockScene();
00720 
00721          ROS_ERROR("Error on changing IM pose");
00722          res.msg = "Error on changing IM pose";
00723          res.completed = false;
00724          reset();
00725          return false;
00726 
00727      }
00728 
00729      interactive_marker_server_->applyChanges();
00730 
00731      unlockScene();
00732 
00733      findIK(p);
00734 
00735      res.msg = "Undoing last change";
00736      res.completed = true;
00737      return true;
00738 
00739     } // undo
00740 
00741     if (req.redo) {
00742 
00743       // TODO: implement
00744       res.completed = false;
00745       res.msg = "Not implemented yet";
00746 
00747     }
00748 
00749   return false;
00750 
00751 }
00752 
00753 bool CArmManipulationEditor::ArmNavSwitchACO(ArmNavSwitchAttCO::Request &req, ArmNavSwitchAttCO::Response &res) {
00754 
00755   // TODO allow the change only if inited=false
00756 
00757   if (inited == true) {
00758 
00759           ROS_ERROR("ACO can bee switched on/off only before start of planning.");
00760           return false;
00761 
00762   }
00763 
00764   aco_ = req.state;
00765 
00766   if (aco_ ) ROS_INFO("ACO set to true");
00767   else ROS_INFO("ACO set to false");
00768 
00769   res.completed = true;
00770   return true;
00771 
00772 }
00773 
00774 bool CArmManipulationEditor::ArmNavStop(ArmNavStop::Request &req, ArmNavStop::Response &res) {
00775 
00776   ROS_INFO("Canceling all goals.");
00777   this->arm_controller_map_[params_.left_arm_group_]->cancelAllGoals();
00778 
00779   reset();
00780 
00781   return true;
00782 
00783 }


srs_assisted_arm_navigation
Author(s): Zdenek Materna (imaterna@fit.vutbr.cz), Michal Spanel (spanel@fit.vutbr.cz)
autogenerated on Mon Oct 6 2014 08:12:08