Go to the documentation of this file.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/state_storage.h>
00039 #include <moveit/warehouse/constraints_storage.h>
00040 #include <boost/program_options/cmdline.hpp>
00041 #include <boost/program_options/options_description.hpp>
00042 #include <boost/program_options/parsers.hpp>
00043 #include <boost/program_options/variables_map.hpp>
00044 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00045 #include <moveit/robot_state/conversions.h>
00046 #include <ros/ros.h>
00047
00048 static const std::string ROBOT_DESCRIPTION="robot_description";
00049
00050 typedef std::pair<geometry_msgs::Point, geometry_msgs::Quaternion> LinkConstraintPair;
00051 typedef std::map<std::string, LinkConstraintPair > LinkConstraintMap;
00052
00053 void collectLinkConstraints(const moveit_msgs::Constraints& constraints, LinkConstraintMap& lcmap )
00054 {
00055 for(std::size_t i = 0; i < constraints.position_constraints.size(); ++i)
00056 {
00057 LinkConstraintPair lcp;
00058 const moveit_msgs::PositionConstraint &pc = constraints.position_constraints[i];
00059 lcp.first = pc.constraint_region.primitive_poses[0].position;
00060 lcmap[constraints.position_constraints[i].link_name] = lcp;
00061 }
00062
00063 for(std::size_t i = 0; i < constraints.orientation_constraints.size(); ++i)
00064 {
00065 if(lcmap.count(constraints.orientation_constraints[i].link_name))
00066 {
00067 lcmap[constraints.orientation_constraints[i].link_name].second = constraints.orientation_constraints[i].orientation;
00068 } else{
00069 ROS_WARN("Orientation constraint for %s has no matching position constraint", constraints.orientation_constraints[i].link_name.c_str());
00070 }
00071 }
00072 }
00073
00074 int main(int argc, char **argv)
00075 {
00076 ros::init(argc, argv, "save_warehouse_as_text", ros::init_options::AnonymousName);
00077
00078 boost::program_options::options_description desc;
00079 desc.add_options()
00080 ("help", "Show help message")
00081 ("host", boost::program_options::value<std::string>(), "Host for the MongoDB.")
00082 ("port", boost::program_options::value<std::size_t>(), "Port for the MongoDB.");
00083
00084 boost::program_options::variables_map vm;
00085 boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
00086 boost::program_options::notify(vm);
00087
00088 if (vm.count("help"))
00089 {
00090 std::cout << desc << std::endl;
00091 return 1;
00092 }
00093
00094 ros::AsyncSpinner spinner(1);
00095 spinner.start();
00096
00097 planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
00098
00099 moveit_warehouse::PlanningSceneStorage pss(vm.count("host") ? vm["host"].as<std::string>() : "",
00100 vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00101
00102 moveit_warehouse::RobotStateStorage rss(vm.count("host") ? vm["host"].as<std::string>() : "",
00103 vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00104
00105 moveit_warehouse::ConstraintsStorage cs(vm.count("host") ? vm["host"].as<std::string>() : "",
00106 vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00107
00108 std::vector<std::string> scene_names;
00109 pss.getPlanningSceneNames(scene_names);
00110
00111 for (std::size_t i = 0 ; i < scene_names.size() ; ++i)
00112 {
00113 moveit_warehouse::PlanningSceneWithMetadata pswm;
00114 if (pss.getPlanningScene(pswm, scene_names[i]))
00115 {
00116 ROS_INFO("Saving scene '%s'", scene_names[i].c_str());
00117 psm.getPlanningScene()->setPlanningSceneMsg(static_cast<const moveit_msgs::PlanningScene&>(*pswm));
00118 std::ofstream fout((scene_names[i] + ".scene").c_str());
00119 psm.getPlanningScene()->saveGeometryToStream(fout);
00120 fout.close();
00121
00122 std::vector<std::string> robotStateNames;
00123 robot_model::RobotModelConstPtr km = psm.getRobotModel();
00124
00125 std::stringstream rsregex;
00126 rsregex << ".*" << scene_names[i] << ".*";
00127 rss.getKnownRobotStates(rsregex.str(), robotStateNames);
00128
00129
00130 std::vector<std::string> constraintNames;
00131
00132 std::stringstream csregex;
00133 csregex << ".*" << scene_names[i] << ".*";
00134 cs.getKnownConstraints(csregex.str(), constraintNames);
00135
00136 if( !(robotStateNames.empty() && constraintNames.empty()) )
00137 {
00138 std::ofstream qfout((scene_names[i] + ".queries").c_str());
00139 qfout << scene_names[i] << std::endl;
00140 if(robotStateNames.size())
00141 {
00142 qfout << "start" << std::endl;
00143 qfout << robotStateNames.size() << std::endl;
00144 for(std::size_t k = 0; k < robotStateNames.size(); ++k)
00145 {
00146 ROS_INFO("Saving start state %s for scene %s", robotStateNames[k].c_str(), scene_names[i].c_str());
00147 qfout << robotStateNames[k] << std::endl;
00148 moveit_warehouse::RobotStateWithMetadata robotState;
00149 rss.getRobotState(robotState, robotStateNames[k]);
00150 robot_state::RobotState ks(km);
00151 robot_state::robotStateMsgToRobotState(*robotState, ks, false);
00152 ks.printStateInfo(qfout);
00153 qfout << "." << std::endl;
00154 }
00155 }
00156
00157 if(constraintNames.size())
00158 {
00159 qfout << "goal" << std::endl;
00160 qfout << constraintNames.size() << std::endl;
00161 for(std::size_t k = 0; k < constraintNames.size(); ++k)
00162 {
00163 ROS_INFO("Saving goal %s for scene %s", constraintNames[k].c_str(), scene_names[i].c_str());
00164 qfout << "link_constraint" << std::endl;
00165 qfout << constraintNames[k] << std::endl;
00166 moveit_warehouse::ConstraintsWithMetadata constraints;
00167 cs.getConstraints(constraints, constraintNames[k]);
00168
00169 LinkConstraintMap lcmap;
00170 collectLinkConstraints(*constraints, lcmap);
00171 for(LinkConstraintMap::iterator iter = lcmap.begin(); iter != lcmap.end(); iter++)
00172 {
00173 std::string link_name = iter->first;
00174 LinkConstraintPair lcp = iter->second;
00175 qfout << link_name << std::endl;
00176 qfout << "xyz " << lcp.first.x << " " << lcp.first.y << " " << lcp.first.z << std::endl;
00177 Eigen::Quaterniond orientation(lcp.second.w, lcp.second.x, lcp.second.y, lcp.second.z);
00178 Eigen::Vector3d rpy = orientation.matrix().eulerAngles(0, 1, 2);
00179 qfout << "rpy " << rpy[0] << " " << rpy[1] << " " << rpy[2] << std::endl;
00180
00181 }
00182 qfout << "." << std::endl;
00183 }
00184 }
00185 qfout.close();
00186 }
00187 }
00188
00189
00190 }
00191
00192 ROS_INFO("Done.");
00193
00194 return 0;
00195 }