00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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(),
00049 *getRobotState(),
00050 params_.left_arm_group_,
00051 params_.left_ik_link_,
00052 planning_scene_utils::getPlanningSceneIdFromName(planning_scene_id),
00053 true,
00054 mpr_id);
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
00074
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
00125
00126
00127
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
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
00223 lockScene();
00224
00225 planned_ = true;
00226 disable_gripper_poses_ = true;
00227
00228
00229
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
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
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
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
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
00448 std::string target = end_eff_link_;
00449
00450 if (req.attached) {
00451
00452
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
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
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
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
00559
00560
00561
00562
00563
00564
00565
00566 lockScene();
00567
00568 if (interactive_marker_server_->setPose("MPR 0_end_control",ps.pose)) {
00569
00570 interactive_marker_server_->applyChanges();
00571
00572 unlockScene();
00573
00574 if (!findIK(ps.pose)) {
00575
00576 ros::Duration(0.25).sleep();
00577
00578
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
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
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
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
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
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();
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 }
00740
00741 if (req.redo) {
00742
00743
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
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 }