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>
47 #include <ros/ros.h>
48 
49 static const std::string ROBOT_DESCRIPTION = "robot_description";
50 
51 typedef std::pair<geometry_msgs::Point, geometry_msgs::Quaternion> LinkConstraintPair;
52 typedef std::map<std::string, LinkConstraintPair> LinkConstraintMap;
53 
54 void collectLinkConstraints(const moveit_msgs::Constraints& constraints, LinkConstraintMap& lcmap)
55 {
56  for (const moveit_msgs::PositionConstraint& position_constraint : constraints.position_constraints)
57  {
59  const moveit_msgs::PositionConstraint& pc = position_constraint;
60  lcp.first = pc.constraint_region.primitive_poses[0].position;
61  lcmap[position_constraint.link_name] = lcp;
62  }
63 
64  for (const moveit_msgs::OrientationConstraint& orientation_constraint : constraints.orientation_constraints)
65  {
66  if (lcmap.count(orientation_constraint.link_name))
67  {
68  lcmap[orientation_constraint.link_name].second = orientation_constraint.orientation;
69  }
70  else
71  {
72  ROS_WARN("Orientation constraint for %s has no matching position constraint",
73  orientation_constraint.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
98  auto db_loader = std::make_unique<warehouse_ros::DatabaseLoader>();
99  warehouse_ros::DatabaseConnection::Ptr conn = db_loader->loadDatabase();
100  if (vm.count("host") && vm.count("port"))
101  conn->setParams(vm["host"].as<std::string>(), vm["port"].as<std::size_t>());
102  if (!conn->connect())
103  return 1;
104 
106  spinner.start();
107 
109 
113 
114  std::vector<std::string> scene_names;
115  pss.getPlanningSceneNames(scene_names);
116 
117  for (const std::string& scene_name : scene_names)
118  {
120  if (pss.getPlanningScene(pswm, scene_name))
121  {
122  ROS_INFO("Saving scene '%s'", scene_name.c_str());
123  psm.getPlanningScene()->setPlanningSceneMsg(static_cast<const moveit_msgs::PlanningScene&>(*pswm));
124  std::ofstream fout((scene_name + ".scene").c_str());
125  psm.getPlanningScene()->saveGeometryToStream(fout);
126  fout.close();
127 
128  std::vector<std::string> robot_state_names;
129  moveit::core::RobotModelConstPtr km = psm.getRobotModel();
130  // Get start states for scene
131  std::stringstream rsregex;
132  rsregex << ".*" << scene_name << ".*";
133  rss.getKnownRobotStates(rsregex.str(), robot_state_names);
134 
135  // Get goal constraints for scene
136  std::vector<std::string> constraint_names;
137 
138  std::stringstream csregex;
139  csregex << ".*" << scene_name << ".*";
140  cs.getKnownConstraints(csregex.str(), constraint_names);
141 
142  if (!(robot_state_names.empty() && constraint_names.empty()))
143  {
144  std::ofstream qfout((scene_name + ".queries").c_str());
145  qfout << scene_name << std::endl;
146  if (!robot_state_names.empty())
147  {
148  qfout << "start" << std::endl;
149  qfout << robot_state_names.size() << std::endl;
150  for (const std::string& robot_state_name : robot_state_names)
151  {
152  ROS_INFO("Saving start state %s for scene %s", robot_state_name.c_str(), scene_name.c_str());
153  qfout << robot_state_name << std::endl;
155  rss.getRobotState(robot_state, robot_state_name);
157  moveit::core::robotStateMsgToRobotState(*robot_state, ks, false);
158  ks.printStateInfo(qfout);
159  qfout << "." << std::endl;
160  }
161  }
162 
163  if (!constraint_names.empty())
164  {
165  qfout << "goal" << std::endl;
166  qfout << constraint_names.size() << std::endl;
167  for (const std::string& constraint_name : constraint_names)
168  {
169  ROS_INFO("Saving goal %s for scene %s", constraint_name.c_str(), scene_name.c_str());
170  qfout << "link_constraint" << std::endl;
171  qfout << constraint_name << std::endl;
173  cs.getConstraints(constraints, constraint_name);
174 
175  LinkConstraintMap lcmap;
176  collectLinkConstraints(*constraints, lcmap);
177  for (std::pair<const std::string, LinkConstraintPair>& iter : lcmap)
178  {
179  std::string link_name = iter.first;
180  LinkConstraintPair lcp = iter.second;
181  qfout << link_name << std::endl;
182  qfout << "xyz " << lcp.first.x << " " << lcp.first.y << " " << lcp.first.z << std::endl;
183  Eigen::Quaterniond orientation(lcp.second.w, lcp.second.x, lcp.second.y, lcp.second.z);
184  Eigen::Vector3d rpy = orientation.matrix().eulerAngles(0, 1, 2);
185  qfout << "rpy " << rpy[0] << " " << rpy[1] << " " << rpy[2] << std::endl;
186  }
187  qfout << "." << std::endl;
188  }
189  }
190  qfout.close();
191  }
192  }
193  }
194 
195  ROS_INFO("Done.");
196 
197  return 0;
198 }
ros::init_options::AnonymousName
AnonymousName
moveit_warehouse::PlanningSceneStorage::getPlanningScene
bool getPlanningScene(PlanningSceneWithMetadata &scene_m, const std::string &scene_name) const
Get the latest planning scene named scene_name.
Definition: planning_scene_storage.cpp:217
ROBOT_DESCRIPTION
static const std::string ROBOT_DESCRIPTION
Definition: save_as_text.cpp:49
boost::shared_ptr
moveit_warehouse::RobotStateStorage
Definition: state_storage.h:82
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
ros.h
state_storage.h
LinkConstraintMap
std::map< std::string, LinkConstraintPair > LinkConstraintMap
Definition: save_as_text.cpp:52
ros::AsyncSpinner
planning_scene_monitor::PlanningSceneMonitor::getPlanningScene
const planning_scene::PlanningScenePtr & getPlanningScene()
moveit::core::RobotState
moveit_warehouse::ConstraintsStorage
Definition: constraints_storage.h:82
moveit_warehouse::ConstraintsStorage::getKnownConstraints
void getKnownConstraints(std::vector< std::string > &names, const std::string &robot="", const std::string &group="") const
Definition: constraints_storage.cpp:106
planning_scene_monitor::PlanningSceneMonitor
database_loader.h
spinner
void spinner()
collectLinkConstraints
void collectLinkConstraints(const moveit_msgs::Constraints &constraints, LinkConstraintMap &lcmap)
Definition: save_as_text.cpp:54
planning_scene_monitor::PlanningSceneMonitor::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
planning_scene_monitor.h
planning_scene_storage.h
constraints_storage.h
ROS_WARN
#define ROS_WARN(...)
LinkConstraintPair
std::pair< geometry_msgs::Point, geometry_msgs::Quaternion > LinkConstraintPair
Definition: save_as_text.cpp:51
moveit_warehouse::PlanningSceneStorage
Definition: planning_scene_storage.h:89
moveit_warehouse::RobotStateStorage::getKnownRobotStates
void getKnownRobotStates(std::vector< std::string > &names, const std::string &robot="") const
Definition: state_storage.cpp:100
main
int main(int argc, char **argv)
Definition: save_as_text.cpp:78
moveit_warehouse::ConstraintsStorage::getConstraints
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.
Definition: constraints_storage.cpp:121
moveit_warehouse::PlanningSceneStorage::getPlanningSceneNames
void getPlanningSceneNames(std::vector< std::string > &names) const
Definition: planning_scene_storage.cpp:186
conversions.h
ROS_INFO
#define ROS_INFO(...)
moveit_warehouse::RobotStateStorage::getRobotState
bool getRobotState(RobotStateWithMetadata &msg_m, const std::string &name, const std::string &robot="") const
Get the constraints named name. Return false on failure.
Definition: state_storage.cpp:113
moveit::core::RobotState::printStateInfo
void printStateInfo(std::ostream &out=std::cout) const
moveit::core::robotStateMsgToRobotState
bool robotStateMsgToRobotState(const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)


warehouse
Author(s): Ioan Sucan
autogenerated on Mon May 27 2024 02:28:07