$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * 00035 *********************************************************************/ 00036 00037 /* \author: Matthew Klingensmith, E. Gil Jones */ 00038 00039 #include <move_arm_warehouse/move_arm_utils.h> 00040 #include <assert.h> 00041 #include <geometric_shapes/shape_operations.h> 00042 #include <planning_environment/util/construct_object.h> 00043 00044 using namespace std; 00045 using namespace arm_navigation_msgs; 00046 using namespace planning_scene_utils; 00047 using namespace collision_space; 00048 using namespace kinematics_msgs; 00049 using namespace arm_navigation_msgs; 00050 using namespace head_monitor_msgs; 00051 using namespace move_arm_warehouse; 00052 using namespace planning_environment; 00053 using namespace planning_models; 00054 using namespace std_msgs; 00055 using namespace trajectory_msgs; 00056 using namespace visualization_msgs; 00057 using namespace arm_navigation_msgs; 00058 using namespace actionlib; 00059 using namespace control_msgs; 00060 using namespace interactive_markers; 00061 00062 #define MARKER_REFRESH_TIME 0.05 00063 #define SAFE_DELETE(x) if(x != NULL) { delete x; x = NULL; } 00064 00065 std_msgs::ColorRGBA makeRandomColor(float brightness, float alpha) 00066 { 00067 std_msgs::ColorRGBA toReturn; 00068 toReturn.a = alpha; 00069 00070 toReturn.r = ((float)(random()) / (float)RAND_MAX) * (1.0f - brightness) + brightness; 00071 toReturn.g = ((float)(random()) / (float)RAND_MAX) * (1.0f - brightness) + brightness; 00072 toReturn.b = ((float)(random()) / (float)RAND_MAX) * (1.0f - brightness) + brightness; 00073 00074 toReturn.r = min(toReturn.r, 1.0f); 00075 toReturn.g = min(toReturn.g, 1.0f); 00076 toReturn.b = min(toReturn.b, 1.0f); 00077 00078 return toReturn; 00079 } 00080 00082 // PLANNING SCENE DATA 00084 00085 PlanningSceneData::PlanningSceneData() 00086 { 00087 setId(0); 00088 setTimeStamp(ros::Time(ros::WallTime::now().toSec())); 00089 } 00090 00091 PlanningSceneData::PlanningSceneData(unsigned int id, const ros::Time& timestamp, const PlanningScene& scene) 00092 { 00093 setId(id); 00094 setPlanningScene(scene); 00095 setTimeStamp(timestamp); 00096 } 00097 00098 void PlanningSceneData::getRobotState(KinematicState* state) 00099 { 00100 // Actually converts a robot state message to a kinematic state message (possibly very slow) 00101 setRobotStateAndComputeTransforms(getPlanningScene().robot_state, *state); 00102 } 00103 00105 //TRAJECTORY DATA 00107 00108 TrajectoryData::TrajectoryData() 00109 { 00110 setCurrentState(NULL); 00111 setSource(""); 00112 setGroupName(""); 00113 setColor(makeRandomColor(0.3f, 0.6f)); 00114 reset(); 00115 setId(0); 00116 showCollisions(); 00117 should_refresh_colors_ = false; 00118 has_refreshed_colors_ = true; 00119 refresh_timer_ = ros::Duration(0.0); 00120 trajectory_error_code_.val = 0; 00121 setRenderType(CollisionMesh); 00122 } 00123 00124 TrajectoryData::TrajectoryData(const unsigned int& id, const string& source, const string& groupName, const JointTrajectory& trajectory) 00125 { 00126 setCurrentState(NULL); 00127 setId(id); 00128 setSource(source); 00129 setGroupName(groupName); 00130 setTrajectory(trajectory); 00131 setColor(makeRandomColor(0.3f, 0.6f)); 00132 reset(); 00133 showCollisions(); 00134 should_refresh_colors_ = false; 00135 has_refreshed_colors_ = true; 00136 refresh_timer_ = ros::Duration(0.0); 00137 trajectory_error_code_.val = 0; 00138 setRenderType(CollisionMesh); 00139 } 00140 00141 void TrajectoryData::moveThroughTrajectory(int step) 00142 { 00143 unsigned int tsize = getTrajectorySize(); 00144 00145 if(tsize == 0 || getCurrentState() == NULL) 00146 { 00147 return; 00148 } 00149 00150 // Possibly negative point 00151 if((int)getCurrentPoint() + step < 0) 00152 { 00153 setCurrentPoint(0); 00154 } 00155 else 00156 { 00157 setCurrentPoint((int)getCurrentPoint() + step); 00158 } 00159 00160 // Possibly overstepped the end of the trajectory. 00161 if(getCurrentPoint() >= tsize - 1) 00162 { 00163 setCurrentPoint(tsize - 1); 00164 stop(); 00165 } 00166 00167 // Create new kinematic state. 00168 updateCurrentState(); 00169 } 00170 00171 void TrajectoryData::updateCurrentState() 00172 { 00173 map<string, double> joint_values; 00174 for(unsigned int i = 0; i < getTrajectory().joint_names.size(); i++) 00175 { 00176 joint_values[getTrajectory().joint_names[i]] = getTrajectory().points[getCurrentPoint()].positions[i]; 00177 } 00178 00179 getCurrentState()->setKinematicState(joint_values); 00180 } 00181 00182 void TrajectoryData::updateCollisionMarkers(CollisionModels* cm_, MotionPlanRequestData& motionPlanRequest, 00183 ros::ServiceClient* distance_state_validity_service_client_) 00184 { 00185 if(areCollisionsVisible()) 00186 { 00187 const KinematicState* state = getCurrentState(); 00188 collision_markers_.markers.clear(); 00189 if(state == NULL) 00190 { 00191 return; 00192 } 00193 std_msgs::ColorRGBA bad_color; 00194 bad_color.a = 1.0f; 00195 bad_color.r = 1.0f; 00196 bad_color.g = 0.0f; 00197 bad_color.b = 0.0f; 00198 00199 collision_space::EnvironmentModel::AllowedCollisionMatrix acm = cm_->getCurrentAllowedCollisionMatrix(); 00200 cm_->disableCollisionsForNonUpdatedLinks(getGroupName()); 00201 // Get all collisions as little red spheres. 00202 cm_->getAllCollisionPointMarkers(*state, collision_markers_, bad_color, ros::Duration(MARKER_REFRESH_TIME)); 00203 00204 const KinematicState::JointStateGroup* jsg = state->getJointStateGroup(getGroupName()); 00205 Constraints empty_constraints; 00206 ArmNavigationErrorCodes code; 00207 00208 // Update validity of the current state. 00209 cm_->isKinematicStateValid(*state, jsg->getJointNames(), code, empty_constraints, 00210 motionPlanRequest.getMotionPlanRequest().path_constraints, true); 00211 00212 cm_->setAlteredAllowedCollisionMatrix(acm); 00213 GetStateValidity::Request val_req; 00214 GetStateValidity::Response val_res; 00215 convertKinematicStateToRobotState(*state, ros::Time(ros::WallTime::now().toSec()), cm_->getWorldFrameId(), 00216 val_req.robot_state); 00217 00218 if(distance_state_validity_service_client_ != NULL) { 00219 if(!distance_state_validity_service_client_->call(val_req, val_res)) 00220 { 00221 ROS_INFO_STREAM("Something wrong with distance server"); 00222 } 00223 } 00224 } 00225 } 00226 00228 // MOTION PLAN REQUEST DATA 00230 00231 MotionPlanRequestData::MotionPlanRequestData(const KinematicState* robot_state) 00232 { 00233 setSource(""); 00234 setStartColor(makeRandomColor(0.3f, 0.6f)); 00235 setGoalColor(makeRandomColor(0.3f, 0.6f)); 00236 setStartEditable(true); 00237 setGoalEditable(true); 00238 setHasGoodIKSolution(true, StartPosition); 00239 setHasGoodIKSolution(true, GoalPosition); 00240 setId(0); 00241 name_ = ""; 00242 show(); 00243 showCollisions(); 00244 // Note: these must be registered as StateRegistry entries after this request has been created. 00245 start_state_ = new KinematicState(*robot_state); 00246 goal_state_ = new KinematicState(*robot_state); 00247 should_refresh_colors_ = false; 00248 has_refreshed_colors_ = true; 00249 refresh_timer_ = ros::Duration(0.0); 00250 are_joint_controls_visible_ = false; 00251 setRenderType(CollisionMesh); 00252 } 00253 00254 MotionPlanRequestData::MotionPlanRequestData(const unsigned int& id, const string& source, const MotionPlanRequest& request, 00255 const KinematicState* robot_state) 00256 { 00257 // Note: these must be registered as StateRegistry entries after this request has been created. 00258 start_state_ = new KinematicState(*robot_state); 00259 goal_state_ = new KinematicState(*robot_state); 00260 00261 setId(id); 00262 setSource(source); 00263 setMotionPlanRequest(request); 00264 00265 setStartColor(makeRandomColor(0.3f, 0.6f)); 00266 setGoalColor(makeRandomColor(0.3f, 0.6f)); 00267 setStartEditable(true); 00268 setGoalEditable(true); 00269 setHasGoodIKSolution(true, StartPosition); 00270 setHasGoodIKSolution(true, GoalPosition); 00271 show(); 00272 showCollisions(); 00273 00274 should_refresh_colors_ = false; 00275 has_refreshed_colors_ = true; 00276 refresh_timer_ = ros::Duration(0.0); 00277 are_joint_controls_visible_ = false; 00278 00279 setRenderType(CollisionMesh); 00280 } 00281 00282 // Kinematic states must be converted to joint constraint message 00283 void MotionPlanRequestData::updateGoalState() 00284 { 00285 vector<JointConstraint>& constraints = getMotionPlanRequest().goal_constraints.joint_constraints; 00286 00287 map<string, double> jointValues; 00288 for(size_t i = 0; i < constraints.size(); i++) 00289 { 00290 JointConstraint& constraint = constraints[i]; 00291 jointValues[constraint.joint_name] = constraint.position; 00292 } 00293 00294 goal_state_->setKinematicState(jointValues); 00295 00296 } 00297 00298 // Kinematic state must be converted to robot state message 00299 void MotionPlanRequestData::updateStartState() 00300 { 00301 setRobotStateAndComputeTransforms(getMotionPlanRequest().start_state, *start_state_); 00302 } 00303 00304 void MotionPlanRequestData::setJointControlsVisible(bool visible, PlanningSceneEditor* editor) 00305 { 00306 are_joint_controls_visible_ = visible; 00307 00308 if(visible) 00309 { 00310 if(isStartVisible() && isStartEditable()) 00311 { 00312 editor->createJointMarkers(*this, StartPosition); 00313 } 00314 else 00315 { 00316 editor->deleteJointMarkers(*this, StartPosition); 00317 } 00318 if(isEndVisible() && isGoalEditable()) 00319 { 00320 editor->createJointMarkers(*this, GoalPosition); 00321 } 00322 else 00323 { 00324 editor->deleteJointMarkers(*this, GoalPosition); 00325 } 00326 } 00327 else 00328 { 00329 editor->deleteJointMarkers(*this, StartPosition); 00330 editor->deleteJointMarkers(*this, GoalPosition); 00331 } 00332 } 00333 00334 void MotionPlanRequestData::setStartStateValues(std::map<std::string, double>& joint_values) 00335 { 00336 setStateChanged(true); 00337 start_state_->setKinematicState(joint_values); 00338 } 00339 00340 void MotionPlanRequestData::setGoalStateValues(std::map<std::string, double>& joint_values) 00341 { 00342 setStateChanged(true); 00343 goal_state_->setKinematicState(joint_values); 00344 00345 getMotionPlanRequest().goal_constraints.joint_constraints.resize(joint_values.size()); 00346 int constraints = 0; 00347 for(std::map<std::string, double>::iterator it = joint_values.begin(); it != joint_values.end(); it++, constraints++) 00348 { 00349 getMotionPlanRequest().goal_constraints.joint_constraints[constraints].joint_name = it->first; 00350 getMotionPlanRequest().goal_constraints.joint_constraints[constraints].position = it->second; 00351 getMotionPlanRequest().goal_constraints.joint_constraints[constraints].tolerance_above = 0.001; 00352 getMotionPlanRequest().goal_constraints.joint_constraints[constraints].tolerance_below = 0.001; 00353 } 00354 } 00355 00356 void MotionPlanRequestData::updateCollisionMarkers(CollisionModels* cm_, 00357 ros::ServiceClient* distance_state_validity_service_client_) 00358 { 00359 if(areCollisionsVisible()) 00360 { 00361 const KinematicState* state = getStartState(); 00362 collision_markers_.markers.clear(); 00363 if(state == NULL) 00364 { 00365 return; 00366 } 00367 00369 // Start state block 00371 std_msgs::ColorRGBA bad_color; 00372 bad_color.a = 1.0f; 00373 bad_color.r = 1.0f; 00374 bad_color.g = 0.0f; 00375 bad_color.b = 0.0f; 00376 collision_space::EnvironmentModel::AllowedCollisionMatrix acm = cm_->getCurrentAllowedCollisionMatrix(); 00377 cm_->disableCollisionsForNonUpdatedLinks(getGroupName()); 00378 // Get all the collision points as little red spheres. 00379 if(isStartVisible()) { 00380 cm_->getAllCollisionPointMarkers(*state, collision_markers_, bad_color, ros::Duration(MARKER_REFRESH_TIME)); 00381 const KinematicState::JointStateGroup* jsg = state->getJointStateGroup(getGroupName()); 00382 ArmNavigationErrorCodes code; 00383 Constraints empty_constraints; 00384 // Ensure that the state is valid. 00385 cm_->isKinematicStateValid(*state, jsg->getJointNames(), code, empty_constraints, 00386 getMotionPlanRequest().path_constraints, true); 00387 00388 GetStateValidity::Request val_req; 00389 GetStateValidity::Response val_res; 00390 convertKinematicStateToRobotState(*state, ros::Time(ros::WallTime::now().toSec()), cm_->getWorldFrameId(), 00391 val_req.robot_state); 00392 00393 if(distance_state_validity_service_client_ != NULL) { 00394 if(!distance_state_validity_service_client_->call(val_req, val_res)) 00395 { 00396 ROS_INFO_STREAM("Something wrong with distance server"); 00397 } 00398 } 00399 } 00401 // End State block 00403 if(isEndVisible()) { 00404 state = getGoalState(); 00405 if(state == NULL) 00406 { 00407 return; 00408 } 00409 cm_->getAllCollisionPointMarkers(*state, collision_markers_, bad_color, ros::Duration(MARKER_REFRESH_TIME)); 00410 const KinematicState::JointStateGroup* jsg = state->getJointStateGroup(getGroupName()); 00411 ArmNavigationErrorCodes code; 00412 Constraints empty_constraints; 00413 cm_->isKinematicStateValid(*state, jsg->getJointNames(), code, empty_constraints, 00414 getMotionPlanRequest().path_constraints, true); 00415 00416 GetStateValidity::Request val_req; 00417 GetStateValidity::Response val_res; 00418 convertKinematicStateToRobotState(*state, ros::Time(ros::WallTime::now().toSec()), cm_->getWorldFrameId(), 00419 val_req.robot_state); 00420 00421 if(distance_state_validity_service_client_ != NULL) { 00422 if(!distance_state_validity_service_client_->call(val_req, val_res)) 00423 { 00424 ROS_INFO_STREAM("Something wrong with distance server"); 00425 } 00426 } 00427 } 00428 cm_->setAlteredAllowedCollisionMatrix(acm); 00429 } 00430 } 00431 00432 // Warning: lots of copying going on here. 00433 std::vector<std::string> MotionPlanRequestData::getJointNamesInGoal() 00434 { 00435 std::vector<JointConstraint>& constraints = getMotionPlanRequest().goal_constraints.joint_constraints; 00436 std::vector<std::string> toReturn; 00437 00438 for(size_t i = 0; i < constraints.size(); i++) 00439 { 00440 JointConstraint& constraint = constraints[i]; 00441 toReturn.push_back(constraint.joint_name); 00442 } 00443 00444 return toReturn; 00445 } 00446 00447 // Warning: calls getJointNamesInGoal 00448 bool MotionPlanRequestData::isJointNameInGoal(std::string joint) 00449 { 00450 vector<string> joints = getJointNamesInGoal(); 00451 for(size_t i = 0; i < joints.size(); i++) 00452 { 00453 if(joints[i] == joint) 00454 { 00455 return true; 00456 } 00457 } 00458 00459 return false; 00460 } 00461 00463 // PLANNING SCENE EDITOR 00465 00466 PlanningSceneEditor::PlanningSceneEditor() 00467 { 00468 setRobotState(NULL, false); 00469 setCollisionModel(NULL, false); 00470 interactive_marker_server_ = NULL; 00471 collision_aware_ik_services_ = NULL; 00472 non_collision_aware_ik_services_ = NULL; 00473 interpolated_ik_services_ = NULL; 00474 selectable_objects_ = NULL; 00475 ik_controllers_ = NULL; 00476 max_collision_object_id_ = 0; 00477 use_interpolated_planner_ = false; 00478 string robot_description_name = nh_.resolveName("robot_description", true); 00479 cm_ = new CollisionModels(robot_description_name); 00480 } 00481 00482 PlanningSceneEditor::PlanningSceneEditor(PlanningSceneParameters& params) 00483 { 00487 params_ = params; 00488 monitor_status_ = idle; 00489 max_collision_object_id_ = 0; 00490 use_interpolated_planner_ = false; 00491 last_collision_object_color_.r = 0.7; 00492 last_collision_object_color_.g = 0.7; 00493 last_collision_object_color_.b = 0.7; 00494 last_collision_object_color_.a = 1.0; 00495 last_mesh_object_color_.r = 0.7; 00496 last_mesh_object_color_.g = 0.7; 00497 last_mesh_object_color_.b = 0.7; 00498 last_mesh_object_color_.a = 1.0; 00499 00500 collision_aware_ik_services_ = new map<string, ros::ServiceClient*> (); 00501 non_collision_aware_ik_services_ = new map<string, ros::ServiceClient*> (); 00502 interpolated_ik_services_ = new map<string, ros::ServiceClient*> (); 00503 selectable_objects_ = new map<string, SelectableObject> (); 00504 ik_controllers_ = new map<string, IKController> (); 00505 00506 string robot_description_name = nh_.resolveName("robot_description", true); 00507 cm_ = new CollisionModels(robot_description_name); 00508 robot_state_ = new KinematicState(cm_->getKinematicModel()); 00509 robot_state_->setKinematicStateToDefault(); 00510 00514 interactive_marker_server_ 00515 = new interactive_markers::InteractiveMarkerServer("planning_scene_warehouse_viewer_controls", "", false); 00516 00517 collision_object_movement_feedback_ptr_ 00518 = boost::bind(&PlanningSceneEditor::collisionObjectMovementCallback, this, _1); 00519 collision_object_selection_feedback_ptr_ = boost::bind(&PlanningSceneEditor::collisionObjectSelectionCallback, this, 00520 _1); 00521 joint_control_feedback_ptr_ = boost::bind(&PlanningSceneEditor::JointControllerCallback, this, _1); 00522 ik_control_feedback_ptr_ = boost::bind(&PlanningSceneEditor::IKControllerCallback, this, _1); 00523 attached_collision_object_feedback_ptr_ = boost::bind(&PlanningSceneEditor::attachedCollisionObjectInteractiveCallback, this, _1); 00524 00528 joint_state_publisher_ = nh_.advertise<sensor_msgs::JointState> ("joint_states", 10); 00529 vis_marker_publisher_ = nh_.advertise<visualization_msgs::Marker> (params.vis_topic_name_, 128); 00530 vis_marker_array_publisher_ = nh_.advertise<visualization_msgs::MarkerArray> (params.vis_topic_name_ + "_array", 128); 00531 move_arm_warehouse_logger_reader_ = new MoveArmWarehouseLoggerReader(); 00532 00533 00537 if(params.sync_robot_state_with_gazebo_) 00538 { 00539 ros::service::waitForService("/gazebo/set_model_configuration"); 00540 ros::service::waitForService(params.list_controllers_service_); 00541 ros::service::waitForService(params.load_controllers_service_); 00542 ros::service::waitForService(params.unload_controllers_service_); 00543 ros::service::waitForService(params.switch_controllers_service_); 00544 ros::service::waitForService("/gazebo/pause_physics"); 00545 ros::service::waitForService("/gazebo/unpause_physics"); 00546 ros::service::waitForService("/gazebo/set_link_properties"); 00547 ros::service::waitForService("/gazebo/get_link_properties"); 00548 } 00549 00550 if(params.left_arm_group_ != "none") 00551 { 00552 ros::service::waitForService(params.left_ik_name_); 00553 } 00554 00555 if(params.right_arm_group_ != "none") 00556 { 00557 ros::service::waitForService(params.right_ik_name_); 00558 } 00559 00560 if(params.planner_service_name_ != "none") 00561 { 00562 ros::service::waitForService(params.planner_service_name_); 00563 } 00564 00565 if(params.proximity_space_service_name_ != "none") 00566 { 00567 ros::service::waitForService(params.proximity_space_service_name_); 00568 } 00569 00570 if(params.proximity_space_validity_name_ != "none") 00571 { 00572 ros::service::waitForService(params.proximity_space_validity_name_); 00573 } 00574 00575 if(params.sync_robot_state_with_gazebo_) 00576 { 00577 gazebo_joint_state_client_ = nh_.serviceClient<gazebo_msgs::SetModelConfiguration>("/gazebo/set_model_configuration", true); 00578 list_controllers_client_ = nh_.serviceClient<pr2_mechanism_msgs::ListControllers>(params.list_controllers_service_, true); 00579 load_controllers_client_ = nh_.serviceClient<pr2_mechanism_msgs::LoadController>(params.load_controllers_service_, true); 00580 unload_controllers_client_ = nh_.serviceClient<pr2_mechanism_msgs::UnloadController>(params.unload_controllers_service_, true); 00581 switch_controllers_client_ = nh_.serviceClient<pr2_mechanism_msgs::SwitchController>(params.switch_controllers_service_, true); 00582 pause_gazebo_client_ = nh_.serviceClient<std_srvs::Empty>("/gazebo/pause_physics", true); 00583 unpause_gazebo_client_ = nh_.serviceClient<std_srvs::Empty>("/gazebo/unpause_physics", true); 00584 set_link_properties_client_ = nh_.serviceClient<gazebo_msgs::SetLinkProperties>("/gazebo/set_link_properties", true); 00585 get_link_properties_client_ = nh_.serviceClient<gazebo_msgs::GetLinkProperties>("/gazebo/get_link_properties", true); 00586 } 00587 00588 if(params.left_arm_group_ != "none") 00589 { 00590 left_ik_service_client_ = nh_.serviceClient<GetConstraintAwarePositionIK> (params.left_ik_name_, true); 00591 non_coll_left_ik_service_client_ = nh_.serviceClient<GetPositionIK> (params.non_coll_left_ik_name_, true); 00592 } 00593 if(params.right_arm_group_ != "none") 00594 { 00595 right_ik_service_client_ = nh_.serviceClient<GetConstraintAwarePositionIK> (params.right_ik_name_, true); 00596 non_coll_right_ik_service_client_ = nh_.serviceClient<GetPositionIK> (params.non_coll_right_ik_name_, true); 00597 } 00598 00599 if(params.planner_service_name_ != "none") 00600 { 00601 planning_service_client_ = nh_.serviceClient<GetMotionPlan> (params.planner_service_name_, true); 00602 } 00603 00604 if(params.trajectory_filter_service_name_ != "none") 00605 { 00606 trajectory_filter_service_client_ 00607 = nh_.serviceClient<FilterJointTrajectoryWithConstraints> (params.trajectory_filter_service_name_); 00608 } 00609 00610 if(params.proximity_space_service_name_ != "none") 00611 { 00612 distance_aware_service_client_ = nh_.serviceClient<GetMotionPlan> (params.proximity_space_service_name_, true); 00613 } 00614 00615 if(params.proximity_space_validity_name_ != "none") 00616 { 00617 distance_state_validity_service_client_ 00618 = nh_.serviceClient<GetStateValidity> (params.proximity_space_validity_name_, true); 00619 } 00620 00621 if(params.proximity_space_planner_name_ != "none") 00622 { 00623 collision_proximity_planner_client_ = nh_.serviceClient<GetMotionPlan> (params.proximity_space_planner_name_, true); 00624 } 00625 00626 set_planning_scene_diff_client_ = nh_.serviceClient<SetPlanningSceneDiff> (params.set_planning_scene_diff_name_); 00627 00628 if(params.use_robot_data_) 00629 { 00630 if(params_.right_arm_group_ != "none") 00631 { 00632 arm_controller_map_[params_.right_arm_group_] = new actionlib::SimpleActionClient< 00633 control_msgs::FollowJointTrajectoryAction>(params.execute_right_trajectory_, true); 00634 00635 while(ros::ok() && !arm_controller_map_[params_.right_arm_group_]->waitForServer(ros::Duration(1.0))) 00636 { 00637 ROS_INFO("Waiting for the right_joint_trajectory_action server to come up."); 00638 } 00639 } 00640 00641 if(params.left_arm_group_ != "none") 00642 { 00643 arm_controller_map_[params.left_arm_group_] = new actionlib::SimpleActionClient< 00644 control_msgs::FollowJointTrajectoryAction>(params.execute_left_trajectory_, true); 00645 00646 while(ros::ok() && !arm_controller_map_[params.left_arm_group_]->waitForServer(ros::Duration(1.0))) 00647 { 00648 ROS_INFO("Waiting for the left_joint_trajectory_action server to come up."); 00649 } 00650 } 00651 } 00652 00653 if(params.left_arm_group_ != "none") 00654 { 00655 (*collision_aware_ik_services_)[params.left_ik_link_] = &left_ik_service_client_; 00656 } 00657 00658 if(params.right_arm_group_ != "none") 00659 { 00660 (*collision_aware_ik_services_)[params.right_ik_link_] = &right_ik_service_client_; 00661 } 00662 00663 if(params.left_arm_group_ != "none") 00664 { 00665 (*non_collision_aware_ik_services_)[params.left_ik_link_] = &non_coll_left_ik_service_client_; 00666 } 00667 00668 if(params.left_arm_group_ != "none") 00669 { 00670 (*non_collision_aware_ik_services_)[params.right_ik_link_] = &non_coll_right_ik_service_client_; 00671 } 00672 00673 if(params.right_interpolate_service_name_ != "none") 00674 { 00675 while(ros::ok() && !ros::service::waitForService(params.right_interpolate_service_name_, ros::Duration(1.0))) 00676 { 00677 ROS_INFO_STREAM("Waiting for the right interpolation server to come up: " << params.right_interpolate_service_name_); 00678 } 00679 right_interpolate_service_client_ = nh_.serviceClient<GetMotionPlan> (params.right_interpolate_service_name_, true); 00680 (*interpolated_ik_services_)[params.right_ik_link_] = &right_interpolate_service_client_; 00681 } 00682 if(params.left_interpolate_service_name_ != "none") 00683 { 00684 while(ros::ok() && !ros::service::waitForService(params.left_interpolate_service_name_, ros::Duration(1.0))) 00685 { 00686 ROS_INFO("Waiting for the left interpolation server to come up."); 00687 } 00688 left_interpolate_service_client_ = nh_.serviceClient<GetMotionPlan> (params.left_interpolate_service_name_, true); 00689 (*interpolated_ik_services_)[params.left_ik_link_] = &left_interpolate_service_client_; 00690 } 00691 00695 menu_entry_maps_["Collision Object"] = MenuEntryMap(); 00696 menu_entry_maps_["Collision Object Selection"] = MenuEntryMap(); 00697 menu_entry_maps_["IK Control"] = MenuEntryMap(); 00698 registerMenuEntry("Collision Object Selection", "Delete", collision_object_selection_feedback_ptr_); 00699 registerMenuEntry("Collision Object Selection", "Select", collision_object_selection_feedback_ptr_); 00700 registerMenuEntry("Collision Object", "Delete", collision_object_movement_feedback_ptr_); 00701 registerMenuEntry("Collision Object", "Deselect", collision_object_movement_feedback_ptr_); 00702 registerMenuEntry("Collision Object", "Attach", collision_object_movement_feedback_ptr_); 00703 registerMenuEntry("Attached Collision Object", "Detach", attached_collision_object_feedback_ptr_); 00704 MenuHandler::EntryHandle resize_mode_entry = registerMenuEntry("Collision Object", "Resize Mode", collision_object_movement_feedback_ptr_); 00705 MenuHandler::EntryHandle off = menu_handler_map_["Collision Object"].insert(resize_mode_entry, "Off", collision_object_movement_feedback_ptr_); 00706 menu_entry_maps_["Collision Object"]["Off"] = off; 00707 menu_handler_map_["Collision Object"].setCheckState(off, MenuHandler::CHECKED); 00708 last_resize_handle_ = off; 00709 MenuHandler::EntryHandle grow = menu_handler_map_["Collision Object"].insert(resize_mode_entry, "Grow", collision_object_movement_feedback_ptr_); 00710 menu_entry_maps_["Collision Object"]["Grow"] = grow; 00711 menu_handler_map_["Collision Object"].setCheckState(grow, MenuHandler::UNCHECKED); 00712 MenuHandler::EntryHandle shrink = menu_handler_map_["Collision Object"].insert(resize_mode_entry, "Shrink", collision_object_movement_feedback_ptr_); 00713 menu_handler_map_["Collision Object"].setCheckState(shrink, MenuHandler::UNCHECKED); 00714 menu_entry_maps_["Collision Object"]["Shrink"] = shrink; 00715 registerMenuEntry("IK Control", "Map to Robot State", ik_control_feedback_ptr_); 00716 registerMenuEntry("IK Control", "Map from Robot State", ik_control_feedback_ptr_); 00717 registerMenuEntry("IK Control", "Go To Last Good State", ik_control_feedback_ptr_); 00718 registerMenuEntry("IK Control", "Randomly Perturb", ik_control_feedback_ptr_); 00719 registerMenuEntry("IK Control", "Plan New Trajectory", ik_control_feedback_ptr_); 00720 registerMenuEntry("IK Control", "Filter Last Trajectory", ik_control_feedback_ptr_); 00721 registerMenuEntry("IK Control", "Delete Request", ik_control_feedback_ptr_); 00722 00723 if(params_.use_robot_data_) 00724 { 00725 registerMenuEntry("IK Control", "Execute Last Trajectory", ik_control_feedback_ptr_); 00726 } 00727 00731 if(params.use_robot_data_) 00732 { 00733 joint_state_subscriber_ = nh_.subscribe("joint_states", 25, &PlanningSceneEditor::jointStateCallback, this); 00734 } 00735 } 00736 00737 void PlanningSceneEditor::jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state) 00738 { 00739 if(robot_state_ == NULL) return; 00740 00741 std::map<std::string, double> joint_state_map; 00742 std::map<std::string, double> joint_velocity_map; 00743 00744 //message already been validated in kmsm 00745 for(unsigned int i = 0; i < joint_state->position.size(); ++i) 00746 { 00747 joint_state_map[joint_state->name[i]] = joint_state->position[i]; 00748 joint_velocity_map[joint_state->name[i]] = joint_state->velocity[i]; 00749 } 00750 //getting base transform 00751 lockScene(); 00752 std::vector<planning_models::KinematicState::JointState*>& joint_state_vector = robot_state_->getJointStateVector(); 00753 for(std::vector<planning_models::KinematicState::JointState*>::iterator it = joint_state_vector.begin(); 00754 it != joint_state_vector.end(); 00755 it++) { 00756 bool tfSets = false; 00757 //see if we need to update any transforms 00758 std::string parent_frame_id = (*it)->getParentFrameId(); 00759 std::string child_frame_id = (*it)->getChildFrameId(); 00760 if(!parent_frame_id.empty() && !child_frame_id.empty()) { 00761 std::string err; 00762 ros::Time tm; 00763 tf::StampedTransform transf; 00764 bool ok = false; 00765 if (transform_listener_.getLatestCommonTime(parent_frame_id, child_frame_id, tm, &err) == tf::NO_ERROR) { 00766 ok = true; 00767 try 00768 { 00769 transform_listener_.lookupTransform(parent_frame_id, child_frame_id, tm, transf); 00770 } 00771 catch(tf::TransformException& ex) 00772 { 00773 ROS_ERROR("Unable to lookup transform from %s to %s. Exception: %s", parent_frame_id.c_str(), child_frame_id.c_str(), ex.what()); 00774 ok = false; 00775 } 00776 } else { 00777 ROS_DEBUG("Unable to lookup transform from %s to %s: no common time.", parent_frame_id.c_str(), child_frame_id.c_str()); 00778 ok = false; 00779 } 00780 if(ok) { 00781 tfSets = (*it)->setJointStateValues(transf); 00782 } 00783 } 00784 (*it)->setJointStateValues(joint_state_map); 00785 } 00786 robot_state_->updateKinematicLinks(); 00787 robot_state_->getKinematicStateValues(robot_state_joint_values_); 00788 unlockScene(); 00789 // Records trajectory if currently executing. 00790 if(monitor_status_ == Executing) 00791 { 00792 trajectory_msgs::JointTrajectoryPoint point; 00793 point.positions.resize(logged_trajectory_.joint_names.size()); 00794 point.velocities.resize(logged_trajectory_.joint_names.size()); 00795 for(unsigned int i = 0; i < logged_trajectory_.joint_names.size(); i++) 00796 { 00797 point.positions[i] = joint_state_map[logged_trajectory_.joint_names[i]]; 00798 point.velocities[i] = joint_velocity_map[logged_trajectory_.joint_names[i]]; 00799 } 00800 point.time_from_start = ros::Time(ros::WallTime::now().toSec()) - logged_trajectory_start_time_; 00801 logged_trajectory_.points.push_back(point); 00802 } 00803 } 00804 00805 PlanningSceneEditor::~PlanningSceneEditor() 00806 { 00807 SAFE_DELETE(robot_state_); 00808 SAFE_DELETE(interactive_marker_server_); 00809 SAFE_DELETE(selectable_objects_); 00810 SAFE_DELETE(ik_controllers_); 00811 SAFE_DELETE(collision_aware_ik_services_); 00812 SAFE_DELETE(non_collision_aware_ik_services_); 00813 SAFE_DELETE(interpolated_ik_services_); 00814 } 00815 00816 void PlanningSceneEditor::makeSelectableAttachedObjectFromPlanningScene(const arm_navigation_msgs::PlanningScene& scene, 00817 arm_navigation_msgs::AttachedCollisionObject& att) 00818 { 00819 std_msgs::ColorRGBA color; 00820 color.r = 0.5; 00821 color.g = 0.5; 00822 color.b = 0.5; 00823 color.a = 1.0; 00824 00825 //need to get this in the right frame for adding the collision object 00826 //intentionally copying object 00827 arm_navigation_msgs::CollisionObject coll = att.object; 00828 { 00829 planning_models::KinematicState state(cm_->getKinematicModel()); 00830 planning_environment::setRobotStateAndComputeTransforms(scene.robot_state, 00831 state); 00832 geometry_msgs::PoseStamped ret_pose; 00833 cm_->convertPoseGivenWorldTransform(*robot_state_, 00834 cm_->getWorldFrameId(), 00835 coll.header, 00836 coll.poses[0], 00837 ret_pose); 00838 coll.header = ret_pose.header; 00839 coll.poses[0] = ret_pose.pose; 00840 } 00841 createSelectableMarkerFromCollisionObject(coll, coll.id, "", color); 00842 attachCollisionObject(att.object.id, 00843 att.link_name, 00844 att.touch_links); 00845 changeToAttached(att.object.id); 00846 } 00847 00848 void PlanningSceneEditor::setCurrentPlanningScene(std::string planning_scene_name, bool loadRequests, bool loadTrajectories) 00849 { 00850 if(planning_scene_map_.find(planning_scene_name) == planning_scene_map_.end()) { 00851 ROS_INFO_STREAM("Haven't got a planning scene for name " << planning_scene_name << " so can't set"); 00852 return; 00853 } 00854 00855 lockScene(); 00856 00857 // Need to do this to clear old scene state. 00858 deleteKinematicStates(); 00859 00860 if(planning_scene_name == "") 00861 { 00862 ROS_INFO_STREAM("No new scene"); 00863 current_planning_scene_name_ = planning_scene_name; 00864 unlockScene(); 00865 return; 00866 } 00867 00871 for(map<string, SelectableObject>::iterator it = selectable_objects_->begin(); it != selectable_objects_->end(); it++) 00872 { 00873 interactive_marker_server_->erase(it->second.selection_marker_.name); 00874 interactive_marker_server_->erase(it->second.control_marker_.name); 00875 } 00876 selectable_objects_->clear(); 00877 00878 for(map<string, IKController>::iterator it = (*ik_controllers_).begin(); it != (*ik_controllers_).end(); it++) 00879 { 00880 interactive_marker_server_->erase(it->second.end_controller_.name); 00881 interactive_marker_server_->erase(it->second.start_controller_.name); 00882 } 00883 interactive_marker_server_->applyChanges(); 00884 00885 (*ik_controllers_).clear(); 00886 00887 00888 std::vector<unsigned int> mprDeletions; 00892 for(map<string, MotionPlanRequestData>::iterator it = motion_plan_map_.begin(); it != motion_plan_map_.end(); it ++) 00893 { 00894 mprDeletions.push_back(it->second.getId()); 00895 } 00896 00897 std::vector<unsigned int> erased_trajectories; 00898 for(size_t i = 0; i < mprDeletions.size(); i++) 00899 { 00900 deleteMotionPlanRequest(mprDeletions[i], erased_trajectories); 00901 } 00902 00903 motion_plan_map_.clear(); 00904 00905 if(!trajectory_map_.empty()) { 00906 ROS_INFO_STREAM("Some trajectories orphaned"); 00907 } 00908 00912 current_planning_scene_name_ = planning_scene_name; 00913 PlanningSceneData& scene = planning_scene_map_[planning_scene_name]; 00914 error_map_.clear(); 00915 scene.getPipelineStages().clear(); 00916 scene.getErrorCodes().clear(); 00917 getPlanningSceneOutcomes(scene.getId(), scene.getPipelineStages(), scene.getErrorCodes(), error_map_); 00918 00922 for(size_t i = 0; i < scene.getPlanningScene().collision_objects.size(); i++) 00923 { 00924 //otherwise might be associated with an attached collision object 00925 std_msgs::ColorRGBA color; 00926 color.r = 0.5; 00927 color.g = 0.5; 00928 color.b = 0.5; 00929 color.a = 1.0; 00930 createSelectableMarkerFromCollisionObject(scene.getPlanningScene().collision_objects[i], scene.getPlanningScene().collision_objects[i].id, scene.getPlanningScene().collision_objects[i].id, color); 00931 } 00932 00936 for(size_t i = 0; i < scene.getPlanningScene().attached_collision_objects.size(); i++) 00937 { 00938 makeSelectableAttachedObjectFromPlanningScene(scene.getPlanningScene(), 00939 scene.getPlanningScene().attached_collision_objects[i]); 00940 } 00941 00945 if(loadRequests) 00946 { 00947 vector<unsigned int> ids; 00948 vector<string> stageNames; 00949 vector<MotionPlanRequest> requests; 00950 move_arm_warehouse_logger_reader_->getAssociatedMotionPlanRequests("", scene.getId(), ids, stageNames, requests); 00951 unsigned int planning_scene_id = getPlanningSceneIdFromName(planning_scene_name); 00952 initMotionPlanRequestData(planning_scene_id, ids, stageNames, requests); 00953 00954 for(size_t j = 0; j < ids.size(); j++) 00955 { 00956 MotionPlanRequest req; 00957 unsigned int motion_id = ids[j]; 00958 00959 MotionPlanRequestData& motion_data = motion_plan_map_[getMotionPlanRequestNameFromId(motion_id)]; 00960 motion_data.setPlanningSceneId(planning_scene_id); 00961 00962 std::vector<JointTrajectory> trajs; 00963 std::vector<string> sources; 00964 std::vector<unsigned int> traj_ids; 00965 std::vector<ros::Duration> durations; 00966 std::vector<int32_t> errors; 00967 00971 if(loadTrajectories) 00972 { 00973 move_arm_warehouse_logger_reader_->getAssociatedJointTrajectories("", scene.getId(), motion_id, trajs, sources, 00974 traj_ids, durations, errors); 00975 00976 for(size_t k = 0; k < trajs.size(); k++) 00977 { 00978 TrajectoryData trajectory_data; 00979 trajectory_data.setTrajectory(trajs[k]); 00980 trajectory_data.setSource(sources[k]); 00981 trajectory_data.setId(traj_ids[k]); 00982 trajectory_data.setMotionPlanRequestId(motion_data.getId()); 00983 trajectory_data.setPlanningSceneId(planning_scene_id); 00984 trajectory_data.setVisible(true); 00985 trajectory_data.setGroupName(motion_data.getGroupName()); 00986 trajectory_data.setDuration(durations[k]); 00987 trajectory_data.trajectory_error_code_.val = errors[k]; 00988 00989 motion_data.addTrajectoryId(trajectory_data.getId()); 00990 00991 if(hasTrajectory(motion_data.getName(), 00992 trajectory_data.getName())) { 00993 ROS_WARN_STREAM("Motion plan request " << motion_data.getName() << " really shouldn't already have a trajectory " << trajectory_data.getName()); 00994 } 00995 00996 trajectory_map_[motion_data.getName()][trajectory_data.getName()] = trajectory_data; 00997 //playTrajectory(motion_data,trajectory_map_[motion_data.getName()][trajectory_data.getName()]); 00998 } 00999 } 01000 } 01001 sendPlanningScene(scene); 01002 } 01003 01004 interactive_marker_server_->applyChanges(); 01005 unlockScene(); 01006 } 01007 01008 void PlanningSceneEditor::getTrajectoryMarkers(visualization_msgs::MarkerArray& arr) 01009 { 01010 trajectory_map_.erase(""); 01011 01012 // For each trajectory... 01013 for(map<string, map<string, TrajectoryData> >::iterator it = trajectory_map_.begin(); it != trajectory_map_.end(); it++) 01014 { 01015 for(map<string, TrajectoryData>::iterator it2 = it->second.begin(); it2 != it->second.end(); it2++) { 01016 if(it2->second.isPlaying()) 01017 { 01018 it2->second.moveThroughTrajectory(2); 01019 } 01020 01021 if(it2->second.getCurrentState() == NULL) 01022 { 01023 continue; 01024 } 01025 01026 it2->second.updateCurrentState(); 01027 01028 // When the color of a trajectory has changed, we have to wait for 01029 // a few milliseconds before the change is registered in rviz. 01030 if(it2->second.shouldRefreshColors()) 01031 { 01032 it2->second.refresh_timer_ += marker_dt_; 01033 01034 if(it2->second.refresh_timer_.toSec() > MARKER_REFRESH_TIME + 0.05) 01035 { 01036 it2->second.setHasRefreshedColors(true); 01037 it2->second.refresh_timer_ = ros::Duration(0.0); 01038 } 01039 } 01040 else 01041 { 01042 if(it2->second.isVisible()) 01043 { 01044 ROS_DEBUG_STREAM("Should be showing trajectory"); 01045 const vector<const KinematicModel::LinkModel*>& updated_links = 01046 cm_->getKinematicModel()->getModelGroup(it2->second.getGroupName())->getUpdatedLinkModels(); 01047 01048 vector<string> lnames; 01049 lnames.resize(updated_links.size()); 01050 for(unsigned int i = 0; i < updated_links.size(); i++) 01051 { 01052 lnames[i] = updated_links[i]->getName(); 01053 } 01054 01055 // Links in group 01056 switch(it2->second.getRenderType()) 01057 { 01058 case VisualMesh: 01059 cm_->getRobotMarkersGivenState(*(it2->second.getCurrentState()), arr, it2->second.getColor(), 01060 getMotionPlanRequestNameFromId(it2->second.getMotionPlanRequestId())+" "+it2->first + "_trajectory", 01061 ros::Duration(MARKER_REFRESH_TIME), 01062 &lnames, 1.0, false); 01063 // Bodies held by robot 01064 cm_->getAttachedCollisionObjectMarkers(*(it2->second.getCurrentState()), arr, 01065 getMotionPlanRequestNameFromId(it2->second.getMotionPlanRequestId())+" "+it2->first + "_trajectory", 01066 it2->second.getColor(), ros::Duration(MARKER_REFRESH_TIME), false, &lnames); 01067 01068 break; 01069 case CollisionMesh: 01070 cm_->getRobotMarkersGivenState(*(it2->second.getCurrentState()), arr, it2->second.getColor(), 01071 getMotionPlanRequestNameFromId(it2->second.getMotionPlanRequestId())+" "+it2->first + "_trajectory", 01072 ros::Duration(MARKER_REFRESH_TIME), 01073 &lnames, 1.0, true); 01074 cm_->getAttachedCollisionObjectMarkers(*(it2->second.getCurrentState()), arr, 01075 getMotionPlanRequestNameFromId(it2->second.getMotionPlanRequestId())+" "+it2->first + "_trajectory", 01076 it2->second.getColor(), ros::Duration(MARKER_REFRESH_TIME), false, &lnames); 01077 break; 01078 case PaddingMesh: 01079 cm_->getRobotPaddedMarkersGivenState((const KinematicState&)*(it2->second.getCurrentState()), 01080 arr, 01081 it2->second.getColor(), 01082 getMotionPlanRequestNameFromId(it2->second.getMotionPlanRequestId())+" "+it2->first + "_trajectory", 01083 ros::Duration(MARKER_REFRESH_TIME)*2.0, 01084 (const vector<string>*)&lnames); 01085 cm_->getAttachedCollisionObjectMarkers(*(it2->second.getCurrentState()), arr, 01086 getMotionPlanRequestNameFromId(it2->second.getMotionPlanRequestId())+" "+it2->first + "_trajectory", 01087 it2->second.getColor(), ros::Duration(MARKER_REFRESH_TIME)*2.0, true, &lnames); 01088 break; 01089 } 01090 01091 } 01092 } 01093 01094 01098 if(it2->second.areCollisionsVisible() && it2->second.isVisible()) 01099 { 01100 01101 if(motion_plan_map_.find(getMotionPlanRequestNameFromId(it2->second.getMotionPlanRequestId())) == motion_plan_map_.end()) { 01102 ROS_INFO_STREAM("Making empty mprs in trajectory"); 01103 continue; 01104 } 01105 01106 // Update markers 01107 if(it2->second.hasStateChanged()) 01108 { 01109 if(params_.proximity_space_validity_name_ == "none") 01110 { 01111 it2->second.updateCollisionMarkers(cm_, motion_plan_map_[getMotionPlanRequestNameFromId(it2->second.getMotionPlanRequestId())], 01112 NULL); 01113 01114 } else { 01115 it2->second.updateCollisionMarkers(cm_, motion_plan_map_[getMotionPlanRequestNameFromId(it2->second.getMotionPlanRequestId())], 01116 &distance_state_validity_service_client_); 01117 01118 } 01119 it2->second.setStateChanged(false); 01120 } 01121 01122 // Then add them to the global array 01123 for(size_t i = 0; i < it2->second.getCollisionMarkers().markers.size(); i++) 01124 { 01125 collision_markers_.markers.push_back(it2->second.getCollisionMarkers().markers[i]); 01126 } 01127 } 01128 } 01129 } 01130 } 01131 01132 void PlanningSceneEditor::getMotionPlanningMarkers(visualization_msgs::MarkerArray& arr) 01133 { 01134 vector<string> removals; 01135 01136 // For each motion plan request ... 01137 for(map<string, MotionPlanRequestData>::iterator it = motion_plan_map_.begin(); it != motion_plan_map_.end(); it++) 01138 { 01139 if(it->second.getName() == "") { 01140 ROS_WARN("Someone's making empty stuff"); 01141 } 01142 MotionPlanRequestData& data = it->second; 01143 01144 // TODO: Find out why this happens. 01145 if(motion_plan_map_.find(it->first) == motion_plan_map_.end() || data.getName() == "") 01146 { 01147 ROS_WARN("Attempting to publish non-existant motion plan request %s Erasing this request!", it->first.c_str()); 01148 removals.push_back(it->first); 01149 continue; 01150 } 01151 01152 // TODO: Find out why this happens. 01153 if(data.getStartState() == NULL || data.getGoalState() == NULL) 01154 { 01155 return; 01156 } 01157 01158 // When a motion plan request has its colors changed, 01159 // we must wait a few milliseconds before rviz registers the change. 01160 if(data.shouldRefreshColors()) 01161 { 01162 data.refresh_timer_ += marker_dt_; 01163 01164 if(data.refresh_timer_.toSec() > MARKER_REFRESH_TIME + 0.05) 01165 { 01166 data.setHasRefreshedColors(true); 01167 data.refresh_timer_ = ros::Duration(0.0); 01168 } 01169 } 01170 else 01171 { 01172 std_msgs::ColorRGBA fail_color; 01173 fail_color.a = 0.9; 01174 fail_color.r = 1.0; 01175 fail_color.g = 0.0; 01176 fail_color.b = 0.0; 01177 01181 if(data.isStartVisible()) 01182 { 01183 const vector<const KinematicModel::LinkModel*>& updated_links = 01184 cm_->getKinematicModel()->getModelGroup(data.getMotionPlanRequest().group_name)->getUpdatedLinkModels(); 01185 01186 vector<string> lnames; 01187 lnames.resize(updated_links.size()); 01188 for(unsigned int i = 0; i < updated_links.size(); i++) 01189 { 01190 lnames[i] = updated_links[i]->getName(); 01191 } 01192 01193 01194 // If we have a good ik solution, publish with the normal color 01195 // else use bright red. 01196 std_msgs::ColorRGBA col; 01197 if(data.hasGoodIKSolution(StartPosition)) 01198 { 01199 col = data.getStartColor(); 01200 } else { 01201 col = fail_color; 01202 } 01203 01204 switch(data.getRenderType()) 01205 { 01206 case VisualMesh: 01207 cm_->getRobotMarkersGivenState(*(data.getStartState()), arr, col, 01208 it->first + "_start", ros::Duration(MARKER_REFRESH_TIME), 01209 &lnames, 1.0, false); 01210 // Bodies held by robot 01211 cm_->getAttachedCollisionObjectMarkers(*(data.getStartState()), arr, it->first + "_start", 01212 col, ros::Duration(MARKER_REFRESH_TIME), false, &lnames); 01213 01214 break; 01215 case CollisionMesh: 01216 cm_->getRobotMarkersGivenState(*(data.getStartState()), arr, col, 01217 it->first + "_start", ros::Duration(MARKER_REFRESH_TIME), 01218 &lnames, 1.0, true); 01219 cm_->getAttachedCollisionObjectMarkers(*(data.getStartState()), arr, it->first + "_start", 01220 col, ros::Duration(MARKER_REFRESH_TIME), false, &lnames); 01221 break; 01222 case PaddingMesh: 01223 cm_->getRobotPaddedMarkersGivenState(*(data.getStartState()), 01224 arr, 01225 col, 01226 it->first + "_start", 01227 ros::Duration(MARKER_REFRESH_TIME), 01228 (const vector<string>*)&lnames); 01229 cm_->getAttachedCollisionObjectMarkers(*(data.getStartState()), arr, it->first + "_start", 01230 col, ros::Duration(MARKER_REFRESH_TIME), true, &lnames); 01231 break; 01232 } 01233 } 01234 01238 if(data.isEndVisible()) 01239 { 01240 const vector<const KinematicModel::LinkModel*>& updated_links = 01241 cm_->getKinematicModel()->getModelGroup(data.getMotionPlanRequest().group_name)->getUpdatedLinkModels(); 01242 01243 vector<string> lnames; 01244 lnames.resize(updated_links.size()); 01245 for(unsigned int i = 0; i < updated_links.size(); i++) 01246 { 01247 lnames[i] = updated_links[i]->getName(); 01248 } 01249 01250 std_msgs::ColorRGBA col; 01251 if(data.hasGoodIKSolution(GoalPosition)) 01252 { 01253 col = data.getGoalColor(); 01254 } else { 01255 col = fail_color; 01256 } 01257 01258 switch(data.getRenderType()) 01259 { 01260 case VisualMesh: 01261 cm_->getRobotMarkersGivenState(*(data.getGoalState()), arr, col, 01262 it->first + "_Goal", ros::Duration(MARKER_REFRESH_TIME), 01263 &lnames, 1.0, false); 01264 01265 // Bodies held by robot 01266 cm_->getAttachedCollisionObjectMarkers(*(data.getGoalState()), arr, it->first + "_Goal", 01267 col, ros::Duration(MARKER_REFRESH_TIME), false, &lnames); 01268 01269 break; 01270 case CollisionMesh: 01271 cm_->getRobotMarkersGivenState(*(data.getGoalState()), arr, col, 01272 it->first + "_Goal", ros::Duration(MARKER_REFRESH_TIME), 01273 &lnames, 1.0, true); 01274 cm_->getAttachedCollisionObjectMarkers(*(data.getGoalState()), arr, it->first + "_Goal", 01275 col, ros::Duration(MARKER_REFRESH_TIME), false, &lnames); 01276 break; 01277 case PaddingMesh: 01278 cm_->getRobotPaddedMarkersGivenState(*(data.getGoalState()), 01279 arr, 01280 col, 01281 it->first + "_Goal", 01282 ros::Duration(MARKER_REFRESH_TIME), 01283 (const vector<string>*)&lnames); 01284 cm_->getAttachedCollisionObjectMarkers(*(data.getGoalState()), arr, it->first + "_Goal", 01285 col, ros::Duration(MARKER_REFRESH_TIME), true, &lnames); 01286 break; 01287 } 01288 } 01289 } 01290 01294 if(it->second.areCollisionsVisible() && (it->second.isStartVisible() || it->second.isEndVisible())) 01295 { 01296 // Update collision markers 01297 if(it->second.hasStateChanged()) 01298 { 01299 if(params_.proximity_space_validity_name_ == "none") 01300 { 01301 it->second.updateCollisionMarkers(cm_, NULL); 01302 } else { 01303 it->second.updateCollisionMarkers(cm_, &distance_state_validity_service_client_); 01304 } 01305 it->second.setStateChanged(false); 01306 } 01307 01308 // Add them to the global array. 01309 for(size_t i = 0; i < it->second.getCollisionMarkers().markers.size(); i++) 01310 { 01311 collision_markers_.markers.push_back(it->second.getCollisionMarkers().markers[i]); 01312 } 01313 } 01314 01315 } 01316 01320 for(size_t i = 0; i < removals.size(); i++) 01321 { 01322 motion_plan_map_.erase(removals[i]); 01323 } 01324 } 01325 01326 void PlanningSceneEditor::createMotionPlanRequest(const planning_models::KinematicState& start_state, 01327 const planning_models::KinematicState& end_state, 01328 const std::string& group_name, 01329 const std::string& end_effector_name, 01330 const bool constrain, 01331 const unsigned int& planning_scene_id, 01332 const bool from_robot_state, 01333 unsigned int& motion_plan_id_out) 01334 01335 { 01336 MotionPlanRequest motion_plan_request; 01337 motion_plan_request.group_name = group_name; 01338 motion_plan_request.num_planning_attempts = 1; 01339 motion_plan_request.allowed_planning_time = ros::Duration(1); 01340 const KinematicState::JointStateGroup* jsg = end_state.getJointStateGroup(group_name); 01341 motion_plan_request.goal_constraints.joint_constraints.resize(jsg->getJointNames().size()); 01342 01343 // Must convert kinematic state to robot state message. 01344 vector<double> joint_values; 01345 jsg->getKinematicStateValues(joint_values); 01346 for(unsigned int i = 0; i < jsg->getJointNames().size(); i++) 01347 { 01348 motion_plan_request.goal_constraints.joint_constraints[i].joint_name = jsg->getJointNames()[i]; 01349 motion_plan_request.goal_constraints.joint_constraints[i].position = joint_values[i]; 01350 motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.001; 01351 motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.001; 01352 } 01353 01354 // Constraining pitch and roll 01355 if(constrain) 01356 { 01357 motion_plan_request.group_name += "_cartesian"; 01358 motion_plan_request.goal_constraints.position_constraints.resize(1); 01359 motion_plan_request.goal_constraints.orientation_constraints.resize(1); 01360 geometry_msgs::PoseStamped end_effector_wrist_pose; 01361 tf::poseTFToMsg(end_state.getLinkState(end_effector_name)->getGlobalLinkTransform(), end_effector_wrist_pose.pose); 01362 end_effector_wrist_pose.header.frame_id = cm_->getWorldFrameId(); 01363 poseStampedToPositionOrientationConstraints(end_effector_wrist_pose, end_effector_name, 01364 motion_plan_request.goal_constraints.position_constraints[0], 01365 motion_plan_request.goal_constraints.orientation_constraints[0]); 01366 motion_plan_request.path_constraints.orientation_constraints.resize(1); 01367 determinePitchRollConstraintsGivenState(end_state, end_effector_name, 01368 motion_plan_request.goal_constraints.orientation_constraints[0], 01369 motion_plan_request.path_constraints.orientation_constraints[0]); 01370 } 01371 01372 // Create start state from kinematic state passed in if robot data is being used 01373 if(!from_robot_state) 01374 { 01375 convertKinematicStateToRobotState(start_state, ros::Time(ros::WallTime::now().toSec()), cm_->getWorldFrameId(), 01376 motion_plan_request.start_state); 01377 } 01378 // Otherwise, use the current robot state. 01379 else 01380 { 01381 convertKinematicStateToRobotState(*robot_state_, ros::Time(ros::WallTime::now().toSec()), cm_->getWorldFrameId(), 01382 motion_plan_request.start_state); 01383 } 01384 01385 if(planning_scene_map_.find(getPlanningSceneNameFromId(planning_scene_id)) == planning_scene_map_.end()) { 01386 ROS_WARN_STREAM("Creating new planning scene for motion plan request - bad!!"); 01387 } 01388 01389 PlanningSceneData& planningSceneData = planning_scene_map_[getPlanningSceneNameFromId(planning_scene_id)]; 01390 01391 // Turn the motion plan request message into a MotionPlanData 01392 unsigned int id = planningSceneData.getNextMotionPlanRequestId(); 01393 MotionPlanRequestData data(id, "Planner", motion_plan_request, robot_state_); 01394 data.setGroupName(motion_plan_request.group_name); 01395 data.setEndEffectorLink(end_effector_name); 01396 data.setGoalEditable(true); 01397 if(from_robot_state) 01398 { 01399 data.setStartEditable(false); 01400 } 01401 01402 // Book keeping for kinematic state storage 01403 StateRegistry start; 01404 start.state = data.getStartState(); 01405 start.source = "Motion Plan Request Data Start create request"; 01406 StateRegistry end; 01407 end.state = data.getGoalState(); 01408 end.source = "Motion Plan Request Data End from create request"; 01409 states_.push_back(start); 01410 states_.push_back(end); 01411 01412 motion_plan_map_[getMotionPlanRequestNameFromId(id)] = data; 01413 data.setPlanningSceneId(planning_scene_id); 01414 01415 // Add request to the planning scene 01416 planningSceneData.addMotionPlanRequestId(id); 01417 01418 motion_plan_id_out = data.getId(); 01419 createIkControllersFromMotionPlanRequest(data, false); 01420 sendPlanningScene(planningSceneData); 01421 } 01422 01423 bool PlanningSceneEditor::planToKinematicState(const KinematicState& state, const string& group_name, const string& end_effector_name, 01424 const bool constrain, unsigned int& trajectory_id_out, unsigned int& planning_scene_id) 01425 { 01426 unsigned int motion_plan_id; 01427 createMotionPlanRequest(*robot_state_, state, group_name, end_effector_name, constrain, planning_scene_id, 01428 false, motion_plan_id); 01429 MotionPlanRequestData& data = motion_plan_map_[getMotionPlanRequestNameFromId(motion_plan_id)]; 01430 return planToRequest(data, trajectory_id_out); 01431 } 01432 01433 bool PlanningSceneEditor::planToRequest(const std::string& request_id, unsigned int& trajectory_id_out) 01434 { 01435 return planToRequest(motion_plan_map_[request_id], trajectory_id_out); 01436 } 01437 01438 bool PlanningSceneEditor::planToRequest(MotionPlanRequestData& data, unsigned int& trajectory_id_out) 01439 { 01440 GetMotionPlan::Request plan_req; 01441 ros::ServiceClient* planner; 01442 std::string source; 01443 if(use_interpolated_planner_) { 01444 source = "Interpolator"; 01445 arm_navigation_msgs::MotionPlanRequest req; 01446 req.group_name = data.getGroupName(); 01447 req.num_planning_attempts = 1; 01448 req.allowed_planning_time = ros::Duration(10.0); 01449 req.start_state = data.getMotionPlanRequest().start_state; 01450 req.start_state.multi_dof_joint_state.child_frame_ids[0] = data.getEndEffectorLink(); 01451 req.start_state.multi_dof_joint_state.frame_ids[0] = cm_->getWorldFrameId(); 01452 tf::poseTFToMsg(data.getStartState()->getLinkState(data.getEndEffectorLink())->getGlobalLinkTransform(), 01453 req.start_state.multi_dof_joint_state.poses[0]); 01454 geometry_msgs::PoseStamped end_effector_wrist_pose; 01455 tf::poseTFToMsg(data.getGoalState()->getLinkState(data.getEndEffectorLink())->getGlobalLinkTransform(), 01456 end_effector_wrist_pose.pose); 01457 end_effector_wrist_pose.header.frame_id = cm_->getWorldFrameId(); 01458 req.goal_constraints.position_constraints.resize(1); 01459 req.goal_constraints.orientation_constraints.resize(1); 01460 01461 arm_navigation_msgs::poseStampedToPositionOrientationConstraints(end_effector_wrist_pose, 01462 data.getEndEffectorLink(), 01463 req.goal_constraints.position_constraints[0], 01464 req.goal_constraints.orientation_constraints[0]); 01465 01466 plan_req.motion_plan_request = req; 01467 planner = (*interpolated_ik_services_)[data.getEndEffectorLink()]; 01468 } else { 01469 source = "Planner"; 01470 planner = &planning_service_client_; 01471 plan_req.motion_plan_request = data.getMotionPlanRequest(); 01472 plan_req.motion_plan_request.allowed_planning_time = ros::Duration(10.0); 01473 } 01474 GetMotionPlan::Response plan_res; 01475 01476 if(!planner->call(plan_req, plan_res)) 01477 { 01478 ROS_INFO("Something wrong with planner client"); 01479 planCallback(plan_res.error_code); 01480 return false; 01481 } 01482 01483 unsigned int id = data.getNextTrajectoryId(); 01484 01485 TrajectoryData trajectory_data; 01486 trajectory_data.setTrajectory(plan_res.trajectory.joint_trajectory); 01487 trajectory_data.setGroupName(data.getMotionPlanRequest().group_name); 01488 trajectory_data.setMotionPlanRequestId(data.getId()); 01489 trajectory_id_out = id; 01490 trajectory_data.setPlanningSceneId(data.getPlanningSceneId()); 01491 trajectory_data.setId(trajectory_id_out); 01492 trajectory_data.setSource(source); 01493 trajectory_data.setDuration(plan_res.planning_time); 01494 trajectory_data.setVisible(true); 01495 trajectory_data.setPlaying(true); 01496 01497 bool success = (plan_res.error_code.val != plan_res.error_code.SUCCESS); 01498 trajectory_data.trajectory_error_code_.val = plan_res.error_code.val; 01499 trajectory_map_[data.getName()][trajectory_data.getName()] = trajectory_data; 01500 data.addTrajectoryId(id); 01501 01502 if(success) { 01503 selected_trajectory_name_ = trajectory_data.getId(); 01504 } else { 01505 ROS_INFO_STREAM("Bad planning error code " << plan_res.error_code.val); 01506 } 01507 planCallback(trajectory_data.trajectory_error_code_); 01508 return success; 01509 } 01510 01511 void PlanningSceneEditor::determinePitchRollConstraintsGivenState(const KinematicState& state, 01512 const std::string& end_effector_link, 01513 OrientationConstraint& goal_constraint, 01514 OrientationConstraint& path_constraint) 01515 { 01516 btTransform cur = state.getLinkState(end_effector_link)->getGlobalLinkTransform(); 01517 //btScalar roll, pitch, yaw; 01518 //cur.getBasis().getRPY(roll,pitch,yaw); 01519 goal_constraint.header.frame_id = cm_->getWorldFrameId(); 01520 goal_constraint.header.stamp = ros::Time(ros::WallTime::now().toSec()); 01521 goal_constraint.link_name = end_effector_link; 01522 tf::quaternionTFToMsg(cur.getRotation(), goal_constraint.orientation); 01523 goal_constraint.absolute_roll_tolerance = 0.04; 01524 goal_constraint.absolute_pitch_tolerance = 0.04; 01525 goal_constraint.absolute_yaw_tolerance = M_PI; 01526 path_constraint.header.frame_id = cm_->getWorldFrameId(); 01527 path_constraint.header.stamp = ros::Time(ros::WallTime::now().toSec()); 01528 path_constraint.link_name = end_effector_link; 01529 tf::quaternionTFToMsg(cur.getRotation(), path_constraint.orientation); 01530 path_constraint.type = path_constraint.HEADER_FRAME; 01531 path_constraint.absolute_roll_tolerance = 0.1; 01532 path_constraint.absolute_pitch_tolerance = 0.1; 01533 path_constraint.absolute_yaw_tolerance = M_PI; 01534 } 01535 01536 void PlanningSceneEditor::printTrajectoryPoint(const vector<string>& joint_names, const vector<double>& joint_values) 01537 { 01538 for(unsigned int i = 0; i < joint_names.size(); i++) 01539 { 01540 ROS_INFO_STREAM("Joint name " << joint_names[i] << " value " << joint_values[i]); 01541 } 01542 } 01543 01544 bool PlanningSceneEditor::filterTrajectory(MotionPlanRequestData& requestData, TrajectoryData& trajectory, 01545 unsigned int& filter_id) 01546 { 01547 FilterJointTrajectoryWithConstraints::Request filter_req; 01548 FilterJointTrajectoryWithConstraints::Response filter_res; 01549 01550 // Filter request has to have robot state message filled 01551 convertKinematicStateToRobotState(*robot_state_, ros::Time(ros::WallTime::now().toSec()), cm_->getWorldFrameId(), 01552 filter_req.start_state); 01553 01554 filter_req.trajectory = trajectory.getTrajectory(); 01555 filter_req.group_name = trajectory.getGroupName(); 01556 filter_req.goal_constraints = requestData.getMotionPlanRequest().goal_constraints; 01557 filter_req.path_constraints = requestData.getMotionPlanRequest().path_constraints; 01558 filter_req.allowed_time = ros::Duration(2.0); 01559 01560 // Time the filtering 01561 ros::Time startTime = ros::Time(ros::WallTime::now().toSec()); 01562 if(!trajectory_filter_service_client_.call(filter_req, filter_res)) 01563 { 01564 ROS_INFO("Problem with trajectory filter"); 01565 filterCallback(filter_res.error_code); 01566 return false; 01567 } 01568 01569 unsigned int id = requestData.getNextTrajectoryId(); 01570 01571 // Convert returned joint trajectory to TrajectoryData 01572 TrajectoryData data(id, "Trajectory Filterer", trajectory.getGroupName(), 01573 filter_res.trajectory); 01574 data.setPlanningSceneId(requestData.getPlanningSceneId()); 01575 data.setMotionPlanRequestId(requestData.getId()); 01576 data.setDuration(ros::Time(ros::WallTime::now().toSec()) - startTime); 01577 requestData.addTrajectoryId(id); 01578 01579 data.trajectory_error_code_.val = filter_res.error_code.val; 01580 trajectory_map_[requestData.getName()][data.getName()] = data; 01581 filter_id = data.getId(); 01582 data.setVisible(true); 01583 data.play(); 01584 selected_trajectory_name_ = data.getName(); 01585 01586 bool success = (filter_res.error_code.val == filter_res.error_code.SUCCESS); 01587 if(!success) { 01588 ROS_INFO_STREAM("Bad trajectory_filter error code " << filter_res.error_code.val); 01589 } else { 01590 playTrajectory(requestData, trajectory_map_[requestData.getName()][data.getName()]); 01591 } 01592 filterCallback(filter_res.error_code); 01593 return success; 01594 } 01595 01596 void PlanningSceneEditor::updateJointStates() 01597 { 01598 01599 // If using robot data, the joint states are handled by whatever is 01600 // running on the robot (presumably) 01601 if(params_.use_robot_data_) 01602 { 01603 return; 01604 } 01605 01606 sensor_msgs::JointState msg; 01607 msg.header.frame_id = cm_->getWorldFrameId(); 01608 msg.header.stamp = ros::Time(ros::WallTime::now().toSec()); 01609 01610 vector<KinematicState::JointState*> jointStates = getRobotState()->getJointStateVector(); 01611 01612 map<string, double> stateMap; 01613 getRobotState()->getKinematicStateValues(stateMap); 01614 getRobotState()->setKinematicState(stateMap); 01615 01616 // Send each joint state out as part of a message to the robot state publisher. 01617 for(size_t i = 0; i < jointStates.size(); i++) 01618 { 01619 KinematicState::JointState* state = jointStates[i]; 01620 msg.name.push_back(state->getName()); 01621 01622 // Assume that joints only have one value. 01623 if(state->getJointStateValues().size() > 0) 01624 { 01625 msg.position.push_back(state->getJointStateValues()[0]); 01626 } 01627 else 01628 { 01629 msg.position.push_back(0.0f); 01630 } 01631 } 01632 joint_state_publisher_.publish(msg); 01633 01634 } 01635 01636 void PlanningSceneEditor::sendMarkers() 01637 { 01638 marker_dt_ = (ros::Time::now() - last_marker_start_time_); 01639 last_marker_start_time_ = ros::Time::now(); 01640 lockScene(); 01641 sendTransformsAndClock(); 01642 visualization_msgs::MarkerArray arr; 01643 01644 getTrajectoryMarkers(arr); 01645 getMotionPlanningMarkers(arr); 01646 01647 vis_marker_array_publisher_.publish(arr); 01648 vis_marker_array_publisher_.publish(collision_markers_); 01649 collision_markers_.markers.clear(); 01650 unlockScene(); 01651 } 01652 01653 bool PlanningSceneEditor::getPlanningSceneOutcomes(const unsigned int id, 01654 vector<string>& pipeline_stages, 01655 vector<ArmNavigationErrorCodes>& error_codes, 01656 map<std::string, ArmNavigationErrorCodes>& error_map) 01657 { 01658 if(!move_arm_warehouse_logger_reader_->getAssociatedOutcomes("", id, pipeline_stages, error_codes)) 01659 { 01660 ROS_DEBUG_STREAM("No outcome associated with planning scene"); 01661 return false; 01662 } 01663 01664 // Fill error map for convenience 01665 for(size_t i = 0; i < error_codes.size(); i++) 01666 { 01667 error_map[pipeline_stages[i]] = error_codes[i]; 01668 } 01669 01670 return true; 01671 } 01672 01673 std::string PlanningSceneEditor::createNewPlanningScene() 01674 { 01675 lock_scene_.lock(); 01676 01677 if(robot_state_ == NULL) 01678 { 01679 robot_state_ = new KinematicState(cm_->getKinematicModel()); 01680 } else { 01681 if(robot_state_joint_values_.empty()) { 01682 robot_state_->setKinematicStateToDefault(); 01683 } else { 01684 robot_state_->setKinematicState(robot_state_joint_values_); 01685 } 01686 } 01687 01688 PlanningSceneData data; 01689 data.setId(generateNewPlanningSceneId()); 01690 data.setTimeStamp(ros::Time(ros::WallTime::now().toSec())); 01691 01692 convertKinematicStateToRobotState(*robot_state_, data.getTimeStamp(), cm_->getWorldFrameId(), 01693 data.getPlanningScene().robot_state); 01694 //end_effector_state_ = planning_state_; 01695 01696 std::vector<string> collisionObjects; 01697 01698 for(map<string, SelectableObject>::iterator it = selectable_objects_->begin(); it != selectable_objects_->end(); it++) 01699 { 01700 collisionObjects.push_back(it->first); 01701 } 01702 01703 //this also does attached collision objects 01704 for(size_t i = 0; i < collisionObjects.size(); i++) 01705 { 01706 deleteCollisionObject(collisionObjects[i]); 01707 } 01708 01709 selectable_objects_->clear(); 01710 01711 char hostname[256]; 01712 gethostname(hostname, 256); 01713 data.setHostName(std::string(hostname)); 01714 01715 planning_scene_map_[data.getName()] = data; 01716 lock_scene_.unlock(); 01717 01718 updateJointStates(); 01719 01720 return data.getName(); 01721 } 01722 01723 void PlanningSceneEditor::loadAllWarehouseData() 01724 { 01725 max_collision_object_id_ = 0; 01726 01727 motion_plan_map_.clear(); 01728 trajectory_map_.clear(); 01729 planning_scene_map_.clear(); 01730 vector<ros::Time> planning_scene_times; 01731 vector<unsigned int> planning_scene_ids; 01732 getAllPlanningSceneTimes(planning_scene_times, 01733 planning_scene_ids); 01734 01735 ROS_INFO_STREAM("Starting load"); 01736 01737 // For each planning scene 01738 for(size_t i = 0; i < planning_scene_times.size(); i++) 01739 { 01740 ros::Time& time = planning_scene_times[i]; 01741 // Load it 01742 loadPlanningScene(time, planning_scene_ids[i]); 01743 01744 ROS_DEBUG_STREAM("Got planning scene " << planning_scene_ids[i] << " from warehouse."); 01745 PlanningSceneData& data = planning_scene_map_[getPlanningSceneNameFromId(planning_scene_ids[i])]; 01746 data.getPipelineStages().clear(); 01747 data.getErrorCodes().clear(); 01748 getPlanningSceneOutcomes(planning_scene_ids[i], data.getPipelineStages(), data.getErrorCodes(), error_map_); 01749 onPlanningSceneLoaded((int)i, (int)planning_scene_times.size()); 01750 } 01751 01752 error_map_.clear(); 01753 } 01754 01755 void PlanningSceneEditor::savePlanningScene(PlanningSceneData& data, bool copy) 01756 { 01757 PlanningScene* actual_planning_scene; 01758 unsigned int id_to_push; 01759 01760 std::string name_to_push = ""; 01761 01762 warehouse_data_loaded_once_ = false; 01763 01764 //overwriting timestamp 01765 data.setTimeStamp(ros::Time(ros::WallTime::now().toSec())); 01766 01767 // Have to do this in case robot state was corrupted by sim time. 01768 if(!copy) { 01769 bool has; 01770 has = move_arm_warehouse_logger_reader_->hasPlanningScene(data.getHostName(), 01771 data.getId()); 01772 //ROS_INFO_STREAM("Has is " << has); 01773 01774 if(has) { 01775 move_arm_warehouse_logger_reader_->removePlanningSceneAndAssociatedDataFromWarehouse(data.getHostName(), 01776 data.getId()); 01777 } 01778 id_to_push = data.getId(); 01779 01780 actual_planning_scene = &(data.getPlanningScene()); 01781 01782 ROS_INFO("Saving Planning Scene %s", data.getName().c_str()); 01783 } else { 01784 //force reload 01785 PlanningSceneData ndata = data; 01786 ndata.setId(generateNewPlanningSceneId()); 01787 id_to_push = ndata.getId(); 01788 ROS_INFO("Copying Planning Scene %s to %s", data.getName().c_str(), ndata.getName().c_str()); 01789 planning_scene_map_[ndata.getName()] = ndata; 01790 name_to_push = ndata.getName(); 01791 actual_planning_scene = &planning_scene_map_[ndata.getName()].getPlanningScene(); 01792 } 01793 01794 move_arm_warehouse_logger_reader_->pushPlanningSceneToWarehouse(*actual_planning_scene, 01795 id_to_push); 01796 01797 for(std::set<unsigned int>::iterator it = data.getRequests().begin(); it != data.getRequests().end(); it++) { 01798 MotionPlanRequestData& req = motion_plan_map_[getMotionPlanRequestNameFromId(*it)]; 01799 move_arm_warehouse_logger_reader_->pushMotionPlanRequestToWarehouse(id_to_push, 01800 req.getId(), 01801 req.getSource(), 01802 req.getMotionPlanRequest()); 01803 ROS_DEBUG_STREAM("Saving Request " << req.getId()); 01804 for(std::set<unsigned int>::iterator it2 = req.getTrajectories().begin(); it2 != req.getTrajectories().end(); it2++) { 01805 TrajectoryData& traj = trajectory_map_[req.getName()][getTrajectoryNameFromId(*it2)]; 01806 move_arm_warehouse_logger_reader_->pushJointTrajectoryToWarehouse(id_to_push, 01807 traj.getSource(), 01808 traj.getDuration(), 01809 traj.getTrajectory(), 01810 traj.getId(), 01811 traj.getMotionPlanRequestId(), 01812 traj.trajectory_error_code_); 01813 move_arm_warehouse_logger_reader_->pushOutcomeToWarehouse(id_to_push, 01814 traj.getSource(), 01815 traj.trajectory_error_code_); 01816 ROS_DEBUG_STREAM("Saving Trajectory " << traj.getId()); 01817 } 01818 } 01819 if(!name_to_push.empty()) { 01820 setCurrentPlanningScene(name_to_push); 01821 } 01822 } 01823 01824 bool PlanningSceneEditor::getAllPlanningSceneTimes(vector<ros::Time>& planning_scene_times, 01825 vector<unsigned int>& planning_scene_ids) 01826 { 01827 move_arm_warehouse_logger_reader_->getAvailablePlanningSceneList("", 01828 planning_scene_ids, 01829 last_creation_time_query_); 01830 planning_scene_times = last_creation_time_query_; 01831 return true; 01832 } 01833 01834 bool PlanningSceneEditor::loadPlanningScene(const ros::Time& time, 01835 const unsigned int id) 01836 { 01837 PlanningSceneData data; 01838 data.setTimeStamp(time); 01839 data.setId(id); 01840 std::string host; 01841 if(!move_arm_warehouse_logger_reader_->getPlanningScene("", id, data.getPlanningScene(), host)) 01842 { 01843 return false; 01844 } 01845 01846 data.setHostName(host); 01847 01848 std::pair<string, PlanningSceneData> p(data.getName(), data); 01849 planning_scene_map_.insert(p); 01850 return true; 01851 } 01852 01853 bool PlanningSceneEditor::getAllAssociatedMotionPlanRequests(const unsigned int id, 01854 vector<unsigned int>& ids, 01855 vector<string>& stages, 01856 vector<MotionPlanRequest>& requests) 01857 { 01858 move_arm_warehouse_logger_reader_->getAssociatedMotionPlanRequests("", id, ids, stages, requests); 01859 return true; 01860 } 01861 01862 void PlanningSceneEditor::deleteKinematicStates() 01863 { 01864 lockScene(); 01865 std::vector<KinematicState*> removals; 01866 for(map<string, map<string, TrajectoryData> >::iterator it = trajectory_map_.begin(); it != trajectory_map_.end(); it++) 01867 { 01868 for(map<string, TrajectoryData>::iterator it2 = it->second.begin(); it2 != it->second.end(); it2++) { 01869 removals.push_back(it2->second.getCurrentState()); 01870 it2->second.reset(); 01871 } 01872 } 01873 01874 for(map<string, MotionPlanRequestData>::iterator it = motion_plan_map_.begin(); it != motion_plan_map_.end(); it++) 01875 { 01876 removals.push_back(it->second.getStartState()); 01877 removals.push_back(it->second.getGoalState()); 01878 it->second.reset(); 01879 } 01880 01881 for(size_t i = 0; i < states_.size(); i++) 01882 { 01883 if(states_[i].state != NULL) 01884 { 01885 bool shouldBreak = false; 01886 for(size_t j = 0; j < removals.size(); j++) 01887 { 01888 if(states_[i].state == removals[j]) 01889 { 01890 shouldBreak = true; 01891 break; 01892 } 01893 } 01894 01895 if(shouldBreak) 01896 { 01897 continue; 01898 } 01899 ROS_INFO("Missed a state from %s!", states_[i].source.c_str()); 01900 delete states_[i].state; 01901 states_[i].state = NULL; 01902 } 01903 } 01904 states_.clear(); 01905 unlockScene(); 01906 } 01907 01908 bool PlanningSceneEditor::sendPlanningScene(PlanningSceneData& data) 01909 { 01910 lockScene(); 01911 last_collision_set_error_code_.val = 0; 01912 01913 SetPlanningSceneDiff::Request planning_scene_req; 01914 SetPlanningSceneDiff::Response planning_scene_res; 01915 01916 planning_scene_req.planning_scene_diff = data.getPlanningScene(); 01917 if(params_.use_robot_data_) { 01918 //just will get latest data from the monitor 01919 arm_navigation_msgs::RobotState emp; 01920 planning_scene_req.planning_scene_diff.robot_state = emp; 01921 } 01922 01923 collision_space::EnvironmentModel::AllowedCollisionMatrix acm; 01924 if(planning_scene_req.planning_scene_diff.allowed_collision_matrix.link_names.empty()) { 01925 acm = cm_->getDefaultAllowedCollisionMatrix(); 01926 } else { 01927 acm = planning_environment::convertFromACMMsgToACM(planning_scene_req.planning_scene_diff.allowed_collision_matrix); 01928 } 01929 planning_scene_req.planning_scene_diff.collision_objects = std::vector<CollisionObject>(); 01930 planning_scene_req.planning_scene_diff.attached_collision_objects = std::vector<AttachedCollisionObject>(); 01931 deleteKinematicStates(); 01932 01933 if(robot_state_ != NULL) 01934 { 01935 cm_->revertPlanningScene(robot_state_); 01936 robot_state_ = NULL; 01937 } 01938 01939 vector<string> removals; 01940 // Handle additions and removals of planning scene objects. 01941 for(map<string, SelectableObject>::iterator it = (*selectable_objects_).begin(); it 01942 != (*selectable_objects_).end(); it++) 01943 { 01944 string name = it->first; 01945 arm_navigation_msgs::CollisionObject& object = it->second.collision_object_; 01946 01947 if(it->second.attach_) { 01948 if(acm.hasEntry(object.id)) { 01949 acm.changeEntry(object.id, false); 01950 } else { 01951 acm.addEntry(object.id, false); 01952 } 01953 //first doing group expansion of touch links 01954 std::vector<std::string>& touch_links = it->second.attached_collision_object_.touch_links; 01955 std::vector<std::string> modded_touch_links; 01956 for(unsigned int i = 0; i < touch_links.size(); i++) { 01957 if(cm_->getKinematicModel()->getModelGroup(touch_links[i])) { 01958 std::vector<std::string> links = cm_->getKinematicModel()->getModelGroup(touch_links[i])->getGroupLinkNames(); 01959 modded_touch_links.insert(modded_touch_links.end(), links.begin(), links.end()); 01960 } else { 01961 modded_touch_links.push_back(touch_links[i]); 01962 } 01963 } 01964 std::string& link_name = it->second.attached_collision_object_.link_name; 01965 if(find(modded_touch_links.begin(), modded_touch_links.end(), link_name) == modded_touch_links.end()) { 01966 modded_touch_links.push_back(link_name); 01967 } 01968 touch_links = modded_touch_links; 01969 acm.changeEntry(object.id, touch_links, true); 01970 it->second.attached_collision_object_.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD; 01971 it->second.attach_ = false; 01972 } else if(it->second.detach_) { 01973 if(acm.hasEntry(object.id)) { 01974 acm.changeEntry(object.id, false); 01975 } 01976 (*selectable_objects_)[name].attached_collision_object_.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::REMOVE; 01977 (*selectable_objects_)[name].collision_object_.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD; 01978 (*selectable_objects_)[name].detach_ = false; 01979 } 01980 01981 if(it->second.attached_collision_object_.object.operation.operation == arm_navigation_msgs::CollisionObjectOperation::ADD) { 01982 planning_scene_req.planning_scene_diff.attached_collision_objects.push_back(it->second.attached_collision_object_); 01983 } else if(object.operation.operation != arm_navigation_msgs::CollisionObjectOperation::REMOVE) { 01984 if(!acm.hasEntry(object.id)) { 01985 acm.addEntry(object.id, false); 01986 } 01987 planning_scene_req.planning_scene_diff.collision_objects.push_back(object); 01988 } else { 01989 if(acm.hasEntry(object.id)) { 01990 acm.removeEntry(object.id); 01991 } 01992 removals.push_back(it->first); 01993 interactive_marker_server_->erase(it->first); 01994 } 01995 } 01996 01997 planning_environment::convertFromACMToACMMsg(acm, planning_scene_req.planning_scene_diff.allowed_collision_matrix); 01998 01999 if(!set_planning_scene_diff_client_.call(planning_scene_req, planning_scene_res)) 02000 { 02001 ROS_WARN("Can't get planning scene"); 02002 unlockScene(); 02003 return false; 02004 } 02005 02006 // Delete collision poles from the map which were removed. 02007 for(size_t i = 0; i < removals.size(); i++) 02008 { 02009 selectable_objects_->erase(removals[i]); 02010 } 02011 02012 data.setPlanningScene(planning_scene_res.planning_scene); 02013 02014 setRobotState(cm_->setPlanningScene(data.getPlanningScene())); 02015 02016 if(getRobotState() == NULL) 02017 { 02018 ROS_ERROR("Something wrong with planning scene"); 02019 unlockScene(); 02020 return false; 02021 } 02022 02023 std_msgs::ColorRGBA add_color; 02024 add_color.a = 1.0f; 02025 add_color.r = 0.0f; 02026 add_color.g = 1.0f; 02027 add_color.b = 0.0f; 02028 02029 //check if there are new objects we don't know about 02030 for(unsigned int i = 0; i < planning_scene_res.planning_scene.collision_objects.size(); i++) { 02031 arm_navigation_msgs::CollisionObject& coll = planning_scene_res.planning_scene.collision_objects[i]; 02032 if(selectable_objects_->find(coll.id) == selectable_objects_->end()) { 02033 createSelectableMarkerFromCollisionObject(coll, 02034 coll.id, 02035 coll.id, 02036 add_color, 02037 true); 02038 } 02039 } 02040 02041 for(unsigned int i = 0; i < planning_scene_res.planning_scene.attached_collision_objects.size(); i++) { 02042 arm_navigation_msgs::CollisionObject& coll = planning_scene_res.planning_scene.attached_collision_objects[i].object; 02043 if(selectable_objects_->find(coll.id) == selectable_objects_->end()) { 02044 makeSelectableAttachedObjectFromPlanningScene(planning_scene_res.planning_scene, 02045 planning_scene_res.planning_scene.attached_collision_objects[i]); 02046 } 02047 } 02048 02049 //Now we may need to update the positions of attached collision objects 02050 for(std::map<std::string, SelectableObject>::iterator it = selectable_objects_->begin(); 02051 it != selectable_objects_->end(); 02052 it++) { 02053 if(it->second.attached_collision_object_.object.operation.operation == arm_navigation_msgs::CollisionObjectOperation::ADD) { 02054 planning_models::KinematicState::AttachedBodyState* att_state = 02055 robot_state_->getAttachedBodyState(it->second.attached_collision_object_.object.id); 02056 if(att_state == NULL) { 02057 ROS_WARN_STREAM("Some problem with " << it->first); 02058 continue; 02059 } 02060 std_msgs::Header header; 02061 header.frame_id = cm_->getWorldFrameId(); 02062 header.stamp = ros::Time::now(); 02063 interactive_marker_server_->setPose(it->second.selection_marker_.name, 02064 toGeometryPose(att_state->getGlobalCollisionBodyTransforms()[0]), 02065 header); 02066 interactive_marker_server_->applyChanges(); 02067 it->second.collision_object_.poses[0] = toGeometryPose(att_state->getGlobalCollisionBodyTransforms()[0]); 02068 } 02069 } 02070 02071 robot_state_->getKinematicStateValues(robot_state_joint_values_); 02072 02073 for(map<string, MotionPlanRequestData>::iterator it = motion_plan_map_.begin(); it != motion_plan_map_.end(); it++) 02074 { 02075 it->second.setStartState(new KinematicState(*robot_state_)); 02076 it->second.setGoalState(new KinematicState(*robot_state_)); 02077 StateRegistry start; 02078 start.state = it->second.getStartState(); 02079 start.source = "Motion Plan Start After Sending Scene"; 02080 states_.push_back(start); 02081 StateRegistry end; 02082 end.state = it->second.getGoalState(); 02083 end.source = "Motion Plan End After Sending Scene"; 02084 states_.push_back(end); 02085 it->second.updateStartState(); 02086 it->second.updateGoalState(); 02087 } 02088 02089 //setShowCollisions(false); 02090 02091 unlockScene(); 02092 return true; 02093 } 02094 02095 void PlanningSceneEditor::initMotionPlanRequestData(const unsigned int& planning_scene_id, 02096 const std::vector<unsigned int>& ids, 02097 const std::vector<std::string>& stages, 02098 const std::vector<arm_navigation_msgs::MotionPlanRequest>& requests) 02099 { 02100 lockScene(); 02101 for(size_t i = 0; i < requests.size(); i++) 02102 { 02103 const MotionPlanRequest& mpr = requests[i]; 02104 cm_->disableCollisionsForNonUpdatedLinks(mpr.group_name); 02105 02106 setRobotStateAndComputeTransforms(mpr.start_state, *robot_state_); 02107 02108 GetMotionPlan::Request plan_req; 02109 plan_req.motion_plan_request = mpr; 02110 GetMotionPlan::Response plan_res; 02111 02112 if(params_.proximity_space_service_name_ != "none") { 02113 if(!distance_aware_service_client_.call(plan_req, plan_res)) 02114 { 02115 ROS_INFO("Something wrong with distance client"); 02116 } 02117 } 02118 02119 MotionPlanRequestData data(robot_state_); 02120 data.setId(ids[i]); 02121 data.setMotionPlanRequest(mpr); 02122 data.setPlanningSceneId(planning_scene_id); 02123 data.setGroupName(mpr.group_name); 02124 data.setSource("Planner"); 02125 02126 StateRegistry start; 02127 start.state = data.getStartState(); 02128 start.source = "Motion Plan Request Data Start from createRequest"; 02129 StateRegistry end; 02130 end.state = data.getGoalState(); 02131 end.source = "Motion Plan Request Data End from createRequest"; 02132 states_.push_back(start); 02133 states_.push_back(end); 02134 02135 const KinematicModel::GroupConfig& config = 02136 cm_->getKinematicModel()->getJointModelGroupConfigMap().at(mpr.group_name); 02137 std::string tip = config.tip_link_; 02138 data.setEndEffectorLink(tip); 02139 02140 PlanningSceneData& planningSceneData = planning_scene_map_[getPlanningSceneNameFromId(planning_scene_id)]; 02141 planningSceneData.addMotionPlanRequestId(ids[i]); 02142 02143 std::vector<unsigned int> erased_trajectories; 02144 if(motion_plan_map_.find(data.getName()) != motion_plan_map_.end()) 02145 { 02146 ROS_INFO_STREAM("Shouldn't be replacing trajectories here"); 02147 deleteMotionPlanRequest(data.getId(), erased_trajectories); 02148 } 02149 motion_plan_map_[getMotionPlanRequestNameFromId(data.getId())] = data; 02150 02151 createIkControllersFromMotionPlanRequest(data, false); 02152 } 02153 unlockScene(); 02154 } 02155 02156 // bool PlanningSceneEditor::getMotionPlanRequest(const ros::Time& time, const unsigned int id, const string& stage, MotionPlanRequest& mpr, 02157 // string& id, string& planning_scene_id) 02158 // { 02159 // if(!move_arm_warehouse_logger_reader_->getAssociatedMotionPlanRequest("", id, time, stage, mpr)) 02160 // { 02161 // ROS_INFO_STREAM("No request with stage " << stage); 02162 // return false; 02163 // } 02164 02165 // lockScene(); 02166 // cm_->disableCollisionsForNonUpdatedLinks(mpr.group_name); 02167 02168 // setRobotStateAndComputeTransforms(mpr.start_state, *robot_state_); 02169 02170 // GetMotionPlan::Request plan_req; 02171 // plan_req.motion_plan_request = mpr; 02172 // GetMotionPlan::Response plan_res; 02173 02174 // if(params_.proximity_space_service_name_ != "none") { 02175 // if(!distance_aware_service_client_.call(plan_req, plan_res)) 02176 // { 02177 // ROS_INFO("Something wrong with distance client"); 02178 // } 02179 // } 02180 02181 // MotionPlanRequestData data(robot_state_); 02182 // data.setId(generateNewMotionPlanId()); 02183 02184 // data.setMotionPlanRequest(mpr); 02185 // data.setPlanningSceneName(planning_scene_id); 02186 // data.setGroupName(mpr.group_name); 02187 // data.setSource("Planner"); 02188 // StateRegistry start; 02189 // start.state = data.getStartState(); 02190 // start.source = "Motion Plan Request Data Start from loadRequest"; 02191 // StateRegistry end; 02192 // end.state = data.getGoalState(); 02193 // end.source = "Motion Plan Request Data End from line loadRequest"; 02194 // states_.push_back(start); 02195 // states_.push_back(end); 02196 02197 // const KinematicModel::GroupConfig& config = 02198 // cm_->getKinematicModel()->getJointModelGroupConfigMap().at(mpr.group_name); 02199 // std::string tip = config.tip_link_; 02200 // data.setEndEffectorLink(tip); 02201 02202 // PlanningSceneData& planningSceneData = planning_scene_map_[planning_scene_id]; 02203 // planningSceneData.getRequests().push_back(data.getId()); 02204 02205 // motion_plan_map_[data.getId()] = data; 02206 // id = data.getId(); 02207 02208 // lock_scene_.unlock(); 02209 02210 // createIkControllersFromMotionPlanRequest(data, false); 02211 // return true; 02212 // } 02213 02214 bool PlanningSceneEditor::getAllAssociatedTrajectorySources(const unsigned int planning_id, 02215 const unsigned int mpr_id, 02216 vector<unsigned int>& trajectory_ids, 02217 vector<string>& trajectory_sources) 02218 { 02219 if(!move_arm_warehouse_logger_reader_->getAssociatedJointTrajectorySources("", planning_id, mpr_id, 02220 trajectory_ids, 02221 trajectory_sources)) 02222 { 02223 return false; 02224 } 02225 return true; 02226 } 02227 02228 bool PlanningSceneEditor::getAllAssociatedPausedStates(const unsigned int id, 02229 vector<ros::Time>& paused_times) 02230 { 02231 if(!move_arm_warehouse_logger_reader_->getAssociatedPausedStates("", id, paused_times)) 02232 { 02233 return false; 02234 } 02235 return true; 02236 } 02237 02238 bool PlanningSceneEditor::getPausedState(const unsigned int id, 02239 const ros::Time& paused_time, 02240 HeadMonitorFeedback& paused_state) 02241 { 02242 if(!move_arm_warehouse_logger_reader_->getAssociatedPausedState("", id, paused_time, paused_state)) 02243 { 02244 return false; 02245 } 02246 return true; 02247 } 02248 02249 bool PlanningSceneEditor::playTrajectory(MotionPlanRequestData& requestData, TrajectoryData& data) 02250 { 02251 lock_scene_.lock(); 02252 for(size_t i = 0; i < states_.size(); i++) 02253 { 02254 if(states_[i].state == data.getCurrentState()) 02255 { 02256 states_[i].state = NULL; 02257 } 02258 } 02259 02260 data.reset(); 02261 02262 data.play(); 02263 data.setVisible(true); 02264 if(data.getTrajectory().points.size() == 0) 02265 { 02266 lock_scene_.unlock(); 02267 return false; 02268 } 02269 02270 if(data.getCurrentState() == NULL) 02271 { 02272 data.setCurrentState(new KinematicState(*robot_state_)); 02273 StateRegistry currentState; 02274 currentState.state = data.getCurrentState(); 02275 currentState.source = "Trajectory from play trajectory"; 02276 states_.push_back(currentState); 02277 } 02278 02279 data.setCurrentPoint(0); 02280 ArmNavigationErrorCodes oldValue; 02281 oldValue.val = data.trajectory_error_code_.val; 02282 ArmNavigationErrorCodes& errorCode = data.trajectory_error_code_; 02283 collision_space::EnvironmentModel::AllowedCollisionMatrix acm = cm_->getCurrentAllowedCollisionMatrix(); 02284 cm_->disableCollisionsForNonUpdatedLinks(data.getGroupName()); 02285 02286 vector<ArmNavigationErrorCodes> trajectory_error_codes; 02287 cm_->isJointTrajectoryValid(*(data.getCurrentState()), data.getTrajectory(), 02288 requestData.getMotionPlanRequest().goal_constraints, 02289 requestData.getMotionPlanRequest().path_constraints, errorCode, 02290 trajectory_error_codes, false); 02291 02292 cm_->setAlteredAllowedCollisionMatrix(acm); 02293 02294 if(errorCode.val != errorCode.SUCCESS) 02295 { 02296 if(trajectory_error_codes.size() > 0) 02297 { 02298 data.setBadPoint(trajectory_error_codes.size() - 1); 02299 } 02300 else 02301 { 02302 data.setBadPoint(0); 02303 } 02304 } 02305 else 02306 { 02307 data.setBadPoint(-1); 02308 errorCode.val = oldValue.val; 02309 } 02310 02311 data.moveThroughTrajectory(0); 02312 lock_scene_.unlock(); 02313 return true; 02314 } 02315 02316 void PlanningSceneEditor::createSelectableMarkerFromCollisionObject(CollisionObject& object, 02317 string name, 02318 string description, 02319 std_msgs::ColorRGBA color, 02320 bool insert_selection) 02321 { 02322 SelectableObject selectable; 02323 selectable.id_ = name; 02324 selectable.collision_object_ = object; 02325 selectable.control_marker_.pose = object.poses[0]; 02326 selectable.control_marker_.header.frame_id = "/" + cm_->getWorldFrameId(); 02327 selectable.control_marker_.header.stamp = ros::Time(ros::WallTime::now().toSec()); 02328 02329 selectable.selection_marker_.pose = object.poses[0]; 02330 selectable.selection_marker_.header.frame_id = "/" + cm_->getWorldFrameId(); 02331 selectable.selection_marker_.header.stamp = ros::Time(ros::WallTime::now().toSec()); 02332 02333 selectable.color_ = color; 02334 02335 InteractiveMarkerControl button; 02336 button.name = "button"; 02337 button.interaction_mode = InteractiveMarkerControl::BUTTON; 02338 button.description = ""; 02339 02340 //min scale initialized 02341 double scale_to_use = .2f; 02342 02343 for(size_t i = 0; i < object.shapes.size(); i++) 02344 { 02345 arm_navigation_msgs::Shape& shape = object.shapes[i]; 02346 Marker mark; 02347 mark.color = color; 02348 planning_environment::setMarkerShapeFromShape(shape, mark); 02349 02350 shapes::Shape* s = planning_environment::constructObject(shape); 02351 bodies::Body* b = bodies::createBodyFromShape(s); 02352 02353 bodies::BoundingSphere bs; 02354 b->computeBoundingSphere(bs); 02355 02356 delete b; 02357 delete s; 02358 02359 if(bs.radius * 2.0 > scale_to_use) { 02360 scale_to_use = bs.radius*2.0; 02361 } 02362 02363 //need to make slightly larger 02364 mark.scale.x = mark.scale.x * 1.01f; 02365 mark.scale.y = mark.scale.y * 1.01f; 02366 mark.scale.z = mark.scale.z * 1.01f; 02367 02368 if(mark.type == Marker::LINE_LIST) { 02369 mark.points.clear(); 02370 mark.type = Marker::TRIANGLE_LIST; 02371 mark.scale.x = 1.01; 02372 mark.scale.y = 1.01; 02373 mark.scale.z = 1.01; 02374 for(unsigned int i = 0; i < shape.triangles.size(); i += 3) { 02375 mark.points.push_back(shape.vertices[shape.triangles[i]]); 02376 mark.points.push_back(shape.vertices[shape.triangles[i+1]]); 02377 mark.points.push_back(shape.vertices[shape.triangles[i+2]]); 02378 } 02379 } 02380 02381 button.markers.push_back(mark); 02382 } 02383 02384 selectable.selection_marker_.controls.push_back(button); 02385 02386 InteractiveMarkerControl sixDof; 02387 sixDof.orientation.w = 1; 02388 sixDof.orientation.x = 1; 02389 sixDof.orientation.y = 0; 02390 sixDof.orientation.z = 0; 02391 sixDof.always_visible = false; 02392 sixDof.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 02393 selectable.control_marker_.controls.push_back(sixDof); 02394 sixDof.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 02395 selectable.control_marker_.controls.push_back(sixDof); 02396 02397 sixDof.orientation.w = 1; 02398 sixDof.orientation.x = 0; 02399 sixDof.orientation.y = 1; 02400 sixDof.orientation.z = 0; 02401 sixDof.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 02402 selectable.control_marker_.controls.push_back(sixDof); 02403 sixDof.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 02404 selectable.control_marker_.controls.push_back(sixDof); 02405 02406 sixDof.orientation.w = 1; 02407 sixDof.orientation.x = 0; 02408 sixDof.orientation.y = 0; 02409 sixDof.orientation.z = 1; 02410 sixDof.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 02411 selectable.control_marker_.controls.push_back(sixDof); 02412 sixDof.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 02413 selectable.control_marker_.controls.push_back(sixDof); 02414 02415 selectable.control_marker_.controls.push_back(button); 02416 selectable.control_marker_.description = description; 02417 02418 selectable.control_marker_.name = name + "_control"; 02419 selectable.selection_marker_.name = name + "_selection"; 02420 02421 selectable.selection_marker_.scale = scale_to_use; 02422 selectable.control_marker_.scale = scale_to_use; 02423 (*selectable_objects_)[name] = selectable; 02424 if(insert_selection) { 02425 interactive_marker_server_->insert(selectable.selection_marker_, collision_object_selection_feedback_ptr_); 02426 menu_handler_map_["Collision Object Selection"].apply(*interactive_marker_server_, selectable.selection_marker_.name); 02427 interactive_marker_server_->applyChanges(); 02428 } 02429 } 02430 02431 void PlanningSceneEditor::JointControllerCallback(const InteractiveMarkerFeedbackConstPtr &feedback) 02432 { 02433 std::string id = ""; 02434 std::string MPR = ""; 02435 PositionType type = StartPosition; 02436 02437 if(feedback->marker_name.rfind("_start_control") != std::string::npos) 02438 { 02439 std::string sub1 = feedback->marker_name.substr(0, feedback->marker_name.rfind("_start_control")); 02440 id = sub1.substr(0, sub1.rfind("_mpr_")); 02441 MPR = sub1.substr(sub1.rfind("_mpr_") + 5, sub1.length()); 02442 type = StartPosition; 02443 } 02444 else if(feedback->marker_name.rfind("_end_control") != std::string::npos) 02445 { 02446 std::string sub1 = feedback->marker_name.substr(0, feedback->marker_name.rfind("_end_control")); 02447 id = sub1.substr(0, sub1.rfind("_mpr_")); 02448 MPR = sub1.substr(sub1.rfind("_mpr_") + 5, sub1.length()); 02449 type = GoalPosition; 02450 } 02451 02452 if(motion_plan_map_.find(MPR) == motion_plan_map_.end()) { 02453 ROS_INFO_STREAM("Making mpr in joint controller callback"); 02454 } 02455 setJointState(motion_plan_map_[MPR], type, id, toBulletTransform(feedback->pose)); 02456 } 02457 02458 void PlanningSceneEditor::IKControllerCallback(const InteractiveMarkerFeedbackConstPtr &feedback) 02459 { 02460 std::string id = ""; 02461 PositionType type = StartPosition; 02462 02463 bool findIKSolution = false; 02464 if(feedback->marker_name.rfind("_start_control") != std::string::npos) 02465 { 02466 id = feedback->marker_name.substr(0, feedback->marker_name.rfind("_start_control")); 02467 type = StartPosition; 02468 } 02469 else if(feedback->marker_name.rfind("_end_control") != std::string::npos) 02470 { 02471 id = feedback->marker_name.substr(0, feedback->marker_name.rfind("_end_control")); 02472 type = GoalPosition; 02473 } 02474 else 02475 { 02476 return; 02477 } 02478 02479 IKController& controller = (*ik_controllers_)[id]; 02480 02481 if(motion_plan_map_.find(getMotionPlanRequestNameFromId(controller.motion_plan_id_)) == motion_plan_map_.end()) { 02482 ROS_INFO_STREAM("Making empty mpr in ik controller callback"); 02483 } 02484 02485 if(feedback->event_type == InteractiveMarkerFeedback::POSE_UPDATE) 02486 { 02487 btTransform pose = toBulletTransform(feedback->pose); 02488 02489 if(type == StartPosition) 02490 { 02491 motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)].getStartState()->updateKinematicStateWithLinkAt(motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)].getEndEffectorLink(), 02492 pose); 02493 findIKSolution = true; 02494 if(selected_motion_plan_name_ != getMotionPlanRequestNameFromId(controller.motion_plan_id_)) 02495 { 02496 selected_motion_plan_name_ = getMotionPlanRequestNameFromId(controller.motion_plan_id_); 02497 updateState(); 02498 } 02499 } 02500 else 02501 { 02502 motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)].getGoalState()->updateKinematicStateWithLinkAt(motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)].getEndEffectorLink(), 02503 pose); 02504 findIKSolution = true; 02505 if(selected_motion_plan_name_ != getMotionPlanRequestNameFromId(controller.motion_plan_id_)) 02506 { 02507 selected_motion_plan_name_ = getMotionPlanRequestNameFromId(controller.motion_plan_id_); 02508 updateState(); 02509 } 02510 } 02511 02512 } 02513 else if(feedback->event_type == InteractiveMarkerFeedback::MENU_SELECT) 02514 { 02515 if(feedback->menu_entry_id == menu_entry_maps_["IK Control"]["Map to Robot State"]) 02516 { 02517 MotionPlanRequestData& data = motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)]; 02518 if(data.hasGoodIKSolution(type)) { 02519 const planning_models::KinematicState* state; 02520 if(type == StartPosition) { 02521 state = data.getStartState(); 02522 } else { 02523 state = data.getGoalState(); 02524 } 02525 planning_environment::convertKinematicStateToRobotState(*state, 02526 ros::Time::now(), 02527 cm_->getWorldFrameId(), 02528 planning_scene_map_[current_planning_scene_name_].getPlanningScene().robot_state); 02529 02530 sendPlanningScene(planning_scene_map_[current_planning_scene_name_]); 02531 } 02532 } else if(feedback->menu_entry_id == menu_entry_maps_["IK Control"]["Map from Robot State"]) 02533 { 02534 MotionPlanRequestData& data = motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)]; 02535 02536 std::map<std::string, double> vals; 02537 robot_state_->getKinematicStateValues(vals); 02538 02539 planning_models::KinematicState* state; 02540 if(type == StartPosition) 02541 { 02542 data.setStartStateValues(vals); 02543 state = data.getStartState(); 02544 convertKinematicStateToRobotState(*state, ros::Time(ros::WallTime::now().toSec()), cm_->getWorldFrameId(), 02545 data.getMotionPlanRequest().start_state); 02546 data.setLastGoodStartPose((state->getLinkState(data.getEndEffectorLink())->getGlobalLinkTransform())); 02547 } 02548 else 02549 { 02550 state = data.getGoalState(); 02551 data.setGoalStateValues(vals); 02552 data.setLastGoodGoalPose((state->getLinkState(data.getEndEffectorLink())->getGlobalLinkTransform())); 02553 } 02554 interactive_marker_server_->setPose(feedback->marker_name, 02555 toGeometryPose(state->getLinkState(data.getEndEffectorLink())->getGlobalLinkTransform()), 02556 feedback->header); 02557 } else if(feedback->menu_entry_id == menu_entry_maps_["IK Control"]["Go To Last Good State"]) 02558 { 02559 MotionPlanRequestData& data = motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)]; 02560 02561 if(type == StartPosition) 02562 { 02563 data.getStartState()->updateKinematicStateWithLinkAt(data.getEndEffectorLink(), (data.getLastGoodStartPose())); 02564 interactive_marker_server_->setPose(feedback->marker_name, toGeometryPose(data.getLastGoodStartPose()), 02565 feedback->header); 02566 findIKSolution = true; 02567 } 02568 else 02569 { 02570 data.getGoalState()->updateKinematicStateWithLinkAt(data.getEndEffectorLink(), (data.getLastGoodGoalPose())); 02571 interactive_marker_server_->setPose(feedback->marker_name, toGeometryPose(data.getLastGoodGoalPose()), 02572 feedback->header); 02573 findIKSolution = true; 02574 } 02575 } 02576 else if(feedback->menu_entry_id == menu_entry_maps_["IK Control"]["Randomly Perturb"]) 02577 { 02578 MotionPlanRequestData& data = motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)]; 02579 02580 randomlyPerturb(data, type); 02581 if(type == StartPosition) 02582 { 02583 interactive_marker_server_->setPose(feedback->marker_name, toGeometryPose(data.getLastGoodStartPose()), 02584 feedback->header); 02585 } 02586 else 02587 { 02588 interactive_marker_server_->setPose(feedback->marker_name, toGeometryPose(data.getLastGoodGoalPose()), 02589 feedback->header); 02590 } 02591 } 02592 else if(feedback->menu_entry_id == menu_entry_maps_["IK Control"]["Plan New Trajectory"]) 02593 { 02594 MotionPlanRequestData& data = motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)]; 02595 unsigned int trajectory; 02596 planToRequest(data, trajectory); 02597 selected_trajectory_name_ = getTrajectoryNameFromId(trajectory); 02598 playTrajectory(data, trajectory_map_[data.getName()][selected_trajectory_name_]); 02599 updateState(); 02600 } 02601 else if(feedback->menu_entry_id == menu_entry_maps_["IK Control"]["Filter Last Trajectory"]) 02602 { 02603 MotionPlanRequestData& data = motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)]; 02604 unsigned int trajectory; 02605 if(selected_trajectory_name_ != "" && hasTrajectory(data.getName(), selected_trajectory_name_)) { 02606 filterTrajectory(data, trajectory_map_[data.getName()][selected_trajectory_name_], trajectory); 02607 selected_trajectory_name_ = getTrajectoryNameFromId(trajectory); 02608 playTrajectory(data, trajectory_map_[data.getName()][selected_trajectory_name_]); 02609 updateState(); 02610 } 02611 } 02612 else if(feedback->menu_entry_id == menu_entry_maps_["IK Control"]["Execute Last Trajectory"]) 02613 { 02614 std::string trajectory; 02615 if(selected_trajectory_name_ != "") 02616 { 02617 executeTrajectory(selected_motion_plan_name_, selected_trajectory_name_); 02618 updateState(); 02619 } 02620 } 02621 else if(feedback->menu_entry_id == menu_entry_maps_["IK Control"]["Delete Request"]) 02622 { 02623 std::vector<unsigned int> erased_trajectories; 02624 deleteMotionPlanRequest(controller.motion_plan_id_, erased_trajectories); 02625 } 02626 } 02627 02628 if(findIKSolution) 02629 { 02630 if(!solveIKForEndEffectorPose(motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)], type, true, false)) 02631 { 02632 if(motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)].hasGoodIKSolution(type)) 02633 { 02634 motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)].refreshColors(); 02635 } 02636 motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)].setHasGoodIKSolution(false, type); 02637 } 02638 else 02639 { 02640 if(!motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)].hasGoodIKSolution(type)) 02641 { 02642 motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)].refreshColors(); 02643 } 02644 motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)].setHasGoodIKSolution(true, type); 02645 } 02646 } 02647 02648 if(motion_plan_map_.find(getMotionPlanRequestNameFromId(controller.motion_plan_id_)) == motion_plan_map_.end()) { 02649 ROS_DEBUG_STREAM("Would be empty mpr in ik controller callback"); 02650 } else if(motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)].areJointControlsVisible()) 02651 { 02652 createJointMarkers(motion_plan_map_[getMotionPlanRequestNameFromId(controller.motion_plan_id_)], type); 02653 } 02654 interactive_marker_server_->applyChanges(); 02655 } 02656 02657 void PlanningSceneEditor::createIkControllersFromMotionPlanRequest(MotionPlanRequestData& data, bool rePose) 02658 { 02659 if(data.isStartEditable()) 02660 { 02661 createIKController(data, StartPosition, rePose); 02662 } 02663 02664 if(data.isGoalEditable()) 02665 { 02666 createIKController(data, GoalPosition, rePose); 02667 } 02668 02669 interactive_marker_server_->applyChanges(); 02670 } 02671 02672 void PlanningSceneEditor::createIKController(MotionPlanRequestData& data, PositionType type, bool rePose) 02673 { 02674 KinematicState* state = NULL; 02675 std::string nametag = ""; 02676 if(type == StartPosition) 02677 { 02678 state = data.getStartState(); 02679 nametag = "_start_control"; 02680 } 02681 else 02682 { 02683 state = data.getGoalState(); 02684 nametag = "_end_control"; 02685 } 02686 02687 btTransform transform = state->getLinkState(data.getEndEffectorLink())->getGlobalLinkTransform(); 02688 InteractiveMarker marker; 02689 02690 02691 if(interactive_marker_server_->get(data.getName() + nametag, marker) && rePose) 02692 { 02693 geometry_msgs::Pose pose = toGeometryPose(transform); 02694 interactive_marker_server_->setPose(data.getName() + nametag, pose); 02695 return; 02696 } 02697 02698 02699 marker.header.frame_id = "/" + cm_->getWorldFrameId(); 02700 marker.pose.position.x = transform.getOrigin().x(); 02701 marker.pose.position.y = transform.getOrigin().y(); 02702 marker.pose.position.z = transform.getOrigin().z(); 02703 marker.pose.orientation.w = transform.getRotation().w(); 02704 marker.pose.orientation.x = transform.getRotation().x(); 02705 marker.pose.orientation.y = transform.getRotation().y(); 02706 marker.pose.orientation.z = transform.getRotation().z(); 02707 marker.scale = 0.225f; 02708 marker.name = data.getName() + nametag; 02709 marker.description = data.getName() + nametag; 02710 02711 InteractiveMarkerControl control; 02712 control.orientation.w = 1; 02713 control.orientation.x = 1; 02714 control.orientation.y = 0; 02715 control.orientation.z = 0; 02716 control.always_visible = false; 02717 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 02718 marker.controls.push_back(control); 02719 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 02720 marker.controls.push_back(control); 02721 02722 control.orientation.w = 1; 02723 control.orientation.x = 0; 02724 control.orientation.y = 1; 02725 control.orientation.z = 0; 02726 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 02727 marker.controls.push_back(control); 02728 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 02729 marker.controls.push_back(control); 02730 02731 control.orientation.w = 1; 02732 control.orientation.x = 0; 02733 control.orientation.y = 0; 02734 control.orientation.z = 1; 02735 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 02736 marker.controls.push_back(control); 02737 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 02738 marker.controls.push_back(control); 02739 02740 interactive_marker_server_->insert(marker, ik_control_feedback_ptr_); 02741 control.interaction_mode = InteractiveMarkerControl::MENU; 02742 //control.markers.push_back(makeMarkerSphere(marker)); 02743 marker.controls.push_back(control); 02744 02745 (*ik_controllers_)[data.getName()].motion_plan_id_ = data.getId(); 02746 if(type == StartPosition) 02747 { 02748 (*ik_controllers_)[data.getName()].start_controller_ = marker; 02749 data.setLastGoodStartPose(toBulletTransform((*ik_controllers_)[data.getName()].start_controller_.pose)); 02750 } 02751 else 02752 { 02753 (*ik_controllers_)[data.getName()].end_controller_ = marker; 02754 data.setLastGoodGoalPose(toBulletTransform((*ik_controllers_)[data.getName()].end_controller_.pose)); 02755 } 02756 02757 menu_handler_map_["IK Control"].apply(*interactive_marker_server_, marker.name); 02758 02759 } 02760 02761 void PlanningSceneEditor::deleteCollisionObject(std::string& name) 02762 { 02763 (*selectable_objects_)[name].collision_object_.operation.operation 02764 = arm_navigation_msgs::CollisionObjectOperation::REMOVE; 02765 (*selectable_objects_)[name].attached_collision_object_.object.operation.operation 02766 = arm_navigation_msgs::CollisionObjectOperation::REMOVE; 02767 interactive_marker_server_->erase((*selectable_objects_)[name].selection_marker_.name); 02768 interactive_marker_server_->erase((*selectable_objects_)[name].control_marker_.name); 02769 interactive_marker_server_->applyChanges(); 02770 } 02771 02772 void PlanningSceneEditor::collisionObjectSelectionCallback(const InteractiveMarkerFeedbackConstPtr &feedback) 02773 { 02774 std::string name = feedback->marker_name.substr(0, feedback->marker_name.rfind("_selection")); 02775 bool should_select = false; 02776 switch (feedback->event_type) 02777 { 02778 case InteractiveMarkerFeedback::BUTTON_CLICK: 02779 { 02780 visualization_msgs::InteractiveMarker mark; 02781 should_select = true; 02782 } 02783 break; 02784 case InteractiveMarkerFeedback::MENU_SELECT: 02785 if(feedback->menu_entry_id == menu_entry_maps_["Collision Object Selection"]["Delete"]) 02786 { 02787 deleteCollisionObject(name); 02788 sendPlanningScene(planning_scene_map_[current_planning_scene_name_]); 02789 selectable_objects_->erase(name); 02790 } 02791 else if(feedback->menu_entry_id == menu_entry_maps_["Collision Object Selection"]["Select"]) 02792 { 02793 should_select = true; 02794 } 02795 break; 02796 } 02797 02798 if(should_select) 02799 { 02800 interactive_marker_server_->erase((*selectable_objects_)[name].selection_marker_.name); 02801 (*selectable_objects_)[name].control_marker_.pose = feedback->pose; 02802 (*selectable_objects_)[name].control_marker_.header.stamp = ros::Time(ros::WallTime::now().toSec()); 02803 02804 interactive_marker_server_->insert((*selectable_objects_)[name].control_marker_, 02805 collision_object_movement_feedback_ptr_); 02806 02807 menu_handler_map_["Collision Object"].apply(*interactive_marker_server_, 02808 (*selectable_objects_)[name].control_marker_.name); 02809 } 02810 interactive_marker_server_->applyChanges(); 02811 02812 } 02813 02814 void PlanningSceneEditor::collisionObjectMovementCallback(const InteractiveMarkerFeedbackConstPtr &feedback) 02815 { 02816 std::string name = feedback->marker_name.substr(0, feedback->marker_name.rfind("_control")); 02817 02818 switch (feedback->event_type) 02819 { 02820 case InteractiveMarkerFeedback::MOUSE_UP: 02821 if(feedback->control_name == "button") return; 02822 if(last_resize_handle_ == menu_entry_maps_["Collision Object"]["Off"]) { 02823 for(size_t i = 0; i < (*selectable_objects_)[name].collision_object_.poses.size(); i++) 02824 { 02825 (*selectable_objects_)[name].collision_object_.poses[i] = feedback->pose; 02826 } 02827 sendPlanningScene(planning_scene_map_[current_planning_scene_name_]); 02828 } else { 02829 CollisionObject coll = (*selectable_objects_)[name].collision_object_; 02830 btTransform orig, cur; 02831 tf::poseMsgToTF(coll.poses[0], orig); 02832 tf::poseMsgToTF(feedback->pose, cur); 02833 btTransform nt = orig.inverse()*cur; 02834 if(last_resize_handle_ == menu_entry_maps_["Collision Object"]["Grow"]) { 02835 if(coll.shapes[0].type == arm_navigation_msgs::Shape::BOX) { 02836 coll.shapes[0].dimensions[0] += fabs(nt.getOrigin().x()); 02837 coll.shapes[0].dimensions[1] += fabs(nt.getOrigin().y()); 02838 coll.shapes[0].dimensions[2] += fabs(nt.getOrigin().z()); 02839 } else if(coll.shapes[0].type == arm_navigation_msgs::Shape::CYLINDER) { 02840 coll.shapes[0].dimensions[0] += fmax(fabs(nt.getOrigin().x()), fabs(nt.getOrigin().y())); 02841 coll.shapes[0].dimensions[1] += fabs(nt.getOrigin().z()); 02842 } else if(coll.shapes[0].type == arm_navigation_msgs::Shape::SPHERE) { 02843 coll.shapes[0].dimensions[0] += fmax(fmax(fabs(nt.getOrigin().x()), fabs(nt.getOrigin().y())), fabs(nt.getOrigin().z())); 02844 } 02845 } else { 02846 //shrinking 02847 if(nt.getOrigin().x() > 0) { 02848 nt.getOrigin().setX(-nt.getOrigin().x()); 02849 } 02850 if(nt.getOrigin().y() > 0) { 02851 nt.getOrigin().setY(-nt.getOrigin().y()); 02852 } 02853 if(nt.getOrigin().z() > 0) { 02854 nt.getOrigin().setZ(-nt.getOrigin().z()); 02855 } 02856 //can't be bigger than the current dimensions 02857 if(coll.shapes[0].type == arm_navigation_msgs::Shape::BOX) { 02858 nt.getOrigin().setX(fmax(nt.getOrigin().x(), -coll.shapes[0].dimensions[0]+.01)); 02859 nt.getOrigin().setY(fmax(nt.getOrigin().y(), -coll.shapes[0].dimensions[1]+.01)); 02860 nt.getOrigin().setZ(fmax(nt.getOrigin().z(), -coll.shapes[0].dimensions[2]+.01)); 02861 coll.shapes[0].dimensions[0] += nt.getOrigin().x(); 02862 coll.shapes[0].dimensions[1] += nt.getOrigin().y(); 02863 coll.shapes[0].dimensions[2] += nt.getOrigin().z(); 02864 } else if(coll.shapes[0].type == arm_navigation_msgs::Shape::CYLINDER) { 02865 nt.getOrigin().setX(fmax(nt.getOrigin().x(), -coll.shapes[0].dimensions[0]+.01)); 02866 nt.getOrigin().setY(fmax(nt.getOrigin().y(), -coll.shapes[0].dimensions[0]+.01)); 02867 nt.getOrigin().setZ(fmax(nt.getOrigin().z(), -coll.shapes[0].dimensions[1]+.01)); 02868 coll.shapes[0].dimensions[0] += fmin(nt.getOrigin().x(), nt.getOrigin().y()); 02869 coll.shapes[0].dimensions[1] += nt.getOrigin().z(); 02870 } else if(coll.shapes[0].type == arm_navigation_msgs::Shape::SPHERE) { 02871 nt.getOrigin().setX(fmax(nt.getOrigin().x(), -coll.shapes[0].dimensions[0]+.01)); 02872 nt.getOrigin().setY(fmax(nt.getOrigin().y(), -coll.shapes[0].dimensions[0]+.01)); 02873 nt.getOrigin().setZ(fmax(nt.getOrigin().z(), -coll.shapes[0].dimensions[0]+.01)); 02874 coll.shapes[0].dimensions[0] += fmin(fmin(nt.getOrigin().x(), nt.getOrigin().y()), nt.getOrigin().z()); 02875 } 02876 } 02877 nt.setOrigin(nt.getOrigin()*.5); 02878 tf::poseTFToMsg(orig*nt, coll.poses[0]); 02879 02880 createSelectableMarkerFromCollisionObject(coll, coll.id, coll.id, (*selectable_objects_)[name].color_, false); 02881 (*selectable_objects_)[name].control_marker_.header.stamp = ros::Time(ros::WallTime::now().toSec()); 02882 02883 interactive_marker_server_->insert((*selectable_objects_)[name].control_marker_, 02884 collision_object_movement_feedback_ptr_); 02885 menu_handler_map_["Collision Object"].apply(*interactive_marker_server_, 02886 (*selectable_objects_)[name].control_marker_.name); 02887 interactive_marker_server_->applyChanges(); 02888 sendPlanningScene(planning_scene_map_[current_planning_scene_name_]); 02889 } 02890 break; 02891 case InteractiveMarkerFeedback::MOUSE_DOWN: 02892 if(feedback->control_name == "button") { 02893 interactive_marker_server_->erase((*selectable_objects_)[name].control_marker_.name); 02894 (*selectable_objects_)[name].selection_marker_.pose = feedback->pose; 02895 (*selectable_objects_)[name].selection_marker_.header.stamp = ros::Time(ros::WallTime::now().toSec()); 02896 interactive_marker_server_->insert((*selectable_objects_)[name].selection_marker_, 02897 collision_object_selection_feedback_ptr_); 02898 menu_handler_map_["Collision Object Selection"].apply(*interactive_marker_server_, 02899 (*selectable_objects_)[name].selection_marker_.name); 02900 } 02901 break; 02902 case InteractiveMarkerFeedback::MENU_SELECT: 02903 if(feedback->menu_entry_id == menu_entry_maps_["Collision Object"]["Delete"]) 02904 { 02905 deleteCollisionObject(name); 02906 sendPlanningScene(planning_scene_map_[current_planning_scene_name_]); 02907 selectable_objects_->erase(name); 02908 } 02909 else if(feedback->menu_entry_id == menu_entry_maps_["Collision Object"]["Deselect"]) 02910 { 02911 interactive_marker_server_->erase((*selectable_objects_)[name].control_marker_.name); 02912 (*selectable_objects_)[name].selection_marker_.pose = feedback->pose; 02913 (*selectable_objects_)[name].selection_marker_.header.stamp = ros::Time(ros::WallTime::now().toSec()); 02914 interactive_marker_server_->insert((*selectable_objects_)[name].selection_marker_, 02915 collision_object_selection_feedback_ptr_); 02916 menu_handler_map_["Collision Object Selection"].apply(*interactive_marker_server_, 02917 (*selectable_objects_)[name].selection_marker_.name); 02918 02919 } else if(feedback->menu_entry_id == menu_entry_maps_["Collision Object"]["Off"] || 02920 feedback->menu_entry_id == menu_entry_maps_["Collision Object"]["Grow"] || 02921 feedback->menu_entry_id == menu_entry_maps_["Collision Object"]["Shrink"]) { 02922 menu_handler_map_["Collision Object"].setCheckState(last_resize_handle_, MenuHandler::UNCHECKED); 02923 last_resize_handle_ = feedback->menu_entry_id; 02924 menu_handler_map_["Collision Object"].setCheckState(last_resize_handle_, MenuHandler::CHECKED); 02925 menu_handler_map_["Collision Object"].reApply(*interactive_marker_server_); 02926 } else if(feedback->menu_entry_id == menu_entry_maps_["Collision Object"]["Attach"]) { 02927 (*selectable_objects_)[name].control_marker_.pose = feedback->pose; 02928 attachObjectCallback(name); 02929 } 02930 break; 02931 case InteractiveMarkerFeedback::POSE_UPDATE: 02932 break; 02933 } 02934 02935 interactive_marker_server_->applyChanges(); 02936 } 02937 02938 void PlanningSceneEditor::changeToAttached(const std::string& name) 02939 { 02940 (*selectable_objects_)[name].selection_marker_.pose = (*selectable_objects_)[name].control_marker_.pose; 02941 (*selectable_objects_)[name].selection_marker_.description = "attached_"+name; 02942 interactive_marker_server_->erase((*selectable_objects_)[name].control_marker_.name); 02943 (*selectable_objects_)[name].selection_marker_.header.stamp = ros::Time(ros::WallTime::now().toSec()); 02944 interactive_marker_server_->insert((*selectable_objects_)[name].selection_marker_, 02945 attached_collision_object_feedback_ptr_); 02946 menu_handler_map_["Attached Collision Object"].apply(*interactive_marker_server_, 02947 (*selectable_objects_)[name].selection_marker_.name); 02948 interactive_marker_server_->applyChanges(); 02949 } 02950 02951 void PlanningSceneEditor::attachedCollisionObjectInteractiveCallback(const InteractiveMarkerFeedbackConstPtr &feedback) 02952 { 02953 std::string name = feedback->marker_name.substr(0, feedback->marker_name.rfind("_selection")); 02954 02955 switch (feedback->event_type) 02956 { 02957 case InteractiveMarkerFeedback::BUTTON_CLICK: 02958 { 02959 if((*selectable_objects_)[name].selection_marker_.description.empty()) { 02960 (*selectable_objects_)[name].selection_marker_.description = "attached_"+(*selectable_objects_)[name].collision_object_.id; 02961 } else { 02962 (*selectable_objects_)[name].selection_marker_.description = ""; 02963 } 02964 interactive_marker_server_->insert((*selectable_objects_)[name].selection_marker_, 02965 attached_collision_object_feedback_ptr_); 02966 std_msgs::Header header; 02967 header.frame_id = cm_->getWorldFrameId(); 02968 header.stamp = ros::Time::now(); 02969 interactive_marker_server_->setPose(feedback->marker_name, 02970 (*selectable_objects_)[name].collision_object_.poses[0], 02971 header); 02972 menu_handler_map_["Attached Collision Object"].apply(*interactive_marker_server_, 02973 (*selectable_objects_)[name].selection_marker_.name); 02974 interactive_marker_server_->applyChanges(); 02975 } 02976 break; 02977 case InteractiveMarkerFeedback::MENU_SELECT: 02978 if(feedback->menu_entry_id == menu_entry_maps_["Attached Collision Object"]["Detach"]) 02979 { 02980 (*selectable_objects_)[name].detach_ = true; 02981 02982 (*selectable_objects_)[name].control_marker_.pose = (*selectable_objects_)[name].collision_object_.poses[0]; 02983 (*selectable_objects_)[name].control_marker_.description = (*selectable_objects_)[name].collision_object_.id; 02984 (*selectable_objects_)[name].selection_marker_.description = ""; 02985 interactive_marker_server_->erase((*selectable_objects_)[name].selection_marker_.name); 02986 interactive_marker_server_->insert((*selectable_objects_)[name].control_marker_, 02987 collision_object_movement_feedback_ptr_); 02988 menu_handler_map_["Collision Object"].apply(*interactive_marker_server_, 02989 (*selectable_objects_)[name].control_marker_.name); 02990 sendPlanningScene(planning_scene_map_[current_planning_scene_name_]); 02991 interactive_marker_server_->applyChanges(); 02992 } 02993 break; 02994 default: 02995 break; 02996 } 02997 } 02998 std::string PlanningSceneEditor::createMeshObject(const std::string& name, 02999 geometry_msgs::Pose pose, 03000 const std::string& filename, 03001 const btVector3& scale, 03002 std_msgs::ColorRGBA color) 03003 { 03004 shapes::Mesh* mesh = shapes::createMeshFromFilename(filename, &scale); 03005 if(mesh == NULL) { 03006 return ""; 03007 } 03008 arm_navigation_msgs::Shape object; 03009 if(!planning_environment::constructObjectMsg(mesh, object)) { 03010 ROS_WARN_STREAM("Object construction fails"); 03011 return ""; 03012 } 03013 delete mesh; 03014 arm_navigation_msgs::CollisionObject collision_object; 03015 collision_object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD; 03016 collision_object.header.stamp = ros::Time(ros::WallTime::now().toSec()); 03017 collision_object.header.frame_id = cm_->getWorldFrameId(); 03018 if(name.empty()) { 03019 collision_object.id = generateNewCollisionObjectId(); 03020 } else { 03021 collision_object.id = name; 03022 } 03023 collision_object.shapes.push_back(object); 03024 collision_object.poses.push_back(pose); 03025 03026 lockScene(); 03027 createSelectableMarkerFromCollisionObject(collision_object, collision_object.id, collision_object.id, color); 03028 03029 ROS_INFO("Created collision object."); 03030 ROS_INFO("Sending planning scene %s", current_planning_scene_name_.c_str()); 03031 03032 sendPlanningScene(planning_scene_map_[current_planning_scene_name_]); 03033 03034 unlockScene(); 03035 return collision_object.id; 03036 } 03037 03038 std::string PlanningSceneEditor::createCollisionObject(const std::string& name, 03039 geometry_msgs::Pose pose, PlanningSceneEditor::GeneratedShape shape, 03040 float scaleX, float scaleY, float scaleZ, std_msgs::ColorRGBA color) 03041 { 03042 lockScene(); 03043 arm_navigation_msgs::CollisionObject collision_object; 03044 collision_object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD; 03045 collision_object.header.stamp = ros::Time(ros::WallTime::now().toSec()); 03046 collision_object.header.frame_id = cm_->getWorldFrameId(); 03047 if(name.empty()) { 03048 collision_object.id = generateNewCollisionObjectId(); 03049 } else { 03050 collision_object.id = name; 03051 } 03052 arm_navigation_msgs::Shape object; 03053 03054 switch (shape) 03055 { 03056 case PlanningSceneEditor::Box: 03057 object.type = arm_navigation_msgs::Shape::BOX; 03058 object.dimensions.resize(3); 03059 object.dimensions[0] = scaleX; 03060 object.dimensions[1] = scaleY; 03061 object.dimensions[2] = scaleZ; 03062 break; 03063 case PlanningSceneEditor::Cylinder: 03064 object.type = arm_navigation_msgs::Shape::CYLINDER; 03065 object.dimensions.resize(2); 03066 object.dimensions[0] = scaleX * 0.5f; 03067 object.dimensions[1] = scaleZ; 03068 break; 03069 case PlanningSceneEditor::Sphere: 03070 object.type = arm_navigation_msgs::Shape::SPHERE; 03071 object.dimensions.resize(1); 03072 object.dimensions[0] = scaleX * 0.5f; 03073 break; 03074 default: 03075 object.type = arm_navigation_msgs::Shape::SPHERE; 03076 object.dimensions.resize(1); 03077 object.dimensions[0] = scaleX * 0.5f; 03078 break; 03079 }; 03080 03081 collision_object.shapes.push_back(object); 03082 collision_object.poses.push_back(pose); 03083 03084 createSelectableMarkerFromCollisionObject(collision_object, collision_object.id, collision_object.id, color); 03085 03086 ROS_INFO("Created collision object."); 03087 ROS_INFO("Sending planning scene %s", current_planning_scene_name_.c_str()); 03088 03089 sendPlanningScene(planning_scene_map_[current_planning_scene_name_]); 03090 03091 unlockScene(); 03092 return collision_object.id; 03093 03094 } 03095 03096 void PlanningSceneEditor::attachCollisionObject(const std::string& name, 03097 const std::string& link_name, 03098 const std::vector<std::string>& touch_links) { 03099 lockScene(); 03100 if(selectable_objects_->find(name) == selectable_objects_->end()) { 03101 ROS_WARN("Must already have selectable object to add collision object"); 03102 unlockScene(); 03103 return; 03104 } 03105 03106 SelectableObject& selectable = (*selectable_objects_)[name]; 03107 03108 selectable.attached_collision_object_.object = selectable.collision_object_; 03109 selectable.attached_collision_object_.link_name = link_name; 03110 selectable.attached_collision_object_.touch_links = touch_links; 03111 selectable.attached_collision_object_.object.operation.operation = selectable.attached_collision_object_.object.operation.ADD; 03112 selectable.attach_ = true; 03113 //now we need to map the collision object pose into the attached object pose 03114 geometry_msgs::Pose p = selectable.attached_collision_object_.object.poses[0]; 03115 geometry_msgs::PoseStamped ret_pose; 03116 ROS_DEBUG_STREAM("Before attach object pose frame " << selectable.attached_collision_object_.object.header.frame_id << " is " 03117 << p.position.x << " " << p.position.y << " " << p.position.z); 03118 cm_->convertPoseGivenWorldTransform(*robot_state_, 03119 link_name, 03120 selectable.attached_collision_object_.object.header, 03121 selectable.attached_collision_object_.object.poses[0], 03122 ret_pose); 03123 selectable.attached_collision_object_.object.header = ret_pose.header; 03124 selectable.attached_collision_object_.object.poses[0] = ret_pose.pose; 03125 ROS_DEBUG_STREAM("Converted attach object pose frame " << ret_pose.header.frame_id << " is " << ret_pose.pose.position.x << " " << ret_pose.pose.position.y << " " << ret_pose.pose.position.z); 03126 sendPlanningScene(planning_scene_map_[current_planning_scene_name_]); 03127 unlockScene(); 03128 } 03129 03130 bool PlanningSceneEditor::solveIKForEndEffectorPose(MotionPlanRequestData& mpr, 03131 planning_scene_utils::PositionType type, bool coll_aware, 03132 bool constrain_pitch_and_roll, double change_redundancy) 03133 { 03134 kinematics_msgs::PositionIKRequest ik_request; 03135 ik_request.ik_link_name = mpr.getEndEffectorLink(); 03136 ik_request.pose_stamped.header.frame_id = cm_->getWorldFrameId(); 03137 ik_request.pose_stamped.header.stamp = ros::Time(ros::WallTime::now().toSec()); 03138 03139 KinematicState* state = NULL; 03140 03141 if(type == StartPosition) 03142 { 03143 state = mpr.getStartState(); 03144 } 03145 else 03146 { 03147 state = mpr.getGoalState(); 03148 } 03149 03150 tf::poseTFToMsg(state->getLinkState(mpr.getEndEffectorLink())->getGlobalLinkTransform(), ik_request.pose_stamped.pose); 03151 03152 convertKinematicStateToRobotState(*state, ros::Time(ros::WallTime::now().toSec()), cm_->getWorldFrameId(), 03153 ik_request.robot_state); 03154 ik_request.ik_seed_state = ik_request.robot_state; 03155 03156 map<string, double> joint_values; 03157 vector<string> joint_names; 03158 03159 if(coll_aware) 03160 { 03161 kinematics_msgs::GetConstraintAwarePositionIK::Request ik_req; 03162 kinematics_msgs::GetConstraintAwarePositionIK::Response ik_res; 03163 if(constrain_pitch_and_roll) 03164 { 03165 arm_navigation_msgs::Constraints goal_constraints; 03166 goal_constraints.orientation_constraints.resize(1); 03167 arm_navigation_msgs::Constraints path_constraints; 03168 path_constraints.orientation_constraints.resize(1); 03169 std::string name = mpr.getEndEffectorLink(); 03170 determinePitchRollConstraintsGivenState(*state, name, goal_constraints.orientation_constraints[0], 03171 path_constraints.orientation_constraints[0]); 03172 03173 arm_navigation_msgs::ArmNavigationErrorCodes err; 03174 if(!cm_->isKinematicStateValid(*state, std::vector<std::string>(), err, goal_constraints, path_constraints)) 03175 { 03176 ROS_INFO_STREAM("Violates rp constraints"); 03177 return false; 03178 } 03179 ik_req.constraints = goal_constraints; 03180 } 03181 ik_req.ik_request = ik_request; 03182 ik_req.timeout = ros::Duration(0.2); 03183 if(!(*collision_aware_ik_services_)[mpr.getEndEffectorLink()]->call(ik_req, ik_res)) 03184 { 03185 ROS_INFO("Problem with ik service call"); 03186 return false; 03187 } 03188 if(ik_res.error_code.val != ik_res.error_code.SUCCESS) 03189 { 03190 ROS_DEBUG_STREAM("Call yields bad error code " << ik_res.error_code.val); 03191 return false; 03192 } 03193 joint_names = ik_res.solution.joint_state.name; 03194 03195 for(unsigned int i = 0; i < ik_res.solution.joint_state.name.size(); i++) 03196 { 03197 joint_values[ik_res.solution.joint_state.name[i]] = ik_res.solution.joint_state.position[i]; 03198 } 03199 03200 } 03201 else 03202 { 03203 kinematics_msgs::GetPositionIK::Request ik_req; 03204 kinematics_msgs::GetPositionIK::Response ik_res; 03205 ik_req.ik_request = ik_request; 03206 ik_req.timeout = ros::Duration(0.2); 03207 if(!(*non_collision_aware_ik_services_)[mpr.getEndEffectorLink()]->call(ik_req, ik_res)) 03208 { 03209 ROS_INFO("Problem with ik service call"); 03210 return false; 03211 } 03212 if(ik_res.error_code.val != ik_res.error_code.SUCCESS) 03213 { 03214 ROS_DEBUG_STREAM("Call yields bad error code " << ik_res.error_code.val); 03215 return false; 03216 } 03217 for(unsigned int i = 0; i < ik_res.solution.joint_state.name.size(); i++) 03218 { 03219 joint_values[ik_res.solution.joint_state.name[i]] = ik_res.solution.joint_state.position[i]; 03220 } 03221 03222 } 03223 03224 lockScene(); 03225 state->setKinematicState(joint_values); 03226 03227 if(coll_aware) 03228 { 03229 Constraints emp_con; 03230 ArmNavigationErrorCodes error_code; 03231 03232 collision_space::EnvironmentModel::AllowedCollisionMatrix acm = cm_->getCurrentAllowedCollisionMatrix(); 03233 cm_->disableCollisionsForNonUpdatedLinks(mpr.getGroupName()); 03234 03235 if(!cm_->isKinematicStateValid(*state, joint_names, error_code, emp_con, emp_con, true)) 03236 { 03237 ROS_INFO_STREAM("Problem with response"); 03238 cm_->setAlteredAllowedCollisionMatrix(acm); 03239 unlockScene(); 03240 return false; 03241 } 03242 cm_->setAlteredAllowedCollisionMatrix(acm); 03243 } 03244 03245 if(type == StartPosition) 03246 { 03247 mpr.setStartStateValues(joint_values); 03248 convertKinematicStateToRobotState(*state, ros::Time(ros::WallTime::now().toSec()), cm_->getWorldFrameId(), 03249 mpr.getMotionPlanRequest().start_state); 03250 mpr.setLastGoodStartPose((state->getLinkState(mpr.getEndEffectorLink())->getGlobalLinkTransform())); 03251 } 03252 else 03253 { 03254 mpr.setGoalStateValues(joint_values); 03255 mpr.setLastGoodGoalPose((state->getLinkState(mpr.getEndEffectorLink())->getGlobalLinkTransform())); 03256 } 03257 unlockScene(); 03258 03259 return true; 03260 } 03261 03262 void PlanningSceneEditor::setJointState(MotionPlanRequestData& data, PositionType position, std::string& jointName, 03263 btTransform value) 03264 { 03265 KinematicState* currentState = NULL; 03266 03267 if(position == StartPosition) 03268 { 03269 currentState = data.getStartState(); 03270 } 03271 else if(position == GoalPosition) 03272 { 03273 currentState = data.getGoalState(); 03274 } 03275 03276 if(currentState == NULL) 03277 { 03278 ROS_ERROR("Robot state for request %s is null!", data.getName().c_str()); 03279 return; 03280 } 03281 03282 string parentLink = currentState->getKinematicModel()->getJointModel(jointName)->getParentLinkModel()->getName(); 03283 string childLink = currentState->getKinematicModel()->getJointModel(jointName)->getChildLinkModel()->getName(); 03284 KinematicState::JointState* jointState = currentState->getJointState(jointName); 03285 const KinematicModel::JointModel* jointModel = jointState->getJointModel(); 03286 03287 bool isRotational = (dynamic_cast<const KinematicModel::RevoluteJointModel*> (jointModel) != NULL); 03288 bool isPrismatic = (dynamic_cast<const KinematicModel::PrismaticJointModel*> (jointModel) != NULL); 03289 03290 KinematicState::LinkState* linkState = currentState->getLinkState(parentLink); 03291 btTransform transformedValue; 03292 03293 if(isPrismatic) 03294 { 03295 value.setRotation(jointState->getVariableTransform().getRotation()); 03296 transformedValue = currentState->getLinkState(childLink)->getLinkModel()->getJointOriginTransform().inverse() 03297 * linkState->getGlobalLinkTransform().inverse() * value; 03298 } 03299 else if(isRotational) 03300 { 03301 transformedValue = currentState->getLinkState(childLink)->getLinkModel()->getJointOriginTransform().inverse() 03302 * linkState->getGlobalLinkTransform().inverse() * value; 03303 } 03304 03305 btTransform oldState = jointState->getVariableTransform(); 03306 jointState->setJointStateValues(transformedValue); 03307 03308 map<string, double> stateMap; 03309 if(currentState->isJointWithinBounds(jointName)) 03310 { 03311 currentState->getKinematicStateValues(stateMap); 03312 currentState->setKinematicState(stateMap); 03313 03314 03315 // Send state to robot model. 03316 if(position == StartPosition) 03317 { 03318 convertKinematicStateToRobotState(*currentState, 03319 data.getMotionPlanRequest().start_state.joint_state.header.stamp, 03320 data.getMotionPlanRequest().start_state.joint_state.header.frame_id, 03321 data.getMotionPlanRequest().start_state); 03322 } 03323 else 03324 { 03325 std::vector<JointConstraint>& constraints = data.getMotionPlanRequest().goal_constraints.joint_constraints; 03326 03327 for(size_t i = 0; i < constraints.size(); i++) 03328 { 03329 JointConstraint& constraint = constraints[i]; 03330 constraint.position = stateMap[constraint.joint_name]; 03331 } 03332 } 03333 03334 03335 createIKController(data, position, true); 03336 createJointMarkers(data, position); 03337 } 03338 else 03339 { 03340 jointState->setJointStateValues(oldState); 03341 } 03342 } 03343 03344 void PlanningSceneEditor::deleteJointMarkers(MotionPlanRequestData& data, PositionType type) 03345 { 03346 vector<string> jointNames = data.getJointNamesInGoal(); 03347 for(size_t i = 0; i < jointNames.size(); i++) 03348 { 03349 if(type == StartPosition) 03350 { 03351 std::string markerName = jointNames[i] + "_mpr_" + data.getName() + "_start_control"; 03352 03353 InteractiveMarker dummy; 03354 if(interactive_marker_server_->get(markerName, dummy)) 03355 { 03356 interactive_marker_server_->erase(markerName); 03357 } 03358 } 03359 else 03360 { 03361 std::string markerName = jointNames[i] + "_mpr_" + data.getName() + "_end_control"; 03362 InteractiveMarker dummy; 03363 if(interactive_marker_server_->get(markerName, dummy)) 03364 { 03365 interactive_marker_server_->erase(markerName); 03366 } 03367 } 03368 } 03369 } 03370 03371 void PlanningSceneEditor::createJointMarkers(MotionPlanRequestData& data, PositionType position) 03372 { 03373 vector<string> jointNames = data.getJointNamesInGoal(); 03374 03375 KinematicState* state = NULL; 03376 std::string sauce = ""; 03377 03378 if(position == StartPosition) 03379 { 03380 state = data.getStartState(); 03381 sauce = "_start_control"; 03382 } 03383 else if(position == GoalPosition) 03384 { 03385 state = data.getGoalState(); 03386 sauce = "_end_control"; 03387 } 03388 03389 // For each joint model, find the location of its axis and make a control there. 03390 for(size_t i = 0; i < jointNames.size(); i++) 03391 { 03392 const string& jointName = jointNames[i]; 03393 KinematicModel::JointModel* model = 03394 (KinematicModel::JointModel*)(state->getKinematicModel()->getJointModel(jointName)); 03395 03396 std::string controlName = jointName + "_mpr_" + data.getName() + sauce; 03397 joint_clicked_map_[controlName] = false; 03398 03399 if(model->getParentLinkModel() != NULL) 03400 { 03401 string parentLinkName = model->getParentLinkModel()->getName(); 03402 string childLinkName = model->getChildLinkModel()->getName(); 03403 btTransform transform = state->getLinkState(parentLinkName)->getGlobalLinkTransform() 03404 * (state->getKinematicModel()->getLinkModel(childLinkName)->getJointOriginTransform() 03405 * (state->getJointState(jointName)->getVariableTransform())); 03406 03407 joint_prev_transform_map_[controlName] = transform; 03408 03409 InteractiveMarker dummy; 03410 if(interactive_marker_server_->get(controlName, dummy)) 03411 { 03412 dummy.header.frame_id = cm_->getWorldFrameId(); 03413 interactive_marker_server_->setPose(controlName, toGeometryPose(transform), dummy.header); 03414 continue; 03415 } 03416 03417 const shapes::Shape* linkShape = model->getChildLinkModel()->getLinkShape(); 03418 const shapes::Mesh* meshShape = dynamic_cast<const shapes::Mesh*> (linkShape); 03419 03420 KinematicModel::RevoluteJointModel* revoluteJoint = dynamic_cast<KinematicModel::RevoluteJointModel*> (model); 03421 KinematicModel::PrismaticJointModel* prismaticJoint = dynamic_cast<KinematicModel::PrismaticJointModel*> (model); 03422 double maxDimension = 0.0f; 03423 if(meshShape != NULL) 03424 { 03425 03426 for(unsigned int i = 0; i < meshShape->vertexCount; i++) 03427 { 03428 double x = meshShape->vertices[3 * i]; 03429 double y = meshShape->vertices[3 * i]; 03430 double z = meshShape->vertices[3 * i]; 03431 03432 if(abs(maxDimension) < abs(sqrt(x * x + y * y + z * z))) 03433 { 03434 maxDimension = abs(x); 03435 } 03436 03437 } 03438 03439 maxDimension *= 3.0; 03440 03441 maxDimension = max(0.15, maxDimension); 03442 maxDimension = min(0.5, maxDimension); 03443 } 03444 else 03445 { 03446 maxDimension = 0.15; 03447 } 03448 03449 if(revoluteJoint != NULL) 03450 { 03451 makeInteractive1DOFRotationMarker(transform, revoluteJoint->axis_, controlName, "", (float)maxDimension, 03452 state->getJointState(jointName)->getJointStateValues()[0]); 03453 } 03454 else if(prismaticJoint != NULL) 03455 { 03456 makeInteractive1DOFTranslationMarker(transform, prismaticJoint->axis_, controlName, "", (float)maxDimension, 03457 state-> getJointState(jointName)->getJointStateValues()[0]); 03458 } 03459 03460 } 03461 } 03462 interactive_marker_server_->applyChanges(); 03463 } 03464 03465 void PlanningSceneEditor::makeInteractive1DOFTranslationMarker(btTransform transform, btVector3 axis, string name, 03466 string description, float scale, float value) 03467 { 03468 InteractiveMarker marker; 03469 marker.header.frame_id = cm_->getWorldFrameId(); 03470 marker.pose.position.x = transform.getOrigin().x(); 03471 marker.pose.position.y = transform.getOrigin().y(); 03472 marker.pose.position.z = transform.getOrigin().z(); 03473 marker.pose.orientation.w = transform.getRotation().w(); 03474 marker.pose.orientation.x = transform.getRotation().x(); 03475 marker.pose.orientation.y = transform.getRotation().y(); 03476 marker.pose.orientation.z = transform.getRotation().z(); 03477 marker.scale = scale; 03478 marker.name = name; 03479 marker.description = description; 03480 InteractiveMarker dummy; 03481 InteractiveMarkerControl control; 03482 if(interactive_marker_server_->get(marker.name, dummy)) 03483 { 03484 interactive_marker_server_->setPose(marker.name, marker.pose, marker.header); 03485 } 03486 else 03487 { 03488 control.orientation.x = axis.x(); 03489 control.orientation.y = axis.z(); 03490 control.orientation.z = axis.y(); 03491 control.orientation.w = 1; 03492 control.independent_marker_orientation = false; 03493 control.always_visible = false; 03494 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 03495 marker.controls.push_back(control); 03496 interactive_marker_server_->insert(marker); 03497 interactive_marker_server_->setCallback(marker.name, joint_control_feedback_ptr_); 03498 } 03499 03500 } 03501 03502 void PlanningSceneEditor::makeInteractive1DOFRotationMarker(btTransform transform, btVector3 axis, string name, 03503 string description, float scale, float angle) 03504 { 03505 InteractiveMarker marker; 03506 marker.header.frame_id = cm_->getWorldFrameId(); 03507 marker.pose.position.x = transform.getOrigin().x(); 03508 marker.pose.position.y = transform.getOrigin().y(); 03509 marker.pose.position.z = transform.getOrigin().z(); 03510 marker.pose.orientation.w = transform.getRotation().w(); 03511 marker.pose.orientation.x = transform.getRotation().x(); 03512 marker.pose.orientation.y = transform.getRotation().y(); 03513 marker.pose.orientation.z = transform.getRotation().z(); 03514 marker.scale = scale; 03515 marker.name = name; 03516 marker.description = description; 03517 03518 InteractiveMarker dummy; 03519 if(interactive_marker_server_->get(marker.name, dummy)) 03520 { 03521 interactive_marker_server_->setPose(marker.name, marker.pose, marker.header); 03522 } 03523 else 03524 { 03525 InteractiveMarkerControl control; 03526 control.orientation.x = axis.x(); 03527 control.orientation.y = axis.z(); 03528 control.orientation.z = axis.y(); 03529 control.orientation.w = 1; 03530 control.independent_marker_orientation = false; 03531 control.always_visible = false; 03532 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 03533 marker.controls.push_back(control); 03534 interactive_marker_server_->insert(marker); 03535 interactive_marker_server_->setCallback(marker.name, joint_control_feedback_ptr_); 03536 } 03537 } 03538 03539 void PlanningSceneEditor::setIKControlsVisible(std::string id, PositionType type, bool visible) 03540 { 03541 if(!visible) 03542 { 03543 if(type == StartPosition) 03544 { 03545 interactive_marker_server_->erase((*ik_controllers_)[id].start_controller_.name); 03546 } 03547 else 03548 { 03549 interactive_marker_server_->erase((*ik_controllers_)[id].end_controller_.name); 03550 } 03551 interactive_marker_server_->applyChanges(); 03552 } 03553 else 03554 { 03555 createIKController(motion_plan_map_[id], type, false); 03556 interactive_marker_server_->applyChanges(); 03557 } 03558 } 03559 03560 void PlanningSceneEditor::executeTrajectory(TrajectoryData& trajectory) 03561 { 03562 if(params_.sync_robot_state_with_gazebo_) 03563 { 03564 pr2_mechanism_msgs::ListControllers listControllers; 03565 03566 if(!list_controllers_client_.call(listControllers.request, listControllers.response)) 03567 { 03568 ROS_ERROR("Failed to get list of controllers!"); 03569 return; 03570 } 03571 03572 std::map<std::string, planning_models::KinematicModel::JointModelGroup*> jointModelGroupMap = 03573 cm_->getKinematicModel()->getJointModelGroupMap(); 03574 planning_models::KinematicModel::JointModelGroup* rightGroup = NULL; 03575 planning_models::KinematicModel::JointModelGroup* leftGroup = NULL; 03576 if(params_.right_arm_group_ != "none") 03577 { 03578 rightGroup = jointModelGroupMap[params_.right_arm_group_]; 03579 } 03580 03581 if(params_.left_arm_group_ != "none") 03582 { 03583 leftGroup = jointModelGroupMap[params_.left_arm_group_]; 03584 } 03585 03586 pr2_mechanism_msgs::SwitchController switchControllers; 03587 switchControllers.request.stop_controllers = listControllers.response.controllers; 03588 if(!switch_controllers_client_.call(switchControllers.request, switchControllers.response)) 03589 { 03590 ROS_ERROR("Failed to shut down controllers!"); 03591 return; 03592 } 03593 03594 ROS_INFO("Shut down controllers."); 03595 03596 MotionPlanRequestData& motionPlanData = motion_plan_map_[getMotionPlanRequestNameFromId(trajectory.getMotionPlanRequestId())]; 03597 03598 gazebo_msgs::SetModelConfiguration modelConfiguration; 03599 modelConfiguration.request.model_name = params_.gazebo_model_name_; 03600 modelConfiguration.request.urdf_param_name = params_.robot_description_param_; 03601 03602 for(size_t i = 0; i < motionPlanData.getStartState()->getJointStateVector().size(); i++) 03603 { 03604 const KinematicState::JointState* jointState = motionPlanData.getStartState()->getJointStateVector()[i]; 03605 if(jointState->getJointStateValues().size() > 0) 03606 { 03607 modelConfiguration.request.joint_names.push_back(jointState->getName()); 03608 modelConfiguration.request.joint_positions.push_back(jointState->getJointStateValues()[0]); 03609 } 03610 } 03611 03612 if(!gazebo_joint_state_client_.call(modelConfiguration.request, modelConfiguration.response)) 03613 { 03614 ROS_ERROR("Failed to call gazebo set joint state client!"); 03615 return; 03616 } 03617 03618 ROS_INFO("Set joint state"); 03619 03620 if(!modelConfiguration.response.success) 03621 { 03622 ROS_ERROR("Failed to set gazebo model configuration to start state!"); 03623 return; 03624 } 03625 ROS_INFO("Gazebo returned: %s", modelConfiguration.response.status_message.c_str()); 03626 03627 pr2_mechanism_msgs::SwitchController restartControllers; 03628 restartControllers.request.start_controllers = listControllers.response.controllers; 03629 if(!switch_controllers_client_.call(restartControllers.request, restartControllers.response)) 03630 { 03631 ROS_ERROR("Failed to restart controllers: service call failed!"); 03632 return; 03633 } 03634 else if(!restartControllers.response.ok) 03635 { 03636 ROS_ERROR("Failed to restart controllers: Response not ok!"); 03637 } 03638 03639 ROS_INFO("Restart controllers."); 03640 03641 ros::Time::sleepUntil(ros::Time::now() + ros::Duration(0.5)); 03642 SimpleActionClient<FollowJointTrajectoryAction>* controller = arm_controller_map_[trajectory.getGroupName()]; 03643 FollowJointTrajectoryGoal goal; 03644 goal.trajectory.joint_names = trajectory.getTrajectory().joint_names; 03645 goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.2); 03646 trajectory_msgs::JointTrajectoryPoint endPoint = trajectory.getTrajectory().points[0]; 03647 endPoint.time_from_start = ros::Duration(1.0); 03648 goal.trajectory.points.push_back(endPoint); 03649 controller->sendGoalAndWait(goal, ros::Duration(1.0), ros::Duration(1.0)); 03650 ros::Time::sleepUntil(ros::Time::now() + ros::Duration(1.0)); 03651 03652 } 03653 03654 SimpleActionClient<FollowJointTrajectoryAction>* controller = arm_controller_map_[trajectory.getGroupName()]; 03655 FollowJointTrajectoryGoal goal; 03656 goal.trajectory = trajectory.getTrajectory(); 03657 goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.2); 03658 controller->sendGoal(goal, boost::bind(&PlanningSceneEditor::controllerDoneCallback, this, _1, _2)); 03659 logged_group_name_ = trajectory.getGroupName(); 03660 logged_motion_plan_request_ = getMotionPlanRequestNameFromId(trajectory.getMotionPlanRequestId()); 03661 logged_trajectory_ = trajectory.getTrajectory(); 03662 logged_trajectory_.points.clear(); 03663 logged_trajectory_start_time_ = ros::Time::now() + ros::Duration(0.2); 03664 monitor_status_ = Executing; 03665 03666 } 03667 03668 void PlanningSceneEditor::randomlyPerturb(MotionPlanRequestData& mpr, PositionType type) 03669 { 03670 lockScene(); 03671 03672 //Joint space method 03673 03674 KinematicState* currentState = NULL; 03675 03676 if(type == StartPosition) 03677 { 03678 currentState = mpr.getStartState(); 03679 } 03680 else 03681 { 03682 currentState = mpr.getGoalState(); 03683 } 03684 03685 vector<KinematicState::JointState*>& jointStates = currentState->getJointStateVector(); 03686 std::map<string, double> originalState; 03687 std::map<string, double> stateMap; 03688 bool goodSolution = false; 03689 int numIterations = 0; 03690 int maxIterations = 100; 03691 while(!goodSolution && numIterations < maxIterations) 03692 { 03693 currentState->getKinematicStateValues(stateMap); 03694 for(size_t i = 0; i < jointStates.size(); i++) 03695 { 03696 KinematicState::JointState* jointState = jointStates[i]; 03697 map<string, pair<double, double> > bounds = jointState->getJointModel()->getAllVariableBounds(); 03698 for(map<string, pair<double, double> >::iterator it = bounds.begin(); it != bounds.end(); it++) 03699 { 03700 if(!mpr.isJointNameInGoal(it->first)) 03701 { 03702 continue; 03703 } 03704 double range = it->second.second - it->second.first; 03705 if(range == std::numeric_limits<double>::infinity()) 03706 { 03707 continue; 03708 } 03709 double randVal = ((double)random() / (double)RAND_MAX) * (range * 0.99) + it->second.first; 03710 stateMap[it->first] = randVal; 03711 } 03712 } 03713 03714 currentState->setKinematicState(stateMap); 03715 03716 if(!cm_->isKinematicStateInCollision(*currentState)) 03717 { 03718 goodSolution = true; 03719 break; 03720 } 03721 numIterations++; 03722 } 03723 03724 if(!goodSolution) 03725 { 03726 currentState->setKinematicState(originalState); 03727 unlockScene(); 03728 return; 03729 } 03730 else 03731 { 03732 ROS_INFO("Found a good random solution in %d iterations", numIterations); 03733 } 03734 03735 if(type == StartPosition) 03736 { 03737 convertKinematicStateToRobotState(*currentState, mpr.getMotionPlanRequest().start_state.joint_state.header.stamp, 03738 mpr.getMotionPlanRequest().start_state.joint_state.header.frame_id, 03739 mpr.getMotionPlanRequest().start_state); 03740 } 03741 else 03742 { 03743 std::vector<JointConstraint>& constraints = mpr.getMotionPlanRequest().goal_constraints.joint_constraints; 03744 for(size_t i = 0; i < constraints.size(); i++) 03745 { 03746 JointConstraint& constraint = constraints[i]; 03747 constraint.position = stateMap[constraint.joint_name]; 03748 } 03749 } 03750 mpr.setHasGoodIKSolution(true, type); 03751 createIKController(mpr, type, false); 03752 mpr.setJointControlsVisible(mpr.areJointControlsVisible(), this); 03753 interactive_marker_server_->applyChanges(); 03754 mpr.refreshColors(); 03755 unlockScene(); 03756 03757 } 03758 03759 void PlanningSceneEditor::controllerDoneCallback(const actionlib::SimpleClientGoalState& state, 03760 const control_msgs::FollowJointTrajectoryResultConstPtr& result) 03761 { 03762 monitor_status_ = idle; 03763 MotionPlanRequestData& mpr = motion_plan_map_[logged_motion_plan_request_]; 03764 TrajectoryData logged(mpr.getNextTrajectoryId(), "Robot Monitor", logged_group_name_, logged_trajectory_); 03765 logged.setBadPoint(-1); 03766 logged.setDuration(ros::Time::now() - logged_trajectory_start_time_); 03767 logged.setMotionPlanRequestId(mpr.getId()); 03768 logged.trajectory_error_code_.val = result->error_code; 03769 mpr.addTrajectoryId(logged.getId()); 03770 trajectory_map_[mpr.getName()][logged.getName()] = logged; 03771 logged_trajectory_.points.clear(); 03772 logged_group_name_ = ""; 03773 logged_motion_plan_request_ = ""; 03774 selected_trajectory_name_ = getTrajectoryNameFromId(logged.getId()); 03775 updateState(); 03776 ROS_INFO("CREATING TRAJECTORY %s", logged.getName().c_str()); 03777 } 03778 03779 void PlanningSceneEditor::getAllRobotStampedTransforms(const planning_models::KinematicState& state, 03780 vector<geometry_msgs::TransformStamped>& trans_vector, const ros::Time& stamp) 03781 { 03782 trans_vector.clear(); 03783 const map<string, geometry_msgs::TransformStamped>& transforms = cm_->getSceneTransformMap(); 03784 geometry_msgs::TransformStamped transvec; 03785 for(map<string, geometry_msgs::TransformStamped>::const_iterator it = transforms.begin(); it 03786 != transforms.end(); it++) 03787 { 03788 if(it->first != cm_->getWorldFrameId()) 03789 { 03790 trans_vector.push_back(it->second); 03791 } 03792 } 03793 for(unsigned int i = 0; i < state.getLinkStateVector().size(); i++) 03794 { 03795 const planning_models::KinematicState::LinkState* ls = state.getLinkStateVector()[i]; 03796 03797 if(ls->getName() != cm_->getWorldFrameId()) 03798 { 03799 geometry_msgs::TransformStamped ts; 03800 ts.header.stamp = stamp; 03801 ts.header.frame_id = cm_->getWorldFrameId(); 03802 03803 ts.child_frame_id = ls->getName(); 03804 tf::transformTFToMsg(ls->getGlobalLinkTransform(), ts.transform); 03805 trans_vector.push_back(ts); 03806 } 03807 } 03808 } 03809 03810 void PlanningSceneEditor::sendTransformsAndClock() 03811 { 03812 if(robot_state_ == NULL) 03813 { 03814 return; 03815 } 03816 03817 if(!params_.use_robot_data_) 03818 { 03819 ros::WallTime cur_time = ros::WallTime::now(); 03820 rosgraph_msgs::Clock c; 03821 c.clock.nsec = cur_time.nsec; 03822 c.clock.sec = cur_time.sec; 03823 //clock_publisher_.publish(c); 03824 03825 03826 getAllRobotStampedTransforms(*robot_state_, robot_transforms_, c.clock); 03827 transform_broadcaster_.sendTransform(robot_transforms_); 03828 } 03829 } 03830 03831 MenuHandler::EntryHandle PlanningSceneEditor::registerSubMenuEntry(string menu, string name, string subMenu, 03832 MenuHandler::FeedbackCallback& callback) 03833 { 03834 03835 MenuHandler::EntryHandle toReturn = menu_handler_map_[menu].insert(menu_entry_maps_[menu][subMenu], name, callback); 03836 menu_entry_maps_[menu][name] = toReturn; 03837 return toReturn; 03838 } 03839 03840 MenuHandler::EntryHandle PlanningSceneEditor::registerMenuEntry(string menu, string entryName, 03841 MenuHandler::FeedbackCallback& callback) 03842 { 03843 MenuHandler::EntryHandle toReturn = menu_handler_map_[menu].insert(entryName, callback); 03844 menu_entry_maps_[menu][entryName] = toReturn; 03845 return toReturn; 03846 } 03847 03848 void PlanningSceneEditor::deleteTrajectory(unsigned int mpr_id, unsigned int traj_id) 03849 { 03850 if(!hasTrajectory(getMotionPlanRequestNameFromId(mpr_id), 03851 getTrajectoryNameFromId(traj_id))) { 03852 ROS_WARN_STREAM("No trajectory " << traj_id << " in trajectories for " << mpr_id << " for deletion"); 03853 return; 03854 } 03855 03856 if(current_planning_scene_name_ == "") { 03857 ROS_WARN_STREAM("Shouldn't be calling without a planning scene"); 03858 return; 03859 } 03860 03861 lockScene(); 03862 03863 if(motion_plan_map_.find(getMotionPlanRequestNameFromId(mpr_id)) 03864 == motion_plan_map_.end()) { 03865 ROS_WARN_STREAM("Can't find mpr id " << mpr_id); 03866 unlockScene(); 03867 return; 03868 } 03869 03870 MotionPlanRequestData& request_data = motion_plan_map_[getMotionPlanRequestNameFromId(mpr_id)]; 03871 03872 if(!request_data.hasTrajectoryId(traj_id)) { 03873 ROS_WARN_STREAM("Motion plan request " << mpr_id << " doesn't have trajectory id " << traj_id << " for deletion"); 03874 unlockScene(); 03875 return; 03876 } 03877 request_data.removeTrajectoryId(traj_id); 03878 03879 TrajectoryData& traj = trajectory_map_[getMotionPlanRequestNameFromId(mpr_id)][getTrajectoryNameFromId(traj_id)]; 03880 03881 for(size_t i = 0; i < states_.size(); i++) 03882 { 03883 if(states_[i].state == traj.getCurrentState()) 03884 { 03885 states_[i].state = NULL; 03886 states_[i].source = "Delete trajectory"; 03887 } 03888 } 03889 03890 traj.reset(); 03891 trajectory_map_[getMotionPlanRequestNameFromId(mpr_id)].erase(getTrajectoryNameFromId(traj_id)); 03892 if(trajectory_map_[getMotionPlanRequestNameFromId(mpr_id)].empty()) { 03893 trajectory_map_.erase(getMotionPlanRequestNameFromId(mpr_id)); 03894 } 03895 03896 unlockScene(); 03897 interactive_marker_server_->applyChanges(); 03898 } 03899 03900 void PlanningSceneEditor::deleteMotionPlanRequest(const unsigned int& id, 03901 std::vector<unsigned int>& erased_trajectories) 03902 { 03903 if(motion_plan_map_.find(getMotionPlanRequestNameFromId(id)) == motion_plan_map_.end()) 03904 { 03905 ROS_WARN_STREAM("Trying to delete non-existent motion plan request " << id); 03906 return; 03907 } 03908 lockScene(); 03909 MotionPlanRequestData& motion_plan_data = motion_plan_map_[getMotionPlanRequestNameFromId(id)]; 03910 for(size_t i = 0; i < states_.size(); i++) 03911 { 03912 if(states_[i].state == motion_plan_data.getStartState() || states_[i].state 03913 == motion_plan_data.getGoalState()) 03914 { 03915 states_[i].state = NULL; 03916 states_[i].source = "Delete motion plan request"; 03917 } 03918 } 03919 erased_trajectories.clear(); 03920 for(std::set<unsigned int>::iterator it = motion_plan_data.getTrajectories().begin(); 03921 it != motion_plan_data.getTrajectories().end(); 03922 it++) { 03923 erased_trajectories.push_back(*it); 03924 } 03925 03926 for(size_t i = 0; i < erased_trajectories.size(); i++) 03927 { 03928 deleteTrajectory(motion_plan_data.getId(), erased_trajectories[i]); 03929 } 03930 03931 deleteJointMarkers(motion_plan_data, StartPosition); 03932 deleteJointMarkers(motion_plan_data, GoalPosition); 03933 interactive_marker_server_->erase(motion_plan_data.getName() + "_start_control"); 03934 interactive_marker_server_->erase(motion_plan_data.getName() + "_end_control"); 03935 03936 motion_plan_data.reset(); 03937 motion_plan_map_.erase(getMotionPlanRequestNameFromId(id)); 03938 03939 if(current_planning_scene_name_ == "") { 03940 ROS_WARN_STREAM("Shouldn't be trying to delete an MPR without a current planning scene"); 03941 } else { 03942 PlanningSceneData& data = planning_scene_map_[current_planning_scene_name_]; 03943 if(!data.hasMotionPlanRequestId(id)) { 03944 ROS_WARN_STREAM("Planning scene " << data.getId() << " doesn't have mpr id " << id << " for delete"); 03945 } else { 03946 data.removeMotionPlanRequestId(id); 03947 } 03948 } 03949 updateState(); 03950 unlockScene(); 03951 interactive_marker_server_->applyChanges(); 03952 } 03953 03954 void PlanningSceneEditor::executeTrajectory(const std::string& mpr_name, 03955 const std::string& traj_name) 03956 { 03957 TrajectoryData& traj = trajectory_map_[mpr_name][traj_name]; 03958 executeTrajectory(traj); 03959 } 03960 03961 bool PlanningSceneEditor::hasTrajectory(const std::string& mpr_name, 03962 const std::string& traj_name) const { 03963 if(trajectory_map_.find(mpr_name) == trajectory_map_.end()) { 03964 return false; 03965 } 03966 03967 if(trajectory_map_.at(mpr_name).find(traj_name) == trajectory_map_.at(mpr_name).end()) { 03968 return false; 03969 } 03970 return true; 03971 }