44 #include <boost/program_options/cmdline.hpp> 45 #include <boost/program_options/options_description.hpp> 46 #include <boost/program_options/parsers.hpp> 47 #include <boost/program_options/variables_map.hpp> 57 if (in.good() && !in.eof())
59 for (
int i = 0; i < count; ++i)
61 std::map<std::string, double> v;
64 if (in.good() && !in.eof())
70 while (joint !=
"." && in.good() && !in.eof())
85 st.setVariablePositions(v);
86 moveit_msgs::RobotState msg;
87 robot_state::robotStateToRobotStateMsg(st, msg);
88 ROS_INFO(
"Parsed start state '%s'", name.c_str());
98 Eigen::Translation3d pos;
99 Eigen::Quaterniond rot;
101 bool have_position =
false;
102 bool have_orientation =
false;
105 std::getline(in, name);
108 std::string link_name;
109 std::getline(in, link_name);
114 while (type !=
"." && in.good() && !in.eof())
118 have_position =
true;
121 pos = Eigen::Translation3d(x, y, z);
123 else if (type ==
"rpy")
125 have_orientation =
true;
128 rot = Eigen::Quaterniond(Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()) *
129 Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY()) *
130 Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()));
133 ROS_ERROR(
"Unknown link constraint element: '%s'", type.c_str());
138 std::string end_link;
139 std::getline(in, end_link);
141 if (have_position && have_orientation)
143 geometry_msgs::PoseStamped pose;
145 pose.header.frame_id = psm->
getRobotModel()->getModelFrame();
148 ROS_INFO(
"Parsed link constraint '%s'", name.c_str());
160 std::string end_link;
161 std::getline(in, end_link);
163 if (in.good() && !in.eof())
165 for (
int i = 0; i < count; ++i)
168 std::getline(in, type);
170 if (in.good() && !in.eof())
172 if (type ==
"link_constraint")
175 ROS_ERROR(
"Unknown goal type: '%s'", type.c_str());
184 std::string scene_name;
186 while (in.good() && !in.eof())
191 if (in.good() && !in.eof())
195 else if (type ==
"goal")
198 ROS_ERROR(
"Unknown query type: '%s'", type.c_str());
203 int main(
int argc,
char** argv)
207 boost::program_options::options_description desc;
208 desc.add_options()(
"help",
"Show help message")(
"queries", boost::program_options::value<std::string>(),
209 "Name of file containing motion planning queries.")(
210 "scene", boost::program_options::value<std::string>(),
"Name of file containing motion planning scene.")(
211 "host", boost::program_options::value<std::string>(),
212 "Host for the DB.")(
"port", boost::program_options::value<std::size_t>(),
"Port for the DB.");
214 boost::program_options::variables_map vm;
215 boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
216 boost::program_options::notify(vm);
218 if (vm.count(
"help") || argc == 1)
220 std::cout << desc << std::endl;
225 if (vm.count(
"host") && vm.count(
"port"))
226 conn->setParams(vm[
"host"].as<std::string>(), vm[
"port"].as<std::size_t>());
227 if (!conn->connect())
237 ROS_ERROR(
"Unable to initialize PlanningSceneMonitor");
245 if (vm.count(
"scene"))
247 std::ifstream fin(vm[
"scene"].as<std::string>().c_str());
250 moveit_msgs::PlanningScene psmsg;
255 if (vm.count(
"queries"))
257 std::ifstream fin(vm[
"queries"].as<std::string>().c_str());
258 if (fin.good() && !fin.eof())
void parseQueries(std::istream &in, planning_scene_monitor::PlanningSceneMonitor *psm, moveit_warehouse::RobotStateStorage *rs, moveit_warehouse::ConstraintsStorage *cs)
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
void addRobotState(const moveit_msgs::RobotState &msg, const std::string &name, const std::string &robot="")
void addConstraints(const moveit_msgs::Constraints &msg, const std::string &robot="", const std::string &group="")
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void parseGoal(std::istream &in, planning_scene_monitor::PlanningSceneMonitor *psm, moveit_warehouse::ConstraintsStorage *cs)
const robot_model::RobotModelConstPtr & getRobotModel() const
void parseLinkConstraint(std::istream &in, planning_scene_monitor::PlanningSceneMonitor *psm, moveit_warehouse::ConstraintsStorage *cs)
static const std::string ROBOT_DESCRIPTION
const planning_scene::PlanningScenePtr & getPlanningScene()
void parseStart(std::istream &in, planning_scene_monitor::PlanningSceneMonitor *psm, moveit_warehouse::RobotStateStorage *rs)
warehouse_ros::DatabaseConnection::Ptr loadDatabase()
Load a database connection.
moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState &state, const robot_model::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
void addPlanningScene(const moveit_msgs::PlanningScene &scene)
int main(int argc, char **argv)