planning_scene_storage.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef MOVEIT_MOVEIT_WAREHOUSE_PLANNING_SCENE_STORAGE_
38 #define MOVEIT_MOVEIT_WAREHOUSE_PLANNING_SCENE_STORAGE_
39 
42 #include <moveit_msgs/PlanningScene.h>
43 #include <moveit_msgs/MotionPlanRequest.h>
44 #include <moveit_msgs/RobotTrajectory.h>
45 
46 namespace moveit_warehouse
47 {
51 
55 
57 
59 {
60 public:
61  static const std::string DATABASE_NAME;
62 
63  static const std::string PLANNING_SCENE_ID_NAME;
64  static const std::string MOTION_PLAN_REQUEST_ID_NAME;
65 
67 
68  void addPlanningScene(const moveit_msgs::PlanningScene& scene);
69  void addPlanningQuery(const moveit_msgs::MotionPlanRequest& planning_query, const std::string& scene_name,
70  const std::string& query_name = "");
71  void addPlanningResult(const moveit_msgs::MotionPlanRequest& planning_query,
72  const moveit_msgs::RobotTrajectory& result, const std::string& scene_name);
73 
74  bool hasPlanningScene(const std::string& name) const;
75  void getPlanningSceneNames(std::vector<std::string>& names) const;
76  void getPlanningSceneNames(const std::string& regex, std::vector<std::string>& names) const;
77 
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;
81 
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);
85  void getPlanningQueries(std::vector<MotionPlanRequestWithMetadata>& planning_queries,
86  const std::string& scene_name) const;
87  void getPlanningQueriesNames(std::vector<std::string>& query_names, const std::string& scene_name) const;
88  void getPlanningQueriesNames(const std::string& regex, std::vector<std::string>& query_names,
89  const std::string& scene_name) const;
90  void getPlanningQueries(std::vector<MotionPlanRequestWithMetadata>& planning_queries,
91  std::vector<std::string>& query_names, const std::string& scene_name) const;
92 
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;
97 
98  void renamePlanningScene(const std::string& old_scene_name, const std::string& new_scene_name);
99  void renamePlanningQuery(const std::string& scene_name, const std::string& old_query_name,
100  const std::string& new_query_name);
101 
102  void removePlanningScene(const std::string& scene_name);
103  void removePlanningQuery(const std::string& scene_name, const std::string& query_name);
104  void removePlanningQueries(const std::string& scene_name);
105  void removePlanningResults(const std::string& scene_name);
106  void removePlanningResults(const std::string& scene_name, const std::string& query_name);
107 
108  void reset();
109 
110 private:
111  void createCollections();
112 
113  std::string getMotionPlanRequestName(const moveit_msgs::MotionPlanRequest& planning_query,
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);
117 
118  PlanningSceneCollection planning_scene_collection_;
119  MotionPlanRequestCollection motion_plan_request_collection_;
120  RobotTrajectoryCollection robot_trajectory_collection_;
121 };
122 }
123 
124 #endif
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_
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
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)
void addPlanningQuery(const moveit_msgs::MotionPlanRequest &planning_query, const std::string &scene_name, const std::string &query_name="")


warehouse
Author(s): Ioan Sucan
autogenerated on Sun Oct 18 2020 13:18:37