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,
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
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
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
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)
00219 {
00220 std::cout << desc << std::endl;
00221 return 1;
00222 }
00223
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 }