39 #include <moveit_msgs/SaveRobotStateToWarehouse.h> 40 #include <moveit_msgs/ListRobotStatesInWarehouse.h> 41 #include <moveit_msgs/GetRobotStateFromWarehouse.h> 42 #include <moveit_msgs/CheckIfRobotStateExistsInWarehouse.h> 43 #include <moveit_msgs/DeleteRobotStateFromWarehouse.h> 44 #include <moveit_msgs/RenameRobotStateInWarehouse.h> 48 bool storeState(moveit_msgs::SaveRobotStateToWarehouse::Request& request,
51 if (request.name.empty())
53 ROS_ERROR(
"You must specify a name to store a state");
54 return (response.success =
false);
56 rs->
addRobotState(request.state, request.name, request.robot);
57 return (response.success =
true);
60 bool listStates(moveit_msgs::ListRobotStatesInWarehouse::Request& request,
63 if (request.regex.empty())
74 bool hasState(moveit_msgs::CheckIfRobotStateExistsInWarehouse::Request& request,
75 moveit_msgs::CheckIfRobotStateExistsInWarehouse::Response& response,
78 response.exists = rs->
hasRobotState(request.name, request.robot);
82 bool getState(moveit_msgs::GetRobotStateFromWarehouse::Request& request,
87 ROS_ERROR_STREAM(
"No state called '" << request.name <<
"' for robot '" << request.robot <<
"'.");
88 moveit_msgs::RobotState dummy;
89 response.state = dummy;
94 response.state =
static_cast<const moveit_msgs::RobotState&
>(*state_buffer);
98 bool renameState(moveit_msgs::RenameRobotStateInWarehouse::Request& request,
103 ROS_ERROR_STREAM(
"No state called '" << request.old_name <<
"' for robot '" << request.robot <<
"'.");
110 bool deleteState(moveit_msgs::DeleteRobotStateFromWarehouse::Request& request,
111 moveit_msgs::DeleteRobotStateFromWarehouse::Response& response,
116 ROS_ERROR_STREAM(
"No state called '" << request.name <<
"' for robot '" << request.robot <<
"'.");
123 int main(
int argc,
char** argv)
125 ros::init(argc, argv,
"moveit_warehouse_services");
134 float connection_timeout;
135 int connection_retries;
137 node.
param<std::string>(
"warehouse_host", host,
"localhost");
138 node.
param<
int>(
"warehouse_port", port, 33829);
139 node.
param<
float>(
"warehouse_db_connection_timeout", connection_timeout, 5.0);
140 node.
param<
int>(
"warehouse_db_connection_retries", connection_retries, 5);
147 conn->setParams(host, port, connection_timeout);
149 ROS_INFO(
"Connecting to warehouse on %s:%d", host.c_str(), port);
151 while (!conn->connect())
154 ROS_WARN(
"Failed to connect to DB on %s:%d (try %d/%d).", host.c_str(), port, tries, connection_retries);
155 if (tries == connection_retries)
157 ROS_FATAL(
"Failed to connect too many times, giving up");
162 catch (std::exception& ex)
170 std::vector<std::string> names;
173 ROS_INFO(
"There are no previously stored robot states");
176 ROS_INFO(
"Previously stored robot states:");
177 for (std::size_t i = 0; i < names.size(); ++i)
178 ROS_INFO(
" * %s", names[i].c_str());
181 boost::function<bool(moveit_msgs::SaveRobotStateToWarehouse::Request & request,
182 moveit_msgs::SaveRobotStateToWarehouse::Response & response)>
183 save_cb = boost::bind(&
storeState, _1, _2, &rs);
185 boost::function<bool(moveit_msgs::ListRobotStatesInWarehouse::Request & request,
186 moveit_msgs::ListRobotStatesInWarehouse::Response & response)>
187 list_cb = boost::bind(&
listStates, _1, _2, &rs);
189 boost::function<bool(moveit_msgs::GetRobotStateFromWarehouse::Request & request,
190 moveit_msgs::GetRobotStateFromWarehouse::Response & response)>
191 get_cb = boost::bind(&
getState, _1, _2, &rs);
193 boost::function<bool(moveit_msgs::CheckIfRobotStateExistsInWarehouse::Request & request,
194 moveit_msgs::CheckIfRobotStateExistsInWarehouse::Response & response)>
195 has_cb = boost::bind(&
hasState, _1, _2, &rs);
197 boost::function<bool(moveit_msgs::RenameRobotStateInWarehouse::Request & request,
198 moveit_msgs::RenameRobotStateInWarehouse::Response & response)>
199 rename_cb = boost::bind(&
renameState, _1, _2, &rs);
201 boost::function<bool(moveit_msgs::DeleteRobotStateFromWarehouse::Request & request,
202 moveit_msgs::DeleteRobotStateFromWarehouse::Response & response)>
203 delete_cb = boost::bind(&
deleteState, _1, _2, &rs);
int main(int argc, char **argv)
void addRobotState(const moveit_msgs::RobotState &msg, const std::string &name, const std::string &robot="")
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void getKnownRobotStates(std::vector< std::string > &names, const std::string &robot="") const
bool hasState(moveit_msgs::CheckIfRobotStateExistsInWarehouse::Request &request, moveit_msgs::CheckIfRobotStateExistsInWarehouse::Response &response, moveit_warehouse::RobotStateStorage *rs)
bool storeState(moveit_msgs::SaveRobotStateToWarehouse::Request &request, moveit_msgs::SaveRobotStateToWarehouse::Response &response, moveit_warehouse::RobotStateStorage *rs)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool renameState(moveit_msgs::RenameRobotStateInWarehouse::Request &request, moveit_msgs::RenameRobotStateInWarehouse::Response &response, moveit_warehouse::RobotStateStorage *rs)
bool deleteState(moveit_msgs::DeleteRobotStateFromWarehouse::Request &request, moveit_msgs::DeleteRobotStateFromWarehouse::Response &response, moveit_warehouse::RobotStateStorage *rs)
warehouse_ros::DatabaseConnection::Ptr loadDatabase()
Load a database connection.
bool hasRobotState(const std::string &name, const std::string &robot="") const
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
static const std::string ROBOT_DESCRIPTION
bool getRobotState(RobotStateWithMetadata &msg_m, const std::string &name, const std::string &robot="") const
Get the constraints named name. Return false on failure.
bool listStates(moveit_msgs::ListRobotStatesInWarehouse::Request &request, moveit_msgs::ListRobotStatesInWarehouse::Response &response, moveit_warehouse::RobotStateStorage *rs)
bool getState(moveit_msgs::GetRobotStateFromWarehouse::Request &request, moveit_msgs::GetRobotStateFromWarehouse::Response &response, moveit_warehouse::RobotStateStorage *rs)
void renameRobotState(const std::string &old_name, const std::string &new_name, const std::string &robot="")
#define ROS_ERROR_STREAM(args)
void removeRobotState(const std::string &name, const std::string &robot="")
ROSCPP_DECL void waitForShutdown()