import_from_text.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 <moveit/kinematic_constraints/utils.h>
00042 #include <moveit/robot_state/conversions.h>
00043 #include <eigen_conversions/eigen_msg.h>
00044 #include <boost/program_options/cmdline.hpp>
00045 #include <boost/program_options/options_description.hpp>
00046 #include <boost/program_options/parsers.hpp>
00047 #include <boost/program_options/variables_map.hpp>
00048 #include <ros/ros.h>
00049 
00050 static const std::string ROBOT_DESCRIPTION="robot_description";
00051 
00052 void parseStart(std::istream &in, planning_scene_monitor::PlanningSceneMonitor *psm, moveit_warehouse::RobotStateStorage *rs)
00053 {
00054   int count;
00055   in >> count;
00056   if (in.good() && !in.eof())
00057   {
00058     for (int i = 0 ; i < count ; ++i)
00059     {
00060       std::map<std::string, double> v;
00061       std::string name;
00062       in >> name;
00063       if (in.good() && !in.eof())
00064       {
00065         std::string joint;
00066         std::string marker;
00067         double value;
00068         in >> joint;
00069         while (joint != "." && in.good() && !in.eof())
00070         {
00071           in >> marker;
00072           if (marker != "=")
00073             joint = ".";
00074           else
00075             in >> value;
00076           v[joint] = value;
00077           if (joint != ".")
00078             in >> joint;
00079         }
00080       }
00081       if (!v.empty())
00082       {
00083         robot_state::RobotState st = psm->getPlanningScene()->getCurrentState();
00084         st.setVariablePositions(v);
00085         moveit_msgs::RobotState msg;
00086         robot_state::robotStateToRobotStateMsg(st, msg);
00087         ROS_INFO("Parsed start state '%s'", name.c_str());
00088         rs->addRobotState(msg, name);
00089       }
00090     }
00091   }
00092 }
00093 
00094 void parseLinkConstraint(std::istream &in, planning_scene_monitor::PlanningSceneMonitor *psm, moveit_warehouse::ConstraintsStorage *cs)
00095 {
00096   Eigen::Translation3d pos;
00097   Eigen::Quaterniond rot;
00098 
00099   bool have_position = false;
00100   bool have_orientation = false;
00101 
00102   std::string name;
00103   std::getline(in, name);
00104 
00105   // The link name is optional, in which case there would be a blank line
00106   std::string link_name;
00107   std::getline(in, link_name);
00108 
00109   std::string type;
00110   in >> type;
00111 
00112   while (type != "." && in.good() && !in.eof())
00113   {
00114     if (type == "xyz")
00115     {
00116       have_position = true;
00117       double x, y, z;
00118       in >> x >> y >> z;
00119       pos = Eigen::Translation3d(x, y, z);
00120     }
00121     else
00122       if (type == "rpy")
00123       {
00124         have_orientation = true;
00125         double r, p, y;
00126         in >> r >> p >> y;
00127         rot = Eigen::Quaterniond(Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX())
00128                                  * Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
00129                                  * Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()));
00130       }
00131       else
00132         ROS_ERROR("Unknown link constraint element: '%s'", type.c_str());
00133     in >> type;
00134   }
00135 
00136   // Convert to getLine method by eating line break
00137   std::string end_link;
00138   std::getline(in, end_link);
00139 
00140   if (have_position && have_orientation)
00141   {
00142     geometry_msgs::PoseStamped pose;
00143     tf::poseEigenToMsg(pos * rot, pose.pose);
00144     pose.header.frame_id = psm->getRobotModel()->getModelFrame();
00145     moveit_msgs::Constraints constr = kinematic_constraints::constructGoalConstraints(link_name, pose);
00146     constr.name = name;
00147     ROS_INFO("Parsed link constraint '%s'", name.c_str());
00148     cs->addConstraints(constr);
00149   }
00150 }
00151 
00152 void parseGoal(std::istream &in, planning_scene_monitor::PlanningSceneMonitor *psm, moveit_warehouse::ConstraintsStorage *cs)
00153 {
00154   int count;
00155   in >> count;
00156 
00157   // Convert to getLine method from here-on, so eat the line break.
00158   std::string end_link;
00159   std::getline(in, end_link);
00160 
00161   if (in.good() && !in.eof())
00162   {
00163     for (int i = 0 ; i < count ; ++i)
00164     {
00165       std::string type;
00166       std::getline(in, type);
00167 
00168       if (in.good() && !in.eof())
00169       {
00170         if (type == "link_constraint")
00171           parseLinkConstraint(in, psm, cs);
00172         else
00173           ROS_ERROR("Unknown goal type: '%s'", type.c_str());
00174       }
00175     }
00176   }
00177 }
00178 
00179 void parseQueries(std::istream &in, planning_scene_monitor::PlanningSceneMonitor *psm,
00180                   moveit_warehouse::RobotStateStorage *rs, moveit_warehouse::ConstraintsStorage *cs)
00181 {
00182   std::string scene_name;
00183   in >> scene_name;
00184   while (in.good() && !in.eof())
00185   {
00186     std::string type;
00187     in >> type;
00188 
00189     if (in.good() && !in.eof())
00190     {
00191       if (type == "start")
00192         parseStart(in, psm, rs);
00193       else
00194         if (type == "goal")
00195           parseGoal(in, psm, cs);
00196         else
00197           ROS_ERROR("Unknown query type: '%s'", type.c_str());
00198     }
00199   }
00200 }
00201 
00202 int main(int argc, char **argv)
00203 {
00204   ros::init(argc, argv, "import_from_text_to_warehouse", ros::init_options::AnonymousName);
00205 
00206   boost::program_options::options_description desc;
00207   desc.add_options()
00208     ("help", "Show help message")
00209     ("queries", boost::program_options::value<std::string>(), "Name of file containing motion planning queries.")
00210     ("scene", boost::program_options::value<std::string>(), "Name of file containing motion planning scene.")
00211     ("host", boost::program_options::value<std::string>(), "Host for the MongoDB.")
00212     ("port", boost::program_options::value<std::size_t>(), "Port for the MongoDB.");
00213 
00214   boost::program_options::variables_map vm;
00215   boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
00216   boost::program_options::notify(vm);
00217 
00218   if (vm.count("help") || argc == 1) // show help if no parameters passed
00219   {
00220     std::cout << desc << std::endl;
00221     return 1;
00222   }
00223 
00224   ros::AsyncSpinner spinner(1);
00225   spinner.start();
00226 
00227   ros::NodeHandle nh;
00228   planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
00229   if (!psm.getPlanningScene())
00230   {
00231     ROS_ERROR("Unable to initialize PlanningSceneMonitor");
00232     return 1;
00233   }
00234 
00235   moveit_warehouse::PlanningSceneStorage pss(vm.count("host") ? vm["host"].as<std::string>() : "",
00236                                              vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00237   moveit_warehouse::ConstraintsStorage cs(vm.count("host") ? vm["host"].as<std::string>() : "",
00238                                           vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00239   moveit_warehouse::RobotStateStorage rs(vm.count("host") ? vm["host"].as<std::string>() : "",
00240                                          vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00241 
00242   if (vm.count("scene"))
00243   {
00244     std::ifstream fin(vm["scene"].as<std::string>().c_str());
00245     psm.getPlanningScene()->loadGeometryFromStream(fin);
00246     fin.close();
00247     moveit_msgs::PlanningScene psmsg;
00248     psm.getPlanningScene()->getPlanningSceneMsg(psmsg);
00249     pss.addPlanningScene(psmsg);
00250   }
00251 
00252   if (vm.count("queries"))
00253   {
00254     std::ifstream fin(vm["queries"].as<std::string>().c_str());
00255     if (fin.good() && !fin.eof())
00256       parseQueries(fin, &psm, &rs, &cs);
00257     fin.close();
00258   }
00259 
00260   return 0;
00261 }


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