save_to_warehouse.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: Ioan Sucan */
00036 
00037 #include <moveit/warehouse/planning_scene_storage.h>
00038 #include <moveit/warehouse/constraints_storage.h>
00039 #include <moveit/warehouse/state_storage.h>
00040 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00041 #include <boost/algorithm/string/join.hpp>
00042 #include <boost/program_options/cmdline.hpp>
00043 #include <boost/program_options/options_description.hpp>
00044 #include <boost/program_options/parsers.hpp>
00045 #include <boost/program_options/variables_map.hpp>
00046 #include <ros/ros.h>
00047 
00048 static const std::string ROBOT_DESCRIPTION="robot_description";
00049 
00050 void onSceneUpdate(planning_scene_monitor::PlanningSceneMonitor *psm, moveit_warehouse::PlanningSceneStorage *pss)
00051 {
00052   ROS_INFO("Received an update to the planning scene...");
00053 
00054   if (!psm->getPlanningScene()->getName().empty())
00055   {
00056     if (!pss->hasPlanningScene(psm->getPlanningScene()->getName()))
00057     {
00058       moveit_msgs::PlanningScene psmsg;
00059       psm->getPlanningScene()->getPlanningSceneMsg(psmsg);
00060       pss->addPlanningScene(psmsg);
00061     }
00062     else
00063       ROS_INFO("Scene '%s' was previously added. Not adding again.", psm->getPlanningScene()->getName().c_str());
00064   }
00065   else
00066     ROS_INFO("Scene name is empty. Not saving.");
00067 }
00068 
00069 void onMotionPlanRequest(const moveit_msgs::MotionPlanRequestConstPtr &req,
00070                          planning_scene_monitor::PlanningSceneMonitor *psm,
00071                          moveit_warehouse::PlanningSceneStorage *pss)
00072 {
00073   if (psm->getPlanningScene()->getName().empty())
00074   {
00075     ROS_INFO("Scene name is empty. Not saving planning request.");
00076     return;
00077   }
00078   pss->addPlanningQuery(*req, psm->getPlanningScene()->getName());
00079 }
00080 
00081 void onConstraints(const moveit_msgs::ConstraintsConstPtr &msg, moveit_warehouse::ConstraintsStorage *cs)
00082 {
00083   if (msg->name.empty())
00084   {
00085     ROS_INFO("No name specified for constraints. Not saving.");
00086     return;
00087   }
00088 
00089   if (cs->hasConstraints(msg->name))
00090   {
00091     ROS_INFO("Replacing constraints '%s'", msg->name.c_str());
00092     cs->removeConstraints(msg->name);
00093     cs->addConstraints(*msg);
00094   }
00095   else
00096   {
00097     ROS_INFO("Adding constraints '%s'", msg->name.c_str());
00098     cs->addConstraints(*msg);
00099   }
00100 }
00101 
00102 void onRobotState(const moveit_msgs::RobotStateConstPtr &msg, moveit_warehouse::RobotStateStorage *rs)
00103 {
00104   std::vector<std::string> names;
00105   rs->getKnownRobotStates(names);
00106   std::set<std::string> names_set(names.begin(), names.end());
00107   std::size_t n = names.size();
00108   while (names_set.find("S" + boost::lexical_cast<std::string>(n)) != names_set.end())
00109     n++;
00110   std::string name = "S" + boost::lexical_cast<std::string>(n);
00111   ROS_INFO("Adding robot state '%s'", name.c_str());
00112   rs->addRobotState(*msg, name);
00113 }
00114 
00115 int main(int argc, char **argv)
00116 {
00117   ros::init(argc, argv, "save_to_warehouse", ros::init_options::AnonymousName);
00118 
00119   boost::program_options::options_description desc;
00120   desc.add_options()
00121     ("help", "Show help message")
00122     ("host", boost::program_options::value<std::string>(), "Host for the MongoDB.")
00123     ("port", boost::program_options::value<std::size_t>(), "Port for the MongoDB.");
00124 
00125   boost::program_options::variables_map vm;
00126   boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
00127   boost::program_options::notify(vm);
00128 
00129   if (vm.count("help"))
00130   {
00131     std::cout << desc << std::endl;
00132     return 1;
00133   }
00134 
00135   ros::AsyncSpinner spinner(1);
00136   spinner.start();
00137 
00138   ros::NodeHandle nh;
00139   boost::shared_ptr<tf::TransformListener> tf(new tf::TransformListener());
00140   planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION, tf);
00141   if (!psm.getPlanningScene())
00142   {
00143     ROS_ERROR("Unable to initialize PlanningSceneMonitor");
00144     return 1;
00145   }
00146 
00147   psm.startSceneMonitor();
00148   psm.startWorldGeometryMonitor();
00149   moveit_warehouse::PlanningSceneStorage pss(vm.count("host") ? vm["host"].as<std::string>() : "",
00150                                              vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00151   moveit_warehouse::ConstraintsStorage cs(vm.count("host") ? vm["host"].as<std::string>() : "",
00152                                           vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00153   moveit_warehouse::RobotStateStorage rs(vm.count("host") ? vm["host"].as<std::string>() : "",
00154                                          vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00155   std::vector<std::string> names;
00156   pss.getPlanningSceneNames(names);
00157   if (names.empty())
00158     ROS_INFO("There are no previously stored scenes");
00159   else
00160   {
00161     ROS_INFO("Previously stored scenes:");
00162     for (std::size_t i = 0 ; i < names.size() ; ++i)
00163       ROS_INFO(" * %s", names[i].c_str());
00164   }
00165   cs.getKnownConstraints(names);
00166   if (names.empty())
00167     ROS_INFO("There are no previously stored constraints");
00168   else
00169   {
00170     ROS_INFO("Previously stored constraints:");
00171     for (std::size_t i = 0 ; i < names.size() ; ++i)
00172       ROS_INFO(" * %s", names[i].c_str());
00173   }
00174   rs.getKnownRobotStates(names);
00175   if (names.empty())
00176     ROS_INFO("There are no previously stored robot states");
00177   else
00178   {
00179     ROS_INFO("Previously stored robot states:");
00180     for (std::size_t i = 0 ; i < names.size() ; ++i)
00181       ROS_INFO(" * %s", names[i].c_str());
00182   }
00183 
00184   psm.addUpdateCallback(boost::bind(&onSceneUpdate, &psm, &pss));
00185 
00186   boost::function<void(const moveit_msgs::MotionPlanRequestConstPtr&)> callback1 = boost::bind(&onMotionPlanRequest, _1, &psm, &pss);
00187   ros::Subscriber mplan_req_sub = nh.subscribe("motion_plan_request", 100, callback1);
00188   boost::function<void(const moveit_msgs::ConstraintsConstPtr&)> callback2 = boost::bind(&onConstraints, _1, &cs);
00189   ros::Subscriber constr_sub = nh.subscribe("constraints", 100, callback2);
00190   boost::function<void(const moveit_msgs::RobotStateConstPtr&)> callback3 = boost::bind(&onRobotState, _1, &rs);
00191   ros::Subscriber state_sub = nh.subscribe("robot_state", 100, callback3);
00192 
00193   std::vector<std::string> topics;
00194   psm.getMonitoredTopics(topics);
00195   ROS_INFO_STREAM("Listening for scene updates on topics " << boost::algorithm::join(topics, ", "));
00196   ROS_INFO_STREAM("Listening for planning requests on topic " << mplan_req_sub.getTopic());
00197   ROS_INFO_STREAM("Listening for named constraints on topic " << constr_sub.getTopic());
00198   ROS_INFO_STREAM("Listening for states on topic " << state_sub.getTopic());
00199 
00200   ros::waitForShutdown();
00201   return 0;
00202 }


warehouse
Author(s): Ioan Sucan
autogenerated on Wed Aug 26 2015 12:43:45