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>(), "Host for the "
84  "DB.")(
85  "port", boost::program_options::value<std::size_t>(), "Port for the DB.");
86 
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);
90 
91  if (vm.count("help"))
92  {
93  std::cout << desc << std::endl;
94  return 1;
95  }
96  // Set up db
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())
101  return 1;
102 
104  spinner.start();
105 
107 
111 
112  std::vector<std::string> scene_names;
113  pss.getPlanningSceneNames(scene_names);
114 
115  for (std::size_t i = 0; i < scene_names.size(); ++i)
116  {
118  if (pss.getPlanningScene(pswm, scene_names[i]))
119  {
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());
123  psm.getPlanningScene()->saveGeometryToStream(fout);
124  fout.close();
125 
126  std::vector<std::string> robotStateNames;
127  robot_model::RobotModelConstPtr km = psm.getRobotModel();
128  // Get start states for scene
129  std::stringstream rsregex;
130  rsregex << ".*" << scene_names[i] << ".*";
131  rss.getKnownRobotStates(rsregex.str(), robotStateNames);
132 
133  // Get goal constraints for scene
134  std::vector<std::string> constraintNames;
135 
136  std::stringstream csregex;
137  csregex << ".*" << scene_names[i] << ".*";
138  cs.getKnownConstraints(csregex.str(), constraintNames);
139 
140  if (!(robotStateNames.empty() && constraintNames.empty()))
141  {
142  std::ofstream qfout((scene_names[i] + ".queries").c_str());
143  qfout << scene_names[i] << std::endl;
144  if (robotStateNames.size())
145  {
146  qfout << "start" << std::endl;
147  qfout << robotStateNames.size() << std::endl;
148  for (std::size_t k = 0; k < robotStateNames.size(); ++k)
149  {
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;
153  rss.getRobotState(robotState, robotStateNames[k]);
154  robot_state::RobotState ks(km);
155  robot_state::robotStateMsgToRobotState(*robotState, ks, false);
156  ks.printStateInfo(qfout);
157  qfout << "." << std::endl;
158  }
159  }
160 
161  if (constraintNames.size())
162  {
163  qfout << "goal" << std::endl;
164  qfout << constraintNames.size() << std::endl;
165  for (std::size_t k = 0; k < constraintNames.size(); ++k)
166  {
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;
171  cs.getConstraints(constraints, constraintNames[k]);
172 
173  LinkConstraintMap lcmap;
174  collectLinkConstraints(*constraints, lcmap);
175  for (LinkConstraintMap::iterator iter = lcmap.begin(); iter != lcmap.end(); iter++)
176  {
177  std::string link_name = iter->first;
178  LinkConstraintPair lcp = iter->second;
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;
184  }
185  qfout << "." << std::endl;
186  }
187  }
188  qfout.close();
189  }
190  }
191  }
192 
193  ROS_INFO("Done.");
194 
195  return 0;
196 }
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
#define ROS_WARN(...)
void spinner()
const planning_scene::PlanningScenePtr & getPlanningScene()
warehouse_ros::DatabaseConnection::Ptr loadDatabase()
Load a database connection.
#define ROS_INFO(...)
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.


warehouse
Author(s): Ioan Sucan
autogenerated on Sun Oct 18 2020 13:18:37