warehouse_services.cpp
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: Dan Greenwald */
36 
37 #include <ros/ros.h>
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>
45 
46 static const std::string ROBOT_DESCRIPTION = "robot_description";
47 
48 bool storeState(moveit_msgs::SaveRobotStateToWarehouse::Request& request,
49  moveit_msgs::SaveRobotStateToWarehouse::Response& response, moveit_warehouse::RobotStateStorage* rs)
50 {
51  if (request.name.empty())
52  {
53  ROS_ERROR("You must specify a name to store a state");
54  return (response.success = false);
55  }
56  rs->addRobotState(request.state, request.name, request.robot);
57  return (response.success = true);
58 }
59 
60 bool listStates(moveit_msgs::ListRobotStatesInWarehouse::Request& request,
61  moveit_msgs::ListRobotStatesInWarehouse::Response& response, moveit_warehouse::RobotStateStorage* rs)
62 {
63  if (request.regex.empty())
64  {
65  rs->getKnownRobotStates(response.states, request.robot);
66  }
67  else
68  {
69  rs->getKnownRobotStates(request.regex, response.states, request.robot);
70  }
71  return true;
72 }
73 
74 bool hasState(moveit_msgs::CheckIfRobotStateExistsInWarehouse::Request& request,
75  moveit_msgs::CheckIfRobotStateExistsInWarehouse::Response& response,
77 {
78  response.exists = rs->hasRobotState(request.name, request.robot);
79  return true;
80 }
81 
82 bool getState(moveit_msgs::GetRobotStateFromWarehouse::Request& request,
83  moveit_msgs::GetRobotStateFromWarehouse::Response& response, moveit_warehouse::RobotStateStorage* rs)
84 {
85  if (!rs->hasRobotState(request.name, request.robot))
86  {
87  ROS_ERROR_STREAM("No state called '" << request.name << "' for robot '" << request.robot << "'.");
88  moveit_msgs::RobotState dummy;
89  response.state = dummy;
90  return false;
91  }
93  rs->getRobotState(state_buffer, request.name, request.robot);
94  response.state = static_cast<const moveit_msgs::RobotState&>(*state_buffer);
95  return true;
96 }
97 
98 bool renameState(moveit_msgs::RenameRobotStateInWarehouse::Request& request,
99  moveit_msgs::RenameRobotStateInWarehouse::Response& response, moveit_warehouse::RobotStateStorage* rs)
100 {
101  if (!rs->hasRobotState(request.old_name, request.robot))
102  {
103  ROS_ERROR_STREAM("No state called '" << request.old_name << "' for robot '" << request.robot << "'.");
104  return false;
105  }
106  rs->renameRobotState(request.old_name, request.new_name, request.robot);
107  return true;
108 }
109 
110 bool deleteState(moveit_msgs::DeleteRobotStateFromWarehouse::Request& request,
111  moveit_msgs::DeleteRobotStateFromWarehouse::Response& response,
113 {
114  if (!rs->hasRobotState(request.name, request.robot))
115  {
116  ROS_ERROR_STREAM("No state called '" << request.name << "' for robot '" << request.robot << "'.");
117  return false;
118  }
119  rs->removeRobotState(request.name, request.robot);
120  return true;
121 }
122 
123 int main(int argc, char** argv)
124 {
125  ros::init(argc, argv, "moveit_warehouse_services");
126 
128  spinner.start();
129 
130  ros::NodeHandle node;
131  std::string host;
132 
133  int port;
134  float connection_timeout;
135  int connection_retries;
136 
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);
141 
143 
144  try
145  {
147  conn->setParams(host, port, connection_timeout);
148 
149  ROS_INFO("Connecting to warehouse on %s:%d", host.c_str(), port);
150  int tries = 0;
151  while (!conn->connect())
152  {
153  ++tries;
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)
156  {
157  ROS_FATAL("Failed to connect too many times, giving up");
158  return 1;
159  }
160  }
161  }
162  catch (std::exception& ex)
163  {
164  ROS_ERROR("%s", ex.what());
165  return 1;
166  }
167 
169 
170  std::vector<std::string> names;
171  rs.getKnownRobotStates(names);
172  if (names.empty())
173  ROS_INFO("There are no previously stored robot states");
174  else
175  {
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());
179  }
180 
181  boost::function<bool(moveit_msgs::SaveRobotStateToWarehouse::Request & request,
182  moveit_msgs::SaveRobotStateToWarehouse::Response & response)>
183  save_cb = boost::bind(&storeState, _1, _2, &rs);
184 
185  boost::function<bool(moveit_msgs::ListRobotStatesInWarehouse::Request & request,
186  moveit_msgs::ListRobotStatesInWarehouse::Response & response)>
187  list_cb = boost::bind(&listStates, _1, _2, &rs);
188 
189  boost::function<bool(moveit_msgs::GetRobotStateFromWarehouse::Request & request,
190  moveit_msgs::GetRobotStateFromWarehouse::Response & response)>
191  get_cb = boost::bind(&getState, _1, _2, &rs);
192 
193  boost::function<bool(moveit_msgs::CheckIfRobotStateExistsInWarehouse::Request & request,
194  moveit_msgs::CheckIfRobotStateExistsInWarehouse::Response & response)>
195  has_cb = boost::bind(&hasState, _1, _2, &rs);
196 
197  boost::function<bool(moveit_msgs::RenameRobotStateInWarehouse::Request & request,
198  moveit_msgs::RenameRobotStateInWarehouse::Response & response)>
199  rename_cb = boost::bind(&renameState, _1, _2, &rs);
200 
201  boost::function<bool(moveit_msgs::DeleteRobotStateFromWarehouse::Request & request,
202  moveit_msgs::DeleteRobotStateFromWarehouse::Response & response)>
203  delete_cb = boost::bind(&deleteState, _1, _2, &rs);
204 
205  ros::ServiceServer save_state_server = node.advertiseService("save_robot_state", save_cb);
206  ros::ServiceServer list_states_server = node.advertiseService("list_robot_states", list_cb);
207  ros::ServiceServer get_state_server = node.advertiseService("get_robot_state", get_cb);
208  ros::ServiceServer has_state_server = node.advertiseService("has_robot_state", has_cb);
209  ros::ServiceServer rename_state_server = node.advertiseService("rename_robot_state", rename_cb);
210  ros::ServiceServer delete_state_server = node.advertiseService("delete_robot_state", delete_cb);
211 
213  return 0;
214 }
#define ROS_FATAL(...)
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)
bool hasState(moveit_msgs::CheckIfRobotStateExistsInWarehouse::Request &request, moveit_msgs::CheckIfRobotStateExistsInWarehouse::Response &response, moveit_warehouse::RobotStateStorage *rs)
#define ROS_ERROR(...)
bool storeState(moveit_msgs::SaveRobotStateToWarehouse::Request &request, moveit_msgs::SaveRobotStateToWarehouse::Response &response, moveit_warehouse::RobotStateStorage *rs)
#define ROS_WARN(...)
void getKnownRobotStates(std::vector< std::string > &names, const std::string &robot="") const
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void spinner()
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)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
warehouse_ros::DatabaseConnection::Ptr loadDatabase()
Load a database connection.
#define ROS_INFO(...)
static const std::string ROBOT_DESCRIPTION
bool hasRobotState(const std::string &name, const std::string &robot="") const
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="")
bool getRobotState(RobotStateWithMetadata &msg_m, const std::string &name, const std::string &robot="") const
Get the constraints named name. Return false on failure.
const std::string response
ROSCPP_DECL void waitForShutdown()


warehouse
Author(s): Ioan Sucan
autogenerated on Fri Aug 20 2021 03:01:32