move_arm_utils.h
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 */
00038 #ifndef MOVE_ARM_UTILS_H
00039 #define MOVE_ARM_UTILS_H
00040 #include <planning_environment/models/collision_models.h>
00041 #include <arm_navigation_msgs/PlanningScene.h>
00042 #include <arm_navigation_msgs/SetPlanningSceneDiff.h>
00043 #include <planning_environment/models/model_utils.h>
00044 #include <rosgraph_msgs/Clock.h>
00045 #include <std_msgs/ColorRGBA.h>
00046 #include <tf/transform_broadcaster.h>
00047 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
00048 #include <kinematics_msgs/GetPositionIK.h>
00049 #include <actionlib/server/simple_action_server.h>
00050 #include <actionlib/client/simple_action_client.h>
00051 #include <actionlib/client/simple_client_goal_state.h>
00052 #include <arm_navigation_msgs/GetMotionPlan.h>
00053 #include <arm_navigation_msgs/GetStateValidity.h>
00054 #include <trajectory_msgs/JointTrajectory.h>
00055 #include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
00056 #include <arm_navigation_msgs/convert_messages.h>
00057 #include <move_arm_warehouse/move_arm_warehouse_logger_reader.h>
00058 #include <arm_navigation_msgs/convert_messages.h>
00059 #include <visualization_msgs/InteractiveMarker.h>
00060 #include <interactive_markers/interactive_marker_server.h>
00061 #include <interactive_markers/menu_handler.h>
00062 #include <interactive_markers/tools.h>
00063 #include <arm_navigation_msgs/CollisionObject.h>
00064 #include <planning_environment/monitors/kinematic_model_state_monitor.h>
00065 #include <control_msgs/FollowJointTrajectoryAction.h>
00066 //#include <gazebo_msgs/SetModelConfiguration.h>
00067 #include <std_srvs/Empty.h>
00068 #include <pr2_mechanism_msgs/ListControllers.h>
00069 #include <pr2_mechanism_msgs/LoadController.h>
00070 #include <pr2_mechanism_msgs/UnloadController.h>
00071 #include <pr2_mechanism_msgs/SwitchController.h>
00072 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00073 // #include <gazebo_msgs/SetLinkProperties.h>
00074 // #include <gazebo_msgs/GetLinkProperties.h>
00075 
00076 typedef std::map<std::string, interactive_markers::MenuHandler::EntryHandle> MenuEntryMap;
00077 typedef std::map<std::string, MenuEntryMap> MenuMap;
00078 typedef std::map<std::string, interactive_markers::MenuHandler> MenuHandlerMap;
00079 
00084 namespace planning_scene_utils
00085 {
00086 
00087 inline static geometry_msgs::Pose toGeometryPose(tf::Transform transform)
00088 {
00089   geometry_msgs::Pose toReturn;
00090   toReturn.position.x = transform.getOrigin().x();
00091   toReturn.position.y = transform.getOrigin().y();
00092   toReturn.position.z = transform.getOrigin().z();
00093   toReturn.orientation.x = transform.getRotation().x();
00094   toReturn.orientation.y = transform.getRotation().y();
00095   toReturn.orientation.z = transform.getRotation().z();
00096   toReturn.orientation.w = transform.getRotation().w();
00097   return toReturn;
00098 }
00099 
00100 inline static tf::Transform toBulletTransform(geometry_msgs::Pose pose)
00101 {
00102   tf::Quaternion quat =
00103     tf::Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
00104   tf::Vector3 vec = tf::Vector3(pose.position.x, pose.position.y, pose.position.z);
00105   return tf::Transform(quat, vec);
00106 }
00107 
00108 inline static std::string getPlanningSceneNameFromId(const unsigned int id) {
00109   std::stringstream ss;
00110   ss << "Planning Scene " << id;
00111   return ss.str();
00112 }
00113 
00114 inline static unsigned int getPlanningSceneIdFromName(const std::string& name) {
00115   std::stringstream ss(name);
00116   std::string temp;
00117   ss >> temp;
00118   ss >> temp;
00119   unsigned int ret;
00120   ss >> ret;
00121   return ret;
00122 }
00123 
00124 inline static std::string getMotionPlanRequestNameFromId(const unsigned int id) {
00125   std::stringstream ss;
00126   ss << "MPR " << id;
00127   return ss.str();
00128 }
00129 
00130 inline static std::string getTrajectoryNameFromId(const unsigned int id) {
00131   std::stringstream ss;
00132   ss << "Trajectory " << id;
00133   return ss.str();
00134 }
00135 
00141 inline static std::string getResultErrorFromCode(int error_code)
00142 {
00143   std::string result;
00144   if(error_code == control_msgs::FollowJointTrajectoryResult::SUCCESSFUL)
00145      result = "Success";
00146   else if(error_code == control_msgs::FollowJointTrajectoryResult::INVALID_GOAL)
00147      result = "Invalid Goal";
00148   else if(error_code == control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS)
00149      result = "Invalid Joints";
00150   else if(error_code == control_msgs::FollowJointTrajectoryResult::OLD_HEADER_TIMESTAMP)
00151      result = "Old header timestamp";
00152   else if(error_code == control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED)
00153      result = "Path tolerance violated";
00154   else if(error_code == control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED)
00155      result = "Goal tolerance violated";
00156   return result;
00157 }
00158 
00164 enum PositionType
00165   {
00166     StartPosition, GoalPosition
00167   };
00168 
00176 enum RenderType
00177   {
00178     CollisionMesh, VisualMesh, PaddingMesh
00179   };
00180 
00187 enum TrajectoryRenderType
00188 {
00189   Kinematic,
00190   Temporal,
00191 };
00192 
00193 
00194 // Must be defined so that subsequent classes can reference.
00195 class PlanningSceneEditor;
00196 
00202 class PlanningSceneData
00203 {
00204 protected:
00205   std::string name_;
00206   unsigned int id_;
00207   std::string host_;
00208   ros::Time timestamp_;
00209   arm_navigation_msgs::PlanningScene planning_scene_;
00210   std::vector<std::string> pipeline_stages_;
00211   std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> error_codes_;
00212   std::set<unsigned int> motion_plan_requests_;
00213 
00214 public:
00215   PlanningSceneData();
00216   PlanningSceneData(unsigned int id, const ros::Time& timestamp, const arm_navigation_msgs::PlanningScene& scene);
00217 
00219   inline std::string getHostName() const
00220   {
00221     return host_;
00222   }
00223 
00225   inline void setHostName(std::string host)
00226   {
00227     host_ = host;
00228   }
00229 
00231   inline std::string getName() const
00232   {
00233     return name_;
00234   }
00235 
00236   inline unsigned int getId() const 
00237   {
00238     return id_;
00239   }
00240 
00242   inline const ros::Time& getTimeStamp() const
00243   {
00244     return timestamp_;
00245   }
00246 
00248   inline arm_navigation_msgs::PlanningScene& getPlanningScene()
00249   {
00250     return planning_scene_;
00251   }
00252 
00254   inline void setTimeStamp(const ros::Time& time)
00255   {
00256     timestamp_ = time;
00257     planning_scene_.robot_state.joint_state.header.stamp = time;
00258   }
00259 
00260   inline void setId(unsigned int id) {
00261     id_ = id;
00262     name_ = getPlanningSceneNameFromId(id);
00263   }
00264 
00266   inline void setPlanningScene(const arm_navigation_msgs::PlanningScene& scene)
00267   {
00268     planning_scene_ = scene;
00269     timestamp_ = scene.robot_state.joint_state.header.stamp;
00270   }
00271 
00275   inline std::vector<std::string>& getPipelineStages()
00276   {
00277     return pipeline_stages_;
00278   }
00279 
00281   inline void setPipelineStages(std::vector<std::string>& stages)
00282   {
00283     pipeline_stages_ = stages;
00284   }
00285 
00287   inline std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& getErrorCodes()
00288   {
00289     return error_codes_;
00290   }
00291 
00293   inline void setErrorCodes(std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& error_codes)
00294   {
00295     error_codes_ = error_codes;
00296   }
00297 
00299   inline std::set<unsigned int>& getRequests()
00300   {
00301     return motion_plan_requests_;
00302   }
00303 
00306   void getRobotState(planning_models::KinematicState* state);
00307 
00308   bool hasMotionPlanRequestId(unsigned int id) const {
00309     return(motion_plan_requests_.find(id) != motion_plan_requests_.end());
00310   }
00311   
00312   void addMotionPlanRequestId(unsigned int id) {
00313     motion_plan_requests_.insert(id);
00314   }
00315 
00316   void removeMotionPlanRequestId(unsigned int id) {
00317     motion_plan_requests_.erase(id);
00318   }
00319 
00320   unsigned int getNextMotionPlanRequestId() const {
00321     if(motion_plan_requests_.empty()) {
00322       return 0;
00323     }
00324     return (*motion_plan_requests_.rbegin())+1;
00325   }
00326 };
00327 
00333 class MotionPlanRequestData
00334 {
00335 protected:
00336   std::string name_;
00337   unsigned int id_;
00338   std::string source_;
00339   unsigned int planning_scene_id_;
00340   std::string end_effector_link_;
00341   std::string group_name_;
00342   arm_navigation_msgs::MotionPlanRequest motion_plan_request_;
00343   bool is_start_editable_;
00344   bool is_goal_editable_;
00345   bool is_start_visible_;
00346   bool is_goal_visible_;
00347   bool should_refresh_colors_;
00348   bool has_refreshed_colors_;
00349   bool has_path_constraints_;
00350   bool has_good_goal_ik_solution_;
00351   bool has_good_start_ik_solution_;
00352   bool are_collisions_visible_;
00353   bool has_state_changed_;
00354   bool are_joint_controls_visible_;
00355 
00356   double roll_tolerance_;
00357   double pitch_tolerance_;
00358   double yaw_tolerance_;
00359 
00360   bool constrain_roll_;
00361   bool constrain_pitch_;
00362   bool constrain_yaw_;
00363 
00364   std_msgs::ColorRGBA start_color_;
00365   std_msgs::ColorRGBA goal_color_;
00366   std::set<unsigned int> trajectories_;
00367   planning_models::KinematicState* start_state_;
00368   planning_models::KinematicState* goal_state_;
00369   tf::Transform last_good_start_pose_;
00370   tf::Transform last_good_goal_pose_;
00371   visualization_msgs::MarkerArray collision_markers_;
00372   RenderType render_type_;
00373   unsigned int next_trajectory_id_;
00374 
00375 public:
00376   MotionPlanRequestData()
00377   {
00378     start_state_ = NULL;
00379     goal_state_ = NULL;
00380   }
00381 
00382   MotionPlanRequestData(const planning_models::KinematicState* robot_state);
00383   MotionPlanRequestData(const unsigned int& id, 
00384                         const std::string& source, 
00385                         const arm_navigation_msgs::MotionPlanRequest& request,
00386                         const planning_models::KinematicState* robot_state, 
00387                         const std::string& end_effector_link);
00388 
00392   ros::Duration refresh_timer_;
00393 
00394 
00397   void setStartStateValues(std::map<std::string, double>& joint_values);
00398 
00401   void setGoalStateValues(std::map<std::string, double>& joint_values);
00402 
00405   void updateStartState();
00406 
00409   void updateGoalState();
00410 
00413   std::vector<std::string> getJointNamesInGoal();
00414 
00419   bool isJointNameInGoal(std::string joint);
00420 
00422   inline RenderType getRenderType() const
00423   {
00424     return render_type_;
00425   }
00426 
00428   inline void setRenderType(RenderType renderType)
00429   {
00430     render_type_ = renderType;
00431   }
00432 
00434   inline bool areJointControlsVisible() const
00435   {
00436     return are_joint_controls_visible_;
00437   }
00438 
00442   void setJointControlsVisible(bool visible, PlanningSceneEditor* editor);
00443 
00445   inline bool hasStateChanged() const
00446   {
00447     return has_state_changed_;
00448   }
00449 
00451   inline void setStateChanged(bool changed)
00452   {
00453     has_state_changed_ = changed;
00454   }
00455 
00457   inline visualization_msgs::MarkerArray& getCollisionMarkers()
00458   {
00459     return collision_markers_;
00460   }
00461 
00463   inline bool areCollisionsVisible() const
00464   {
00465     return are_collisions_visible_;
00466   }
00467 
00469   inline void setCollisionsVisible(bool visible)
00470   {
00471     are_collisions_visible_ = visible;
00472   }
00473 
00475   inline void showCollisions() 
00476   {
00477     setCollisionsVisible(true);
00478   }
00479 
00481   inline void hideCollisions()
00482   {
00483     setCollisionsVisible(false);
00484   }
00485 
00487   inline tf::Transform getLastGoodStartPose() const
00488   {
00489     return last_good_start_pose_;
00490   }
00491 
00493   inline tf::Transform getLastGoodGoalPose() const
00494   {
00495     return last_good_goal_pose_;
00496   }
00497 
00499   inline void setLastGoodStartPose(tf::Transform pose)
00500   {
00501     last_good_start_pose_ = pose;
00502   }
00503 
00505   inline void setLastGoodGoalPose(tf::Transform pose)
00506   {
00507     last_good_goal_pose_ = pose;
00508   }
00509 
00511   inline void setStartState(planning_models::KinematicState* state)
00512   {
00513     start_state_ = state;
00514     setStateChanged(true);
00515   }
00516 
00518   inline void setGoalState(planning_models::KinematicState* state)
00519   {
00520     goal_state_ = state;
00521     setStateChanged(true);
00522   }
00523 
00525   inline void reset()
00526   {
00527     if(start_state_ != NULL)
00528     {
00529       delete start_state_;
00530       start_state_ = NULL;
00531     }
00532 
00533     if(goal_state_ != NULL)
00534     {
00535       delete goal_state_;
00536       goal_state_ = NULL;
00537     }
00538   }
00539 
00541   inline bool hasGoodIKSolution(const PositionType& type) const
00542   {
00543     if(type == StartPosition) {
00544       return has_good_start_ik_solution_;
00545     } else {
00546       return has_good_goal_ik_solution_;
00547     }
00548   }
00549 
00551   inline void setHasGoodIKSolution(const bool& solution, const PositionType& type)
00552   {
00553     if(type == StartPosition) {
00554       has_good_start_ik_solution_ = solution;
00555     } else {
00556       has_good_goal_ik_solution_ = solution;
00557     }
00558   }
00559 
00561   inline planning_models::KinematicState* getStartState()
00562   {
00563     return start_state_;
00564   }
00565 
00567   inline planning_models::KinematicState* getGoalState()
00568   {
00569     return goal_state_;
00570   }
00571 
00574   inline std::string getGroupName() const
00575   { 
00576     return group_name_;
00577   }
00578 
00580   inline std::string getEndEffectorLink() const
00581   {
00582     return end_effector_link_;
00583   }
00584 
00587   inline void setGroupName(const std::string& name)
00588   {
00589     group_name_ = name;
00590   }
00591 
00593   inline void setEndEffectorLink(const std::string& name)
00594   {
00595     end_effector_link_ = name;
00596   }
00597 
00599   inline const std::string& getName() const
00600   {
00601     return name_;
00602   }
00603   
00604   unsigned int getId() const {
00605     return id_;
00606   }
00607 
00609   inline void setId(const unsigned int id)
00610   {
00611     id_ = id;
00612     name_ = getMotionPlanRequestNameFromId(id_);
00613   }
00614 
00616   inline bool shouldRefreshColors() const 
00617   {
00618     return should_refresh_colors_;
00619   }
00620 
00622   inline bool hasRefreshedColors() const
00623   {
00624     return has_refreshed_colors_;
00625   }
00626 
00628   inline void setHasRefreshedColors(bool refresh)
00629   {
00630     has_refreshed_colors_ = refresh;
00631 
00632     if(refresh)
00633     {
00634       should_refresh_colors_ = false;
00635     }
00636   }
00637 
00640   inline void refreshColors()
00641   {
00642     should_refresh_colors_ = true;
00643     has_refreshed_colors_ = false;
00644     refresh_timer_ = ros::Duration(0.0);
00645   }
00646 
00649   inline bool isStartVisible() const
00650   {
00651     return is_start_visible_;
00652   }
00653 
00656   inline bool isEndVisible() const
00657   {
00658     return is_goal_visible_;
00659   }
00660 
00662   inline void setStartVisible(bool visible)
00663   {
00664     is_start_visible_ = visible;
00665   }
00666 
00668   inline void setEndVisible(bool visible)
00669   {
00670     is_goal_visible_ = visible;
00671   }
00672 
00674   inline void show()
00675   {
00676     setStartVisible(true);
00677     setEndVisible(true);
00678   }
00679 
00681   inline void hide()
00682   {
00683     setStartVisible(false);
00684     setEndVisible(false);
00685   }
00686 
00688   inline void showStart()
00689   {
00690     setStartVisible(true);
00691   }
00692 
00694   inline void showGoal()
00695   {
00696     setEndVisible(true);
00697   }
00698 
00700   inline void hideStart()
00701   {
00702     setStartVisible(false);
00703   }
00704 
00706   inline void hideGoal()
00707   {
00708     setEndVisible(false);
00709   }
00710 
00712   inline const std_msgs::ColorRGBA& getStartColor() const
00713   {
00714     return start_color_;
00715   }
00716 
00718   inline const std_msgs::ColorRGBA& getGoalColor() const
00719   {
00720     return goal_color_;
00721   }
00722 
00724   inline void setStartColor(std_msgs::ColorRGBA color)
00725   {
00726     start_color_ = color;
00727   }
00728 
00730   inline void setGoalColor(std_msgs::ColorRGBA color)
00731   {
00732     goal_color_ = color;
00733   }
00734 
00737   inline void setStartEditable(bool editable)
00738   {
00739     is_start_editable_ = editable;
00740   }
00741 
00744   inline void setGoalEditable(bool editable)
00745   {
00746     is_goal_editable_ = editable;
00747   }
00748 
00751   inline bool isStartEditable() const
00752   {
00753     return is_start_editable_;
00754   }
00755 
00758   inline bool isGoalEditable() const
00759   {
00760     return is_goal_editable_;
00761   }
00762 
00764   inline void setSource(const std::string& source)
00765   {
00766     source_ = source;
00767   }
00768 
00770   inline const std::string& getSource() const
00771   {
00772     return source_;
00773   }
00774 
00776   inline arm_navigation_msgs::MotionPlanRequest& getMotionPlanRequest()
00777   {
00778     return motion_plan_request_;
00779   }
00780 
00782   inline void setMotionPlanRequest(const arm_navigation_msgs::MotionPlanRequest& request)
00783   {
00784     motion_plan_request_ = request;
00785     setGroupName(request.group_name);
00786     updateStartState();
00787     updateGoalState();
00788   }
00789 
00791   inline void setPlanningSceneId(const unsigned int id)
00792   {
00793     planning_scene_id_ = id;
00794   }
00795   inline unsigned int getPlanningSceneId() const
00796   {
00797     return planning_scene_id_;
00798   }
00799 
00801   inline std::string getPlanningSceneName() const 
00802   {
00803     return getPlanningSceneNameFromId(planning_scene_id_);
00804   }
00805 
00807   inline std::set<unsigned int>& getTrajectories()
00808   {
00809     return trajectories_;
00810   }
00811 
00812   unsigned int getNextTrajectoryId() const {
00813     if(trajectories_.empty()) return 0;
00814     return (*trajectories_.rbegin())+1;
00815   }
00816 
00817   void addTrajectoryId(unsigned int id) {
00818     trajectories_.insert(id);
00819   }
00820   
00821   void removeTrajectoryId(unsigned int id) {
00822     trajectories_.erase(id);
00823   }
00824 
00825   bool hasTrajectoryId(unsigned int id) const {
00826     return (trajectories_.find(id) != trajectories_.end());
00827   }
00828 
00829   bool hasPathConstraints() const {
00830     return has_path_constraints_;
00831   }
00832   
00833   void setPathConstraints(bool has) {
00834     has_path_constraints_ = has;
00835   }
00836 
00837   bool getConstrainRoll() const {
00838     return constrain_roll_;
00839   }
00840 
00841   void setConstrainRoll(bool s) {
00842     constrain_roll_ = s;
00843   }
00844 
00845   bool getConstrainPitch() const {
00846     return constrain_pitch_;
00847   }
00848 
00849   void setConstrainPitch(bool s) {
00850     constrain_pitch_ = s;
00851   }
00852 
00853   bool getConstrainYaw() const {
00854     return constrain_yaw_;
00855   }
00856 
00857   void setConstrainYaw(bool s) {
00858     constrain_yaw_ = s;
00859   }
00860 
00861   double getRollTolerance() const {
00862     return roll_tolerance_;
00863   }
00864 
00865   void setRollTolerance(double s) {
00866     roll_tolerance_ = s;
00867   }
00868 
00869   double getPitchTolerance() const {
00870     return pitch_tolerance_;
00871   }
00872 
00873   void setPitchTolerance(double s) {
00874     pitch_tolerance_ = s;
00875   }
00876 
00877   double getYawTolerance() const {
00878     return yaw_tolerance_;
00879   }
00880 
00881   void setYawTolerance(double s) {
00882     yaw_tolerance_ = s;
00883   }
00884 
00885   void setGoalAndPathPositionOrientationConstraints(arm_navigation_msgs::MotionPlanRequest& mpr,
00886                                                     planning_scene_utils::PositionType type) const;
00887 
00889   void updateCollisionMarkers(planning_environment::CollisionModels* cm_,
00890                               ros::ServiceClient* distance_state_validity_service_client_);
00891 };
00892 
00893 
00899 class TrajectoryData
00900 {
00901 public:
00902 
00903   enum MarkerType {
00904     VISUAL,
00905     COLLISION,
00906     PADDED
00907   };
00908 
00909 protected:
00910   std::string name_;
00911   unsigned int id_;
00912   std::string source_;
00913   std::string group_name_;
00914   unsigned int planning_scene_id_;
00915   unsigned int motion_plan_request_Id_;
00916   trajectory_msgs::JointTrajectory trajectory_;
00917   trajectory_msgs::JointTrajectory trajectory_error_;
00918   bool is_visible_;
00919   MarkerType marker_type_;
00920   bool is_playing_;
00921   ros::Time playback_start_time_;
00922   bool collisions_visible_;
00923   bool state_changed_;
00924   std_msgs::ColorRGBA color_;
00925   unsigned int current_trajectory_point_;
00926   unsigned int trajectory_bad_point_;
00927   planning_models::KinematicState* current_state_;
00928   ros::Duration duration_;
00929   bool should_refresh_colors_;
00930   bool has_refreshed_colors_;
00931   visualization_msgs::MarkerArray collision_markers_;
00932   RenderType render_type_;
00933   TrajectoryRenderType trajectory_render_type_;
00934   ros::Duration time_to_stop_;
00935 public:
00936 
00938   ros::Duration refresh_timer_;
00939 
00941   arm_navigation_msgs::ArmNavigationErrorCodes trajectory_error_code_;
00942 
00943   TrajectoryData();
00944   TrajectoryData(const unsigned int& id, const std::string& source, const std::string& group_name,
00945                  const trajectory_msgs::JointTrajectory& trajectory);
00946   TrajectoryData(const unsigned int& id, const std::string& source, const std::string& group_name,
00947                  const trajectory_msgs::JointTrajectory& trajectory, const trajectory_msgs::JointTrajectory& trajectory_error);
00948 
00951   void advanceThroughTrajectory(int amount);
00952 
00955   void advanceToNextClosestPoint(ros::Time time);
00956 
00958   void updateCurrentState();
00959 
00961   inline bool hasStateChanged() const
00962   { 
00963     return state_changed_;
00964   }
00965 
00967   inline void setStateChanged(bool changed)
00968   {
00969     state_changed_ = changed;
00970   }
00971 
00973   inline visualization_msgs::MarkerArray& getCollisionMarkers()
00974   {
00975     return collision_markers_;
00976   }
00977 
00979   inline bool areCollisionsVisible() const
00980   {
00981     return collisions_visible_;
00982   }
00983 
00985   inline void setCollisionsVisible(bool shown)
00986   {
00987     collisions_visible_ = shown;
00988   }
00989 
00991   inline void showCollisions()
00992   {
00993     setCollisionsVisible(true);
00994   }
00995 
00997   inline void hideCollisions()
00998   {
00999     setCollisionsVisible(false);
01000   }
01001 
01003   inline size_t getTrajectorySize() const
01004   {
01005     return trajectory_.points.size();
01006   }
01007 
01009   inline bool shouldRefreshColors() const
01010   {
01011     return should_refresh_colors_;
01012   }
01013 
01015   inline bool hasRefreshedColors() const
01016   {
01017     return has_refreshed_colors_;
01018   }
01019 
01021   inline void setHasRefreshedColors(bool refresh)
01022   {
01023     has_refreshed_colors_ = refresh;
01024 
01025     if(refresh)
01026     {
01027       should_refresh_colors_ = false;
01028     }
01029   }
01030 
01033   inline void refreshColors()
01034   {
01035     should_refresh_colors_ = true;
01036     has_refreshed_colors_ = false;
01037     refresh_timer_ = ros::Duration(0.0);
01038   }
01039 
01041   inline RenderType getRenderType() const
01042   {
01043     return render_type_;
01044   }
01045 
01047   inline void setRenderType(RenderType renderType)
01048   {
01049     render_type_ = renderType;
01050   }
01051 
01053   inline TrajectoryRenderType getTrajectoryRenderType() const
01054   {
01055     return trajectory_render_type_;
01056   }
01057 
01059   inline void setTrajectoryRenderType(TrajectoryRenderType renderType)
01060   {
01061     trajectory_render_type_ = renderType;
01062   }
01063 
01065   inline void reset()
01066   {
01067 
01068     if(current_state_ != NULL)
01069     {
01070       delete current_state_;
01071       current_state_ = NULL;
01072     }
01073 
01074     is_playing_ = false;
01075     is_visible_ = false;
01076     current_trajectory_point_ = 0;
01077     state_changed_ = false;
01078   }
01079 
01082   inline planning_models::KinematicState* getCurrentState()
01083   {
01084     return current_state_;
01085   }
01086 
01088   inline void setCurrentState(planning_models::KinematicState* state)
01089   {
01090     current_state_ = state;
01091     state_changed_ = true;
01092   }
01093 
01095   inline void setMotionPlanRequestId(const unsigned int& Id)
01096   {
01097     motion_plan_request_Id_ = Id;
01098   }
01099 
01101   inline const unsigned int& getMotionPlanRequestId() const
01102   {
01103     return motion_plan_request_Id_;
01104   }
01105 
01106   inline void setPlanningSceneId(const unsigned int id) {
01107     planning_scene_id_ = id;
01108   }
01109 
01110   unsigned int getPlanningScendId() const {
01111     return planning_scene_id_;
01112   }
01113 
01115   inline void setCurrentPoint(unsigned int point)
01116   {
01117     current_trajectory_point_ = point;
01118     state_changed_ = true;
01119   }
01120 
01122   inline unsigned int getCurrentPoint() const
01123   {
01124     return current_trajectory_point_;
01125   }
01126 
01128   inline unsigned int getBadPoint() const
01129   {
01130     return trajectory_bad_point_;
01131   }
01132 
01134   inline void setGroupname(const std::string& group_name)
01135   {
01136     group_name_ = group_name;
01137   }
01138 
01140   inline bool isPlaying() const
01141   {
01142     return is_playing_;
01143   }
01144 
01146   inline void play()
01147   {
01148     is_playing_ = true;
01149     playback_start_time_ = ros::Time::now();
01150   }
01151 
01153   inline void stop()
01154   {
01155     is_playing_ = false;
01156   }
01157 
01159   inline bool isVisible() const
01160   {
01161     return is_visible_;
01162   }
01163 
01165   inline void setVisible(bool visible)
01166   {
01167     is_visible_ = visible;
01168   }
01169 
01170   inline MarkerType getMarkerType() const 
01171   {
01172     return marker_type_;
01173   }
01174 
01176   inline void setMarkerType(MarkerType mt) 
01177   {
01178     marker_type_ = mt;
01179   }
01180 
01182   inline void show()
01183   {
01184     setVisible(true);
01185   }
01186 
01188   inline void hide()
01189   {
01190     setVisible(false);
01191   }
01192 
01195   inline const ros::Duration& getDuration() const
01196   {
01197     return duration_;
01198   }
01199 
01201   inline void setDuration(const ros::Duration& duration)
01202   {
01203     duration_ = duration;
01204   }
01205 
01207   inline const std_msgs::ColorRGBA& getColor() const
01208   {
01209     return color_;
01210   }
01211 
01213   inline void setColor(const std_msgs::ColorRGBA& color)
01214   {
01215     color_ = color;
01216   }
01217 
01219   inline std::string getSource()
01220   {
01221     return source_;
01222   }
01223 
01225   inline trajectory_msgs::JointTrajectory& getTrajectory()
01226   {
01227     return trajectory_;
01228   }
01229 
01231   inline void setSource(const std::string& source)
01232   {
01233     source_ = source;
01234   }
01235 
01237   inline void setTrajectory(const trajectory_msgs::JointTrajectory& trajectory)
01238   {
01239     trajectory_ = trajectory;
01240   }
01241 
01243   inline trajectory_msgs::JointTrajectory& getTrajectoryError()
01244   {
01245     return trajectory_error_;
01246   }
01247 
01249   inline void setTrajectoryError(const trajectory_msgs::JointTrajectory& trajectory_error)
01250   {
01251     trajectory_error_ = trajectory_error;
01252   }
01253 
01255   inline unsigned int getId() const
01256   {
01257     return id_;
01258   }
01259 
01261   inline void setId(const unsigned int& id)
01262   {
01263     id_ = id;
01264     name_ = getTrajectoryNameFromId(id_);
01265   }
01266 
01267   const std::string& getName() const {
01268     return name_;
01269   }
01270 
01272   inline const std::string& getGroupName() const
01273   {
01274     return group_name_;
01275   }
01276 
01278   inline void setBadPoint(unsigned int point)
01279   {
01280     trajectory_bad_point_ = point;
01281   }
01282 
01284   inline void setGroupName(std::string name)
01285   {
01286     group_name_ = name;
01287   }
01288 
01291   void updateCollisionMarkers(planning_environment::CollisionModels* cm_, MotionPlanRequestData& motionPlanRequest,
01292                               ros::ServiceClient* distance_state_validity_service_client_);
01293 };
01294 
01300 struct PlanningSceneParameters
01301 {
01302   std::string left_ik_name_;
01303   std::string right_ik_name_;
01304   std::string non_coll_left_ik_name_;
01305   std::string non_coll_right_ik_name_;
01306   std::string left_interpolate_service_name_;
01307   std::string right_interpolate_service_name_;
01308   std::string planner_1_service_name_;
01309   std::string planner_2_service_name_;
01310   std::string proximity_space_service_name_;
01311   std::string proximity_space_validity_name_;
01312   std::string set_planning_scene_diff_name_;
01313   std::string trajectory_filter_1_service_name_;
01314   std::string trajectory_filter_2_service_name_;
01315   std::string proximity_space_planner_name_;
01316   std::string vis_topic_name_;
01317   std::string right_ik_link_;
01318   std::string left_ik_link_;
01319   std::string left_redundancy_;
01320   std::string right_redundancy_;
01321   std::string right_arm_group_;
01322   std::string left_arm_group_;
01323   std::string execute_left_trajectory_;
01324   std::string execute_right_trajectory_;
01325   std::string list_controllers_service_;
01326   std::string unload_controllers_service_;
01327   std::string load_controllers_service_;
01328   std::string switch_controllers_service_;
01329   std::string gazebo_model_name_;
01330   std::string robot_description_param_;
01331   bool use_robot_data_;
01332   bool sync_robot_state_with_gazebo_;
01333 };
01334 
01339 class PlanningSceneEditor
01340 {
01341 public:
01346   enum GeneratedShape
01347     {
01348       Box, Cylinder, Sphere
01349     };
01350 protected:
01351 
01360   enum MonitorStatus
01361     {
01362       idle, Executing, WaitingForStop, Done
01363     };
01364 
01373   struct StateRegistry
01374   {
01375     planning_models::KinematicState* state;
01376     std::string source;
01377   };
01378 
01384   struct SelectableObject
01385   {
01386     SelectableObject() {
01387       attach_ = false;
01388       detach_ = false;
01389       attached_collision_object_.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::REMOVE;
01390       collision_object_.operation.operation = arm_navigation_msgs::CollisionObjectOperation::REMOVE;
01391     }
01392 
01393     bool attach_;
01394     bool detach_;
01395     arm_navigation_msgs::AttachedCollisionObject attached_collision_object_;
01396     arm_navigation_msgs::CollisionObject collision_object_;
01397     visualization_msgs::InteractiveMarker selection_marker_;
01398     visualization_msgs::InteractiveMarker control_marker_;
01399     std_msgs::ColorRGBA color_;
01400 
01401     std::string id_;
01402   };
01403 
01409   struct IKController
01410   {
01411     unsigned int motion_plan_id_;
01412     visualization_msgs::InteractiveMarker start_controller_;
01413     visualization_msgs::InteractiveMarker end_controller_;
01414   };
01415 
01420   virtual void updateState()
01421   {
01422   }
01423 
01428   virtual void planCallback(arm_navigation_msgs::ArmNavigationErrorCodes& errorCode) = 0;
01429 
01434   virtual void filterCallback(arm_navigation_msgs::ArmNavigationErrorCodes& errorCode) = 0;
01435 
01436   virtual void attachObjectCallback(const std::string& name) = 0;
01437   // virtual void detachObjectCallback(arm_navigation_msgs::CollisionObject& object) = 0;
01438 
01439   void changeToAttached(const std::string& name);
01440 
01446   virtual void selectedTrajectoryCurrentPointChanged( unsigned int new_current_point ) {}
01447 
01448   boost::recursive_mutex lock_scene_;
01449   arm_navigation_msgs::ArmNavigationErrorCodes last_collision_set_error_code_;
01450   move_arm_warehouse::MoveArmWarehouseLoggerReader* move_arm_warehouse_logger_reader_;
01451   planning_environment::CollisionModels* cm_;
01452   planning_models::KinematicState* robot_state_;
01453   PlanningSceneParameters params_;
01454   ros::NodeHandle nh_;
01455   ros::Publisher clock_publisher_;
01456   ros::Publisher joint_state_publisher_;
01457   ros::Publisher vis_marker_array_publisher_;
01458   ros::Publisher vis_marker_publisher_;
01459   ros::Subscriber joint_state_subscriber_;
01460   ros::Subscriber r_arm_controller_state_subscriber_;
01461   ros::Subscriber l_arm_controller_state_subscriber_;
01462   ros::ServiceClient collision_proximity_planner_client_;
01463   ros::ServiceClient distance_aware_service_client_;
01464   ros::ServiceClient distance_state_validity_service_client_;
01465   ros::ServiceClient set_planning_scene_diff_client_;
01466   ros::ServiceClient left_ik_service_client_;
01467   ros::ServiceClient left_interpolate_service_client_;
01468   ros::ServiceClient non_coll_left_ik_service_client_;
01469   ros::ServiceClient non_coll_right_ik_service_client_;
01470   ros::ServiceClient planning_1_service_client_;
01471   ros::ServiceClient planning_2_service_client_;
01472   ros::ServiceClient right_ik_service_client_;
01473   ros::ServiceClient right_interpolate_service_client_;
01474   ros::ServiceClient trajectory_filter_1_service_client_;
01475   ros::ServiceClient trajectory_filter_2_service_client_;
01476   ros::ServiceClient gazebo_joint_state_client_;
01477   ros::ServiceClient list_controllers_client_;
01478   ros::ServiceClient load_controllers_client_;
01479   ros::ServiceClient unload_controllers_client_;
01480   ros::ServiceClient switch_controllers_client_;
01481   ros::ServiceClient pause_gazebo_client_;
01482   ros::ServiceClient unpause_gazebo_client_;
01483   ros::ServiceClient set_link_properties_client_;
01484   ros::ServiceClient get_link_properties_client_;
01485 
01486   std::map<std::string, double> robot_state_joint_values_;
01487   std::vector<ros::Time> last_creation_time_query_;
01488   tf::TransformBroadcaster transform_broadcaster_;
01489   tf::TransformListener transform_listener_;
01490   std::map<std::string, actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>*> arm_controller_map_;
01491   unsigned int max_trajectory_id_;
01492   unsigned int max_collision_object_id_;
01493 
01494   bool warehouse_data_loaded_once_;
01495   int  active_planner_index_;
01496   bool use_primary_filter_;
01497 
01498   trajectory_msgs::JointTrajectory logged_trajectory_;
01499   trajectory_msgs::JointTrajectory logged_trajectory_controller_error_;
01500   ros::Time logged_trajectory_start_time_;
01501   int logged_trajectory_controller_error_code_;
01502 
01503   bool send_collision_markers_;
01504   std::string collision_marker_state_;
01505   visualization_msgs::MarkerArray collision_markers_;
01506   planning_models::KinematicState* paused_collision_state_;
01507   std_msgs::ColorRGBA point_color_;
01508   std::vector<geometry_msgs::TransformStamped> robot_transforms_;
01509 
01510   interactive_markers::MenuHandler::FeedbackCallback attached_collision_object_feedback_ptr_;
01511   interactive_markers::MenuHandler::FeedbackCallback collision_object_selection_feedback_ptr_;
01512   interactive_markers::MenuHandler::FeedbackCallback collision_object_movement_feedback_ptr_;
01513   interactive_markers::MenuHandler::FeedbackCallback ik_control_feedback_ptr_;
01514   interactive_markers::MenuHandler::FeedbackCallback joint_control_feedback_ptr_;
01515 
01516   interactive_markers::InteractiveMarkerServer* interactive_marker_server_;
01517 
01518   std::map<std::string, SelectableObject>* selectable_objects_;
01519   std::map<std::string, IKController>* ik_controllers_;
01520 
01521   std::string current_planning_scene_name_;
01522   std::string selected_motion_plan_name_;
01523   std::string selected_trajectory_name_;
01524   std::string logged_group_name_;
01525   std::string logged_motion_plan_request_;
01526   std::map<std::string, MenuEntryMap> menu_entry_maps_;
01527   MenuHandlerMap menu_handler_map_;
01528 
01529   std::map<std::string, ros::ServiceClient*>* collision_aware_ik_services_;
01530   std::map<std::string, ros::ServiceClient*>* non_collision_aware_ik_services_;
01531   std::map<std::string, ros::ServiceClient*>* interpolated_ik_services_;
01532   std::map<std::string, arm_navigation_msgs::ArmNavigationErrorCodes> error_map_;
01533   std::vector<StateRegistry> states_;
01534 
01535   interactive_markers::MenuHandler::EntryHandle last_resize_handle_;
01536 
01537   std_msgs::ColorRGBA last_collision_object_color_;
01538   std_msgs::ColorRGBA last_mesh_object_color_;
01539 
01540   MonitorStatus monitor_status_;
01541   ros::Time time_of_controller_done_callback_;
01542   ros::Time time_of_last_moving_notification_;
01543 
01544   ros::Time last_marker_start_time_;
01545   ros::Duration marker_dt_;
01546 
01547   void attachCollisionObject(const std::string& name, 
01548                              const std::string& link_name,
01549                              const std::vector<std::string>& touch_links);
01550   
01554   void createSelectableMarkerFromCollisionObject(arm_navigation_msgs::CollisionObject& object, std::string name,
01555                                                  std::string description, std_msgs::ColorRGBA color, bool insert_selectable=true);
01556 
01557   
01558   
01559 public:
01560 
01562   std::map<std::string, PlanningSceneData> planning_scene_map_;
01563 
01565   std::map<std::string, std::map<std::string, TrajectoryData> > trajectory_map_;
01566 
01568   std::map<std::string, MotionPlanRequestData> motion_plan_map_;
01569 
01571   std::map<std::string, bool> joint_clicked_map_;
01572 
01574   std::map<std::string, tf::Transform> joint_prev_transform_map_;
01575 
01576   PlanningSceneEditor();
01577   PlanningSceneEditor(PlanningSceneParameters& params);
01578   ~PlanningSceneEditor();
01579 
01587   bool filterTrajectory(MotionPlanRequestData& requestData, TrajectoryData& trajectory, unsigned int& filter_id);
01588 
01597   bool getAllAssociatedMotionPlanRequests(const unsigned int id, 
01598                                           std::vector<unsigned int>& ids,
01599                                           std::vector<std::string>& stages,
01600                                           std::vector<arm_navigation_msgs::MotionPlanRequest>& requests);
01601 
01608   bool getAllAssociatedPausedStates(const unsigned int id, 
01609                                     std::vector<ros::Time>& paused_times);
01610 
01617   bool getAllAssociatedTrajectorySources(const unsigned int planning_id,
01618                                          const unsigned int mpr_id,
01619                                          std::vector<unsigned int>& trajectory_ids,
01620                                          std::vector<std::string>& trajectory_sources);
01621 
01627   bool getAllPlanningSceneTimes(std::vector<ros::Time>& planning_scene_times,
01628                                 std::vector<unsigned int>& planning_scene_ids);
01629 
01630 
01640   bool getMotionPlanRequest(const ros::Time& time, const std::string& stage,
01641                             arm_navigation_msgs::MotionPlanRequest& mpr, std::string& id,
01642                             std::string& planning_scene_id);
01643 
01651   bool getPausedState(const unsigned int id, 
01652                       const ros::Time& paused_time,
01653                       head_monitor_msgs::HeadMonitorFeedback& paused_state);
01654 
01663   bool getPlanningSceneOutcomes(const unsigned int id, 
01664                                 std::vector<std::string>& pipeline_stages,
01665                                 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& error_codes,
01666                                 std::map<std::string, arm_navigation_msgs::ArmNavigationErrorCodes>& error_map);
01667 
01668 
01675   bool loadPlanningScene(const ros::Time& time, 
01676                          const unsigned int id);
01677 
01688   bool planToKinematicState(const planning_models::KinematicState& state, const std::string& group_name,
01689                             const std::string& end_effector_name, unsigned int& trajectoryid_Out,
01690                             unsigned int& planning_scene_id);
01691 
01698   bool planToRequest(MotionPlanRequestData& data, unsigned int& trajectoryid_Out);
01699 
01706   bool planToRequest(const std::string& requestid, unsigned int& trajectoryid_Out);
01707 
01714   bool playTrajectory(MotionPlanRequestData& requestData, TrajectoryData& data);
01715 
01721   bool sendPlanningScene(PlanningSceneData& data);
01722 
01733   bool solveIKForEndEffectorPose(MotionPlanRequestData& mpr, PositionType type, bool coll_aware = true,
01734                                  double change_redundancy = 0.0);
01735 
01736 
01744   interactive_markers::MenuHandler::EntryHandle registerMenuEntry(std::string menu, std::string entryName,
01745                                                                   interactive_markers::MenuHandler::FeedbackCallback& callback);
01746 
01747 
01748 
01757   interactive_markers::MenuHandler::EntryHandle registerSubMenuEntry(std::string menu, std::string name,
01758                                                                      std::string subMenu,
01759                                                                      interactive_markers::MenuHandler::FeedbackCallback& callback);
01760 
01765   std::string createNewPlanningScene();
01766 
01773   virtual void onPlanningSceneLoaded(int scene, int numScenes)
01774   {
01775   }
01776 
01781   void collisionObjectMovementCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
01782 
01787   void collisionObjectSelectionCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
01788 
01789   void attachedCollisionObjectInteractiveCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
01790 
01801   std::string createCollisionObject(const std::string& name, geometry_msgs::Pose pose, GeneratedShape shape, float scaleX, float scaleY,
01802                                     float scaleZ, std_msgs::ColorRGBA color);
01803 
01804   std::string createMeshObject(const std::string& name, 
01805                                geometry_msgs::Pose pose,
01806                                const std::string& filename,
01807                                const tf::Vector3& scale,
01808                                std_msgs::ColorRGBA color);
01815   void createIKController(MotionPlanRequestData& data, PositionType type, bool rePose = true);
01816 
01823   void createIkControllersFromMotionPlanRequest(MotionPlanRequestData& data, bool rePose = true);
01824 
01830   void createJointMarkers(MotionPlanRequestData& data, planning_scene_utils::PositionType position);
01831 
01832 
01844   void createMotionPlanRequest(const planning_models::KinematicState& start_state,
01845                                const planning_models::KinematicState& end_state, 
01846                                const std::string& group_name,
01847                                const std::string& end_effector_name, 
01848                                const unsigned int& planning_scene_name,
01849                                const bool fromRobotState, 
01850                                unsigned int& motionPlan_id_Out);
01851 
01852 
01860   void initMotionPlanRequestData(const unsigned int& planning_scene_id, 
01861                                  const std::vector<unsigned int>& ids,
01862                                  const std::vector<std::string>& stages,
01863                                  const std::vector<arm_navigation_msgs::MotionPlanRequest>& requests);
01864 
01865 
01871   void deleteJointMarkers(MotionPlanRequestData& data, PositionType type);
01872 
01877   void deleteKinematicStates();
01878 
01883   void deleteMotionPlanRequest(const unsigned int& id,
01884                                std::vector<unsigned int>& erased_trajectories);
01885 
01890   void deleteTrajectory(unsigned int mpr_id, unsigned int traj_id);
01891 
01892 
01900   // void determineOrientationConstraintsGivenState(const MotionPlanRequestData& mpr,
01901   //                                                const planning_models::KinematicState& state,
01902   //                                                arm_navigation_msgs::OrientationConstraint& goal_constraint,
01903   //                                                arm_navigation_msgs::OrientationConstraint& path_constraint);
01904 
01909   void executeTrajectory(const std::string& mpr_name, const std::string& traj_name);
01910 
01915   void executeTrajectory(TrajectoryData& data);
01916 
01923   void getAllRobotStampedTransforms(const planning_models::KinematicState& state,
01924                                     std::vector<geometry_msgs::TransformStamped>& trans_vector, const ros::Time& stamp);
01925 
01930   void getMotionPlanningMarkers(visualization_msgs::MarkerArray& arr);
01931 
01936   void getTrajectoryMarkers(visualization_msgs::MarkerArray& arr);
01937 
01942   void IKControllerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
01943 
01948   void JointControllerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
01949 
01955   void controllerDoneCallback(const actionlib::SimpleClientGoalState& state,
01956                               const control_msgs::FollowJointTrajectoryResultConstPtr& result);
01957 
01961   void armHasStoppedMoving();
01962 
01967   void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state);
01968 
01973 void jointTrajectoryControllerStateCallback(const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr& joint_state);
01974 
01975 
01979   void loadAllWarehouseData();
01980 
01990   void makeInteractive1DOFRotationMarker(tf::Transform transform, tf::Vector3 axis, std::string name, std::string description,
01991                                          float scale = 1.0f, float angle = 0.0f);
01992 
02002   void makeInteractive1DOFTranslationMarker(tf::Transform transform, tf::Vector3 axis, std::string name, std::string description,
02003                                             float scale = 1.0f, float value = 0.0f);
02004 
02008   void printTrajectoryPoint(const std::vector<std::string>& joint_names, const std::vector<double>& joint_values);
02009 
02015   void randomlyPerturb(MotionPlanRequestData& mpr, PositionType type);
02016 
02021   void savePlanningScene(PlanningSceneData& data, bool copy = false);
02022 
02026   void sendMarkers();
02027 
02031   void sendTransformsAndClock();
02032 
02033   void makeSelectableAttachedObjectFromPlanningScene(const arm_navigation_msgs::PlanningScene& scene,
02034                                                      arm_navigation_msgs::AttachedCollisionObject& att);
02035 
02042   void setCurrentPlanningScene(std::string id, bool loadRequests = true, bool loadTrajectories = true);
02043 
02050   void setIKControlsVisible(std::string id, PositionType type, bool visible);
02051 
02060   void setJointState(MotionPlanRequestData& data, PositionType position, std::string& jointName, tf::Transform value);
02061 
02066   void updateJointStates();
02067 
02068   bool hasTrajectory(const std::string& mpr_name,
02069                      const std::string& traj_name) const;
02070 
02075   inline std::string generateNewCollisionObjectId()
02076   {
02077     std::stringstream stream;
02078     stream << "collision_object_";
02079     max_collision_object_id_++;
02080     stream << max_collision_object_id_;
02081     return stream.str();
02082   }
02083 
02088   inline unsigned int generateNewPlanningSceneId()
02089   {
02090     return move_arm_warehouse_logger_reader_->determineNextPlanningSceneId();
02091   }
02092 
02093 
02098   void deleteCollisionObject(std::string& name);
02099 
02100 
02101   inline void lockScene()
02102   {
02103     lock_scene_.lock();
02104   }
02105 
02106   inline void unlockScene()
02107   {
02108     lock_scene_.unlock();
02109   }
02110 
02111   inline planning_environment::CollisionModels* getCollisionModel()
02112   {
02113     return cm_;
02114   }
02115 
02116   inline void setCollisionModel(planning_environment::CollisionModels* model, bool shouldDelete = false)
02117   {
02118     if(shouldDelete && cm_ != NULL)
02119     {
02120       delete cm_;
02121       cm_ = model;
02122     }
02123     cm_ = model;
02124   }
02125 
02126   inline planning_models::KinematicState* getRobotState()
02127   {
02128     return robot_state_;
02129   }
02130 
02131   inline void setRobotState(planning_models::KinematicState* robot_state, bool shouldDelete = true)
02132   {
02133     if(shouldDelete && robot_state_ != NULL)
02134     {
02135       delete robot_state_;
02136       robot_state_ = NULL;
02137     }
02138 
02139     robot_state_ = robot_state;
02140   }
02141 
02142   inline move_arm_warehouse::MoveArmWarehouseLoggerReader* getLoggerReader()
02143   {
02144     return move_arm_warehouse_logger_reader_;
02145   }
02146 
02147   inline void setLoggerReader(move_arm_warehouse::MoveArmWarehouseLoggerReader* loggerReader,
02148                               bool shouldDelete = true)
02149   {
02150     if(move_arm_warehouse_logger_reader_ != NULL)
02151     {
02152       delete move_arm_warehouse_logger_reader_;
02153       move_arm_warehouse_logger_reader_ = NULL;
02154     }
02155 
02156     move_arm_warehouse_logger_reader_ = loggerReader;
02157   }
02158 };
02159 
02160 }
02161 #endif


move_arm_warehouse
Author(s): Ioan Sucan, Sachin Chitta(sachinc@willowgarage.com)
autogenerated on Thu Dec 12 2013 11:09:11