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 <srs_assisted_arm_navigation/move_arm_utils/move_arm_warehouse_logger_reader.h>
00059 #include <arm_navigation_msgs/convert_messages.h>
00060 #include <visualization_msgs/InteractiveMarker.h>
00061 #include <interactive_markers/interactive_marker_server.h>
00062 #include <interactive_markers/menu_handler.h>
00063 #include <interactive_markers/tools.h>
00064 #include <arm_navigation_msgs/CollisionObject.h>
00065 #include <planning_environment/monitors/kinematic_model_state_monitor.h>
00066 #include <control_msgs/FollowJointTrajectoryAction.h>
00067 //#include <gazebo_msgs/SetModelConfiguration.h>
00068 #include <std_srvs/Empty.h>
00069 #include <pr2_mechanism_msgs/ListControllers.h>
00070 #include <pr2_mechanism_msgs/LoadController.h>
00071 #include <pr2_mechanism_msgs/UnloadController.h>
00072 #include <pr2_mechanism_msgs/SwitchController.h>
00073 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00074 // #include <gazebo_msgs/SetLinkProperties.h>
00075 // #include <gazebo_msgs/GetLinkProperties.h>
00076 
00077 typedef map<std::string, interactive_markers::MenuHandler::EntryHandle> MenuEntryMap;
00078 typedef map<std::string, MenuEntryMap> MenuMap;
00079 typedef map<std::string, interactive_markers::MenuHandler> MenuHandlerMap;
00080 
00085 namespace planning_scene_utils
00086 {
00087 
00088 inline static geometry_msgs::Pose toGeometryPose(tf::Transform transform)
00089 {
00090   geometry_msgs::Pose toReturn;
00091   toReturn.position.x = transform.getOrigin().x();
00092   toReturn.position.y = transform.getOrigin().y();
00093   toReturn.position.z = transform.getOrigin().z();
00094   toReturn.orientation.x = transform.getRotation().x();
00095   toReturn.orientation.y = transform.getRotation().y();
00096   toReturn.orientation.z = transform.getRotation().z();
00097   toReturn.orientation.w = transform.getRotation().w();
00098   return toReturn;
00099 }
00100 
00101 inline static tf::Transform toBulletTransform(geometry_msgs::Pose pose)
00102 {
00103   tf::Quaternion quat =
00104     tf::Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
00105   tf::Vector3 vec = tf::Vector3(pose.position.x, pose.position.y, pose.position.z);
00106   return tf::Transform(quat, vec);
00107 }
00108 
00109 inline static std::string getPlanningSceneNameFromId(const unsigned int id) {
00110   std::stringstream ss;
00111   ss << "Planning Scene " << id;
00112   return ss.str();
00113 }
00114 
00115 inline static unsigned int getPlanningSceneIdFromName(const std::string& name) {
00116   std::stringstream ss(name);
00117   std::string temp;
00118   ss >> temp;
00119   ss >> temp;
00120   unsigned int ret;
00121   ss >> ret;
00122   return ret;
00123 }
00124 
00125 inline static std::string getMotionPlanRequestNameFromId(const unsigned int id) {
00126   std::stringstream ss;
00127   ss << "MPR " << id;
00128   return ss.str();
00129 }
00130 
00131 inline static std::string getTrajectoryNameFromId(const unsigned int id) {
00132   std::stringstream ss;
00133   ss << "Trajectory " << id;
00134   return ss.str();
00135 }
00136 
00142 inline static std::string getResultErrorFromCode(int error_code)
00143 {
00144   std::string result;
00145   if(error_code == control_msgs::FollowJointTrajectoryResult::SUCCESSFUL)
00146      result = "Success";
00147   else if(error_code == control_msgs::FollowJointTrajectoryResult::INVALID_GOAL)
00148      result = "Invalid Goal";
00149   else if(error_code == control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS)
00150      result = "Invalid Joints";
00151   else if(error_code == control_msgs::FollowJointTrajectoryResult::OLD_HEADER_TIMESTAMP)
00152      result = "Old header timestamp";
00153   else if(error_code == control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED)
00154      result = "Path tolerance violated";
00155   else if(error_code == control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED)
00156      result = "Goal tolerance violated";
00157   return result;
00158 }
00159 
00165 enum PositionType
00166   {
00167     StartPosition, GoalPosition
00168   };
00169 
00177 enum RenderType
00178   {
00179     CollisionMesh, VisualMesh, PaddingMesh
00180   };
00181 
00188 enum TrajectoryRenderType
00189 {
00190   Kinematic,
00191   Temporal,
00192 };
00193 
00194 
00195 // Must be defined so that subsequent classes can reference.
00196 class PlanningSceneEditor;
00197 
00203 class PlanningSceneData
00204 {
00205 protected:
00206   std::string name_;
00207   unsigned int id_;
00208   std::string host_;
00209   ros::Time timestamp_;
00210   arm_navigation_msgs::PlanningScene planning_scene_;
00211   std::vector<std::string> pipeline_stages_;
00212   std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> error_codes_;
00213   std::set<unsigned int> motion_plan_requests_;
00214 
00215 public:
00216   PlanningSceneData();
00217   PlanningSceneData(unsigned int id, const ros::Time& timestamp, const arm_navigation_msgs::PlanningScene& scene);
00218 
00220   inline std::string getHostName() const
00221   {
00222     return host_;
00223   }
00224 
00226   inline void setHostName(std::string host)
00227   {
00228     host_ = host;
00229   }
00230 
00232   inline std::string getName() const
00233   {
00234     return name_;
00235   }
00236 
00237   inline unsigned int getId() const 
00238   {
00239     return id_;
00240   }
00241 
00243   inline const ros::Time& getTimeStamp() const
00244   {
00245     return timestamp_;
00246   }
00247 
00249   inline arm_navigation_msgs::PlanningScene& getPlanningScene()
00250   {
00251     return planning_scene_;
00252   }
00253 
00255   inline void setTimeStamp(const ros::Time& time)
00256   {
00257     timestamp_ = time;
00258     planning_scene_.robot_state.joint_state.header.stamp = time;
00259   }
00260 
00261   inline void setId(unsigned int id) {
00262     id_ = id;
00263     name_ = getPlanningSceneNameFromId(id);
00264   }
00265 
00267   inline void setPlanningScene(const arm_navigation_msgs::PlanningScene& scene)
00268   {
00269     planning_scene_ = scene;
00270     timestamp_ = scene.robot_state.joint_state.header.stamp;
00271   }
00272 
00276   inline std::vector<std::string>& getPipelineStages()
00277   {
00278     return pipeline_stages_;
00279   }
00280 
00282   inline void setPipelineStages(std::vector<std::string>& stages)
00283   {
00284     pipeline_stages_ = stages;
00285   }
00286 
00288   inline std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& getErrorCodes()
00289   {
00290     return error_codes_;
00291   }
00292 
00294   inline void setErrorCodes(std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& error_codes)
00295   {
00296     error_codes_ = error_codes;
00297   }
00298 
00300   inline std::set<unsigned int>& getRequests()
00301   {
00302     return motion_plan_requests_;
00303   }
00304 
00307   void getRobotState(planning_models::KinematicState* state);
00308 
00309   bool hasMotionPlanRequestId(unsigned int id) const {
00310     return(motion_plan_requests_.find(id) != motion_plan_requests_.end());
00311   }
00312   
00313   void addMotionPlanRequestId(unsigned int id) {
00314     motion_plan_requests_.insert(id);
00315   }
00316 
00317   void removeMotionPlanRequestId(unsigned int id) {
00318     motion_plan_requests_.erase(id);
00319   }
00320 
00321   unsigned int getNextMotionPlanRequestId() const {
00322     if(motion_plan_requests_.empty()) {
00323       return 0;
00324     }
00325     return (*motion_plan_requests_.rbegin())+1;
00326   }
00327 };
00328 
00334 class MotionPlanRequestData
00335 {
00336 protected:
00337   std::string name_;
00338   unsigned int id_;
00339   std::string source_;
00340   unsigned int planning_scene_id_;
00341   std::string end_effector_link_;
00342   std::string group_name_;
00343   arm_navigation_msgs::MotionPlanRequest motion_plan_request_;
00344   bool is_start_editable_;
00345   bool is_goal_editable_;
00346   bool is_start_visible_;
00347   bool is_goal_visible_;
00348   bool should_refresh_colors_;
00349   bool has_refreshed_colors_;
00350   bool has_path_constraints_;
00351   bool has_good_goal_ik_solution_;
00352   bool has_good_start_ik_solution_;
00353   bool are_collisions_visible_;
00354   bool has_state_changed_;
00355   bool are_joint_controls_visible_;
00356 
00357   double roll_tolerance_;
00358   double pitch_tolerance_;
00359   double yaw_tolerance_;
00360 
00361   bool constrain_roll_;
00362   bool constrain_pitch_;
00363   bool constrain_yaw_;
00364 
00365   std_msgs::ColorRGBA start_color_;
00366   std_msgs::ColorRGBA goal_color_;
00367   std::set<unsigned int> trajectories_;
00368   planning_models::KinematicState* start_state_;
00369   planning_models::KinematicState* goal_state_;
00370   tf::Transform last_good_start_pose_;
00371   tf::Transform last_good_goal_pose_;
00372   visualization_msgs::MarkerArray collision_markers_;
00373   RenderType render_type_;
00374   unsigned int next_trajectory_id_;
00375 
00376 public:
00377   MotionPlanRequestData()
00378   {
00379     start_state_ = NULL;
00380     goal_state_ = NULL;
00381   }
00382 
00383   MotionPlanRequestData(const planning_models::KinematicState* robot_state);
00384   MotionPlanRequestData(const unsigned int& id, 
00385                         const std::string& source, 
00386                         const arm_navigation_msgs::MotionPlanRequest& request,
00387                         const planning_models::KinematicState* robot_state, 
00388                         const std::string& end_effector_link);
00389 
00393   ros::Duration refresh_timer_;
00394 
00395 
00398   void setStartStateValues(std::map<std::string, double>& joint_values);
00399 
00402   void setGoalStateValues(std::map<std::string, double>& joint_values);
00403 
00406   void updateStartState();
00407 
00410   void updateGoalState();
00411 
00414   std::vector<std::string> getJointNamesInGoal();
00415 
00420   bool isJointNameInGoal(std::string joint);
00421 
00423   inline RenderType getRenderType() const
00424   {
00425     return render_type_;
00426   }
00427 
00429   inline void setRenderType(RenderType renderType)
00430   {
00431     render_type_ = renderType;
00432   }
00433 
00435   inline bool areJointControlsVisible() const
00436   {
00437     return are_joint_controls_visible_;
00438   }
00439 
00443   void setJointControlsVisible(bool visible, PlanningSceneEditor* editor);
00444 
00446   inline bool hasStateChanged() const
00447   {
00448     return has_state_changed_;
00449   }
00450 
00452   inline void setStateChanged(bool changed)
00453   {
00454     has_state_changed_ = changed;
00455   }
00456 
00458   inline visualization_msgs::MarkerArray& getCollisionMarkers()
00459   {
00460     return collision_markers_;
00461   }
00462 
00464   inline bool areCollisionsVisible() const
00465   {
00466     return are_collisions_visible_;
00467   }
00468 
00470   inline void setCollisionsVisible(bool visible)
00471   {
00472     are_collisions_visible_ = visible;
00473   }
00474 
00476   inline void showCollisions() 
00477   {
00478     setCollisionsVisible(true);
00479   }
00480 
00482   inline void hideCollisions()
00483   {
00484     setCollisionsVisible(false);
00485   }
00486 
00488   inline tf::Transform getLastGoodStartPose() const
00489   {
00490     return last_good_start_pose_;
00491   }
00492 
00494   inline tf::Transform getLastGoodGoalPose() const
00495   {
00496     return last_good_goal_pose_;
00497   }
00498 
00500   inline void setLastGoodStartPose(tf::Transform pose)
00501   {
00502     last_good_start_pose_ = pose;
00503   }
00504 
00506   inline void setLastGoodGoalPose(tf::Transform pose)
00507   {
00508     last_good_goal_pose_ = pose;
00509   }
00510 
00512   inline void setStartState(planning_models::KinematicState* state)
00513   {
00514     start_state_ = state;
00515     setStateChanged(true);
00516   }
00517 
00519   inline void setGoalState(planning_models::KinematicState* state)
00520   {
00521     goal_state_ = state;
00522     setStateChanged(true);
00523   }
00524 
00526   inline void reset()
00527   {
00528     if(start_state_ != NULL)
00529     {
00530       delete start_state_;
00531       start_state_ = NULL;
00532     }
00533 
00534     if(goal_state_ != NULL)
00535     {
00536       delete goal_state_;
00537       goal_state_ = NULL;
00538     }
00539   }
00540 
00542   inline bool hasGoodIKSolution(const PositionType& type) const
00543   {
00544     if(type == StartPosition) {
00545       return has_good_start_ik_solution_;
00546     } else {
00547       return has_good_goal_ik_solution_;
00548     }
00549   }
00550 
00552   inline void setHasGoodIKSolution(const bool& solution, const PositionType& type)
00553   {
00554     if(type == StartPosition) {
00555       has_good_start_ik_solution_ = solution;
00556     } else {
00557       has_good_goal_ik_solution_ = solution;
00558     }
00559   }
00560 
00562   inline planning_models::KinematicState* getStartState()
00563   {
00564     return start_state_;
00565   }
00566 
00568   inline planning_models::KinematicState* getGoalState()
00569   {
00570     return goal_state_;
00571   }
00572 
00575   inline std::string getGroupName() const
00576   { 
00577     return group_name_;
00578   }
00579 
00581   inline std::string getEndEffectorLink() const
00582   {
00583     return end_effector_link_;
00584   }
00585 
00588   inline void setGroupName(const std::string& name)
00589   {
00590     group_name_ = name;
00591   }
00592 
00594   inline void setEndEffectorLink(const std::string& name)
00595   {
00596     end_effector_link_ = name;
00597   }
00598 
00600   inline const std::string& getName() const
00601   {
00602     return name_;
00603   }
00604   
00605   unsigned int getId() const {
00606     return id_;
00607   }
00608 
00610   inline void setId(const unsigned int id)
00611   {
00612     id_ = id;
00613     name_ = getMotionPlanRequestNameFromId(id_);
00614   }
00615 
00617   inline bool shouldRefreshColors() const 
00618   {
00619     return should_refresh_colors_;
00620   }
00621 
00623   inline bool hasRefreshedColors() const
00624   {
00625     return has_refreshed_colors_;
00626   }
00627 
00629   inline void setHasRefreshedColors(bool refresh)
00630   {
00631     has_refreshed_colors_ = refresh;
00632 
00633     if(refresh)
00634     {
00635       should_refresh_colors_ = false;
00636     }
00637   }
00638 
00641   inline void refreshColors()
00642   {
00643     should_refresh_colors_ = true;
00644     has_refreshed_colors_ = false;
00645     refresh_timer_ = ros::Duration(0.0);
00646   }
00647 
00650   inline bool isStartVisible() const
00651   {
00652     return is_start_visible_;
00653   }
00654 
00657   inline bool isEndVisible() const
00658   {
00659     return is_goal_visible_;
00660   }
00661 
00663   inline void setStartVisible(bool visible)
00664   {
00665     is_start_visible_ = visible;
00666   }
00667 
00669   inline void setEndVisible(bool visible)
00670   {
00671     is_goal_visible_ = visible;
00672   }
00673 
00675   inline void show()
00676   {
00677     setStartVisible(true);
00678     setEndVisible(true);
00679   }
00680 
00682   inline void hide()
00683   {
00684     setStartVisible(false);
00685     setEndVisible(false);
00686   }
00687 
00689   inline void showStart()
00690   {
00691     setStartVisible(true);
00692   }
00693 
00695   inline void showGoal()
00696   {
00697     setEndVisible(true);
00698   }
00699 
00701   inline void hideStart()
00702   {
00703     setStartVisible(false);
00704   }
00705 
00707   inline void hideGoal()
00708   {
00709     setEndVisible(false);
00710   }
00711 
00713   inline const std_msgs::ColorRGBA& getStartColor() const
00714   {
00715     return start_color_;
00716   }
00717 
00719   inline const std_msgs::ColorRGBA& getGoalColor() const
00720   {
00721     return goal_color_;
00722   }
00723 
00725   inline void setStartColor(std_msgs::ColorRGBA color)
00726   {
00727     start_color_ = color;
00728   }
00729 
00731   inline void setGoalColor(std_msgs::ColorRGBA color)
00732   {
00733     goal_color_ = color;
00734   }
00735 
00738   inline void setStartEditable(bool editable)
00739   {
00740     is_start_editable_ = editable;
00741   }
00742 
00745   inline void setGoalEditable(bool editable)
00746   {
00747     is_goal_editable_ = editable;
00748   }
00749 
00752   inline bool isStartEditable() const
00753   {
00754     return is_start_editable_;
00755   }
00756 
00759   inline bool isGoalEditable() const
00760   {
00761     return is_goal_editable_;
00762   }
00763 
00765   inline void setSource(const std::string& source)
00766   {
00767     source_ = source;
00768   }
00769 
00771   inline const std::string& getSource() const
00772   {
00773     return source_;
00774   }
00775 
00777   inline arm_navigation_msgs::MotionPlanRequest& getMotionPlanRequest()
00778   {
00779     return motion_plan_request_;
00780   }
00781 
00783   inline void setMotionPlanRequest(const arm_navigation_msgs::MotionPlanRequest& request)
00784   {
00785     motion_plan_request_ = request;
00786     setGroupName(request.group_name);
00787     updateStartState();
00788     updateGoalState();
00789   }
00790 
00792   inline void setPlanningSceneId(const unsigned int id)
00793   {
00794     planning_scene_id_ = id;
00795   }
00796   inline unsigned int getPlanningSceneId() const
00797   {
00798     return planning_scene_id_;
00799   }
00800 
00802   inline std::string getPlanningSceneName() const 
00803   {
00804     return getPlanningSceneNameFromId(planning_scene_id_);
00805   }
00806 
00808   inline std::set<unsigned int>& getTrajectories()
00809   {
00810     return trajectories_;
00811   }
00812 
00813   unsigned int getNextTrajectoryId() const {
00814     if(trajectories_.empty()) return 0;
00815     return (*trajectories_.rbegin())+1;
00816   }
00817 
00818   void addTrajectoryId(unsigned int id) {
00819     trajectories_.insert(id);
00820   }
00821   
00822   void removeTrajectoryId(unsigned int id) {
00823     trajectories_.erase(id);
00824   }
00825 
00826   bool hasTrajectoryId(unsigned int id) const {
00827     return (trajectories_.find(id) != trajectories_.end());
00828   }
00829 
00830   bool hasPathConstraints() const {
00831     return has_path_constraints_;
00832   }
00833   
00834   void setPathConstraints(bool has) {
00835     has_path_constraints_ = has;
00836   }
00837 
00838   bool getConstrainRoll() const {
00839     return constrain_roll_;
00840   }
00841 
00842   void setConstrainRoll(bool s) {
00843     constrain_roll_ = s;
00844   }
00845 
00846   bool getConstrainPitch() const {
00847     return constrain_pitch_;
00848   }
00849 
00850   void setConstrainPitch(bool s) {
00851     constrain_pitch_ = s;
00852   }
00853 
00854   bool getConstrainYaw() const {
00855     return constrain_yaw_;
00856   }
00857 
00858   void setConstrainYaw(bool s) {
00859     constrain_yaw_ = s;
00860   }
00861 
00862   double getRollTolerance() const {
00863     return roll_tolerance_;
00864   }
00865 
00866   void setRollTolerance(double s) {
00867     roll_tolerance_ = s;
00868   }
00869 
00870   double getPitchTolerance() const {
00871     return pitch_tolerance_;
00872   }
00873 
00874   void setPitchTolerance(double s) {
00875     pitch_tolerance_ = s;
00876   }
00877 
00878   double getYawTolerance() const {
00879     return yaw_tolerance_;
00880   }
00881 
00882   void setYawTolerance(double s) {
00883     yaw_tolerance_ = s;
00884   }
00885 
00886   void setGoalAndPathPositionOrientationConstraints(arm_navigation_msgs::MotionPlanRequest& mpr,
00887                                                     planning_scene_utils::PositionType type) const;
00888 
00890   void updateCollisionMarkers(planning_environment::CollisionModels* cm_,
00891                               ros::ServiceClient* distance_state_validity_service_client_);
00892 };
00893 
00894 
00900 class TrajectoryData
00901 {
00902 public:
00903 
00904   enum MarkerType {
00905     VISUAL,
00906     COLLISION,
00907     PADDED
00908   };
00909 
00910 protected:
00911   std::string name_;
00912   unsigned int id_;
00913   std::string source_;
00914   std::string group_name_;
00915   unsigned int planning_scene_id_;
00916   unsigned int motion_plan_request_Id_;
00917   trajectory_msgs::JointTrajectory trajectory_;
00918   trajectory_msgs::JointTrajectory trajectory_error_;
00919   bool is_visible_;
00920   MarkerType marker_type_;
00921   bool is_playing_;
00922   ros::Time playback_start_time_;
00923   bool collisions_visible_;
00924   bool state_changed_;
00925   std_msgs::ColorRGBA color_;
00926   unsigned int current_trajectory_point_;
00927   unsigned int trajectory_bad_point_;
00928   planning_models::KinematicState* current_state_;
00929   ros::Duration duration_;
00930   bool should_refresh_colors_;
00931   bool has_refreshed_colors_;
00932   visualization_msgs::MarkerArray collision_markers_;
00933   RenderType render_type_;
00934   TrajectoryRenderType trajectory_render_type_;
00935   ros::Duration time_to_stop_;
00936 public:
00937 
00939   ros::Duration refresh_timer_;
00940 
00942   arm_navigation_msgs::ArmNavigationErrorCodes trajectory_error_code_;
00943 
00944   TrajectoryData();
00945   TrajectoryData(const unsigned int& id, const std::string& source, const std::string& group_name,
00946                  const trajectory_msgs::JointTrajectory& trajectory);
00947   TrajectoryData(const unsigned int& id, const std::string& source, const std::string& group_name,
00948                  const trajectory_msgs::JointTrajectory& trajectory, const trajectory_msgs::JointTrajectory& trajectory_error);
00949 
00952   void advanceThroughTrajectory(int amount);
00953 
00956   void advanceToNextClosestPoint(ros::Time time);
00957 
00959   void updateCurrentState();
00960 
00962   inline bool hasStateChanged() const
00963   { 
00964     return state_changed_;
00965   }
00966 
00968   inline void setStateChanged(bool changed)
00969   {
00970     state_changed_ = changed;
00971   }
00972 
00974   inline visualization_msgs::MarkerArray& getCollisionMarkers()
00975   {
00976     return collision_markers_;
00977   }
00978 
00980   inline bool areCollisionsVisible() const
00981   {
00982     return collisions_visible_;
00983   }
00984 
00986   inline void setCollisionsVisible(bool shown)
00987   {
00988     collisions_visible_ = shown;
00989   }
00990 
00992   inline void showCollisions()
00993   {
00994     setCollisionsVisible(true);
00995   }
00996 
00998   inline void hideCollisions()
00999   {
01000     setCollisionsVisible(false);
01001   }
01002 
01004   inline size_t getTrajectorySize() const
01005   {
01006     return trajectory_.points.size();
01007   }
01008 
01010   inline bool shouldRefreshColors() const
01011   {
01012     return should_refresh_colors_;
01013   }
01014 
01016   inline bool hasRefreshedColors() const
01017   {
01018     return has_refreshed_colors_;
01019   }
01020 
01022   inline void setHasRefreshedColors(bool refresh)
01023   {
01024     has_refreshed_colors_ = refresh;
01025 
01026     if(refresh)
01027     {
01028       should_refresh_colors_ = false;
01029     }
01030   }
01031 
01034   inline void refreshColors()
01035   {
01036     should_refresh_colors_ = true;
01037     has_refreshed_colors_ = false;
01038     refresh_timer_ = ros::Duration(0.0);
01039   }
01040 
01042   inline RenderType getRenderType() const
01043   {
01044     return render_type_;
01045   }
01046 
01048   inline void setRenderType(RenderType renderType)
01049   {
01050     render_type_ = renderType;
01051   }
01052 
01054   inline TrajectoryRenderType getTrajectoryRenderType() const
01055   {
01056     return trajectory_render_type_;
01057   }
01058 
01060   inline void setTrajectoryRenderType(TrajectoryRenderType renderType)
01061   {
01062     trajectory_render_type_ = renderType;
01063   }
01064 
01066   inline void reset()
01067   {
01068 
01069     if(current_state_ != NULL)
01070     {
01071       delete current_state_;
01072       current_state_ = NULL;
01073     }
01074 
01075     is_playing_ = false;
01076     is_visible_ = false;
01077     current_trajectory_point_ = 0;
01078     state_changed_ = false;
01079   }
01080 
01083   inline planning_models::KinematicState* getCurrentState()
01084   {
01085     return current_state_;
01086   }
01087 
01089   inline void setCurrentState(planning_models::KinematicState* state)
01090   {
01091     current_state_ = state;
01092     state_changed_ = true;
01093   }
01094 
01096   inline void setMotionPlanRequestId(const unsigned int& Id)
01097   {
01098     motion_plan_request_Id_ = Id;
01099   }
01100 
01102   inline const unsigned int& getMotionPlanRequestId() const
01103   {
01104     return motion_plan_request_Id_;
01105   }
01106 
01107   inline void setPlanningSceneId(const unsigned int id) {
01108     planning_scene_id_ = id;
01109   }
01110 
01111   unsigned int getPlanningScendId() const {
01112     return planning_scene_id_;
01113   }
01114 
01116   inline void setCurrentPoint(unsigned int point)
01117   {
01118     current_trajectory_point_ = point;
01119     state_changed_ = true;
01120   }
01121 
01123   inline unsigned int getCurrentPoint() const
01124   {
01125     return current_trajectory_point_;
01126   }
01127 
01129   inline unsigned int getBadPoint() const
01130   {
01131     return trajectory_bad_point_;
01132   }
01133 
01135   inline void setGroupname(const std::string& group_name)
01136   {
01137     group_name_ = group_name;
01138   }
01139 
01141   inline bool isPlaying() const
01142   {
01143     return is_playing_;
01144   }
01145 
01147   inline void play()
01148   {
01149     is_playing_ = true;
01150     playback_start_time_ = ros::Time::now();
01151   }
01152 
01154   inline void stop()
01155   {
01156     is_playing_ = false;
01157   }
01158 
01160   inline bool isVisible() const
01161   {
01162     return is_visible_;
01163   }
01164 
01166   inline void setVisible(bool visible)
01167   {
01168     is_visible_ = visible;
01169   }
01170 
01171   inline MarkerType getMarkerType() const 
01172   {
01173     return marker_type_;
01174   }
01175 
01177   inline void setMarkerType(MarkerType mt) 
01178   {
01179     marker_type_ = mt;
01180   }
01181 
01183   inline void show()
01184   {
01185     setVisible(true);
01186   }
01187 
01189   inline void hide()
01190   {
01191     setVisible(false);
01192   }
01193 
01196   inline const ros::Duration& getDuration() const
01197   {
01198     return duration_;
01199   }
01200 
01202   inline void setDuration(const ros::Duration& duration)
01203   {
01204     duration_ = duration;
01205   }
01206 
01208   inline const std_msgs::ColorRGBA& getColor() const
01209   {
01210     return color_;
01211   }
01212 
01214   inline void setColor(const std_msgs::ColorRGBA& color)
01215   {
01216     color_ = color;
01217   }
01218 
01220   inline std::string getSource()
01221   {
01222     return source_;
01223   }
01224 
01226   inline trajectory_msgs::JointTrajectory& getTrajectory()
01227   {
01228     return trajectory_;
01229   }
01230 
01232   inline void setSource(const std::string& source)
01233   {
01234     source_ = source;
01235   }
01236 
01238   inline void setTrajectory(const trajectory_msgs::JointTrajectory& trajectory)
01239   {
01240     trajectory_ = trajectory;
01241   }
01242 
01244   inline trajectory_msgs::JointTrajectory& getTrajectoryError()
01245   {
01246     return trajectory_error_;
01247   }
01248 
01250   inline void setTrajectoryError(const trajectory_msgs::JointTrajectory& trajectory_error)
01251   {
01252     trajectory_error_ = trajectory_error;
01253   }
01254 
01256   inline unsigned int getId() const
01257   {
01258     return id_;
01259   }
01260 
01262   inline void setId(const unsigned int& id)
01263   {
01264     id_ = id;
01265     name_ = getTrajectoryNameFromId(id_);
01266   }
01267 
01268   const std::string& getName() const {
01269     return name_;
01270   }
01271 
01273   inline const std::string& getGroupName() const
01274   {
01275     return group_name_;
01276   }
01277 
01279   inline void setBadPoint(unsigned int point)
01280   {
01281     trajectory_bad_point_ = point;
01282   }
01283 
01285   inline void setGroupName(std::string name)
01286   {
01287     group_name_ = name;
01288   }
01289 
01292   void updateCollisionMarkers(planning_environment::CollisionModels* cm_, MotionPlanRequestData& motionPlanRequest,
01293                               ros::ServiceClient* distance_state_validity_service_client_);
01294 };
01295 
01301 struct PlanningSceneParameters
01302 {
01303   std::string left_ik_name_;
01304   std::string right_ik_name_;
01305   std::string non_coll_left_ik_name_;
01306   std::string non_coll_right_ik_name_;
01307   std::string left_interpolate_service_name_;
01308   std::string right_interpolate_service_name_;
01309   std::string planner_1_service_name_;
01310   std::string planner_2_service_name_;
01311   std::string proximity_space_service_name_;
01312   std::string proximity_space_validity_name_;
01313   std::string set_planning_scene_diff_name_;
01314   std::string trajectory_filter_1_service_name_;
01315   std::string trajectory_filter_2_service_name_;
01316   std::string proximity_space_planner_name_;
01317   std::string vis_topic_name_;
01318   std::string right_ik_link_;
01319   std::string left_ik_link_;
01320   std::string left_redundancy_;
01321   std::string right_redundancy_;
01322   std::string right_arm_group_;
01323   std::string left_arm_group_;
01324   std::string execute_left_trajectory_;
01325   std::string execute_right_trajectory_;
01326   std::string list_controllers_service_;
01327   std::string unload_controllers_service_;
01328   std::string load_controllers_service_;
01329   std::string switch_controllers_service_;
01330   std::string gazebo_model_name_;
01331   std::string robot_description_param_;
01332   bool use_robot_data_;
01333   bool sync_robot_state_with_gazebo_;
01334 };
01335 
01340 class PlanningSceneEditor
01341 {
01342 public:
01347   enum GeneratedShape
01348     {
01349       Box, Cylinder, Sphere
01350     };
01351 protected:
01352 
01361   enum MonitorStatus
01362     {
01363       idle, Executing, WaitingForStop, Done
01364     };
01365 
01374   struct StateRegistry
01375   {
01376     planning_models::KinematicState* state;
01377     std::string source;
01378   };
01379 
01385   struct SelectableObject
01386   {
01387     SelectableObject() {
01388       attach_ = false;
01389       detach_ = false;
01390       attached_collision_object_.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::REMOVE;
01391       collision_object_.operation.operation = arm_navigation_msgs::CollisionObjectOperation::REMOVE;
01392     }
01393 
01394     bool attach_;
01395     bool detach_;
01396     arm_navigation_msgs::AttachedCollisionObject attached_collision_object_;
01397     arm_navigation_msgs::CollisionObject collision_object_;
01398     visualization_msgs::InteractiveMarker selection_marker_;
01399     visualization_msgs::InteractiveMarker control_marker_;
01400     std_msgs::ColorRGBA color_;
01401 
01402     std::string id_;
01403   };
01404 
01410   struct IKController
01411   {
01412     unsigned int motion_plan_id_;
01413     visualization_msgs::InteractiveMarker start_controller_;
01414     visualization_msgs::InteractiveMarker end_controller_;
01415   };
01416 
01421   virtual void updateState()
01422   {
01423   }
01424 
01429   virtual void planCallback(arm_navigation_msgs::ArmNavigationErrorCodes& errorCode) = 0;
01430 
01435   virtual void filterCallback(arm_navigation_msgs::ArmNavigationErrorCodes& errorCode) = 0;
01436 
01437   virtual void attachObjectCallback(const std::string& name) = 0;
01438   // virtual void detachObjectCallback(arm_navigation_msgs::CollisionObject& object) = 0;
01439 
01440   void changeToAttached(const std::string& name);
01441 
01447   virtual void selectedTrajectoryCurrentPointChanged( unsigned int new_current_point ) {}
01448 
01449   boost::recursive_mutex lock_scene_;
01450   arm_navigation_msgs::ArmNavigationErrorCodes last_collision_set_error_code_;
01451   move_arm_warehouse::MoveArmWarehouseLoggerReader* move_arm_warehouse_logger_reader_;
01452   planning_environment::CollisionModels* cm_;
01453   planning_models::KinematicState* robot_state_;
01454   PlanningSceneParameters params_;
01455   ros::NodeHandle nh_;
01456   ros::Publisher clock_publisher_;
01457   ros::Publisher joint_state_publisher_;
01458   ros::Publisher vis_marker_array_publisher_;
01459   ros::Publisher vis_marker_publisher_;
01460   ros::Subscriber joint_state_subscriber_;
01461   ros::Subscriber r_arm_controller_state_subscriber_;
01462   ros::Subscriber l_arm_controller_state_subscriber_;
01463   ros::ServiceClient collision_proximity_planner_client_;
01464   ros::ServiceClient distance_aware_service_client_;
01465   ros::ServiceClient distance_state_validity_service_client_;
01466   ros::ServiceClient set_planning_scene_diff_client_;
01467   ros::ServiceClient left_ik_service_client_;
01468   ros::ServiceClient left_interpolate_service_client_;
01469   ros::ServiceClient non_coll_left_ik_service_client_;
01470   ros::ServiceClient non_coll_right_ik_service_client_;
01471   ros::ServiceClient planning_1_service_client_;
01472   ros::ServiceClient planning_2_service_client_;
01473   ros::ServiceClient right_ik_service_client_;
01474   ros::ServiceClient right_interpolate_service_client_;
01475   ros::ServiceClient trajectory_filter_1_service_client_;
01476   ros::ServiceClient trajectory_filter_2_service_client_;
01477   ros::ServiceClient gazebo_joint_state_client_;
01478   ros::ServiceClient list_controllers_client_;
01479   ros::ServiceClient load_controllers_client_;
01480   ros::ServiceClient unload_controllers_client_;
01481   ros::ServiceClient switch_controllers_client_;
01482   ros::ServiceClient pause_gazebo_client_;
01483   ros::ServiceClient unpause_gazebo_client_;
01484   ros::ServiceClient set_link_properties_client_;
01485   ros::ServiceClient get_link_properties_client_;
01486 
01487   std::map<std::string, double> robot_state_joint_values_;
01488   std::vector<ros::Time> last_creation_time_query_;
01489   tf::TransformBroadcaster transform_broadcaster_;
01490   tf::TransformListener transform_listener_;
01491   std::map<std::string, actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>*> arm_controller_map_;
01492   unsigned int max_trajectory_id_;
01493   unsigned int max_collision_object_id_;
01494 
01495   bool warehouse_data_loaded_once_;
01496   int  active_planner_index_;
01497   bool use_primary_filter_;
01498 
01499   trajectory_msgs::JointTrajectory logged_trajectory_;
01500   trajectory_msgs::JointTrajectory logged_trajectory_controller_error_;
01501   ros::Time logged_trajectory_start_time_;
01502   int logged_trajectory_controller_error_code_;
01503 
01504   bool send_collision_markers_;
01505   std::string collision_marker_state_;
01506   visualization_msgs::MarkerArray collision_markers_;
01507   planning_models::KinematicState* paused_collision_state_;
01508   std_msgs::ColorRGBA point_color_;
01509   std::vector<geometry_msgs::TransformStamped> robot_transforms_;
01510 
01511   interactive_markers::MenuHandler::FeedbackCallback attached_collision_object_feedback_ptr_;
01512   interactive_markers::MenuHandler::FeedbackCallback collision_object_selection_feedback_ptr_;
01513   interactive_markers::MenuHandler::FeedbackCallback collision_object_movement_feedback_ptr_;
01514   interactive_markers::MenuHandler::FeedbackCallback ik_control_feedback_ptr_;
01515   interactive_markers::MenuHandler::FeedbackCallback joint_control_feedback_ptr_;
01516 
01517   interactive_markers::InteractiveMarkerServer* interactive_marker_server_;
01518 
01519   std::map<std::string, SelectableObject>* selectable_objects_;
01520   std::map<std::string, IKController>* ik_controllers_;
01521 
01522   std::string current_planning_scene_name_;
01523   std::string selected_motion_plan_name_;
01524   std::string selected_trajectory_name_;
01525   std::string logged_group_name_;
01526   std::string logged_motion_plan_request_;
01527   std::map<string, MenuEntryMap> menu_entry_maps_;
01528   MenuHandlerMap menu_handler_map_;
01529 
01530   std::map<string, ros::ServiceClient*>* collision_aware_ik_services_;
01531   std::map<string, ros::ServiceClient*>* non_collision_aware_ik_services_;
01532   std::map<string, ros::ServiceClient*>* interpolated_ik_services_;
01533   std::map<string, arm_navigation_msgs::ArmNavigationErrorCodes> error_map_;
01534   std::vector<StateRegistry> states_;
01535 
01536   interactive_markers::MenuHandler::EntryHandle last_resize_handle_;
01537 
01538   std_msgs::ColorRGBA last_collision_object_color_;
01539   std_msgs::ColorRGBA last_mesh_object_color_;
01540 
01541   MonitorStatus monitor_status_;
01542   ros::Time time_of_controller_done_callback_;
01543   ros::Time time_of_last_moving_notification_;
01544 
01545   ros::Time last_marker_start_time_;
01546   ros::Duration marker_dt_;
01547 
01548   void attachCollisionObject(const std::string& name, 
01549                              const std::string& link_name,
01550                              const std::vector<std::string>& touch_links);
01551   
01555   void createSelectableMarkerFromCollisionObject(arm_navigation_msgs::CollisionObject& object, std::string name,
01556                                                  std::string description, std_msgs::ColorRGBA color, bool insert_selectable=true);
01557 
01558   
01559   
01560 public:
01561 
01563   std::map<std::string, PlanningSceneData> planning_scene_map_;
01564 
01566   std::map<std::string, std::map<std::string, TrajectoryData> > trajectory_map_;
01567 
01569   std::map<std::string, MotionPlanRequestData> motion_plan_map_;
01570 
01572   std::map<std::string, bool> joint_clicked_map_;
01573 
01575   std::map<std::string, tf::Transform> joint_prev_transform_map_;
01576 
01577   PlanningSceneEditor();
01578   PlanningSceneEditor(PlanningSceneParameters& params);
01579   ~PlanningSceneEditor();
01580 
01588   bool filterTrajectory(MotionPlanRequestData& requestData, TrajectoryData& trajectory, unsigned int& filter_id);
01589 
01598   bool getAllAssociatedMotionPlanRequests(const unsigned int id, 
01599                                           std::vector<unsigned int>& ids,
01600                                           std::vector<std::string>& stages,
01601                                           std::vector<arm_navigation_msgs::MotionPlanRequest>& requests);
01602 
01609   bool getAllAssociatedPausedStates(const unsigned int id, 
01610                                     std::vector<ros::Time>& paused_times);
01611 
01618   bool getAllAssociatedTrajectorySources(const unsigned int planning_id,
01619                                          const unsigned int mpr_id,
01620                                          std::vector<unsigned int>& trajectory_ids,
01621                                          std::vector<std::string>& trajectory_sources);
01622 
01628   bool getAllPlanningSceneTimes(std::vector<ros::Time>& planning_scene_times,
01629                                 vector<unsigned int>& planning_scene_ids);
01630 
01631 
01641   bool getMotionPlanRequest(const ros::Time& time, const std::string& stage,
01642                             arm_navigation_msgs::MotionPlanRequest& mpr, std::string& id,
01643                             std::string& planning_scene_id);
01644 
01652   bool getPausedState(const unsigned int id, 
01653                       const ros::Time& paused_time,
01654                       head_monitor_msgs::HeadMonitorFeedback& paused_state);
01655 
01664   bool getPlanningSceneOutcomes(const unsigned int id, 
01665                                 std::vector<std::string>& pipeline_stages,
01666                                 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& error_codes,
01667                                 std::map<std::string, arm_navigation_msgs::ArmNavigationErrorCodes>& error_map);
01668 
01669 
01676   bool loadPlanningScene(const ros::Time& time, 
01677                          const unsigned int id);
01678 
01689   bool planToKinematicState(const planning_models::KinematicState& state, const std::string& group_name,
01690                             const std::string& end_effector_name, unsigned int& trajectoryid_Out,
01691                             unsigned int& planning_scene_id);
01692 
01699   bool planToRequest(MotionPlanRequestData& data, unsigned int& trajectoryid_Out);
01700 
01707   bool planToRequest(const std::string& requestid, unsigned int& trajectoryid_Out);
01708 
01715   bool playTrajectory(MotionPlanRequestData& requestData, TrajectoryData& data);
01716 
01722   bool sendPlanningScene(PlanningSceneData& data);
01723 
01734   bool solveIKForEndEffectorPose(MotionPlanRequestData& mpr, PositionType type, bool coll_aware = true,
01735                                  double change_redundancy = 0.0);
01736 
01737 
01745   interactive_markers::MenuHandler::EntryHandle registerMenuEntry(std::string menu, std::string entryName,
01746                                                                   interactive_markers::MenuHandler::FeedbackCallback& callback);
01747 
01748 
01749 
01758   interactive_markers::MenuHandler::EntryHandle registerSubMenuEntry(std::string menu, std::string name,
01759                                                                      std::string subMenu,
01760                                                                      interactive_markers::MenuHandler::FeedbackCallback& callback);
01761 
01766   std::string createNewPlanningScene();
01767 
01774   virtual void onPlanningSceneLoaded(int scene, int numScenes)
01775   {
01776   }
01777 
01782   void collisionObjectMovementCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
01783 
01788   void collisionObjectSelectionCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
01789 
01790   void attachedCollisionObjectInteractiveCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
01791 
01802   std::string createCollisionObject(const std::string& name, geometry_msgs::Pose pose, GeneratedShape shape, float scaleX, float scaleY,
01803                                     float scaleZ, std_msgs::ColorRGBA color,
01804                                     bool selectable);
01805 
01806   std::string createMeshObject(const std::string& name, 
01807                                geometry_msgs::Pose pose,
01808                                const std::string& filename,
01809                                const tf::Vector3& scale,
01810                                std_msgs::ColorRGBA color);
01817   void createIKController(MotionPlanRequestData& data, PositionType type, bool rePose = true);
01818 
01825   void createIkControllersFromMotionPlanRequest(MotionPlanRequestData& data, bool rePose = true);
01826 
01832   void createJointMarkers(MotionPlanRequestData& data, planning_scene_utils::PositionType position);
01833 
01834 
01846   void createMotionPlanRequest(const planning_models::KinematicState& start_state,
01847                                const planning_models::KinematicState& end_state, 
01848                                const std::string& group_name,
01849                                const std::string& end_effector_name, 
01850                                const unsigned int& planning_scene_name,
01851                                const bool fromRobotState, 
01852                                unsigned int& motionPlan_id_Out);
01853 
01854 
01862   void initMotionPlanRequestData(const unsigned int& planning_scene_id, 
01863                                  const std::vector<unsigned int>& ids,
01864                                  const std::vector<std::string>& stages,
01865                                  const std::vector<arm_navigation_msgs::MotionPlanRequest>& requests);
01866 
01867 
01873   void deleteJointMarkers(MotionPlanRequestData& data, PositionType type);
01874 
01879   void deleteKinematicStates();
01880 
01885   void deleteMotionPlanRequest(const unsigned int& id,
01886                                std::vector<unsigned int>& erased_trajectories);
01887 
01892   void deleteTrajectory(unsigned int mpr_id, unsigned int traj_id);
01893 
01894 
01902   // void determineOrientationConstraintsGivenState(const MotionPlanRequestData& mpr,
01903   //                                                const planning_models::KinematicState& state,
01904   //                                                arm_navigation_msgs::OrientationConstraint& goal_constraint,
01905   //                                                arm_navigation_msgs::OrientationConstraint& path_constraint);
01906 
01911   void executeTrajectory(const std::string& mpr_name, const std::string& traj_name);
01912 
01917   void executeTrajectory(TrajectoryData& data);
01918 
01925   void getAllRobotStampedTransforms(const planning_models::KinematicState& state,
01926                                     vector<geometry_msgs::TransformStamped>& trans_vector, const ros::Time& stamp);
01927 
01932   void getMotionPlanningMarkers(visualization_msgs::MarkerArray& arr);
01933 
01938   void getTrajectoryMarkers(visualization_msgs::MarkerArray& arr);
01939 
01944   void IKControllerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
01945 
01950   void JointControllerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
01951 
01957   void controllerDoneCallback(const actionlib::SimpleClientGoalState& state,
01958                               const control_msgs::FollowJointTrajectoryResultConstPtr& result);
01959 
01963   void armHasStoppedMoving();
01964 
01969   void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state);
01970 
01975 void jointTrajectoryControllerStateCallback(const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr& joint_state);
01976 
01977 
01981   void loadAllWarehouseData();
01982 
01992   void makeInteractive1DOFRotationMarker(tf::Transform transform, tf::Vector3 axis, string name, string description,
01993                                          float scale = 1.0f, float angle = 0.0f);
01994 
02004   void makeInteractive1DOFTranslationMarker(tf::Transform transform, tf::Vector3 axis, string name, string description,
02005                                             float scale = 1.0f, float value = 0.0f);
02006 
02010   void printTrajectoryPoint(const std::vector<std::string>& joint_names, const std::vector<double>& joint_values);
02011 
02017   void randomlyPerturb(MotionPlanRequestData& mpr, PositionType type);
02018 
02023   void savePlanningScene(PlanningSceneData& data, bool copy = false);
02024 
02028   void sendMarkers();
02029 
02033   void sendTransformsAndClock();
02034 
02035   void makeSelectableAttachedObjectFromPlanningScene(const arm_navigation_msgs::PlanningScene& scene,
02036                                                      arm_navigation_msgs::AttachedCollisionObject& att);
02037 
02044   void setCurrentPlanningScene(std::string id, bool loadRequests = true, bool loadTrajectories = true);
02045 
02052   void setIKControlsVisible(std::string id, PositionType type, bool visible);
02053 
02062   void setJointState(MotionPlanRequestData& data, PositionType position, std::string& jointName, tf::Transform value);
02063 
02068   void updateJointStates();
02069 
02070   bool hasTrajectory(const std::string& mpr_name,
02071                      const std::string& traj_name) const;
02072 
02077   inline std::string generateNewCollisionObjectId()
02078   {
02079     std::stringstream stream;
02080     stream << "collision_object_";
02081     max_collision_object_id_++;
02082     stream << max_collision_object_id_;
02083     return stream.str();
02084   }
02085 
02090   inline unsigned int generateNewPlanningSceneId()
02091   {
02092     return move_arm_warehouse_logger_reader_->determineNextPlanningSceneId();
02093   }
02094 
02095 
02100   void deleteCollisionObject(std::string& name);
02101 
02102 
02103   inline void lockScene()
02104   {
02105     lock_scene_.lock();
02106   }
02107 
02108   inline void unlockScene()
02109   {
02110     lock_scene_.unlock();
02111   }
02112 
02113   inline planning_environment::CollisionModels* getCollisionModel()
02114   {
02115     return cm_;
02116   }
02117 
02118   inline void setCollisionModel(planning_environment::CollisionModels* model, bool shouldDelete = false)
02119   {
02120     if(shouldDelete && cm_ != NULL)
02121     {
02122       delete cm_;
02123       cm_ = model;
02124     }
02125     cm_ = model;
02126   }
02127 
02128   inline planning_models::KinematicState* getRobotState()
02129   {
02130     return robot_state_;
02131   }
02132 
02133   inline void setRobotState(planning_models::KinematicState* robot_state, bool shouldDelete = true)
02134   {
02135     if(shouldDelete && robot_state_ != NULL)
02136     {
02137       delete robot_state_;
02138       robot_state_ = NULL;
02139     }
02140 
02141     robot_state_ = robot_state;
02142   }
02143 
02144   inline move_arm_warehouse::MoveArmWarehouseLoggerReader* getLoggerReader()
02145   {
02146     return move_arm_warehouse_logger_reader_;
02147   }
02148 
02149   inline void setLoggerReader(move_arm_warehouse::MoveArmWarehouseLoggerReader* loggerReader,
02150                               bool shouldDelete = true)
02151   {
02152     if(move_arm_warehouse_logger_reader_ != NULL)
02153     {
02154       delete move_arm_warehouse_logger_reader_;
02155       move_arm_warehouse_logger_reader_ = NULL;
02156     }
02157 
02158     move_arm_warehouse_logger_reader_ = loggerReader;
02159   }
02160 };
02161 
02162 }
02163 #endif


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