00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <ros/ros.h>
00038 #include <moveit/warehouse/state_storage.h>
00039 #include <moveit_msgs/SaveRobotStateToWarehouse.h>
00040 #include <moveit_msgs/ListRobotStatesInWarehouse.h>
00041 #include <moveit_msgs/GetRobotStateFromWarehouse.h>
00042 #include <moveit_msgs/CheckIfRobotStateExistsInWarehouse.h>
00043 #include <moveit_msgs/DeleteRobotStateFromWarehouse.h>
00044 #include <moveit_msgs/RenameRobotStateInWarehouse.h>
00045
00046 static const std::string ROBOT_DESCRIPTION = "robot_description";
00047
00048 bool storeState(moveit_msgs::SaveRobotStateToWarehouse::Request& request,
00049 moveit_msgs::SaveRobotStateToWarehouse::Response& response, moveit_warehouse::RobotStateStorage* rs)
00050 {
00051 const moveit_msgs::RobotState& state = request.state;
00052 if (request.name.empty())
00053 {
00054 ROS_ERROR("You must specify a name to store a state");
00055 return response.success = false;
00056 }
00057 rs->addRobotState(request.state, request.name, request.robot);
00058 return response.success = true;
00059 }
00060
00061 bool listStates(moveit_msgs::ListRobotStatesInWarehouse::Request& request,
00062 moveit_msgs::ListRobotStatesInWarehouse::Response& response, moveit_warehouse::RobotStateStorage* rs)
00063 {
00064 if (request.regex.empty())
00065 {
00066 rs->getKnownRobotStates(response.states, request.robot);
00067 }
00068 else
00069 {
00070 rs->getKnownRobotStates(request.regex, response.states, request.robot);
00071 }
00072 return true;
00073 }
00074
00075 bool hasState(moveit_msgs::CheckIfRobotStateExistsInWarehouse::Request& request,
00076 moveit_msgs::CheckIfRobotStateExistsInWarehouse::Response& response,
00077 moveit_warehouse::RobotStateStorage* rs)
00078 {
00079 response.exists = rs->hasRobotState(request.name, request.robot);
00080 return true;
00081 }
00082
00083 bool getState(moveit_msgs::GetRobotStateFromWarehouse::Request& request,
00084 moveit_msgs::GetRobotStateFromWarehouse::Response& response, moveit_warehouse::RobotStateStorage* rs)
00085 {
00086 if (!rs->hasRobotState(request.name, request.robot))
00087 {
00088 ROS_ERROR_STREAM("No state called '" << request.name << "' for robot '" << request.robot << "'.");
00089 moveit_msgs::RobotState dummy;
00090 response.state = dummy;
00091 return false;
00092 }
00093 moveit_warehouse::RobotStateWithMetadata state_buffer;
00094 rs->getRobotState(state_buffer, request.name, request.robot);
00095 response.state = static_cast<const moveit_msgs::RobotState&>(*state_buffer);
00096 return true;
00097 }
00098
00099 bool renameState(moveit_msgs::RenameRobotStateInWarehouse::Request& request,
00100 moveit_msgs::RenameRobotStateInWarehouse::Response& response, moveit_warehouse::RobotStateStorage* rs)
00101 {
00102 if (!rs->hasRobotState(request.old_name, request.robot))
00103 {
00104 ROS_ERROR_STREAM("No state called '" << request.old_name << "' for robot '" << request.robot << "'.");
00105 return false;
00106 }
00107 rs->renameRobotState(request.old_name, request.new_name, request.robot);
00108 return true;
00109 }
00110
00111 bool deleteState(moveit_msgs::DeleteRobotStateFromWarehouse::Request& request,
00112 moveit_msgs::DeleteRobotStateFromWarehouse::Response& response,
00113 moveit_warehouse::RobotStateStorage* rs)
00114 {
00115 if (!rs->hasRobotState(request.name, request.robot))
00116 {
00117 ROS_ERROR_STREAM("No state called '" << request.name << "' for robot '" << request.robot << "'.");
00118 return false;
00119 }
00120 rs->removeRobotState(request.name, request.robot);
00121 return true;
00122 }
00123
00124 int main(int argc, char** argv)
00125 {
00126 ros::init(argc, argv, "moveit_warehouse_services");
00127
00128 ros::AsyncSpinner spinner(1);
00129 spinner.start();
00130
00131 ros::NodeHandle node;
00132 std::string host;
00133 int port;
00134 node.param<std::string>("warehouse_host", host, "localhost");
00135 node.param<int>("warehouse_port", port, 33829);
00136
00137 warehouse_ros::DatabaseConnection::Ptr conn = moveit_warehouse::loadDatabase();
00138 conn->setParams(host, port, 5.0);
00139
00140 ROS_INFO("Connecting to warehouse on %s:%d", host.c_str(), port);
00141 moveit_warehouse::RobotStateStorage rs(conn);
00142
00143 std::vector<std::string> names;
00144 rs.getKnownRobotStates(names);
00145 if (names.empty())
00146 ROS_INFO("There are no previously stored robot states");
00147 else
00148 {
00149 ROS_INFO("Previously stored robot states:");
00150 for (std::size_t i = 0; i < names.size(); ++i)
00151 ROS_INFO(" * %s", names[i].c_str());
00152 }
00153
00154 boost::function<bool(moveit_msgs::SaveRobotStateToWarehouse::Request & request,
00155 moveit_msgs::SaveRobotStateToWarehouse::Response & response)>
00156 save_cb = boost::bind(&storeState, _1, _2, &rs);
00157
00158 boost::function<bool(moveit_msgs::ListRobotStatesInWarehouse::Request & request,
00159 moveit_msgs::ListRobotStatesInWarehouse::Response & response)>
00160 list_cb = boost::bind(&listStates, _1, _2, &rs);
00161
00162 boost::function<bool(moveit_msgs::GetRobotStateFromWarehouse::Request & request,
00163 moveit_msgs::GetRobotStateFromWarehouse::Response & response)>
00164 get_cb = boost::bind(&getState, _1, _2, &rs);
00165
00166 boost::function<bool(moveit_msgs::CheckIfRobotStateExistsInWarehouse::Request & request,
00167 moveit_msgs::CheckIfRobotStateExistsInWarehouse::Response & response)>
00168 has_cb = boost::bind(&hasState, _1, _2, &rs);
00169
00170 boost::function<bool(moveit_msgs::RenameRobotStateInWarehouse::Request & request,
00171 moveit_msgs::RenameRobotStateInWarehouse::Response & response)>
00172 rename_cb = boost::bind(&renameState, _1, _2, &rs);
00173
00174 boost::function<bool(moveit_msgs::DeleteRobotStateFromWarehouse::Request & request,
00175 moveit_msgs::DeleteRobotStateFromWarehouse::Response & response)>
00176 delete_cb = boost::bind(&deleteState, _1, _2, &rs);
00177
00178 ros::ServiceServer save_state_server = node.advertiseService("save_robot_state", save_cb);
00179 ros::ServiceServer list_states_server = node.advertiseService("list_robot_states", list_cb);
00180 ros::ServiceServer get_state_server = node.advertiseService("get_robot_state", get_cb);
00181 ros::ServiceServer has_state_server = node.advertiseService("has_robot_state", has_cb);
00182 ros::ServiceServer rename_state_server = node.advertiseService("rename_robot_state", rename_cb);
00183 ros::ServiceServer delete_state_server = node.advertiseService("delete_robot_state", delete_cb);
00184
00185 ros::waitForShutdown();
00186 return 0;
00187 }