save_as_text.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
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>
46 #include <ros/ros.h>
47 
48 static const std::string ROBOT_DESCRIPTION = "robot_description";
49 
50 typedef std::pair<geometry_msgs::Point, geometry_msgs::Quaternion> LinkConstraintPair;
51 typedef std::map<std::string, LinkConstraintPair> LinkConstraintMap;
52 
53 void collectLinkConstraints(const moveit_msgs::Constraints& constraints, LinkConstraintMap& lcmap)
54 {
55  for (std::size_t i = 0; i < constraints.position_constraints.size(); ++i)
56  {
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;
61  }
62 
63  for (std::size_t i = 0; i < constraints.orientation_constraints.size(); ++i)
64  {
65  if (lcmap.count(constraints.orientation_constraints[i].link_name))
66  {
67  lcmap[constraints.orientation_constraints[i].link_name].second =
68  constraints.orientation_constraints[i].orientation;
69  }
70  else
71  {
72  ROS_WARN("Orientation constraint for %s has no matching position constraint",
73  constraints.orientation_constraints[i].link_name.c_str());
74  }
75  }
76 }
77 
78 int main(int argc, char** argv)
79 {
80  ros::init(argc, argv, "save_warehouse_as_text", ros::init_options::AnonymousName);
81 
82  boost::program_options::options_description desc;
83  desc.add_options()("help", "Show help message")("host", boost::program_options::value<std::string>(),
84  "Host for the "
85  "DB.")("port", boost::program_options::value<std::size_t>(),
86  "Port for the DB.");
87 
88  boost::program_options::variables_map vm;
89  boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
90  boost::program_options::notify(vm);
91 
92  if (vm.count("help"))
93  {
94  std::cout << desc << std::endl;
95  return 1;
96  }
97  // Set up db
99  if (vm.count("host") && vm.count("port"))
100  conn->setParams(vm["host"].as<std::string>(), vm["port"].as<std::size_t>());
101  if (!conn->connect())
102  return 1;
103 
105  spinner.start();
106 
108 
112 
113  std::vector<std::string> scene_names;
114  pss.getPlanningSceneNames(scene_names);
115 
116  for (std::size_t i = 0; i < scene_names.size(); ++i)
117  {
119  if (pss.getPlanningScene(pswm, scene_names[i]))
120  {
121  ROS_INFO("Saving scene '%s'", scene_names[i].c_str());
122  psm.getPlanningScene()->setPlanningSceneMsg(static_cast<const moveit_msgs::PlanningScene&>(*pswm));
123  std::ofstream fout((scene_names[i] + ".scene").c_str());
124  psm.getPlanningScene()->saveGeometryToStream(fout);
125  fout.close();
126 
127  std::vector<std::string> robot_state_names;
128  robot_model::RobotModelConstPtr km = psm.getRobotModel();
129  // Get start states for scene
130  std::stringstream rsregex;
131  rsregex << ".*" << scene_names[i] << ".*";
132  rss.getKnownRobotStates(rsregex.str(), robot_state_names);
133 
134  // Get goal constraints for scene
135  std::vector<std::string> constraint_names;
136 
137  std::stringstream csregex;
138  csregex << ".*" << scene_names[i] << ".*";
139  cs.getKnownConstraints(csregex.str(), constraint_names);
140 
141  if (!(robot_state_names.empty() && constraint_names.empty()))
142  {
143  std::ofstream qfout((scene_names[i] + ".queries").c_str());
144  qfout << scene_names[i] << std::endl;
145  if (!robot_state_names.empty())
146  {
147  qfout << "start" << std::endl;
148  qfout << robot_state_names.size() << std::endl;
149  for (std::size_t k = 0; k < robot_state_names.size(); ++k)
150  {
151  ROS_INFO("Saving start state %s for scene %s", robot_state_names[k].c_str(), scene_names[i].c_str());
152  qfout << robot_state_names[k] << std::endl;
154  rss.getRobotState(robot_state, robot_state_names[k]);
155  robot_state::RobotState ks(km);
156  robot_state::robotStateMsgToRobotState(*robot_state, ks, false);
157  ks.printStateInfo(qfout);
158  qfout << "." << std::endl;
159  }
160  }
161 
162  if (!constraint_names.empty())
163  {
164  qfout << "goal" << std::endl;
165  qfout << constraint_names.size() << std::endl;
166  for (std::size_t k = 0; k < constraint_names.size(); ++k)
167  {
168  ROS_INFO("Saving goal %s for scene %s", constraint_names[k].c_str(), scene_names[i].c_str());
169  qfout << "link_constraint" << std::endl;
170  qfout << constraint_names[k] << std::endl;
172  cs.getConstraints(constraints, constraint_names[k]);
173 
174  LinkConstraintMap lcmap;
175  collectLinkConstraints(*constraints, lcmap);
176  for (LinkConstraintMap::iterator iter = lcmap.begin(); iter != lcmap.end(); iter++)
177  {
178  std::string link_name = iter->first;
179  LinkConstraintPair lcp = iter->second;
180  qfout << link_name << std::endl;
181  qfout << "xyz " << lcp.first.x << " " << lcp.first.y << " " << lcp.first.z << std::endl;
182  Eigen::Quaterniond orientation(lcp.second.w, lcp.second.x, lcp.second.y, lcp.second.z);
183  Eigen::Vector3d rpy = orientation.matrix().eulerAngles(0, 1, 2);
184  qfout << "rpy " << rpy[0] << " " << rpy[1] << " " << rpy[2] << std::endl;
185  }
186  qfout << "." << std::endl;
187  }
188  }
189  qfout.close();
190  }
191  }
192  }
193 
194  ROS_INFO("Done.");
195 
196  return 0;
197 }
static const std::string ROBOT_DESCRIPTION
std::map< std::string, LinkConstraintPair > LinkConstraintMap
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void collectLinkConstraints(const moveit_msgs::Constraints &constraints, LinkConstraintMap &lcmap)
const robot_model::RobotModelConstPtr & getRobotModel() const
#define ROS_WARN(...)
void getKnownRobotStates(std::vector< std::string > &names, const std::string &robot="") const
void spinner()
const planning_scene::PlanningScenePtr & getPlanningScene()
void getPlanningSceneNames(std::vector< std::string > &names) const
warehouse_ros::DatabaseConnection::Ptr loadDatabase()
Load a database connection.
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.
#define ROS_INFO(...)
int main(int argc, char **argv)
std::pair< geometry_msgs::Point, geometry_msgs::Quaternion > LinkConstraintPair
void getKnownConstraints(std::vector< std::string > &names, const std::string &robot="", const std::string &group="") const
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.


warehouse
Author(s): Ioan Sucan
autogenerated on Fri Aug 20 2021 03:01:32