00001 /* 00002 * planning_scene_interface.h 00003 * 00004 * Created on: 26 Jul 2012 00005 * Author: andreas 00006 */ 00007 00008 #ifndef PLANNING_SCENE_INTERFACE_H_ 00009 #define PLANNING_SCENE_INTERFACE_H_ 00010 00011 #include <ros/ros.h> 00012 #include <sensor_msgs/JointState.h> 00013 #include <arm_navigation_msgs/SetPlanningSceneDiff.h> 00014 #include "tidyup_utils/hand_description.h" 00015 #include <map> 00016 00017 class PlanningSceneInterface 00018 { 00019 public: 00020 static PlanningSceneInterface* instance(); 00021 // bool setPlanningSceneDiff(const arm_navigation_msgs::PlanningScene& scene); 00022 00023 bool sendDiff(); 00024 bool resetPlanningScene(); 00025 00026 // const arm_navigation_msgs::PlanningScene& getPlanningScene() const {return spsdService.response.planning_scene;} 00027 const arm_navigation_msgs::PlanningScene& getCurrentScene() const {return spsdService.request.planning_scene_diff;} 00028 const arm_navigation_msgs::RobotState& getRobotState(){return spsdService.request.planning_scene_diff.robot_state;} 00029 const std::vector <arm_navigation_msgs::CollisionObject>& getCollisionObjects(){return spsdService.request.planning_scene_diff.collision_objects;} 00030 const std::vector <arm_navigation_msgs::AttachedCollisionObject>& getAttachedCollisionObjects() const {return spsdService.request.planning_scene_diff.attached_collision_objects;} 00031 const arm_navigation_msgs::CollisionObject* getCollisionObject(const std::string& id); 00032 const arm_navigation_msgs::AttachedCollisionObject* getAttachedCollisionObject(const std::string& id); 00033 00034 void setRobotState(const arm_navigation_msgs::RobotState& state); 00035 void addObject(const arm_navigation_msgs::CollisionObject& object); 00036 void removeObject(const std::string& id); 00037 void updateObject(const std::string& id, const geometry_msgs::Pose& pose); 00038 void attachObjectToGripper(const std::string& id, const std::string& arm); 00039 void detachObjectAndAdd(const std::string& id); 00040 00041 const std::string& getGlobalFrame() const {return globalFrame;} 00042 00043 void printDiffToCurrent(const arm_navigation_msgs::PlanningScene& other) const; 00044 static void printDiff(const arm_navigation_msgs::PlanningScene& scene, const arm_navigation_msgs::PlanningScene& other); 00045 static void printDiff(const arm_navigation_msgs::RobotState& state, const arm_navigation_msgs::RobotState& other); 00046 static void printDiff(const std::vector<arm_navigation_msgs::CollisionObject>& objectList, 00047 const std::vector<arm_navigation_msgs::CollisionObject>& other); 00048 static void printDiff(const std::vector<arm_navigation_msgs::AttachedCollisionObject>& objectList, 00049 const std::vector<arm_navigation_msgs::AttachedCollisionObject>& other); 00050 static bool isDifferent(const geometry_msgs::Pose& pose, const geometry_msgs::Pose& other); 00051 static void printAttachedObject(const arm_navigation_msgs::AttachedCollisionObject& object); 00052 static void printObjects(const arm_navigation_msgs::PlanningScene& scene); 00053 void test(); 00054 00055 00056 private: 00057 PlanningSceneInterface(); 00058 00059 arm_navigation_msgs::RobotState& getRobotState_() {return spsdService.request.planning_scene_diff.robot_state;} 00060 std::vector <arm_navigation_msgs::CollisionObject>& getCollisionObjects_() {return spsdService.request.planning_scene_diff.collision_objects;} 00061 std::vector <arm_navigation_msgs::AttachedCollisionObject>& getAttachedCollisionObjects_() {return spsdService.request.planning_scene_diff.attached_collision_objects;} 00062 arm_navigation_msgs::CollisionObject* getCollisionObject_(const std::string& id); 00063 arm_navigation_msgs::AttachedCollisionObject* getAttachedCollisionObject_(const std::string& id); 00064 00065 static PlanningSceneInterface* singleton_instance; 00066 arm_navigation_msgs::SetPlanningSceneDiff spsdService; 00067 ros::ServiceClient setPlanningSceneService; 00068 00069 std::map<std::string, HandDescription> handDescriptions; 00070 std::string globalFrame; 00071 00072 std::string logName; 00073 }; 00074 00075 00076 #endif /* PLANNING_SCENE_INTERFACE_H_ */
