37 #ifndef MOVEIT_MOVEIT_WAREHOUSE_PLANNING_SCENE_STORAGE_ 38 #define MOVEIT_MOVEIT_WAREHOUSE_PLANNING_SCENE_STORAGE_ 42 #include <moveit_msgs/PlanningScene.h> 43 #include <moveit_msgs/MotionPlanRequest.h> 44 #include <moveit_msgs/RobotTrajectory.h> 69 void addPlanningQuery(
const moveit_msgs::MotionPlanRequest& planning_query,
const std::string& scene_name,
70 const std::string& query_name =
"");
72 const moveit_msgs::RobotTrajectory& result,
const std::string& scene_name);
79 bool getPlanningScene(PlanningSceneWithMetadata& scene_m,
const std::string& scene_name)
const;
80 bool getPlanningSceneWorld(moveit_msgs::PlanningSceneWorld& world,
const std::string& scene_name)
const;
82 bool hasPlanningQuery(
const std::string& scene_name,
const std::string& query_name)
const;
83 bool getPlanningQuery(MotionPlanRequestWithMetadata& query_m,
const std::string& scene_name,
84 const std::string& query_name);
86 const std::string& scene_name)
const;
89 const std::string& scene_name)
const;
91 std::vector<std::string>& query_names,
const std::string& scene_name)
const;
93 void getPlanningResults(std::vector<RobotTrajectoryWithMetadata>& planning_results,
const std::string& scene_name,
94 const moveit_msgs::MotionPlanRequest& planning_query)
const;
95 void getPlanningResults(std::vector<RobotTrajectoryWithMetadata>& planning_results,
const std::string& scene_name,
96 const std::string& query_name)
const;
98 void renamePlanningScene(
const std::string& old_scene_name,
const std::string& new_scene_name);
100 const std::string& new_query_name);
114 const std::string& scene_name)
const;
115 std::string
addNewPlanningRequest(
const moveit_msgs::MotionPlanRequest& planning_query,
const std::string& scene_name,
116 const std::string& query_name);
void removePlanningQuery(const std::string &scene_name, const std::string &query_name)
void renamePlanningQuery(const std::string &scene_name, const std::string &old_query_name, const std::string &new_query_name)
warehouse_ros::MessageCollection< moveit_msgs::PlanningScene >::Ptr PlanningSceneCollection
bool hasPlanningScene(const std::string &name) const
void getPlanningSceneNames(std::vector< std::string > &names) const
warehouse_ros::MessageWithMetadata< moveit_msgs::RobotTrajectory >::ConstPtr RobotTrajectoryWithMetadata
void removePlanningQueries(const std::string &scene_name)
PlanningSceneStorage(warehouse_ros::DatabaseConnection::Ptr conn)
warehouse_ros::MessageCollection< moveit_msgs::MotionPlanRequest >::Ptr MotionPlanRequestCollection
bool hasPlanningQuery(const std::string &scene_name, const std::string &query_name) const
void renamePlanningScene(const std::string &old_scene_name, const std::string &new_scene_name)
This class provides the mechanism to connect to a database and reads needed ROS parameters when appro...
RobotTrajectoryCollection robot_trajectory_collection_
MOVEIT_CLASS_FORWARD(ConstraintsStorage)
MotionPlanRequestCollection motion_plan_request_collection_
static const std::string DATABASE_NAME
void getPlanningResults(std::vector< RobotTrajectoryWithMetadata > &planning_results, const std::string &scene_name, const moveit_msgs::MotionPlanRequest &planning_query) const
static const std::string MOTION_PLAN_REQUEST_ID_NAME
warehouse_ros::MessageWithMetadata< moveit_msgs::PlanningScene >::ConstPtr PlanningSceneWithMetadata
PlanningSceneCollection planning_scene_collection_
bool getPlanningSceneWorld(moveit_msgs::PlanningSceneWorld &world, const std::string &scene_name) const
void removePlanningScene(const std::string &scene_name)
void addPlanningScene(const moveit_msgs::PlanningScene &scene)
bool getPlanningQuery(MotionPlanRequestWithMetadata &query_m, const std::string &scene_name, const std::string &query_name)
warehouse_ros::MessageCollection< moveit_msgs::RobotTrajectory >::Ptr RobotTrajectoryCollection
bool getPlanningScene(PlanningSceneWithMetadata &scene_m, const std::string &scene_name) const
Get the latest planning scene named scene_name.
void getPlanningQueriesNames(std::vector< std::string > &query_names, const std::string &scene_name) const
std::string getMotionPlanRequestName(const moveit_msgs::MotionPlanRequest &planning_query, const std::string &scene_name) const
void getPlanningQueries(std::vector< MotionPlanRequestWithMetadata > &planning_queries, const std::string &scene_name) const
std::string addNewPlanningRequest(const moveit_msgs::MotionPlanRequest &planning_query, const std::string &scene_name, const std::string &query_name)
warehouse_ros::MessageWithMetadata< moveit_msgs::MotionPlanRequest >::ConstPtr MotionPlanRequestWithMetadata
void addPlanningResult(const moveit_msgs::MotionPlanRequest &planning_query, const moveit_msgs::RobotTrajectory &result, const std::string &scene_name)
void removePlanningResults(const std::string &scene_name)
static const std::string PLANNING_SCENE_ID_NAME
void addPlanningQuery(const moveit_msgs::MotionPlanRequest &planning_query, const std::string &scene_name, const std::string &query_name="")