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


move_arm_warehouse
Author(s): Ioan Sucan, Sachin Chitta(sachinc@willowgarage.com)
autogenerated on Fri Dec 6 2013 21:12:34