38 #include <boost/regex.hpp> 58 conn_->openCollectionPtr<moveit_msgs::MotionPlanRequest>(
DATABASE_NAME,
"motion_plan_request");
60 conn_->openCollectionPtr<moveit_msgs::RobotTrajectory>(
DATABASE_NAME,
"robot_trajectory");
83 ROS_DEBUG(
"%s scene '%s'", replace ?
"Replaced" :
"Added", scene.name.c_str());
91 return !planning_scenes.empty();
95 const moveit_msgs::MotionPlanRequest& planning_query,
const std::string& scene_name)
const 103 if (existing_requests.empty())
111 const void* data_arg = buffer_arg.get();
113 for (std::size_t i = 0; i < existing_requests.size(); ++i)
116 static_cast<const moveit_msgs::MotionPlanRequest&>(*existing_requests[i]));
117 if (serial_size != serial_size_arg)
122 const void* data = buffer.get();
123 if (memcmp(data_arg, data, serial_size) == 0)
131 const std::string& scene_name,
132 const std::string& query_name)
137 if (!query_name.empty() &&
id.empty())
140 if (
id != query_name ||
id ==
"")
145 const moveit_msgs::MotionPlanRequest& planning_query,
const std::string& scene_name,
const std::string& query_name)
147 std::string
id = query_name;
150 std::set<std::string> used;
154 for (std::size_t i = 0; i < existing_requests.size(); ++i)
156 std::size_t
index = existing_requests.size();
159 id =
"Motion Plan Request " + boost::lexical_cast<std::string>(index);
161 }
while (used.find(
id) != used.end());
167 ROS_DEBUG(
"Saved planning query '%s' for scene '%s'",
id.c_str(), scene_name.c_str());
172 const moveit_msgs::RobotTrajectory& result,
173 const std::string& scene_name)
188 std::vector<PlanningSceneWithMetadata> planning_scenes =
190 for (std::size_t i = 0; i < planning_scenes.size(); ++i)
196 std::vector<std::string>& names)
const 203 const std::string& scene_name)
const 208 world = scene_m->world;
216 const std::string& scene_name)
const 221 if (planning_scenes.empty())
223 ROS_WARN(
"Planning scene '%s' was not found in the database", scene_name.c_str());
226 scene_m = planning_scenes.back();
228 const_cast<moveit_msgs::PlanningScene*
>(
static_cast<const moveit_msgs::PlanningScene*
>(scene_m.get()))->name =
234 const std::string& scene_name,
235 const std::string& query_name)
241 if (planning_queries.empty())
243 ROS_ERROR(
"Planning query '%s' not found for scene '%s'", query_name.c_str(), scene_name.c_str());
248 query_m = planning_queries.front();
254 std::vector<MotionPlanRequestWithMetadata>& planning_queries,
const std::string& scene_name)
const 262 const std::string& scene_name)
const 268 for (std::size_t i = 0; i < planning_queries.size(); ++i)
274 std::vector<std::string>& query_names,
275 const std::string& scene_name)
const 281 std::vector<std::string> fnames;
282 boost::regex
r(regex);
283 for (std::size_t i = 0; i < query_names.size(); ++i)
286 if (boost::regex_match(query_names[i].c_str(), match, r))
288 fnames.push_back(query_names[i]);
291 query_names.swap(fnames);
296 std::vector<MotionPlanRequestWithMetadata>& planning_queries, std::vector<std::string>& query_names,
297 const std::string& scene_name)
const 302 query_names.resize(planning_queries.size());
303 for (std::size_t i = 0; i < planning_queries.size(); ++i)
307 query_names[i].clear();
311 std::vector<RobotTrajectoryWithMetadata>& planning_results,
const std::string& scene_name,
312 const moveit_msgs::MotionPlanRequest& planning_query)
const 316 planning_results.clear();
322 std::vector<RobotTrajectoryWithMetadata>& planning_results,
const std::string& scene_name,
323 const std::string& planning_query)
const 332 const std::string& query_name)
const 338 return !queries.empty();
342 const std::string& new_scene_name)
349 ROS_DEBUG(
"Renamed planning scene from '%s' to '%s'", old_scene_name.c_str(), new_scene_name.c_str());
353 const std::string& old_query_name,
354 const std::string& new_query_name)
362 ROS_DEBUG(
"Renamed planning query for scene '%s' from '%s' to '%s'", scene_name.c_str(), old_query_name.c_str(),
363 new_query_name.c_str());
372 ROS_DEBUG(
"Removed %u PlanningScene messages (named '%s')", rem, scene_name.c_str());
381 ROS_DEBUG(
"Removed %u MotionPlanRequest messages for scene '%s'", rem, scene_name.c_str());
385 const std::string& query_name)
392 ROS_DEBUG(
"Removed %u MotionPlanRequest messages for scene '%s', query '%s'", rem, scene_name.c_str(),
401 ROS_DEBUG(
"Removed %u RobotTrajectory messages for scene '%s'", rem, scene_name.c_str());
405 const std::string& query_name)
411 ROS_DEBUG(
"Removed %u RobotTrajectory messages for scene '%s', query '%s'", rem, scene_name.c_str(),
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)
bool hasPlanningScene(const std::string &name) const
void getPlanningSceneNames(std::vector< std::string > &names) const
void filterNames(const std::string ®ex, std::vector< std::string > &names) const
Keep only the names that match regex.
void removePlanningQueries(const std::string &scene_name)
PlanningSceneStorage(warehouse_ros::DatabaseConnection::Ptr conn)
bool hasPlanningQuery(const std::string &scene_name, const std::string &query_name) const
warehouse_ros::DatabaseConnection::Ptr conn_
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_
MotionPlanRequestCollection motion_plan_request_collection_
static const std::string DATABASE_NAME
void serialize(Stream &stream, const T &t)
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
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)
bool getPlanningScene(PlanningSceneWithMetadata &scene_m, const std::string &scene_name) const
Get the latest planning scene named scene_name.
uint32_t serializationLength(const T &t)
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)
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="")