00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
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
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)
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 }