45 using warehouse_ros::Metadata;
46 using warehouse_ros::Query;
54 void moveit_warehouse::PlanningSceneWorldStorage::createCollections()
56 planning_scene_world_collection_ =
57 conn_->openCollectionPtr<moveit_msgs::msg::PlanningSceneWorld>(DATABASE_NAME,
"planning_scene_worlds");
62 planning_scene_world_collection_.reset();
63 conn_->dropDatabase(DATABASE_NAME);
68 const std::string&
name)
71 if (hasPlanningSceneWorld(
name))
73 removePlanningSceneWorld(
name);
76 Metadata::Ptr metadata = planning_scene_world_collection_->createMetadata();
77 metadata->append(PLANNING_SCENE_WORLD_ID_NAME,
name);
78 planning_scene_world_collection_->insert(msg, metadata);
79 RCLCPP_DEBUG(logger_,
"%s planning scene world '%s'", replace ?
"Replaced" :
"Added",
name.c_str());
84 Query::Ptr q = planning_scene_world_collection_->createQuery();
85 q->append(PLANNING_SCENE_WORLD_ID_NAME,
name);
86 std::vector<PlanningSceneWorldWithMetadata> psw = planning_scene_world_collection_->queryList(q,
true);
91 std::vector<std::string>& names)
const
93 getKnownPlanningSceneWorlds(names);
94 filterNames(regex, names);
100 Query::Ptr q = planning_scene_world_collection_->createQuery();
101 std::vector<PlanningSceneWorldWithMetadata> planning_scene_worlds =
102 planning_scene_world_collection_->queryList(q,
true, PLANNING_SCENE_WORLD_ID_NAME,
true);
105 if (planning_scene_world->lookupField(PLANNING_SCENE_WORLD_ID_NAME))
106 names.push_back(planning_scene_world->lookupString(PLANNING_SCENE_WORLD_ID_NAME));
111 const std::string&
name)
const
113 Query::Ptr q = planning_scene_world_collection_->createQuery();
114 q->append(PLANNING_SCENE_WORLD_ID_NAME,
name);
115 std::vector<PlanningSceneWorldWithMetadata> psw = planning_scene_world_collection_->queryList(q,
false);
128 const std::string& new_name)
130 Query::Ptr q = planning_scene_world_collection_->createQuery();
131 q->append(PLANNING_SCENE_WORLD_ID_NAME, old_name);
132 Metadata::Ptr m = planning_scene_world_collection_->createMetadata();
133 m->append(PLANNING_SCENE_WORLD_ID_NAME, new_name);
134 planning_scene_world_collection_->modifyMetadata(q, m);
135 RCLCPP_DEBUG(logger_,
"Renamed planning scene world from '%s' to '%s'", old_name.c_str(), new_name.c_str());
140 Query::Ptr q = planning_scene_world_collection_->createQuery();
141 q->append(PLANNING_SCENE_WORLD_ID_NAME,
name);
142 unsigned int rem = planning_scene_world_collection_->removeMessages(q);
143 RCLCPP_DEBUG(logger_,
"Removed %u PlanningSceneWorld messages (named '%s')", rem,
name.c_str());
This class provides the mechanism to connect to a database and reads needed ROS parameters when appro...
void getKnownPlanningSceneWorlds(std::vector< std::string > &names) const
bool hasPlanningSceneWorld(const std::string &name) const
void renamePlanningSceneWorld(const std::string &old_name, const std::string &new_name)
bool getPlanningSceneWorld(PlanningSceneWorldWithMetadata &msg_m, const std::string &name) const
Get the constraints named name. Return false on failure.
void removePlanningSceneWorld(const std::string &name)
static const std::string DATABASE_NAME
static const std::string PLANNING_SCENE_WORLD_ID_NAME
PlanningSceneWorldStorage(warehouse_ros::DatabaseConnection::Ptr conn)
void addPlanningSceneWorld(const moveit_msgs::msg::PlanningSceneWorld &msg, const std::string &name)
rclcpp::Logger getLogger()
warehouse_ros::MessageWithMetadata< moveit_msgs::msg::PlanningSceneWorld >::ConstPtr PlanningSceneWorldWithMetadata
Main namespace for MoveIt.