moveit_config_data.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
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 Willow Garage 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: Dave Coleman */
00036 
00037 #include <moveit/setup_assistant/tools/moveit_config_data.h>
00038 // Reading/Writing Files
00039 #include <iostream>  // For writing yaml and launch files
00040 #include <fstream>
00041 #include <yaml-cpp/yaml.h>             // outputing yaml config files
00042 #include <boost/filesystem.hpp>        // for creating folders/files
00043 #include <boost/algorithm/string.hpp>  // for string find and replace in templates
00044 
00045 #ifdef HAVE_NEW_YAMLCPP
00046 #include <boost/optional.hpp>  // optional
00047 #endif
00048 
00049 // ROS
00050 #include <ros/console.h>
00051 #include <ros/package.h>  // for getting file path for loading images
00052 
00053 #ifdef HAVE_NEW_YAMLCPP
00054 namespace YAML
00055 {
00056 // Create a legacy Iterator that can be used with the yaml-cpp 0.3 API.
00057 class Iterator
00058 {
00059 public:
00060   typedef YAML::iterator iterator_t;
00061   typedef YAML::const_iterator const_iterator_t;
00062 
00063   Iterator(iterator_t iter) : iter_(iter)
00064   {
00065   }
00066 
00067   const Node& first() const
00068   {
00069     return iter_->first;
00070   }
00071 
00072   const Node& second() const
00073   {
00074     return iter_->second;
00075   }
00076 
00077   detail::iterator_value operator*()
00078   {
00079     return *iter_;
00080   }
00081 
00082   Iterator operator++()
00083   {
00084     return Iterator(++iter_);
00085   }
00086 
00087   bool operator==(iterator_t const& other) const
00088   {
00089     return iter_ == other;
00090   }
00091 
00092   bool operator!=(iterator_t const& other) const
00093   {
00094     return iter_ != other;
00095   }
00096 
00097 private:
00098   iterator_t iter_;
00099 };
00100 }
00101 #endif
00102 
00103 namespace moveit_setup_assistant
00104 {
00105 // File system
00106 namespace fs = boost::filesystem;
00107 
00108 #ifdef HAVE_NEW_YAMLCPP
00109 typedef boost::optional<YAML::Node> yaml_node_t;
00110 
00111 // Helper function to find a value (yaml-cpp 0.5)
00112 template <typename T>
00113 yaml_node_t findValue(const YAML::Node& node, const T& key)
00114 {
00115   if (node[key])
00116     return node[key];
00117   return yaml_node_t();
00118 }
00119 
00120 // The >> operator disappeared in yaml-cpp 0.5, so this function is
00121 // added to provide support for code written under the yaml-cpp 0.3 API.
00122 template <typename T>
00123 void operator>>(const YAML::Node& node, T& i)
00124 {
00125   i = node.as<T>();
00126 }
00127 
00128 #else
00129 typedef const YAML::Node* yaml_node_t;
00130 
00131 // Helper function to find a value (yaml-cpp 0.3)
00132 template <typename T>
00133 yaml_node_t findValue(const YAML::Node& node, const T& key)
00134 {
00135   return node.FindValue(key);
00136 }
00137 #endif
00138 
00139 // yaml-cpp 0.5 also changed how you load the YAML document.  This
00140 // function hides the changes.
00141 void loadYaml(std::istream& in_stream, YAML::Node& doc_out)
00142 {
00143 #ifdef HAVE_NEW_YAMLCPP
00144   doc_out = YAML::Load(in_stream);
00145 #else
00146   YAML::Parser parser(in_stream);
00147   parser.GetNextDocument(doc_out);
00148 #endif
00149 }
00150 
00151 // ******************************************************************************************
00152 // Constructor
00153 // ******************************************************************************************
00154 MoveItConfigData::MoveItConfigData() : config_pkg_generated_timestamp_(0), urdf_requires_jade_xacro_(false)
00155 {
00156   // Create an instance of SRDF writer and URDF model for all widgets to share
00157   srdf_.reset(new srdf::SRDFWriter());
00158   urdf_model_.reset(new urdf::Model());
00159 
00160   // Not in debug mode
00161   debug_ = false;
00162 
00163   // Get MoveIt Setup Assistant package path
00164   setup_assistant_path_ = ros::package::getPath("moveit_setup_assistant");
00165   if (setup_assistant_path_.empty())
00166   {
00167     setup_assistant_path_ = ".";
00168   }
00169 }
00170 
00171 // ******************************************************************************************
00172 // Destructor
00173 // ******************************************************************************************
00174 MoveItConfigData::~MoveItConfigData()
00175 {
00176 }
00177 
00178 // ******************************************************************************************
00179 // Provide a kinematic model. Load a new one if necessary
00180 // ******************************************************************************************
00181 robot_model::RobotModelConstPtr MoveItConfigData::getRobotModel()
00182 {
00183   if (!robot_model_)
00184   {
00185     // Initialize with a URDF Model Interface and a SRDF Model
00186     robot_model_.reset(new robot_model::RobotModel(urdf_model_, srdf_->srdf_model_));
00187     robot_model_const_ = robot_model_;
00188   }
00189 
00190   return robot_model_const_;
00191 }
00192 
00193 // ******************************************************************************************
00194 // Update the Kinematic Model with latest SRDF modifications
00195 // ******************************************************************************************
00196 void MoveItConfigData::updateRobotModel()
00197 {
00198   ROS_INFO("Updating kinematic model");
00199 
00200   // Tell SRDF Writer to create new SRDF Model, use original URDF model
00201   srdf_->updateSRDFModel(*urdf_model_);
00202 
00203   // Create new kin model
00204   robot_model_.reset(new robot_model::RobotModel(urdf_model_, srdf_->srdf_model_));
00205   robot_model_const_ = robot_model_;
00206 
00207   // Reset the planning scene
00208   planning_scene_.reset();
00209 }
00210 
00211 // ******************************************************************************************
00212 // Provide a shared planning scene
00213 // ******************************************************************************************
00214 planning_scene::PlanningScenePtr MoveItConfigData::getPlanningScene()
00215 {
00216   if (!planning_scene_)
00217   {
00218     // make sure kinematic model exists
00219     getRobotModel();
00220 
00221     // Allocate an empty planning scene
00222     planning_scene_.reset(new planning_scene::PlanningScene(robot_model_));
00223   }
00224   return planning_scene_;
00225 }
00226 
00227 // ******************************************************************************************
00228 // Load the allowed collision matrix from the SRDF's list of link pairs
00229 // ******************************************************************************************
00230 void MoveItConfigData::loadAllowedCollisionMatrix()
00231 {
00232   // Clear the allowed collision matrix
00233   allowed_collision_matrix_.clear();
00234 
00235   // Update the allowed collision matrix, in case there has been a change
00236   for (std::vector<srdf::Model::DisabledCollision>::const_iterator pair_it = srdf_->disabled_collisions_.begin();
00237        pair_it != srdf_->disabled_collisions_.end(); ++pair_it)
00238   {
00239     allowed_collision_matrix_.setEntry(pair_it->link1_, pair_it->link2_, true);
00240   }
00241 }
00242 
00243 // ******************************************************************************************
00244 // Output MoveIt Setup Assistant hidden settings file
00245 // ******************************************************************************************
00246 bool MoveItConfigData::outputSetupAssistantFile(const std::string& file_path)
00247 {
00248   YAML::Emitter emitter;
00249   emitter << YAML::BeginMap;
00250 
00251   // Output every available planner ---------------------------------------------------
00252   emitter << YAML::Key << "moveit_setup_assistant_config";
00253 
00254   emitter << YAML::Value << YAML::BeginMap;
00255 
00256   // URDF Path Location
00257   emitter << YAML::Key << "URDF";
00258   emitter << YAML::Value << YAML::BeginMap;
00259   emitter << YAML::Key << "package" << YAML::Value << urdf_pkg_name_;
00260   emitter << YAML::Key << "relative_path" << YAML::Value << urdf_pkg_relative_path_;
00261   if (urdf_requires_jade_xacro_)
00262     emitter << YAML::Key << "use_jade_xacro" << YAML::Value << urdf_requires_jade_xacro_;
00263   emitter << YAML::EndMap;
00264 
00266   emitter << YAML::Key << "SRDF";
00267   emitter << YAML::Value << YAML::BeginMap;
00268   emitter << YAML::Key << "relative_path" << YAML::Value << srdf_pkg_relative_path_;
00269   emitter << YAML::EndMap;
00270 
00272   emitter << YAML::Key << "CONFIG";
00273   emitter << YAML::Value << YAML::BeginMap;
00274   emitter << YAML::Key << "author_name" << YAML::Value << author_name_;
00275   emitter << YAML::Key << "author_email" << YAML::Value << author_email_;
00276   emitter << YAML::Key << "generated_timestamp" << YAML::Value << std::time(NULL);  // TODO: is this cross-platform?
00277   emitter << YAML::EndMap;
00278 
00279   emitter << YAML::EndMap;
00280 
00281   std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
00282   if (!output_stream.good())
00283   {
00284     ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
00285     return false;
00286   }
00287 
00288   output_stream << emitter.c_str();
00289   output_stream.close();
00290 
00291   return true;  // file created successfully
00292 }
00293 
00294 // ******************************************************************************************
00295 // Output OMPL Planning config files
00296 // ******************************************************************************************
00297 bool MoveItConfigData::outputOMPLPlanningYAML(const std::string& file_path)
00298 {
00299   YAML::Emitter emitter;
00300   emitter << YAML::BeginMap;
00301 
00302   // Output every available planner ---------------------------------------------------
00303   emitter << YAML::Key << "planner_configs";
00304 
00305   emitter << YAML::Value << YAML::BeginMap;
00306 
00307   std::vector<OMPLPlannerDescription> planner_des;
00308 
00309   OMPLPlannerDescription SBL("SBL", "geometric");
00310   SBL.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()");
00311   planner_des.push_back(SBL);
00312 
00313   OMPLPlannerDescription EST("EST", "geometric");
00314   EST.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()");
00315   EST.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05");
00316   planner_des.push_back(EST);
00317 
00318   OMPLPlannerDescription LBKPIECE("LBKPIECE", "geometric");
00319   LBKPIECE.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on "
00320                                         "setup()");
00321   LBKPIECE.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9");
00322   LBKPIECE.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5");
00323   planner_des.push_back(LBKPIECE);
00324 
00325   OMPLPlannerDescription BKPIECE("BKPIECE", "geometric");
00326   BKPIECE.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on "
00327                                        "setup()");
00328   BKPIECE.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9");
00329   BKPIECE.addParameter("failed_expansion_score_factor", "0.5", "When extending motion fails, scale score by factor. "
00330                                                                "default: 0.5");
00331   BKPIECE.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5");
00332   planner_des.push_back(BKPIECE);
00333 
00334   OMPLPlannerDescription KPIECE("KPIECE", "geometric");
00335   KPIECE.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on "
00336                                       "setup()");
00337   KPIECE.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05 ");
00338   KPIECE.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9 (0.0,1.]");
00339   KPIECE.addParameter("failed_expansion_score_factor", "0.5", "When extending motion fails, scale score by factor. "
00340                                                               "default: 0.5");
00341   KPIECE.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5");
00342   planner_des.push_back(KPIECE);
00343 
00344   OMPLPlannerDescription RRT("RRT", "geometric");
00345   RRT.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()");
00346   RRT.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05");
00347   planner_des.push_back(RRT);
00348 
00349   OMPLPlannerDescription RRTConnect("RRTConnect", "geometric");
00350   RRTConnect.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on "
00351                                           "setup()");
00352   planner_des.push_back(RRTConnect);
00353 
00354   OMPLPlannerDescription RRTstar("RRTstar", "geometric");
00355   RRTstar.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on "
00356                                        "setup()");
00357   RRTstar.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05");
00358   RRTstar.addParameter("delay_collision_checking", "1", "Stop collision checking as soon as C-free parent found. "
00359                                                         "default 1");
00360   planner_des.push_back(RRTstar);
00361 
00362   OMPLPlannerDescription TRRT("TRRT", "geometric");
00363   TRRT.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()");
00364   TRRT.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05");
00365   TRRT.addParameter("max_states_failed", "10", "when to start increasing temp. default: 10");
00366   TRRT.addParameter("temp_change_factor", "2.0", "how much to increase or decrease temp. default: 2.0");
00367   TRRT.addParameter("min_temperature", "10e-10", "lower limit of temp change. default: 10e-10");
00368   TRRT.addParameter("init_temperature", "10e-6", "initial temperature. default: 10e-6");
00369   TRRT.addParameter("frountier_threshold", "0.0", "dist new state to nearest neighbor to disqualify as frontier. "
00370                                                   "default: 0.0 set in setup() ");
00371   TRRT.addParameter("frountierNodeRatio", "0.1", "1/10, or 1 nonfrontier for every 10 frontier. default: 0.1");
00372   TRRT.addParameter("k_constant", "0.0", "value used to normalize expresssion. default: 0.0 set in setup()");
00373   planner_des.push_back(TRRT);
00374 
00375   OMPLPlannerDescription PRM("PRM", "geometric");
00376   PRM.addParameter("max_nearest_neighbors", "10", "use k nearest neighbors. default: 10");
00377   planner_des.push_back(PRM);
00378 
00379   OMPLPlannerDescription PRMstar("PRMstar", "geometric");  // no delcares in code
00380   planner_des.push_back(PRMstar);
00381 
00382   // Add Planners with parameter values
00383   std::vector<std::string> pconfigs;
00384   for (std::size_t i = 0; i < planner_des.size(); ++i)
00385   {
00386     std::string defaultconfig = planner_des[i].name_ + "kConfigDefault";
00387     emitter << YAML::Key << defaultconfig;
00388     emitter << YAML::Value << YAML::BeginMap;
00389     emitter << YAML::Key << "type" << YAML::Value << "geometric::" + planner_des[i].name_;
00390     for (std::size_t j = 0; j < planner_des[i].parameter_list_.size(); j++)
00391     {
00392       emitter << YAML::Key << planner_des[i].parameter_list_[j].name;
00393       emitter << YAML::Value << planner_des[i].parameter_list_[j].value;
00394       emitter << YAML::Comment(planner_des[i].parameter_list_[j].comment.c_str());
00395     }
00396     emitter << YAML::EndMap;
00397 
00398     pconfigs.push_back(defaultconfig);
00399   }
00400 
00401   // End of every avail planner
00402   emitter << YAML::EndMap;
00403 
00404   // Output every group and the planners it can use ----------------------------------
00405   for (std::vector<srdf::Model::Group>::iterator group_it = srdf_->groups_.begin(); group_it != srdf_->groups_.end();
00406        ++group_it)
00407   {
00408     emitter << YAML::Key << group_it->name_;
00409     emitter << YAML::Value << YAML::BeginMap;
00410     // Output associated planners
00411     emitter << YAML::Key << "planner_configs";
00412     emitter << YAML::Value << YAML::BeginSeq;
00413     for (std::size_t i = 0; i < pconfigs.size(); ++i)
00414       emitter << pconfigs[i];
00415     emitter << YAML::EndSeq;
00416 
00417     // Output projection_evaluator
00418     std::string projection_joints = decideProjectionJoints(group_it->name_);
00419     if (!projection_joints.empty())
00420     {
00421       emitter << YAML::Key << "projection_evaluator";
00422       emitter << YAML::Value << projection_joints;
00423       emitter << YAML::Key << "longest_valid_segment_fraction";
00424       emitter << YAML::Value << "0.05";
00425     }
00426 
00427     emitter << YAML::EndMap;
00428   }
00429 
00430   emitter << YAML::EndMap;
00431 
00432   std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
00433   if (!output_stream.good())
00434   {
00435     ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
00436     return false;
00437   }
00438 
00439   output_stream << emitter.c_str();
00440   output_stream.close();
00441 
00442   return true;  // file created successfully
00443 }
00444 
00445 // ******************************************************************************************
00446 // Output kinematic config files
00447 // ******************************************************************************************
00448 bool MoveItConfigData::outputKinematicsYAML(const std::string& file_path)
00449 {
00450   YAML::Emitter emitter;
00451   emitter << YAML::BeginMap;
00452 
00453   // Output every group and the kinematic solver it can use ----------------------------------
00454   for (std::vector<srdf::Model::Group>::iterator group_it = srdf_->groups_.begin(); group_it != srdf_->groups_.end();
00455        ++group_it)
00456   {
00457     // Only save kinematic data if the solver is not "None"
00458     if (group_meta_data_[group_it->name_].kinematics_solver_.empty() ||
00459         group_meta_data_[group_it->name_].kinematics_solver_ == "None")
00460       continue;
00461 
00462     emitter << YAML::Key << group_it->name_;
00463     emitter << YAML::Value << YAML::BeginMap;
00464 
00465     // Kinematic Solver
00466     emitter << YAML::Key << "kinematics_solver";
00467     emitter << YAML::Value << group_meta_data_[group_it->name_].kinematics_solver_;
00468 
00469     // Search Resolution
00470     emitter << YAML::Key << "kinematics_solver_search_resolution";
00471     emitter << YAML::Value << group_meta_data_[group_it->name_].kinematics_solver_search_resolution_;
00472 
00473     // Solver Timeout
00474     emitter << YAML::Key << "kinematics_solver_timeout";
00475     emitter << YAML::Value << group_meta_data_[group_it->name_].kinematics_solver_timeout_;
00476 
00477     // Solver Attempts
00478     emitter << YAML::Key << "kinematics_solver_attempts";
00479     emitter << YAML::Value << group_meta_data_[group_it->name_].kinematics_solver_attempts_;
00480 
00481     emitter << YAML::EndMap;
00482   }
00483 
00484   emitter << YAML::EndMap;
00485 
00486   std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
00487   if (!output_stream.good())
00488   {
00489     ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
00490     return false;
00491   }
00492 
00493   output_stream << emitter.c_str();
00494   output_stream.close();
00495 
00496   return true;  // file created successfully
00497 }
00498 
00499 bool MoveItConfigData::outputFakeControllersYAML(const std::string& file_path)
00500 {
00501   YAML::Emitter emitter;
00502   emitter << YAML::BeginMap;
00503 
00504   emitter << YAML::Key << "controller_list";
00505   emitter << YAML::Value << YAML::BeginSeq;
00506 
00507   // Union all the joints in groups
00508   std::set<const robot_model::JointModel*> joints;
00509 
00510   // Loop through groups
00511   for (std::vector<srdf::Model::Group>::iterator group_it = srdf_->groups_.begin(); group_it != srdf_->groups_.end();
00512        ++group_it)
00513   {
00514     // Get list of associated joints
00515     const robot_model::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group_it->name_);
00516     emitter << YAML::BeginMap;
00517     const std::vector<const robot_model::JointModel*>& joint_models = joint_model_group->getActiveJointModels();
00518     emitter << YAML::Key << "name";
00519     emitter << YAML::Value << "fake_" + group_it->name_ + "_controller";
00520     emitter << YAML::Key << "joints";
00521     emitter << YAML::Value << YAML::BeginSeq;
00522 
00523     // Iterate through the joints
00524     for (std::vector<const robot_model::JointModel*>::const_iterator joint_it = joint_models.begin();
00525          joint_it != joint_models.end(); ++joint_it)
00526     {
00527       if ((*joint_it)->isPassive() || (*joint_it)->getMimic() != NULL ||
00528           (*joint_it)->getType() == robot_model::JointModel::FIXED)
00529       {
00530         continue;
00531       }
00532       emitter << (*joint_it)->getName();
00533     }
00534     emitter << YAML::EndSeq;
00535     emitter << YAML::EndMap;
00536   }
00537   emitter << YAML::EndSeq;
00538   emitter << YAML::EndMap;
00539 
00540   std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
00541   if (!output_stream.good())
00542   {
00543     ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
00544     return false;
00545   }
00546 
00547   output_stream << emitter.c_str();
00548   output_stream.close();
00549 
00550   return true;  // file created successfully
00551 }
00552 
00553 // ******************************************************************************************
00554 // Output joint limits config files
00555 // ******************************************************************************************
00556 bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path)
00557 {
00558   YAML::Emitter emitter;
00559   emitter << YAML::BeginMap;
00560 
00561   emitter << YAML::Key << "joint_limits";
00562   emitter << YAML::Value << YAML::BeginMap;
00563 
00564   // Union all the joints in groups. Uses a custom comparator to allow the joints to be sorted by name
00565   std::set<const robot_model::JointModel*, joint_model_compare> joints;
00566 
00567   // Loop through groups
00568   for (std::vector<srdf::Model::Group>::iterator group_it = srdf_->groups_.begin(); group_it != srdf_->groups_.end();
00569        ++group_it)
00570   {
00571     // Get list of associated joints
00572     const robot_model::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group_it->name_);
00573 
00574     const std::vector<const robot_model::JointModel*>& joint_models = joint_model_group->getJointModels();
00575 
00576     // Iterate through the joints
00577     for (std::vector<const robot_model::JointModel*>::const_iterator joint_it = joint_models.begin();
00578          joint_it != joint_models.end(); ++joint_it)
00579     {
00580       // Check that this joint only represents 1 variable.
00581       if ((*joint_it)->getVariableCount() == 1)
00582         joints.insert(*joint_it);
00583     }
00584   }
00585 
00586   // Add joints to yaml file, if no more than 1 dof
00587   for (std::set<const robot_model::JointModel*>::iterator joint_it = joints.begin(); joint_it != joints.end();
00588        ++joint_it)
00589   {
00590     emitter << YAML::Key << (*joint_it)->getName();
00591     emitter << YAML::Value << YAML::BeginMap;
00592 
00593     const robot_model::VariableBounds& b = (*joint_it)->getVariableBounds()[0];
00594 
00595     // Output property
00596     emitter << YAML::Key << "has_velocity_limits";
00597     if (b.velocity_bounded_)
00598       emitter << YAML::Value << "true";
00599     else
00600       emitter << YAML::Value << "false";
00601 
00602     // Output property
00603     emitter << YAML::Key << "max_velocity";
00604     emitter << YAML::Value << std::min(fabs(b.max_velocity_), fabs(b.min_velocity_));
00605 
00606     // Output property
00607     emitter << YAML::Key << "has_acceleration_limits";
00608     if (b.acceleration_bounded_)
00609       emitter << YAML::Value << "true";
00610     else
00611       emitter << YAML::Value << "false";
00612 
00613     // Output property
00614     emitter << YAML::Key << "max_acceleration";
00615     emitter << YAML::Value << std::min(fabs(b.max_acceleration_), fabs(b.min_acceleration_));
00616 
00617     emitter << YAML::EndMap;
00618   }
00619 
00620   emitter << YAML::EndMap;
00621 
00622   std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
00623   if (!output_stream.good())
00624   {
00625     ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
00626     return false;
00627   }
00628   // Add documentation into joint_limits.yaml
00629   output_stream << "# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or "
00630                    "augmented as needed"
00631                 << std::endl;
00632   output_stream << "# Specific joint properties can be changed with the keys [max_position, min_position, "
00633                    "max_velocity, max_acceleration]"
00634                 << std::endl;
00635   output_stream << "# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]" << std::endl;
00636   output_stream << emitter.c_str();
00637   output_stream.close();
00638 
00639   return true;  // file created successfully
00640 }
00641 
00642 // ******************************************************************************************
00643 // Set list of collision link pairs in SRDF; sorted; with optional filter
00644 // ******************************************************************************************
00645 
00646 class SortableDisabledCollision
00647 {
00648 public:
00649   SortableDisabledCollision(const srdf::Model::DisabledCollision& dc)
00650     : dc_(dc), key_(dc.link1_ < dc.link2_ ? (dc.link1_ + "|" + dc.link2_) : (dc.link2_ + "|" + dc.link1_))
00651   {
00652   }
00653   operator const srdf::Model::DisabledCollision() const
00654   {
00655     return dc_;
00656   }
00657   bool operator<(const SortableDisabledCollision& other) const
00658   {
00659     return key_ < other.key_;
00660   }
00661 
00662 private:
00663   const srdf::Model::DisabledCollision dc_;
00664   const std::string key_;
00665 };
00666 
00667 void MoveItConfigData::setCollisionLinkPairs(const moveit_setup_assistant::LinkPairMap& link_pairs, size_t skip_mask)
00668 {
00669   // Create temp disabled collision
00670   srdf::Model::DisabledCollision dc;
00671 
00672   std::set<SortableDisabledCollision> disabled_collisions;
00673   disabled_collisions.insert(srdf_->disabled_collisions_.begin(), srdf_->disabled_collisions_.end());
00674 
00675   // copy the data in this class's LinkPairMap datastructure to srdf::Model::DisabledCollision format
00676   for (moveit_setup_assistant::LinkPairMap::const_iterator pair_it = link_pairs.begin(); pair_it != link_pairs.end();
00677        ++pair_it)
00678   {
00679     // Only copy those that are actually disabled
00680     if (pair_it->second.disable_check)
00681     {
00682       if ((1 << pair_it->second.reason) & skip_mask)
00683         continue;
00684 
00685       dc.link1_ = pair_it->first.first;
00686       dc.link2_ = pair_it->first.second;
00687       dc.reason_ = moveit_setup_assistant::disabledReasonToString(pair_it->second.reason);
00688 
00689       disabled_collisions.insert(SortableDisabledCollision(dc));
00690     }
00691   }
00692 
00693   srdf_->disabled_collisions_.assign(disabled_collisions.begin(), disabled_collisions.end());
00694 }
00695 
00696 // ******************************************************************************************
00697 // Decide the best two joints to be used for the projection evaluator
00698 // ******************************************************************************************
00699 std::string MoveItConfigData::decideProjectionJoints(std::string planning_group)
00700 {
00701   std::string joint_pair = "";
00702 
00703   // Retrieve pointer to the shared kinematic model
00704   const robot_model::RobotModelConstPtr& model = getRobotModel();
00705 
00706   // Error check
00707   if (!model->hasJointModelGroup(planning_group))
00708     return joint_pair;
00709 
00710   // Get the joint model group
00711   const robot_model::JointModelGroup* group = model->getJointModelGroup(planning_group);
00712 
00713   // get vector of joint names
00714   const std::vector<std::string> joints = group->getJointModelNames();
00715 
00716   if (joints.size() >= 2)
00717   {
00718     // Check that the first two joints have only 1 variable
00719     if (group->getJointModel(joints[0])->getVariableCount() == 1 &&
00720         group->getJointModel(joints[1])->getVariableCount() == 1)
00721     {
00722       // Just choose the first two joints.
00723       joint_pair = "joints(" + joints[0] + "," + joints[1] + ")";
00724     }
00725   }
00726 
00727   return joint_pair;
00728 }
00729 
00730 // ******************************************************************************************
00731 // Input kinematics.yaml file
00732 // ******************************************************************************************
00733 bool MoveItConfigData::inputKinematicsYAML(const std::string& file_path)
00734 {
00735   // Load file
00736   std::ifstream input_stream(file_path.c_str());
00737   if (!input_stream.good())
00738   {
00739     ROS_ERROR_STREAM("Unable to open file for reading " << file_path);
00740     return false;
00741   }
00742 
00743   // Begin parsing
00744   try
00745   {
00746     YAML::Node doc;
00747     loadYaml(input_stream, doc);
00748 
00749     yaml_node_t prop_name;
00750 
00751 // Loop through all groups
00752 #ifdef HAVE_NEW_YAMLCPP
00753     for (YAML::const_iterator group_it = doc.begin(); group_it != doc.end(); ++group_it)
00754 #else
00755     for (YAML::Iterator group_it = doc.begin(); group_it != doc.end(); ++group_it)
00756 #endif
00757     {
00758 #ifdef HAVE_NEW_YAMLCPP
00759       const std::string group_name = group_it->first.as<std::string>();
00760 #else
00761       std::string group_name;
00762       group_it.first() >> group_name;
00763 #endif
00764 
00765       // Create new meta data
00766       GroupMetaData new_meta_data;
00767 
00768 // kinematics_solver
00769 #ifdef HAVE_NEW_YAMLCPP
00770       if (prop_name = findValue(group_it->second, "kinematics_solver"))
00771 #else
00772       if (prop_name = findValue(group_it.second(), "kinematics_solver"))
00773 #endif
00774       {
00775         *prop_name >> new_meta_data.kinematics_solver_;
00776       }
00777 
00778 // kinematics_solver_search_resolution
00779 #ifdef HAVE_NEW_YAMLCPP
00780       if (prop_name = findValue(group_it->second, "kinematics_solver_search_resolution"))
00781 #else
00782       if (prop_name = findValue(group_it.second(), "kinematics_solver_search_resolution"))
00783 #endif
00784       {
00785         *prop_name >> new_meta_data.kinematics_solver_search_resolution_;
00786       }
00787       else
00788       {
00789         new_meta_data.kinematics_solver_attempts_ = DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION_;
00790       }
00791 
00792 // kinematics_solver_timeout
00793 #ifdef HAVE_NEW_YAMLCPP
00794       if (prop_name = findValue(group_it->second, "kinematics_solver_timeout"))
00795 #else
00796       if (prop_name = findValue(group_it.second(), "kinematics_solver_timeout"))
00797 #endif
00798       {
00799         *prop_name >> new_meta_data.kinematics_solver_timeout_;
00800       }
00801       else
00802       {
00803         new_meta_data.kinematics_solver_attempts_ = DEFAULT_KIN_SOLVER_TIMEOUT_;
00804       }
00805 
00806 // kinematics_solver_attempts
00807 #ifdef HAVE_NEW_YAMLCPP
00808       if (prop_name = findValue(group_it->second, "kinematics_solver_attempts"))
00809 #else
00810       if (prop_name = findValue(group_it.second(), "kinematics_solver_attempts"))
00811 #endif
00812       {
00813         *prop_name >> new_meta_data.kinematics_solver_attempts_;
00814       }
00815       else
00816       {
00817         new_meta_data.kinematics_solver_attempts_ = DEFAULT_KIN_SOLVER_ATTEMPTS_;
00818       }
00819 
00820       // Assign meta data to vector
00821       group_meta_data_[group_name] = new_meta_data;
00822     }
00823   }
00824   catch (YAML::ParserException& e)  // Catch errors
00825   {
00826     ROS_ERROR_STREAM(e.what());
00827     return false;
00828   }
00829 
00830   return true;  // file created successfully
00831 }
00832 
00833 // ******************************************************************************************
00834 // Set package path; try to resolve path from package name if directory does not exist
00835 // ******************************************************************************************
00836 bool MoveItConfigData::setPackagePath(const std::string& pkg_path)
00837 {
00838   std::string full_package_path;
00839 
00840   // check that the folder exists
00841   if (!fs::is_directory(pkg_path))
00842   {
00843     // does not exist, check if its a package
00844     full_package_path = ros::package::getPath(pkg_path);
00845 
00846     // check that the folder exists
00847     if (!fs::is_directory(full_package_path))
00848     {
00849       return false;
00850     }
00851   }
00852   else
00853   {
00854     // they inputted a full path
00855     full_package_path = pkg_path;
00856   }
00857 
00858   config_pkg_path_ = full_package_path;
00859   return true;
00860 }
00861 
00862 // ******************************************************************************************
00863 // Resolve path to .setup_assistant file
00864 // ******************************************************************************************
00865 
00866 bool MoveItConfigData::getSetupAssistantYAMLPath(std::string& path)
00867 {
00868   path = appendPaths(config_pkg_path_, ".setup_assistant");
00869 
00870   // Check if the old package is a setup assistant package
00871   return fs::is_regular_file(path);
00872 }
00873 
00874 // ******************************************************************************************
00875 // Make the full URDF path using the loaded .setup_assistant data
00876 // ******************************************************************************************
00877 bool MoveItConfigData::createFullURDFPath()
00878 {
00879   boost::trim(urdf_pkg_name_);
00880 
00881   // Check if a package name was provided
00882   if (urdf_pkg_name_.empty() || urdf_pkg_name_ == "\"\"")
00883   {
00884     urdf_path_ = urdf_pkg_relative_path_;
00885     urdf_pkg_name_.clear();
00886   }
00887   else
00888   {
00889     // Check that ROS can find the package
00890     std::string robot_desc_pkg_path = ros::package::getPath(urdf_pkg_name_);
00891 
00892     if (robot_desc_pkg_path.empty())
00893     {
00894       urdf_path_.clear();
00895       return false;
00896     }
00897 
00898     // Append the relative URDF url path
00899     urdf_path_ = appendPaths(robot_desc_pkg_path, urdf_pkg_relative_path_);
00900   }
00901 
00902   // Check that this file exits -------------------------------------------------
00903   return fs::is_regular_file(urdf_path_);
00904 }
00905 
00906 // ******************************************************************************************
00907 // Make the full SRDF path using the loaded .setup_assistant data
00908 // ******************************************************************************************
00909 bool MoveItConfigData::createFullSRDFPath(const std::string& package_path)
00910 {
00911   srdf_path_ = appendPaths(package_path, srdf_pkg_relative_path_);
00912 
00913   return fs::is_regular_file(srdf_path_);
00914 }
00915 
00916 // ******************************************************************************************
00917 // Input .setup_assistant file - contains data used for the MoveIt Setup Assistant
00918 // ******************************************************************************************
00919 bool MoveItConfigData::inputSetupAssistantYAML(const std::string& file_path)
00920 {
00921   // Load file
00922   std::ifstream input_stream(file_path.c_str());
00923   if (!input_stream.good())
00924   {
00925     ROS_ERROR_STREAM("Unable to open file for reading " << file_path);
00926     return false;
00927   }
00928 
00929   // Begin parsing
00930   try
00931   {
00932     YAML::Node doc;
00933     loadYaml(input_stream, doc);
00934 
00935     yaml_node_t title_node, urdf_node, package_node, jade_xacro_node, srdf_node, relative_node, config_node,
00936         timestamp_node, author_name_node, author_email_node;
00937 
00938     // Get title node
00939     if (title_node = findValue(doc, "moveit_setup_assistant_config"))
00940     {
00941       // URDF Properties
00942       if (urdf_node = findValue(*title_node, "URDF"))
00943       {
00944         // Load first property
00945         if (package_node = findValue(*urdf_node, "package"))
00946         {
00947           *package_node >> urdf_pkg_name_;
00948         }
00949         else
00950         {
00951           return false;  // if we do not find this value we cannot continue
00952         }
00953 
00954         // Load second property
00955         if (relative_node = findValue(*urdf_node, "relative_path"))
00956         {
00957           *relative_node >> urdf_pkg_relative_path_;
00958         }
00959         else
00960         {
00961           return false;  // if we do not find this value we cannot continue
00962         }
00963         // should Jade+ xacro extensions be enabled for this package?
00964         if (jade_xacro_node = findValue(*urdf_node, "use_jade_xacro"))
00965         {
00966           *jade_xacro_node >> urdf_requires_jade_xacro_;
00967         }
00968         else
00969         {
00970           // value for this node is not required
00971         }
00972       }
00973       // SRDF Properties
00974       if (srdf_node = findValue(*title_node, "SRDF"))
00975       {
00976         // Load first property
00977         if (relative_node = findValue(*srdf_node, "relative_path"))
00978         {
00979           *relative_node >> srdf_pkg_relative_path_;
00980         }
00981         else
00982         {
00983           return false;  // if we do not find this value we cannot continue
00984         }
00985       }
00986       // Package generation time
00987       if (config_node = findValue(*title_node, "CONFIG"))
00988       {
00989         // Load author contact details
00990         if (author_name_node = findValue(*config_node, "author_name"))
00991         {
00992           *author_name_node >> author_name_;
00993         }
00994         if (author_email_node = findValue(*config_node, "author_email"))
00995         {
00996           *author_email_node >> author_email_;
00997         }
00998         // Load first property
00999         if (timestamp_node = findValue(*config_node, "generated_timestamp"))
01000         {
01001           *timestamp_node >> config_pkg_generated_timestamp_;
01002         }
01003         else
01004         {
01005           // if we do not find this value it is fine, not required
01006         }
01007       }
01008       return true;
01009     }
01010   }
01011   catch (YAML::ParserException& e)  // Catch errors
01012   {
01013     ROS_ERROR_STREAM(e.what());
01014   }
01015 
01016   return false;  // if it gets to this point an error has occured
01017 }
01018 
01019 // ******************************************************************************************
01020 // Helper Function for joining a file path and a file name, or two file paths, etc, in a cross-platform way
01021 // ******************************************************************************************
01022 std::string MoveItConfigData::appendPaths(const std::string& path1, const std::string& path2)
01023 {
01024   fs::path result = path1;
01025   result /= path2;
01026   return result.make_preferred().native().c_str();
01027 }
01028 
01029 srdf::Model::Group* MoveItConfigData::findGroupByName(const std::string& name)
01030 {
01031   // Find the group we are editing based on the goup name string
01032   srdf::Model::Group* searched_group = NULL;  // used for holding our search results
01033 
01034   for (std::vector<srdf::Model::Group>::iterator group_it = srdf_->groups_.begin(); group_it != srdf_->groups_.end();
01035        ++group_it)
01036   {
01037     if (group_it->name_ == name)  // string match
01038     {
01039       searched_group = &(*group_it);  // convert to pointer from iterator
01040       break;                          // we are done searching
01041     }
01042   }
01043 
01044   // Check if subgroup was found
01045   if (searched_group == NULL)  // not found
01046     ROS_FATAL_STREAM("An internal error has occured while searching for groups. Group '" << name << "' was not found "
01047                                                                                                     "in the SRDF.");
01048 
01049   return searched_group;
01050 }
01051 
01052 }  // namespace


moveit_setup_assistant
Author(s): Dave Coleman
autogenerated on Wed Jun 19 2019 19:25:13