99bool getState(
const std::shared_ptr<moveit_msgs::srv::GetRobotStateFromWarehouse::Request>& request,
100 const std::shared_ptr<moveit_msgs::srv::GetRobotStateFromWarehouse::Response>& response,
105 RCLCPP_ERROR_STREAM(getLogger(),
"No state called '" << request->name <<
"' for robot '" << request->robot <<
"'.");
106 moveit_msgs::msg::RobotState dummy;
107 response->state = dummy;
111 rs.
getRobotState(state_buffer, request->name, request->robot);
112 response->state =
static_cast<const moveit_msgs::msg::RobotState&
>(*state_buffer);
145 rclcpp::init(argc, argv);
146 rclcpp::NodeOptions node_options;
147 node_options.allow_undeclared_parameters(
true);
148 node_options.automatically_declare_parameters_from_overrides(
true);
149 rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(
"moveit_warehouse_services", node_options);
155 double connection_timeout;
156 int connection_retries;
158 node->get_parameter_or(std::string(
"warehouse_host"), host, std::string(
"localhost"));
159 node->get_parameter_or(std::string(
"warehouse_port"), port, 33829);
160 node->get_parameter_or(std::string(
"warehouse_db_connection_timeout"), connection_timeout, 5.0);
161 node->get_parameter_or(std::string(
"warehouse_db_connection_retries"), connection_retries, 5);
163 warehouse_ros::DatabaseConnection::Ptr conn;
168 conn->setParams(host, port, connection_timeout);
170 RCLCPP_INFO(node->get_logger(),
"Connecting to warehouse on %s:%d", host.c_str(), port);
172 while (!conn->connect())
175 RCLCPP_WARN(node->get_logger(),
"Failed to connect to DB on %s:%d (try %d/%d).", host.c_str(), port, tries,
177 if (tries == connection_retries)
179 RCLCPP_FATAL(node->get_logger(),
"Failed to connect too many times, giving up");
184 catch (std::exception& ex)
186 RCLCPP_ERROR(node->get_logger(),
"%s", ex.what());
192 std::vector<std::string> names;
196 RCLCPP_INFO(node->get_logger(),
"There are no previously stored robot states");
200 RCLCPP_INFO(node->get_logger(),
"Previously stored robot states:");
201 for (
const std::string& name : names)
202 RCLCPP_INFO(node->get_logger(),
" * %s", name.c_str());
205 auto save_cb = [&](
const std::shared_ptr<moveit_msgs::srv::SaveRobotStateToWarehouse::Request>& request,
206 const std::shared_ptr<moveit_msgs::srv::SaveRobotStateToWarehouse::Response>& response) ->
bool {
210 auto list_cb = [&](
const std::shared_ptr<moveit_msgs::srv::ListRobotStatesInWarehouse::Request>& request,
211 const std::shared_ptr<moveit_msgs::srv::ListRobotStatesInWarehouse::Response>& response) ->
bool {
215 auto get_cb = [&](
const std::shared_ptr<moveit_msgs::srv::GetRobotStateFromWarehouse::Request>& request,
216 const std::shared_ptr<moveit_msgs::srv::GetRobotStateFromWarehouse::Response>& response) ->
bool {
217 return getState(request, response, rs);
221 [&](
const std::shared_ptr<moveit_msgs::srv::CheckIfRobotStateExistsInWarehouse::Request>& request,
222 const std::shared_ptr<moveit_msgs::srv::CheckIfRobotStateExistsInWarehouse::Response>& response) ->
bool {
223 return hasState(request, response, rs);
227 [&](
const std::shared_ptr<moveit_msgs::srv::RenameRobotStateInWarehouse::Request>& request,
228 const std::shared_ptr<moveit_msgs::srv::RenameRobotStateInWarehouse::Response>& response) ->
bool {
233 [&](
const std::shared_ptr<moveit_msgs::srv::DeleteRobotStateFromWarehouse::Request>& request,
234 const std::shared_ptr<moveit_msgs::srv::DeleteRobotStateFromWarehouse::Response>& response) ->
bool {
238 auto save_state_server =
239 node->create_service<moveit_msgs::srv::SaveRobotStateToWarehouse>(
"save_robot_state", save_cb);
240 auto list_states_server =
241 node->create_service<moveit_msgs::srv::ListRobotStatesInWarehouse>(
"list_robot_states", list_cb);
242 auto get_state_server = node->create_service<moveit_msgs::srv::GetRobotStateFromWarehouse>(
"get_robot_state", get_cb);
243 auto has_state_server =
244 node->create_service<moveit_msgs::srv::CheckIfRobotStateExistsInWarehouse>(
"has_robot_state", has_cb);
245 auto rename_state_server =
246 node->create_service<moveit_msgs::srv::RenameRobotStateInWarehouse>(
"rename_robot_state", rename_cb);
247 auto delete_state_server =
248 node->create_service<moveit_msgs::srv::DeleteRobotStateFromWarehouse>(
"delete_robot_state", delete_cb);