warehouse_services.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dan Greenwald */
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 }


warehouse
Author(s): Ioan Sucan
autogenerated on Mon Jul 24 2017 02:21:59