generate_state_database.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * Copyright (c) 2018, Michael 'v4hn' Goerner
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Authors: Ioan Sucan, Michael Goerner */
37 
38 #include <tf/transform_datatypes.h>
42 
44 
46 
47 #include <boost/math/constants/constants.hpp>
48 #include <sstream>
49 
50 static const std::string LOGNAME = "generate_state_database";
51 
52 static const std::string ROBOT_DESCRIPTION = "robot_description";
53 
54 static const std::string CONSTRAINT_PARAMETER = "constraints";
55 
57 {
59  {
60  use_current_scene = nh.param("use_current_scene", false);
61 
62  // number of states in joint space approximation
63  construction_opts.samples = nh.param("state_cnt", 10000);
64 
65  // generate edges together with states?
66  construction_opts.edges_per_sample = nh.param("edges_per_sample", 0);
67  construction_opts.max_edge_length = nh.param("max_edge_length", 0.2);
68 
69  // verify constraint validity on edges
70  construction_opts.explicit_motions = nh.param("explicit_motions", true);
71  construction_opts.explicit_points_resolution = nh.param("explicit_points_resolution", 0.05);
72  construction_opts.max_explicit_points = nh.param("max_explicit_points", 200);
73 
74  // local planning in JointModel state space
76  nh.param<std::string>("state_space_parameterization", "JointModel");
77 
78  output_folder = nh.param<std::string>("output_folder", "constraint_approximations_database");
79 
80  if (!nh.getParam("planning_group", planning_group))
81  {
82  ROS_FATAL_NAMED(LOGNAME, "~planning_group parameter has to be specified.");
83  return false;
84  }
85 
86  XmlRpc::XmlRpcValue constraint_description;
87  if (!nh.getParam(CONSTRAINT_PARAMETER, constraint_description) ||
89  {
91  "Could not find valid constraint description in parameter '"
93  << "'. "
94  "Please upload correct correct constraint description or remap the parameter.");
95  return false;
96  }
97 
98  return true;
99  };
100 
101  std::string planning_group;
102 
103  // path to folder for generated database
104  std::string output_folder;
105 
106  // request the current scene via get_planning_scene service
108 
109  // constraints the approximation should satisfy
110  moveit_msgs::Constraints constraints;
111 
112  // internal parameters of approximation generator
114 };
115 
116 void computeDB(const planning_scene::PlanningScenePtr& scene, struct GenerateStateDatabaseParameters& params)
117 {
118  // required by addConstraintApproximation
119  scene->getCurrentStateNonConst().update();
120 
121  ompl_interface::OMPLInterface ompl_interface(scene->getRobotModel());
122 
123  ROS_INFO_STREAM_NAMED(LOGNAME, "Generating Joint Space Constraint Approximation Database for constraint:\n"
124  << params.constraints);
125 
127  ompl_interface.getConstraintsLibrary().addConstraintApproximation(params.constraints, params.planning_group,
128  scene, params.construction_opts);
129 
130  if (!result.approx)
131  {
132  ROS_FATAL_NAMED(LOGNAME, "Failed to generate approximation.");
133  return;
134  }
135  ompl_interface.getConstraintsLibrary().saveConstraintApproximations(params.output_folder);
136  ROS_INFO_STREAM_NAMED(LOGNAME,
137  "Successfully generated Joint Space Constraint Approximation Database for constraint:\n"
138  << params.constraints);
139  ROS_INFO_STREAM_NAMED(LOGNAME, "The database has been saved in your local folder '" << params.output_folder << "'");
140 }
141 
142 int main(int argc, char** argv)
143 {
144  ros::init(argc, argv, "construct_tool_constraint_database", ros::init_options::AnonymousName);
145 
147  spinner.start();
148 
149  ros::NodeHandle nh("~");
150 
152  if (!params.setFromHandle(nh))
153  return 1;
154 
156  if (!psm.getRobotModel())
157  return 1;
158 
159  if (params.use_current_scene)
160  {
161  ROS_INFO_NAMED(LOGNAME, "Requesting current planning scene to generate database");
162  if (!psm.requestPlanningSceneState())
163  {
164  ROS_FATAL_NAMED(LOGNAME, "Abort. The current scene could not be retrieved.");
165  return 1;
166  }
167  }
168 
170  {
171  ROS_FATAL_NAMED(LOGNAME, "Abort. Constraint description is an empty set of constraints.");
172  return 1;
173  }
174 
175  computeDB(psm.getPlanningScene(), params);
176 
177  return 0;
178 }
bool isEmpty(const moveit_msgs::Constraints &constr)
#define ROS_INFO_NAMED(name,...)
void computeDB(const planning_scene::PlanningScenePtr &scene, struct GenerateStateDatabaseParameters &params)
std::string resolveName(const std::string &name, bool remap=true) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
const robot_model::RobotModelConstPtr & getRobotModel() const
#define ROS_INFO_STREAM_NAMED(name, args)
The MoveIt! interface to OMPL.
void spinner()
const planning_scene::PlanningScenePtr & getPlanningScene()
static const std::string ROBOT_DESCRIPTION
ompl_interface::ConstraintApproximationConstructionOptions construction_opts
#define ROS_FATAL_STREAM_NAMED(name, args)
static const std::string CONSTRAINT_PARAMETER
bool param(const std::string &param_name, T &param_val, const T &default_val) const
bool setFromHandle(ros::NodeHandle &nh)
bool constructConstraints(XmlRpc::XmlRpcValue &params, moveit_msgs::Constraints &constraints)
static const std::string LOGNAME
bool getParam(const std::string &key, std::string &s) const
#define ROS_FATAL_NAMED(name,...)
bool requestPlanningSceneState(const std::string &service_name=DEFAULT_PLANNING_SCENE_SERVICE)
int main(int argc, char **argv)


ompl
Author(s): Ioan Sucan
autogenerated on Sun Sep 15 2019 03:58:41