moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
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
38#include <moveit_msgs/srv/save_robot_state_to_warehouse.hpp>
39#include <moveit_msgs/srv/list_robot_states_in_warehouse.hpp>
40#include <moveit_msgs/srv/get_robot_state_from_warehouse.hpp>
41#include <moveit_msgs/srv/check_if_robot_state_exists_in_warehouse.hpp>
42#include <moveit_msgs/srv/delete_robot_state_from_warehouse.hpp>
43#include <moveit_msgs/srv/rename_robot_state_in_warehouse.hpp>
44#include <rclcpp/executors.hpp>
45#include <rclcpp/logger.hpp>
46#include <rclcpp/logging.hpp>
47#include <rclcpp/node.hpp>
48#include <rclcpp/node_options.hpp>
49#include <rclcpp/parameter_value.hpp>
50#include <rclcpp/utilities.hpp>
52
53namespace
54{
55rclcpp::Logger getLogger()
56{
57 return moveit::getLogger("moveit.ros.warehouse_services");
58}
59} // namespace
60
61static const std::string ROBOT_DESCRIPTION = "robot_description";
62
63bool storeState(const std::shared_ptr<moveit_msgs::srv::SaveRobotStateToWarehouse::Request>& request,
64 const std::shared_ptr<moveit_msgs::srv::SaveRobotStateToWarehouse::Response>& response,
66{
67 if (request->name.empty())
68 {
69 RCLCPP_ERROR(getLogger(), "You must specify a name to store a state");
70 return (response->success = false);
71 }
72 rs.addRobotState(request->state, request->name, request->robot);
73 return (response->success = true);
74}
75
76bool listStates(const std::shared_ptr<moveit_msgs::srv::ListRobotStatesInWarehouse::Request>& request,
77 const std::shared_ptr<moveit_msgs::srv::ListRobotStatesInWarehouse::Response>& response,
79{
80 if (request->regex.empty())
81 {
82 rs.getKnownRobotStates(response->states, request->robot);
83 }
84 else
85 {
86 rs.getKnownRobotStates(request->regex, response->states, request->robot);
87 }
88 return true;
89}
90
91bool hasState(const std::shared_ptr<moveit_msgs::srv::CheckIfRobotStateExistsInWarehouse::Request>& request,
92 const std::shared_ptr<moveit_msgs::srv::CheckIfRobotStateExistsInWarehouse::Response>& response,
94{
95 response->exists = rs.hasRobotState(request->name, request->robot);
96 return true;
97}
98
99bool getState(const std::shared_ptr<moveit_msgs::srv::GetRobotStateFromWarehouse::Request>& request,
100 const std::shared_ptr<moveit_msgs::srv::GetRobotStateFromWarehouse::Response>& response,
102{
103 if (!rs.hasRobotState(request->name, request->robot))
104 {
105 RCLCPP_ERROR_STREAM(getLogger(), "No state called '" << request->name << "' for robot '" << request->robot << "'.");
106 moveit_msgs::msg::RobotState dummy;
107 response->state = dummy;
108 return false;
109 }
111 rs.getRobotState(state_buffer, request->name, request->robot);
112 response->state = static_cast<const moveit_msgs::msg::RobotState&>(*state_buffer);
113 return true;
114}
115
116bool renameState(const std::shared_ptr<moveit_msgs::srv::RenameRobotStateInWarehouse::Request>& request,
117 const std::shared_ptr<moveit_msgs::srv::RenameRobotStateInWarehouse::Response>& /*response*/,
119{
120 if (!rs.hasRobotState(request->old_name, request->robot))
121 {
122 RCLCPP_ERROR_STREAM(getLogger(),
123 "No state called '" << request->old_name << "' for robot '" << request->robot << "'.");
124 return false;
125 }
126 rs.renameRobotState(request->old_name, request->new_name, request->robot);
127 return true;
128}
129
130bool deleteState(const std::shared_ptr<moveit_msgs::srv::DeleteRobotStateFromWarehouse::Request>& request,
131 const std::shared_ptr<moveit_msgs::srv::DeleteRobotStateFromWarehouse::Response>& /*response*/,
133{
134 if (!rs.hasRobotState(request->name, request->robot))
135 {
136 RCLCPP_ERROR_STREAM(getLogger(), "No state called '" << request->name << "' for robot '" << request->robot << "'.");
137 return false;
138 }
139 rs.removeRobotState(request->name, request->robot);
140 return true;
141}
142
143int main(int argc, char** argv)
144{
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);
150 moveit::setNodeLoggerName(node->get_name());
151
152 std::string host;
153
154 int port;
155 double connection_timeout;
156 int connection_retries;
157
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);
162
163 warehouse_ros::DatabaseConnection::Ptr conn;
164
165 try
166 {
168 conn->setParams(host, port, connection_timeout);
169
170 RCLCPP_INFO(node->get_logger(), "Connecting to warehouse on %s:%d", host.c_str(), port);
171 int tries = 0;
172 while (!conn->connect())
173 {
174 ++tries;
175 RCLCPP_WARN(node->get_logger(), "Failed to connect to DB on %s:%d (try %d/%d).", host.c_str(), port, tries,
176 connection_retries);
177 if (tries == connection_retries)
178 {
179 RCLCPP_FATAL(node->get_logger(), "Failed to connect too many times, giving up");
180 return 1;
181 }
182 }
183 }
184 catch (std::exception& ex)
185 {
186 RCLCPP_ERROR(node->get_logger(), "%s", ex.what());
187 return 1;
188 }
189
191
192 std::vector<std::string> names;
193 rs.getKnownRobotStates(names);
194 if (names.empty())
195 {
196 RCLCPP_INFO(node->get_logger(), "There are no previously stored robot states");
197 }
198 else
199 {
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());
203 }
204
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 {
207 return storeState(request, response, rs);
208 };
209
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 {
212 return listStates(request, response, rs);
213 };
214
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);
218 };
219
220 auto has_cb =
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);
224 };
225
226 auto rename_cb =
227 [&](const std::shared_ptr<moveit_msgs::srv::RenameRobotStateInWarehouse::Request>& request,
228 const std::shared_ptr<moveit_msgs::srv::RenameRobotStateInWarehouse::Response>& response) -> bool {
229 return renameState(request, response, rs);
230 };
231
232 auto delete_cb =
233 [&](const std::shared_ptr<moveit_msgs::srv::DeleteRobotStateFromWarehouse::Request>& request,
234 const std::shared_ptr<moveit_msgs::srv::DeleteRobotStateFromWarehouse::Response>& response) -> bool {
235 return deleteState(request, response, rs);
236 };
237
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);
249
250 rclcpp::spin(node);
251 return 0;
252}
void renameRobotState(const std::string &old_name, const std::string &new_name, const std::string &robot="")
bool hasRobotState(const std::string &name, const std::string &robot="") const
void getKnownRobotStates(std::vector< std::string > &names, const std::string &robot="") const
bool getRobotState(RobotStateWithMetadata &msg_m, const std::string &name, const std::string &robot="") const
Get the constraints named name. Return false on failure.
void removeRobotState(const std::string &name, const std::string &robot="")
void addRobotState(const moveit_msgs::msg::RobotState &msg, const std::string &name, const std::string &robot="")
warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotState >::ConstPtr RobotStateWithMetadata
warehouse_ros::DatabaseConnection::Ptr loadDatabase(const rclcpp::Node::SharedPtr &node)
Load a database connection.
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
Definition logger.cpp:79
void setNodeLoggerName(const std::string &name)
Call once after creating a node to initialize logging namespaces.
Definition logger.cpp:73
bool storeState(const std::shared_ptr< moveit_msgs::srv::SaveRobotStateToWarehouse::Request > &request, const std::shared_ptr< moveit_msgs::srv::SaveRobotStateToWarehouse::Response > &response, moveit_warehouse::RobotStateStorage &rs)
int main(int argc, char **argv)
bool hasState(const std::shared_ptr< moveit_msgs::srv::CheckIfRobotStateExistsInWarehouse::Request > &request, const std::shared_ptr< moveit_msgs::srv::CheckIfRobotStateExistsInWarehouse::Response > &response, moveit_warehouse::RobotStateStorage &rs)
bool listStates(const std::shared_ptr< moveit_msgs::srv::ListRobotStatesInWarehouse::Request > &request, const std::shared_ptr< moveit_msgs::srv::ListRobotStatesInWarehouse::Response > &response, moveit_warehouse::RobotStateStorage &rs)
bool getState(const std::shared_ptr< moveit_msgs::srv::GetRobotStateFromWarehouse::Request > &request, const std::shared_ptr< moveit_msgs::srv::GetRobotStateFromWarehouse::Response > &response, moveit_warehouse::RobotStateStorage &rs)
bool renameState(const std::shared_ptr< moveit_msgs::srv::RenameRobotStateInWarehouse::Request > &request, const std::shared_ptr< moveit_msgs::srv::RenameRobotStateInWarehouse::Response > &, moveit_warehouse::RobotStateStorage &rs)
bool deleteState(const std::shared_ptr< moveit_msgs::srv::DeleteRobotStateFromWarehouse::Request > &request, const std::shared_ptr< moveit_msgs::srv::DeleteRobotStateFromWarehouse::Response > &, moveit_warehouse::RobotStateStorage &rs)