48 using warehouse_ros::Metadata;
49 using warehouse_ros::Query;
57 void moveit_warehouse::TrajectoryConstraintsStorage::createCollections()
59 constraints_collection_ =
60 conn_->openCollectionPtr<moveit_msgs::msg::TrajectoryConstraints>(DATABASE_NAME,
"trajectory_constraints");
65 constraints_collection_.reset();
66 conn_->dropDatabase(DATABASE_NAME);
71 const moveit_msgs::msg::TrajectoryConstraints& msg,
const std::string&
name,
const std::string& robot,
72 const std::string& group)
75 if (hasTrajectoryConstraints(
name, robot, group))
77 removeTrajectoryConstraints(
name, robot, group);
80 Metadata::Ptr metadata = constraints_collection_->createMetadata();
81 metadata->append(CONSTRAINTS_ID_NAME,
name);
82 metadata->append(ROBOT_NAME, robot);
83 metadata->append(CONSTRAINTS_GROUP_NAME, group);
84 constraints_collection_->insert(msg, metadata);
85 RCLCPP_DEBUG(logger_,
"%s constraints '%s'", replace ?
"Replaced" :
"Added",
name.c_str());
89 const std::string& robot,
90 const std::string& group)
const
92 Query::Ptr q = constraints_collection_->createQuery();
93 q->append(CONSTRAINTS_ID_NAME,
name);
95 q->append(ROBOT_NAME, robot);
97 q->append(CONSTRAINTS_GROUP_NAME, group);
98 std::vector<TrajectoryConstraintsWithMetadata> constr = constraints_collection_->queryList(q,
true);
99 return !constr.empty();
103 std::vector<std::string>& names,
104 const std::string& robot,
105 const std::string& group)
const
107 getKnownTrajectoryConstraints(names, robot, group);
108 filterNames(regex, names);
112 const std::string& robot,
113 const std::string& group)
const
116 Query::Ptr q = constraints_collection_->createQuery();
118 q->append(ROBOT_NAME, robot);
120 q->append(CONSTRAINTS_GROUP_NAME, group);
121 std::vector<TrajectoryConstraintsWithMetadata> constr =
122 constraints_collection_->queryList(q,
true, CONSTRAINTS_ID_NAME,
true);
125 if (traj_constraint->lookupField(CONSTRAINTS_ID_NAME))
126 names.push_back(traj_constraint->lookupString(CONSTRAINTS_ID_NAME));
131 const std::string&
name,
132 const std::string& robot,
133 const std::string& group)
const
135 Query::Ptr q = constraints_collection_->createQuery();
136 q->append(CONSTRAINTS_ID_NAME,
name);
138 q->append(ROBOT_NAME, robot);
140 q->append(CONSTRAINTS_GROUP_NAME, group);
141 std::vector<TrajectoryConstraintsWithMetadata> constr = constraints_collection_->queryList(q,
false);
148 msg_m = constr.back();
154 const std::string& new_name,
155 const std::string& robot,
156 const std::string& group)
158 Query::Ptr q = constraints_collection_->createQuery();
159 q->append(CONSTRAINTS_ID_NAME, old_name);
161 q->append(ROBOT_NAME, robot);
163 q->append(CONSTRAINTS_GROUP_NAME, group);
164 Metadata::Ptr m = constraints_collection_->createMetadata();
165 m->append(CONSTRAINTS_ID_NAME, new_name);
166 constraints_collection_->modifyMetadata(q, m);
167 RCLCPP_DEBUG(logger_,
"Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str());
171 const std::string& robot,
172 const std::string& group)
174 Query::Ptr q = constraints_collection_->createQuery();
175 q->append(CONSTRAINTS_ID_NAME,
name);
177 q->append(ROBOT_NAME, robot);
179 q->append(CONSTRAINTS_GROUP_NAME, group);
180 unsigned int rem = constraints_collection_->removeMessages(q);
181 RCLCPP_DEBUG(logger_,
"Removed %u TrajectoryConstraints messages (named '%s')", rem,
name.c_str());
This class provides the mechanism to connect to a database and reads needed ROS parameters when appro...
TrajectoryConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn)
static const std::string DATABASE_NAME
bool hasTrajectoryConstraints(const std::string &name, const std::string &robot="", const std::string &group="") const
void renameTrajectoryConstraints(const std::string &old_name, const std::string &new_name, const std::string &robot="", const std::string &group="")
void getKnownTrajectoryConstraints(std::vector< std::string > &names, const std::string &robot="", const std::string &group="") const
static const std::string CONSTRAINTS_ID_NAME
void removeTrajectoryConstraints(const std::string &name, const std::string &robot="", const std::string &group="")
static const std::string ROBOT_NAME
void addTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints &msg, const std::string &name, const std::string &robot="", const std::string &group="")
static const std::string CONSTRAINTS_GROUP_NAME
bool getTrajectoryConstraints(TrajectoryConstraintsWithMetadata &msg_m, const std::string &name, const std::string &robot="", const std::string &group="") const
Get the constraints named name. Return false on failure.
rclcpp::Logger getLogger()
warehouse_ros::MessageWithMetadata< moveit_msgs::msg::TrajectoryConstraints >::ConstPtr TrajectoryConstraintsWithMetadata
Main namespace for MoveIt.