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


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