collisions_updater.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015, Fraunhofer IPA
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Fraunhofer IPA nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Mathias Lüdtke */
00036 
00037 #include <ros/ros.h>
00038 #include <moveit/setup_assistant/tools/moveit_config_data.h>
00039 #include <moveit/setup_assistant/tools/file_loader.h>
00040 
00041 #include <boost/program_options.hpp>
00042 
00043 namespace po = boost::program_options;
00044 
00045 bool loadSetupAssistantConfig(moveit_setup_assistant::MoveItConfigData& config_data, const std::string& pkg_path)
00046 {
00047   if (!config_data.setPackagePath(pkg_path))
00048   {
00049     ROS_ERROR_STREAM("Could not set package path '" << pkg_path << "'");
00050     return false;
00051   }
00052 
00053   std::string setup_assistant_path;
00054   if (!config_data.getSetupAssistantYAMLPath(setup_assistant_path))
00055   {
00056     ROS_ERROR_STREAM("Could not resolve path to .setup_assistant");
00057     return false;
00058   }
00059 
00060   if (!config_data.inputSetupAssistantYAML(setup_assistant_path))
00061   {
00062     ROS_ERROR_STREAM("Could not parse .setup_assistant file from '" << setup_assistant_path << "'");
00063     return false;
00064   }
00065 
00066   config_data.createFullURDFPath();  // might fail at this point
00067 
00068   config_data.createFullSRDFPath(config_data.config_pkg_path_);  // might fail at this point
00069 
00070   return true;
00071 }
00072 
00073 bool setup(moveit_setup_assistant::MoveItConfigData& config_data, bool keep_old,
00074            const std::vector<std::string>& xacro_args)
00075 {
00076   std::string urdf_string;
00077   if (!moveit_setup_assistant::loadXmlFileToString(urdf_string, config_data.urdf_path_, xacro_args))
00078   {
00079     ROS_ERROR_STREAM("Could not load URDF from '" << config_data.urdf_path_ << "'");
00080     return false;
00081   }
00082   if (!config_data.urdf_model_->initString(urdf_string))
00083   {
00084     ROS_ERROR_STREAM("Could not parse URDF from '" << config_data.urdf_path_ << "'");
00085     return false;
00086   }
00087 
00088   std::string srdf_string;
00089   if (!moveit_setup_assistant::loadXmlFileToString(srdf_string, config_data.srdf_path_, xacro_args))
00090   {
00091     ROS_ERROR_STREAM("Could not load SRDF from '" << config_data.srdf_path_ << "'");
00092     return false;
00093   }
00094   if (!config_data.srdf_->initString(*config_data.urdf_model_, srdf_string))
00095   {
00096     ROS_ERROR_STREAM("Could not parse SRDF from '" << config_data.srdf_path_ << "'");
00097     return false;
00098   }
00099 
00100   if (!keep_old)
00101     config_data.srdf_->disabled_collisions_.clear();
00102 
00103   return true;
00104 }
00105 
00106 moveit_setup_assistant::LinkPairMap compute(moveit_setup_assistant::MoveItConfigData& config_data, uint32_t trials,
00107                                             double min_collision_fraction, bool verbose)
00108 {
00109   // TODO: spin thread and print progess if verbose
00110   unsigned int collision_progress;
00111   return moveit_setup_assistant::computeDefaultCollisions(config_data.getPlanningScene(), &collision_progress,
00112                                                           trials > 0, trials, min_collision_fraction, verbose);
00113 }
00114 
00115 int main(int argc, char* argv[])
00116 {
00117   std::string config_pkg_path;
00118   std::string urdf_path;
00119   std::string srdf_path;
00120 
00121   std::string output_path;
00122 
00123   bool include_default = false, include_always = false, keep_old = false, verbose = false;
00124 
00125   double min_collision_fraction = 1.0;
00126 
00127   uint32_t never_trials = 0;
00128 
00129   po::options_description desc("Allowed options");
00130   desc.add_options()("help", "show help")("config-pkg", po::value(&config_pkg_path), "path to moveit config package")(
00131       "urdf", po::value(&urdf_path),
00132       "path to URDF ( or xacro)")("srdf", po::value(&srdf_path),
00133                                   "path to SRDF ( or xacro)")("output", po::value(&output_path), "output path for SRDF")
00134 
00135       ("xacro-args", po::value<std::vector<std::string> >()->composing(), "additional arguments for xacro")
00136 
00137           ("default", po::bool_switch(&include_default), "disable default colliding pairs")(
00138               "always", po::bool_switch(&include_always), "disable always colliding pairs")
00139 
00140               ("keep", po::bool_switch(&keep_old), "keep disabled link from SRDF")("verbose", po::bool_switch(&verbose),
00141                                                                                    "verbose output")
00142 
00143                   ("trials", po::value(&never_trials), "number of trials for searching never colliding pairs")(
00144                       "min-collision-fraction", po::value(&min_collision_fraction),
00145                       "fraction of small sample size to determine links that are alwas colliding");
00146 
00147   po::positional_options_description pos_desc;
00148   pos_desc.add("xacro-args", -1);
00149 
00150   po::variables_map vm;
00151   po::store(po::command_line_parser(argc, argv).options(desc).positional(pos_desc).run(), vm);
00152   po::notify(vm);
00153 
00154   if (vm.count("help"))
00155   {
00156     std::cout << desc << std::endl;
00157     return 1;
00158   }
00159 
00160   moveit_setup_assistant::MoveItConfigData config_data;
00161 
00162   if (!config_pkg_path.empty())
00163   {
00164     if (!loadSetupAssistantConfig(config_data, config_pkg_path))
00165     {
00166       ROS_ERROR_STREAM("Could not load config at '" << config_pkg_path << "'");
00167       return 1;
00168     }
00169   }
00170   else if (urdf_path.empty() || srdf_path.empty())
00171   {
00172     ROS_ERROR_STREAM("Please provide config package or URDF and SRDF path");
00173     return 1;
00174   }
00175   else if (moveit_setup_assistant::isXacroFile(srdf_path) && output_path.empty())
00176   {
00177     ROS_ERROR_STREAM("Please provide a different output file for SRDF xacro input file");
00178     return 1;
00179   }
00180 
00181   // overwrite config paths if applicable
00182   if (!urdf_path.empty())
00183     config_data.urdf_path_ = urdf_path;
00184   if (!srdf_path.empty())
00185     config_data.srdf_path_ = srdf_path;
00186 
00187   std::vector<std::string> xacro_args;
00188   if (vm.count("xacro-args"))
00189     xacro_args = vm["xacro-args"].as<std::vector<std::string> >();
00190 
00191   if (!setup(config_data, keep_old, xacro_args))
00192   {
00193     ROS_ERROR_STREAM("Could not setup updater");
00194     return 1;
00195   }
00196 
00197   moveit_setup_assistant::LinkPairMap link_pairs = compute(config_data, never_trials, min_collision_fraction, verbose);
00198 
00199   size_t skip_mask = 0;
00200   if (!include_default)
00201     skip_mask |= (1 << moveit_setup_assistant::DEFAULT);
00202   if (!include_always)
00203     skip_mask |= (1 << moveit_setup_assistant::ALWAYS);
00204 
00205   config_data.setCollisionLinkPairs(link_pairs, skip_mask);
00206 
00207   config_data.srdf_->writeSRDF(output_path.empty() ? config_data.srdf_path_ : output_path);
00208 
00209   return 0;
00210 }


moveit_setup_assistant
Author(s): Dave Coleman
autogenerated on Mon Jul 24 2017 02:22:29