collisions_updater.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, Fraunhofer IPA
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 Fraunhofer IPA 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: Mathias Lüdtke */
36 
37 #include <ros/ros.h>
40 
41 #include <boost/program_options.hpp>
42 
43 namespace po = boost::program_options;
44 
45 bool loadSetupAssistantConfig(moveit_setup_assistant::MoveItConfigData& config_data, const std::string& pkg_path)
46 {
47  if (!config_data.setPackagePath(pkg_path))
48  {
49  ROS_ERROR_STREAM("Could not set package path '" << pkg_path << "'");
50  return false;
51  }
52 
53  std::string setup_assistant_path;
54  if (!config_data.getSetupAssistantYAMLPath(setup_assistant_path))
55  {
56  ROS_ERROR_STREAM("Could not resolve path to .setup_assistant");
57  return false;
58  }
59 
60  if (!config_data.inputSetupAssistantYAML(setup_assistant_path))
61  {
62  ROS_ERROR_STREAM("Could not parse .setup_assistant file from '" << setup_assistant_path << "'");
63  return false;
64  }
65 
66  config_data.createFullURDFPath(); // might fail at this point
67 
68  config_data.createFullSRDFPath(config_data.config_pkg_path_); // might fail at this point
69 
70  return true;
71 }
72 
73 bool setup(moveit_setup_assistant::MoveItConfigData& config_data, bool keep_old,
74  const std::vector<std::string>& xacro_args)
75 {
76  std::string urdf_string;
77  if (!rdf_loader::RDFLoader::loadXmlFileToString(urdf_string, config_data.urdf_path_, xacro_args))
78  {
79  ROS_ERROR_STREAM("Could not load URDF from '" << config_data.urdf_path_ << "'");
80  return false;
81  }
82  if (!config_data.urdf_model_->initString(urdf_string))
83  {
84  ROS_ERROR_STREAM("Could not parse URDF from '" << config_data.urdf_path_ << "'");
85  return false;
86  }
87 
88  std::string srdf_string;
89  if (!rdf_loader::RDFLoader::loadXmlFileToString(srdf_string, config_data.srdf_path_, xacro_args))
90  {
91  ROS_ERROR_STREAM("Could not load SRDF from '" << config_data.srdf_path_ << "'");
92  return false;
93  }
94  if (!config_data.srdf_->initString(*config_data.urdf_model_, srdf_string))
95  {
96  ROS_ERROR_STREAM("Could not parse SRDF from '" << config_data.srdf_path_ << "'");
97  return false;
98  }
99 
100  if (!keep_old)
101  config_data.srdf_->disabled_collisions_.clear();
102 
103  return true;
104 }
105 
107  double min_collision_fraction, bool verbose)
108 {
109  // TODO: spin thread and print progess if verbose
110  unsigned int collision_progress;
111  return moveit_setup_assistant::computeDefaultCollisions(config_data.getPlanningScene(), &collision_progress,
112  trials > 0, trials, min_collision_fraction, verbose);
113 }
114 
115 int main(int argc, char* argv[])
116 {
117  std::string config_pkg_path;
118  std::string urdf_path;
119  std::string srdf_path;
120 
121  std::string output_path;
122 
123  bool include_default = false, include_always = false, keep_old = false, verbose = false;
124 
125  double min_collision_fraction = 1.0;
126 
127  uint32_t never_trials = 0;
128 
129  po::options_description desc("Allowed options");
130  desc.add_options()("help", "show help")("config-pkg", po::value(&config_pkg_path), "path to moveit config package")(
131  "urdf", po::value(&urdf_path),
132  "path to URDF ( or xacro)")("srdf", po::value(&srdf_path),
133  "path to SRDF ( or xacro)")("output", po::value(&output_path), "output path for SRDF")
134 
135  ("xacro-args", po::value<std::vector<std::string> >()->composing(), "additional arguments for xacro")
136 
137  ("default", po::bool_switch(&include_default), "disable default colliding pairs")(
138  "always", po::bool_switch(&include_always), "disable always colliding pairs")
139 
140  ("keep", po::bool_switch(&keep_old), "keep disabled link from SRDF")("verbose", po::bool_switch(&verbose),
141  "verbose output")
142 
143  ("trials", po::value(&never_trials), "number of trials for searching never colliding pairs")(
144  "min-collision-fraction", po::value(&min_collision_fraction),
145  "fraction of small sample size to determine links that are alwas colliding");
146 
147  po::positional_options_description pos_desc;
148  pos_desc.add("xacro-args", -1);
149 
150  po::variables_map vm;
151  po::store(po::command_line_parser(argc, argv).options(desc).positional(pos_desc).run(), vm);
152  po::notify(vm);
153 
154  if (vm.count("help"))
155  {
156  std::cout << desc << std::endl;
157  return 1;
158  }
159 
161 
162  if (!config_pkg_path.empty())
163  {
164  if (!loadSetupAssistantConfig(config_data, config_pkg_path))
165  {
166  ROS_ERROR_STREAM("Could not load config at '" << config_pkg_path << "'");
167  return 1;
168  }
169  }
170  else if (urdf_path.empty() || srdf_path.empty())
171  {
172  ROS_ERROR_STREAM("Please provide config package or URDF and SRDF path");
173  return 1;
174  }
175  else if (rdf_loader::RDFLoader::isXacroFile(srdf_path) && output_path.empty())
176  {
177  ROS_ERROR_STREAM("Please provide a different output file for SRDF xacro input file");
178  return 1;
179  }
180 
181  // overwrite config paths if applicable
182  if (!urdf_path.empty())
183  config_data.urdf_path_ = urdf_path;
184  if (!srdf_path.empty())
185  config_data.srdf_path_ = srdf_path;
186 
187  std::vector<std::string> xacro_args;
188  if (vm.count("xacro-args"))
189  xacro_args = vm["xacro-args"].as<std::vector<std::string> >();
190 
191  if (!setup(config_data, keep_old, xacro_args))
192  {
193  ROS_ERROR_STREAM("Could not setup updater");
194  return 1;
195  }
196 
197  moveit_setup_assistant::LinkPairMap link_pairs = compute(config_data, never_trials, min_collision_fraction, verbose);
198 
199  size_t skip_mask = 0;
200  if (!include_default)
201  skip_mask |= (1 << moveit_setup_assistant::DEFAULT);
202  if (!include_always)
203  skip_mask |= (1 << moveit_setup_assistant::ALWAYS);
204 
205  config_data.setCollisionLinkPairs(link_pairs, skip_mask);
206 
207  config_data.srdf_->writeSRDF(output_path.empty() ? config_data.srdf_path_ : output_path);
208 
209  return 0;
210 }
bool verbose
int main(int argc, char *argv[])
desc
urdf::ModelSharedPtr urdf_model_
URDF robot model.
bool setPackagePath(const std::string &pkg_path)
bool inputSetupAssistantYAML(const std::string &file_path)
bool createFullURDFPath()
Make the full URDF path using the loaded .setup_assistant data.
static bool isXacroFile(const std::string &path)
options
planning_scene::PlanningScenePtr getPlanningScene()
Provide a shared planning scene.
srdf::SRDFWriterPtr srdf_
SRDF Data and Writer.
void setCollisionLinkPairs(const moveit_setup_assistant::LinkPairMap &link_pairs, size_t skip_mask=0)
Set list of collision link pairs in SRDF; sorted; with optional filter.
std::string srdf_path_
Full file-system path to srdf.
bool setup(moveit_setup_assistant::MoveItConfigData &config_data, bool keep_old, const std::vector< std::string > &xacro_args)
bool createFullSRDFPath(const std::string &package_path)
Make the full SRDF path using the loaded .setup_assistant data.
static bool loadXmlFileToString(std::string &buffer, const std::string &path, const std::vector< std::string > &xacro_args)
LinkPairMap computeDefaultCollisions(const planning_scene::PlanningSceneConstPtr &parent_scene, unsigned int *progress, const bool include_never_colliding, const unsigned int trials, const double min_collision_faction, const bool verbose)
Generate an adjacency list of links that are always and never in collision, to speed up collision det...
This class is shared with all widgets and contains the common configuration data needed for generatin...
#define ROS_ERROR_STREAM(args)
bool loadSetupAssistantConfig(moveit_setup_assistant::MoveItConfigData &config_data, const std::string &pkg_path)
std::string urdf_path_
Full file-system path to urdf.
void run(ClassLoader *loader)
moveit_setup_assistant::LinkPairMap compute(moveit_setup_assistant::MoveItConfigData &config_data, uint32_t trials, double min_collision_fraction, bool verbose)
std::string config_pkg_path_
Loaded configuration package path - if an existing package was loaded, holds that path...
std::map< std::pair< std::string, std::string >, LinkPairData > LinkPairMap
LinkPairMap is an adjacency list structure containing links in string-based form. Used for disabled l...


moveit_setup_assistant
Author(s): Dave Coleman
autogenerated on Sun Oct 18 2020 13:19:28