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


warehouse
Author(s): Ioan Sucan
autogenerated on Mon Apr 23 2018 10:25:48