40 #include <boost/program_options/cmdline.hpp> 41 #include <boost/program_options/options_description.hpp> 42 #include <boost/program_options/parsers.hpp> 43 #include <boost/program_options/variables_map.hpp> 55 for (std::size_t i = 0; i < constraints.position_constraints.size(); ++i)
58 const moveit_msgs::PositionConstraint& pc = constraints.position_constraints[i];
59 lcp.first = pc.constraint_region.primitive_poses[0].position;
60 lcmap[constraints.position_constraints[i].link_name] = lcp;
63 for (std::size_t i = 0; i < constraints.orientation_constraints.size(); ++i)
65 if (lcmap.count(constraints.orientation_constraints[i].link_name))
67 lcmap[constraints.orientation_constraints[i].link_name].second =
68 constraints.orientation_constraints[i].orientation;
72 ROS_WARN(
"Orientation constraint for %s has no matching position constraint",
73 constraints.orientation_constraints[i].link_name.c_str());
78 int main(
int argc,
char** argv)
82 boost::program_options::options_description desc;
83 desc.add_options()(
"help",
"Show help message")(
"host", boost::program_options::value<std::string>(),
"Host for the " 85 "port", boost::program_options::value<std::size_t>(),
"Port for the DB.");
87 boost::program_options::variables_map vm;
88 boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
89 boost::program_options::notify(vm);
93 std::cout << desc << std::endl;
98 if (vm.count(
"host") && vm.count(
"port"))
99 conn->setParams(vm[
"host"].as<std::string>(), vm[
"port"].as<std::size_t>());
100 if (!conn->connect())
112 std::vector<std::string> scene_names;
115 for (std::size_t i = 0; i < scene_names.size(); ++i)
120 ROS_INFO(
"Saving scene '%s'", scene_names[i].c_str());
121 psm.
getPlanningScene()->setPlanningSceneMsg(static_cast<const moveit_msgs::PlanningScene&>(*pswm));
122 std::ofstream fout((scene_names[i] +
".scene").c_str());
126 std::vector<std::string> robotStateNames;
129 std::stringstream rsregex;
130 rsregex <<
".*" << scene_names[i] <<
".*";
134 std::vector<std::string> constraintNames;
136 std::stringstream csregex;
137 csregex <<
".*" << scene_names[i] <<
".*";
140 if (!(robotStateNames.empty() && constraintNames.empty()))
142 std::ofstream qfout((scene_names[i] +
".queries").c_str());
143 qfout << scene_names[i] << std::endl;
144 if (robotStateNames.size())
146 qfout <<
"start" << std::endl;
147 qfout << robotStateNames.size() << std::endl;
148 for (std::size_t k = 0; k < robotStateNames.size(); ++k)
150 ROS_INFO(
"Saving start state %s for scene %s", robotStateNames[k].c_str(), scene_names[i].c_str());
151 qfout << robotStateNames[k] << std::endl;
154 robot_state::RobotState ks(km);
155 robot_state::robotStateMsgToRobotState(*robotState, ks,
false);
156 ks.printStateInfo(qfout);
157 qfout <<
"." << std::endl;
161 if (constraintNames.size())
163 qfout <<
"goal" << std::endl;
164 qfout << constraintNames.size() << std::endl;
165 for (std::size_t k = 0; k < constraintNames.size(); ++k)
167 ROS_INFO(
"Saving goal %s for scene %s", constraintNames[k].c_str(), scene_names[i].c_str());
168 qfout <<
"link_constraint" << std::endl;
169 qfout << constraintNames[k] << std::endl;
175 for (LinkConstraintMap::iterator iter = lcmap.begin(); iter != lcmap.end(); iter++)
177 std::string link_name = iter->first;
179 qfout << link_name << std::endl;
180 qfout <<
"xyz " << lcp.first.x <<
" " << lcp.first.y <<
" " << lcp.first.z << std::endl;
181 Eigen::Quaterniond orientation(lcp.second.w, lcp.second.x, lcp.second.y, lcp.second.z);
182 Eigen::Vector3d rpy = orientation.matrix().eulerAngles(0, 1, 2);
183 qfout <<
"rpy " << rpy[0] <<
" " << rpy[1] <<
" " << rpy[2] << std::endl;
185 qfout <<
"." << std::endl;
static const std::string ROBOT_DESCRIPTION
void getPlanningSceneNames(std::vector< std::string > &names) const
std::map< std::string, LinkConstraintPair > LinkConstraintMap
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
const robot_model::RobotModelConstPtr & getRobotModel() const
void collectLinkConstraints(const moveit_msgs::Constraints &constraints, LinkConstraintMap &lcmap)
void getKnownRobotStates(std::vector< std::string > &names, const std::string &robot="") const
const planning_scene::PlanningScenePtr & getPlanningScene()
warehouse_ros::DatabaseConnection::Ptr loadDatabase()
Load a database connection.
void getKnownConstraints(std::vector< std::string > &names, const std::string &robot="", const std::string &group="") const
int main(int argc, char **argv)
std::pair< geometry_msgs::Point, geometry_msgs::Quaternion > LinkConstraintPair
bool getConstraints(ConstraintsWithMetadata &msg_m, const std::string &name, const std::string &robot="", const std::string &group="") const
Get the constraints named name. Return false on failure.
bool getPlanningScene(PlanningSceneWithMetadata &scene_m, const std::string &scene_name) const
Get the latest planning scene named scene_name.
bool getRobotState(RobotStateWithMetadata &msg_m, const std::string &name, const std::string &robot="") const
Get the constraints named name. Return false on failure.