moveit_config_data.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: Dave Coleman */
36 
37 #include <moveit/setup_assistant/tools/moveit_config_data.h>
38 #include <moveit/move_group_interface/move_group_interface.h>
39 
40 // Reading/Writing Files
41 #include <iostream> // For writing yaml and launch files
42 #include <fstream>
43 #include <boost/filesystem/path.hpp> // for creating folders/files
44 #include <boost/filesystem/operations.hpp> // is_regular_file, is_directory, etc.
45 #include <boost/algorithm/string/trim.hpp>
46 #include <boost/algorithm/string/predicate.hpp>
47 
48 // ROS
49 #include <ros/console.h>
50 #include <ros/package.h> // for getting file path for loading images
51 
52 // OMPL version
53 #include <ompl/config.h>
54 
55 namespace moveit_setup_assistant
56 {
57 // File system
58 namespace fs = boost::filesystem;
59 
60 // ******************************************************************************************
61 // Constructor
62 // ******************************************************************************************
63 MoveItConfigData::MoveItConfigData() : config_pkg_generated_timestamp_(0)
64 {
65  // Create an instance of SRDF writer and URDF model for all widgets to share
66  srdf_ = std::make_shared<srdf::SRDFWriter>();
67  urdf_model_ = std::make_shared<urdf::Model>();
68 
69  // Not in debug mode
70  debug_ = false;
71 
72  // Get MoveIt Setup Assistant package path
73  setup_assistant_path_ = ros::package::getPath("moveit_setup_assistant");
74  if (setup_assistant_path_.empty())
75  {
76  setup_assistant_path_ = ".";
77  }
78 }
79 
80 // ******************************************************************************************
81 // Destructor
82 // ******************************************************************************************
83 MoveItConfigData::~MoveItConfigData() = default;
84 
85 // ******************************************************************************************
86 // Load a robot model
87 // ******************************************************************************************
88 void MoveItConfigData::setRobotModel(const moveit::core::RobotModelPtr& robot_model)
89 {
90  robot_model_ = robot_model;
91 }
92 
93 // ******************************************************************************************
94 // Provide a kinematic model. Load a new one if necessary
95 // ******************************************************************************************
96 moveit::core::RobotModelConstPtr MoveItConfigData::getRobotModel()
97 {
98  if (!robot_model_)
99  {
100  // Initialize with a URDF Model Interface and a SRDF Model
101  robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model_, srdf_->srdf_model_);
102  }
103 
104  return robot_model_;
105 }
106 
107 // ******************************************************************************************
108 // Update the Kinematic Model with latest SRDF modifications
109 // ******************************************************************************************
110 void MoveItConfigData::updateRobotModel()
111 {
112  ROS_INFO("Updating kinematic model");
113 
114  // Tell SRDF Writer to create new SRDF Model, use original URDF model
115  srdf_->updateSRDFModel(*urdf_model_);
116 
117  // Create new kin model
118  robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model_, srdf_->srdf_model_);
119 
120  // Reset the planning scene
121  planning_scene_.reset();
122 }
123 
124 // ******************************************************************************************
125 // Provide a shared planning scene
126 // ******************************************************************************************
127 planning_scene::PlanningScenePtr MoveItConfigData::getPlanningScene()
128 {
129  if (!planning_scene_)
130  {
131  // make sure kinematic model exists
132  getRobotModel();
133 
134  // Allocate an empty planning scene
135  planning_scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
136  }
137  return planning_scene_;
138 }
139 
140 // ******************************************************************************************
141 // Load the allowed collision matrix from the SRDF's list of link pairs
142 // ******************************************************************************************
143 void MoveItConfigData::loadAllowedCollisionMatrix(const srdf::SRDFWriter& srdf)
144 {
145  allowed_collision_matrix_.clear();
146 
147  // load collision defaults
148  for (const std::string& name : srdf.no_default_collision_links_)
149  allowed_collision_matrix_.setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS);
150  // re-enable specific collision pairs
151  for (auto const& collision : srdf.enabled_collision_pairs_)
152  allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, false);
153  // *finally* disable selected collision pairs
154  for (auto const& collision : srdf.disabled_collision_pairs_)
155  allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, true);
156 }
157 
158 // ******************************************************************************************
159 // Output MoveIt Setup Assistant hidden settings file
160 // ******************************************************************************************
161 bool MoveItConfigData::outputSetupAssistantFile(const std::string& file_path)
162 {
163  YAML::Emitter emitter;
164  emitter << YAML::BeginMap;
165 
166  // Output every available planner ---------------------------------------------------
167  emitter << YAML::Key << "moveit_setup_assistant_config";
168 
169  emitter << YAML::Value << YAML::BeginMap;
170 
171  // URDF Path Location
172  emitter << YAML::Key << "URDF";
173  emitter << YAML::Value << YAML::BeginMap;
174  emitter << YAML::Key << "package" << YAML::Value << urdf_pkg_name_;
175  emitter << YAML::Key << "relative_path" << YAML::Value << urdf_pkg_relative_path_;
176  emitter << YAML::Key << "xacro_args" << YAML::Value << xacro_args_;
177  emitter << YAML::EndMap;
178 
180  emitter << YAML::Key << "SRDF";
181  emitter << YAML::Value << YAML::BeginMap;
182  emitter << YAML::Key << "relative_path" << YAML::Value << srdf_pkg_relative_path_;
183  emitter << YAML::EndMap;
184 
186  emitter << YAML::Key << "CONFIG";
187  emitter << YAML::Value << YAML::BeginMap;
188  emitter << YAML::Key << "author_name" << YAML::Value << author_name_;
189  emitter << YAML::Key << "author_email" << YAML::Value << author_email_;
190  auto cur_time = std::time(nullptr);
191  emitter << YAML::Key << "generated_timestamp" << YAML::Value << cur_time; // TODO: is this cross-platform?
192  emitter << YAML::EndMap;
193 
194  emitter << YAML::EndMap;
195 
196  std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
197  if (!output_stream.good())
198  {
199  ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
200  return false;
201  }
202 
203  output_stream << emitter.c_str();
204  output_stream.close();
205 
207  // NOTE: Needed for when people run the MSA generator multiple times in a row.
208  config_pkg_generated_timestamp_ = cur_time;
209 
210  return true; // file created successfully
211 }
212 
213 // ******************************************************************************************
214 // Output Gazebo URDF file
215 // ******************************************************************************************
216 bool MoveItConfigData::outputGazeboURDFFile(const std::string& file_path)
217 {
218  std::ofstream os(file_path.c_str(), std::ios_base::trunc);
219  if (!os.good())
220  {
221  ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
222  return false;
223  }
224 
225  os << gazebo_urdf_string_ << std::endl;
226  os.close();
227 
228  return true;
229 }
230 
231 // ******************************************************************************************
232 // Output OMPL Planning config files
233 // ******************************************************************************************
234 bool MoveItConfigData::outputOMPLPlanningYAML(const std::string& file_path)
235 {
236  YAML::Emitter emitter;
237  emitter << YAML::BeginMap;
238 
239  // Output every available planner ---------------------------------------------------
240  emitter << YAML::Key << "planner_configs";
241 
242  emitter << YAML::Value << YAML::BeginMap;
243 
244  std::vector<OMPLPlannerDescription> planner_des = getOMPLPlanners();
245 
246  // Add Planners with parameter values
247  std::vector<std::string> pconfigs;
248  for (OMPLPlannerDescription& planner_de : planner_des)
249  {
250  std::string defaultconfig = planner_de.name_;
251  emitter << YAML::Key << defaultconfig;
252  emitter << YAML::Value << YAML::BeginMap;
253  emitter << YAML::Key << "type" << YAML::Value << "geometric::" + planner_de.name_;
254  for (OmplPlanningParameter& ompl_planner_param : planner_de.parameter_list_)
255  {
256  emitter << YAML::Key << ompl_planner_param.name;
257  emitter << YAML::Value << ompl_planner_param.value;
258  emitter << YAML::Comment(ompl_planner_param.comment);
259  }
260  emitter << YAML::EndMap;
261 
262  pconfigs.push_back(defaultconfig);
263  }
264 
265  // End of every avail planner
266  emitter << YAML::EndMap;
267 
268  // Output every group and the planners it can use ----------------------------------
269  for (srdf::Model::Group& group : srdf_->groups_)
270  {
271  emitter << YAML::Key << group.name_;
272  emitter << YAML::Value << YAML::BeginMap;
273  // Output associated planners
274  if (!group_meta_data_[group.name_].default_planner_.empty())
275  emitter << YAML::Key << "default_planner_config" << YAML::Value << group_meta_data_[group.name_].default_planner_;
276  emitter << YAML::Key << "planner_configs";
277  emitter << YAML::Value << YAML::BeginSeq;
278  for (const std::string& pconfig : pconfigs)
279  emitter << pconfig;
280  emitter << YAML::EndSeq;
281 
282  // Output projection_evaluator
283  std::string projection_joints = decideProjectionJoints(group.name_);
284  if (!projection_joints.empty())
285  {
286  emitter << YAML::Key << "projection_evaluator";
287  emitter << YAML::Value << projection_joints;
288  // OMPL collision checking discretization
289  emitter << YAML::Key << "longest_valid_segment_fraction";
290  emitter << YAML::Value << "0.005";
291  }
292 
293  emitter << YAML::EndMap;
294  }
295 
296  emitter << YAML::EndMap;
297 
298  std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
299  if (!output_stream.good())
300  {
301  ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
302  return false;
303  }
304 
305  output_stream << emitter.c_str() << std::endl;
306  output_stream.close();
307 
308  return true; // file created successfully
309 }
310 
311 // ******************************************************************************************
312 // Output STOMP Planning config files
313 // ******************************************************************************************
314 bool MoveItConfigData::outputSTOMPPlanningYAML(const std::string& file_path)
315 {
316  YAML::Emitter emitter;
317  emitter << YAML::BeginMap;
318 
319  // Add STOMP default for every group
320  for (srdf::Model::Group& group : srdf_->groups_)
321  {
322  emitter << YAML::Key << "stomp/" + group.name_;
323  emitter << YAML::BeginMap;
324  emitter << YAML::Key << "group_name";
325  emitter << YAML::Value << group.name_;
326 
327  emitter << YAML::Key << "optimization";
328  emitter << YAML::BeginMap;
329  emitter << YAML::Key << "num_timesteps";
330  emitter << YAML::Value << "60";
331  emitter << YAML::Key << "num_iterations";
332  emitter << YAML::Value << "40";
333  emitter << YAML::Key << "num_iterations_after_valid";
334  emitter << YAML::Value << "0";
335  emitter << YAML::Key << "num_rollouts";
336  emitter << YAML::Value << "30";
337  emitter << YAML::Key << "max_rollouts";
338  emitter << YAML::Value << "30";
339  emitter << YAML::Key << "initialization_method";
340  emitter << YAML::Value << "1";
341  emitter << YAML::Comment("[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]");
342  emitter << YAML::Key << "control_cost_weight";
343  emitter << YAML::Value << "0.0";
344  emitter << YAML::EndMap;
345 
346  emitter << YAML::Key << "task";
347  emitter << YAML::BeginMap;
348 
349  emitter << YAML::Key << "noise_generator";
350  emitter << YAML::BeginSeq;
351  emitter << YAML::BeginMap;
352  emitter << YAML::Key << "class";
353  emitter << YAML::Value << "stomp_moveit/NormalDistributionSampling";
354  emitter << YAML::Key << "stddev";
355  emitter << YAML::Flow;
356  const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_);
357  const std::vector<const moveit::core::JointModel*>& joint_models = joint_model_group->getActiveJointModels();
358  std::vector<float> stddev(joint_models.size(), 0.05);
359  emitter << YAML::Value << stddev;
360  emitter << YAML::EndMap;
361  emitter << YAML::EndSeq;
362 
363  emitter << YAML::Key << "cost_functions";
364  emitter << YAML::BeginSeq;
365  emitter << YAML::BeginMap;
366  emitter << YAML::Key << "class";
367  emitter << YAML::Value << "stomp_moveit/CollisionCheck";
368  emitter << YAML::Key << "collision_penalty";
369  emitter << YAML::Value << "1.0";
370  emitter << YAML::Key << "cost_weight";
371  emitter << YAML::Value << "1.0";
372  emitter << YAML::Key << "kernel_window_percentage";
373  emitter << YAML::Value << "0.2";
374  emitter << YAML::Key << "longest_valid_joint_move";
375  emitter << YAML::Value << "0.05";
376  emitter << YAML::EndMap;
377  emitter << YAML::EndSeq;
378 
379  emitter << YAML::Key << "noisy_filters";
380  emitter << YAML::BeginSeq;
381  emitter << YAML::BeginMap;
382  emitter << YAML::Key << "class";
383  emitter << YAML::Value << "stomp_moveit/JointLimits";
384  emitter << YAML::Key << "lock_start";
385  emitter << YAML::Value << "True";
386  emitter << YAML::Key << "lock_goal";
387  emitter << YAML::Value << "True";
388  emitter << YAML::EndMap;
389  emitter << YAML::BeginMap;
390  emitter << YAML::Key << "class";
391  emitter << YAML::Value << "stomp_moveit/MultiTrajectoryVisualization";
392  emitter << YAML::Key << "line_width";
393  emitter << YAML::Value << "0.02";
394  emitter << YAML::Key << "rgb";
395  emitter << YAML::Flow;
396  std::vector<float> noisy_filters_rgb{ 255, 255, 0 };
397  emitter << YAML::Value << noisy_filters_rgb;
398  emitter << YAML::Key << "marker_array_topic";
399  emitter << YAML::Value << "stomp_trajectories";
400  emitter << YAML::Key << "marker_namespace";
401  emitter << YAML::Value << "noisy";
402  emitter << YAML::EndMap;
403  emitter << YAML::EndSeq;
404 
405  emitter << YAML::Key << "update_filters";
406  emitter << YAML::BeginSeq;
407  emitter << YAML::BeginMap;
408  emitter << YAML::Key << "class";
409  emitter << YAML::Value << "stomp_moveit/PolynomialSmoother";
410  emitter << YAML::Key << "poly_order";
411  emitter << YAML::Value << "6";
412  emitter << YAML::EndMap;
413  emitter << YAML::BeginMap;
414  emitter << YAML::Key << "class";
415  emitter << YAML::Value << "stomp_moveit/TrajectoryVisualization";
416  emitter << YAML::Key << "line_width";
417  emitter << YAML::Value << "0.05";
418  emitter << YAML::Key << "rgb";
419  emitter << YAML::Flow;
420  std::vector<float> update_filters_rgb{ 0, 191, 255 };
421  emitter << YAML::Value << update_filters_rgb;
422  emitter << YAML::Key << "error_rgb";
423  emitter << YAML::Flow;
424  std::vector<float> update_filters_error_rgb{ 255, 0, 0 };
425  emitter << YAML::Value << update_filters_error_rgb;
426  emitter << YAML::Key << "publish_intermediate";
427  emitter << YAML::Value << "True";
428  emitter << YAML::Key << "marker_topic";
429  emitter << YAML::Value << "stomp_trajectory";
430  emitter << YAML::Key << "marker_namespace";
431  emitter << YAML::Value << "optimized";
432  emitter << YAML::EndMap;
433  emitter << YAML::EndSeq;
434 
435  emitter << YAML::EndMap;
436  emitter << YAML::EndMap;
437  }
438 
439  emitter << YAML::EndMap;
440 
441  std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
442  if (!output_stream.good())
443  {
444  ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
445  return false;
446  }
447 
448  output_stream << emitter.c_str();
449  output_stream.close();
450 
451  return true; // file created successfully
452 }
453 
454 // ******************************************************************************************
455 // Output kinematic config files
456 // ******************************************************************************************
457 bool MoveItConfigData::outputKinematicsYAML(const std::string& file_path)
458 {
459  YAML::Emitter emitter;
460  emitter << YAML::BeginMap;
461 
462  // Output every group and the kinematic solver it can use ----------------------------------
463  for (srdf::Model::Group& group : srdf_->groups_)
464  {
465  // Only save kinematic data if the solver is not "None"
466  if (group_meta_data_[group.name_].kinematics_solver_.empty() ||
467  group_meta_data_[group.name_].kinematics_solver_ == "None")
468  continue;
469 
470  emitter << YAML::Key << group.name_;
471  emitter << YAML::Value << YAML::BeginMap;
472 
473  // Kinematic Solver
474  emitter << YAML::Key << "kinematics_solver";
475  emitter << YAML::Value << group_meta_data_[group.name_].kinematics_solver_;
476 
477  // Search Resolution
478  emitter << YAML::Key << "kinematics_solver_search_resolution";
479  emitter << YAML::Value << group_meta_data_[group.name_].kinematics_solver_search_resolution_;
480 
481  // Solver Timeout
482  emitter << YAML::Key << "kinematics_solver_timeout";
483  emitter << YAML::Value << group_meta_data_[group.name_].kinematics_solver_timeout_;
484 
485  // Goal joint tolerance
486  emitter << YAML::Key << "goal_joint_tolerance";
487  emitter << YAML::Value << group_meta_data_[group.name_].goal_joint_tolerance_;
488 
489  // Goal position tolerance
490  emitter << YAML::Key << "goal_position_tolerance";
491  emitter << YAML::Value << group_meta_data_[group.name_].goal_position_tolerance_;
492 
493  // Goal orientation tolerance
494  emitter << YAML::Key << "goal_orientation_tolerance";
495  emitter << YAML::Value << group_meta_data_[group.name_].goal_orientation_tolerance_;
496 
497  emitter << YAML::EndMap;
498  }
499 
500  emitter << YAML::EndMap;
501 
502  std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
503  if (!output_stream.good())
504  {
505  ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
506  return false;
507  }
508 
509  output_stream << emitter.c_str();
510  output_stream.close();
511 
512  return true; // file created successfully
513 }
514 
515 // ******************************************************************************************
516 // Helper function to get the controller that is controlling the joint
517 // ******************************************************************************************
518 std::string MoveItConfigData::getJointHardwareInterface(const std::string& joint_name)
519 {
520  for (ControllerConfig& ros_control_config : controller_configs_)
521  {
522  std::vector<std::string>::iterator joint_it =
523  std::find(ros_control_config.joints_.begin(), ros_control_config.joints_.end(), joint_name);
524  if (joint_it != ros_control_config.joints_.end())
525  {
526  if (ros_control_config.type_.substr(0, 8) == "position")
527  return "hardware_interface/PositionJointInterface";
528  else if (ros_control_config.type_.substr(0, 8) == "velocity")
529  return "hardware_interface/VelocityJointInterface";
530  // As of writing this, available joint command interfaces are position, velocity and effort.
531  else
532  return "hardware_interface/EffortJointInterface";
533  }
534  }
535  // If the joint was not found in any controller return EffortJointInterface
536  return "hardware_interface/EffortJointInterface";
537 }
538 
539 bool MoveItConfigData::outputFakeControllersYAML(const std::string& file_path)
540 {
541  YAML::Emitter emitter;
542  emitter << YAML::BeginMap;
543 
544  emitter << YAML::Key << "controller_list";
545  emitter << YAML::Value << YAML::BeginSeq;
546 
547  // Loop through groups
548  for (srdf::Model::Group& group : srdf_->groups_)
549  {
550  // Get list of associated joints
551  const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_);
552  emitter << YAML::BeginMap;
553  const std::vector<const moveit::core::JointModel*>& joint_models = joint_model_group->getActiveJointModels();
554  emitter << YAML::Key << "name";
555  emitter << YAML::Value << "fake_" + group.name_ + "_controller";
556  emitter << YAML::Key << "type";
557  emitter << YAML::Value << "$(arg fake_execution_type)";
558  emitter << YAML::Key << "joints";
559  emitter << YAML::Value << YAML::BeginSeq;
560 
561  // Iterate through the joints
562  for (const moveit::core::JointModel* joint : joint_models)
563  {
564  if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == moveit::core::JointModel::FIXED)
565  continue;
566  emitter << joint->getName();
567  }
568  emitter << YAML::EndSeq;
569  emitter << YAML::EndMap;
570  }
571 
572  emitter << YAML::EndSeq;
573 
574  // Add an initial pose for each group
575  emitter << YAML::Key << "initial" << YAML::Comment("Define initial robot poses per group");
576 
577  bool poses_found = false;
578  std::string default_group_name;
579  for (const srdf::Model::Group& group : srdf_->groups_)
580  {
581  if (default_group_name.empty())
582  default_group_name = group.name_;
583  for (const srdf::Model::GroupState& group_state : srdf_->group_states_)
584  {
585  if (group.name_ == group_state.group_)
586  {
587  if (!poses_found)
588  {
589  poses_found = true;
590  emitter << YAML::Value << YAML::BeginSeq;
591  }
592  emitter << YAML::BeginMap;
593  emitter << YAML::Key << "group";
594  emitter << YAML::Value << group.name_;
595  emitter << YAML::Key << "pose";
596  emitter << YAML::Value << group_state.name_;
597  emitter << YAML::EndMap;
598  break;
599  }
600  }
601  }
602  if (poses_found)
603  emitter << YAML::EndSeq;
604  else
605  {
606  // Add commented lines to show how the feature can be used
607  if (default_group_name.empty())
608  default_group_name = "group";
609  emitter << YAML::Newline;
610  emitter << YAML::Comment(" - group: " + default_group_name) << YAML::Newline;
611  emitter << YAML::Comment(" pose: home") << YAML::Newline;
612 
613  // Add empty list for valid yaml
614  emitter << YAML::BeginSeq;
615  emitter << YAML::EndSeq;
616  }
617 
618  emitter << YAML::EndMap;
619 
620  std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
621  if (!output_stream.good())
622  {
623  ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
624  return false;
625  }
626 
627  output_stream << emitter.c_str();
628  output_stream.close();
629 
630  return true; // file created successfully
631 }
632 
633 std::map<std::string, double> MoveItConfigData::getInitialJoints() const
634 {
635  std::map<std::string, double> joints;
636  for (const srdf::Model::Group& group : srdf_->groups_)
637  {
638  // use first pose of each group as initial pose
639  for (const srdf::Model::GroupState& group_state : srdf_->group_states_)
640  {
641  if (group.name_ != group_state.group_)
642  continue;
643  for (const auto& pair : group_state.joint_values_)
644  {
645  if (pair.second.size() != 1)
646  continue; // only handle simple joints here
647  joints[pair.first] = pair.second.front();
648  }
649  break;
650  }
651  }
652  return joints;
653 }
654 
655 std::vector<OMPLPlannerDescription> MoveItConfigData::getOMPLPlanners() const
656 {
657  std::vector<OMPLPlannerDescription> planner_des;
658 
659  OMPLPlannerDescription aps("AnytimePathShortening", "geometric");
660  aps.addParameter("shortcut", "true", "Attempt to shortcut all new solution paths");
661  aps.addParameter("hybridize", "true", "Compute hybrid solution trajectories");
662  aps.addParameter("max_hybrid_paths", "24", "Number of hybrid paths generated per iteration");
663  aps.addParameter("num_planners", "4", "The number of default planners to use for planning");
664  aps.addParameter("planners", "", // added in OMPL 1.5.0
665  "A comma-separated list of planner types (e.g., \"PRM,EST,RRTConnect\""
666  "Optionally, planner parameters can be passed to change the default:"
667  "\"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]\"");
668  planner_des.push_back(aps);
669 
670  OMPLPlannerDescription sbl("SBL", "geometric");
671  sbl.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()");
672  planner_des.push_back(sbl);
673 
674  OMPLPlannerDescription est("EST", "geometric");
675  est.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()");
676  est.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05");
677  planner_des.push_back(est);
678 
679  OMPLPlannerDescription lbkpiece("LBKPIECE", "geometric");
680  lbkpiece.addParameter("range", "0.0",
681  "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on "
682  "setup()");
683  lbkpiece.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9");
684  lbkpiece.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5");
685  planner_des.push_back(lbkpiece);
686 
687  OMPLPlannerDescription bkpiece("BKPIECE", "geometric");
688  bkpiece.addParameter("range", "0.0",
689  "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on "
690  "setup()");
691  bkpiece.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9");
692  bkpiece.addParameter("failed_expansion_score_factor", "0.5",
693  "When extending motion fails, scale score by factor. "
694  "default: 0.5");
695  bkpiece.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5");
696  planner_des.push_back(bkpiece);
697 
698  OMPLPlannerDescription kpiece("KPIECE", "geometric");
699  kpiece.addParameter("range", "0.0",
700  "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on "
701  "setup()");
702  kpiece.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05");
703  kpiece.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9 (0.0,1.]");
704  kpiece.addParameter("failed_expansion_score_factor", "0.5",
705  "When extending motion fails, scale score by factor. "
706  "default: 0.5");
707  kpiece.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5");
708  planner_des.push_back(kpiece);
709 
710  OMPLPlannerDescription rrt("RRT", "geometric");
711  rrt.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()");
712  rrt.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05");
713  planner_des.push_back(rrt);
714 
715  OMPLPlannerDescription rrt_connect("RRTConnect", "geometric");
716  rrt_connect.addParameter("range", "0.0",
717  "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on "
718  "setup()");
719  planner_des.push_back(rrt_connect);
720 
721  OMPLPlannerDescription rr_tstar("RRTstar", "geometric");
722  rr_tstar.addParameter("range", "0.0",
723  "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on "
724  "setup()");
725  rr_tstar.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05");
726  rr_tstar.addParameter("delay_collision_checking", "1",
727  "Stop collision checking as soon as C-free parent found. "
728  "default 1");
729  planner_des.push_back(rr_tstar);
730 
731  OMPLPlannerDescription trrt("TRRT", "geometric");
732  trrt.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()");
733  trrt.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05");
734  trrt.addParameter("max_states_failed", "10", "when to start increasing temp. default: 10");
735  trrt.addParameter("temp_change_factor", "2.0", "how much to increase or decrease temp. default: 2.0");
736  trrt.addParameter("min_temperature", "10e-10", "lower limit of temp change. default: 10e-10");
737  trrt.addParameter("init_temperature", "10e-6", "initial temperature. default: 10e-6");
738  trrt.addParameter("frontier_threshold", "0.0",
739  "dist new state to nearest neighbor to disqualify as frontier. "
740  "default: 0.0 set in setup()");
741  trrt.addParameter("frontier_node_ratio", "0.1", "1/10, or 1 nonfrontier for every 10 frontier. default: 0.1");
742  trrt.addParameter("k_constant", "0.0", "value used to normalize expresssion. default: 0.0 set in setup()");
743  planner_des.push_back(trrt);
744 
745  OMPLPlannerDescription prm("PRM", "geometric");
746  prm.addParameter("max_nearest_neighbors", "10", "use k nearest neighbors. default: 10");
747  planner_des.push_back(prm);
748 
749  OMPLPlannerDescription pr_mstar("PRMstar", "geometric"); // no declares in code
750  planner_des.push_back(pr_mstar);
751 
752  OMPLPlannerDescription fmt("FMT", "geometric");
753  fmt.addParameter("num_samples", "1000", "number of states that the planner should sample. default: 1000");
754  fmt.addParameter("radius_multiplier", "1.1", "multiplier used for the nearest neighbors search radius. default: 1.1");
755  fmt.addParameter("nearest_k", "1", "use Knearest strategy. default: 1");
756  fmt.addParameter("cache_cc", "1", "use collision checking cache. default: 1");
757  fmt.addParameter("heuristics", "0", "activate cost to go heuristics. default: 0");
758  fmt.addParameter("extended_fmt", "1",
759  "activate the extended FMT*: adding new samples if planner does not finish "
760  "successfully. default: 1");
761  planner_des.push_back(fmt);
762 
763  OMPLPlannerDescription bfmt("BFMT", "geometric");
764  bfmt.addParameter("num_samples", "1000", "number of states that the planner should sample. default: 1000");
765  bfmt.addParameter("radius_multiplier", "1.0",
766  "multiplier used for the nearest neighbors search radius. default: "
767  "1.0");
768  bfmt.addParameter("nearest_k", "1", "use the Knearest strategy. default: 1");
769  bfmt.addParameter("balanced", "0",
770  "exploration strategy: balanced true expands one tree every iteration. False will "
771  "select the tree with lowest maximum cost to go. default: 1");
772  bfmt.addParameter("optimality", "1",
773  "termination strategy: optimality true finishes when the best possible path is "
774  "found. Otherwise, the algorithm will finish when the first feasible path is "
775  "found. default: 1");
776  bfmt.addParameter("heuristics", "1", "activates cost to go heuristics. default: 1");
777  bfmt.addParameter("cache_cc", "1", "use the collision checking cache. default: 1");
778  bfmt.addParameter("extended_fmt", "1",
779  "Activates the extended FMT*: adding new samples if planner does not finish "
780  "successfully. default: 1");
781  planner_des.push_back(bfmt);
782 
783  OMPLPlannerDescription pdst("PDST", "geometric");
784  rrt.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05");
785  planner_des.push_back(pdst);
786 
787  OMPLPlannerDescription stride("STRIDE", "geometric");
788  stride.addParameter("range", "0.0",
789  "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on "
790  "setup()");
791  stride.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05");
792  stride.addParameter("use_projected_distance", "0",
793  "whether nearest neighbors are computed based on distances in a "
794  "projection of the state rather distances in the state space "
795  "itself. default: 0");
796  stride.addParameter("degree", "16",
797  "desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). "
798  "default: 16");
799  stride.addParameter("max_degree", "18", "max degree of a node in the GNAT. default: 12");
800  stride.addParameter("min_degree", "12", "min degree of a node in the GNAT. default: 12");
801  stride.addParameter("max_pts_per_leaf", "6", "max points per leaf in the GNAT. default: 6");
802  stride.addParameter("estimated_dimension", "0.0", "estimated dimension of the free space. default: 0.0");
803  stride.addParameter("min_valid_path_fraction", "0.2", "Accept partially valid moves above fraction. default: 0.2");
804  planner_des.push_back(stride);
805 
806  OMPLPlannerDescription bi_trrt("BiTRRT", "geometric");
807  bi_trrt.addParameter("range", "0.0",
808  "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on "
809  "setup()");
810  bi_trrt.addParameter("temp_change_factor", "0.1", "how much to increase or decrease temp. default: 0.1");
811  bi_trrt.addParameter("init_temperature", "100", "initial temperature. default: 100");
812  bi_trrt.addParameter("frontier_threshold", "0.0",
813  "dist new state to nearest neighbor to disqualify as frontier. "
814  "default: 0.0 set in setup()");
815  bi_trrt.addParameter("frontier_node_ratio", "0.1", "1/10, or 1 nonfrontier for every 10 frontier. default: 0.1");
816  bi_trrt.addParameter("cost_threshold", "1e300",
817  "the cost threshold. Any motion cost that is not better will not be "
818  "expanded. default: inf");
819  planner_des.push_back(bi_trrt);
820 
821  OMPLPlannerDescription lbtrrt("LBTRRT", "geometric");
822  lbtrrt.addParameter("range", "0.0",
823  "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on "
824  "setup()");
825  lbtrrt.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05");
826  lbtrrt.addParameter("epsilon", "0.4", "optimality approximation factor. default: 0.4");
827  planner_des.push_back(lbtrrt);
828 
829  OMPLPlannerDescription bi_est("BiEST", "geometric");
830  bi_est.addParameter("range", "0.0",
831  "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on "
832  "setup()");
833  planner_des.push_back(bi_est);
834 
835  OMPLPlannerDescription proj_est("ProjEST", "geometric");
836  proj_est.addParameter("range", "0.0",
837  "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on "
838  "setup()");
839  proj_est.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05");
840  planner_des.push_back(proj_est);
841 
842  OMPLPlannerDescription lazy_prm("LazyPRM", "geometric");
843  lazy_prm.addParameter("range", "0.0",
844  "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on "
845  "setup()");
846  planner_des.push_back(lazy_prm);
847 
848  OMPLPlannerDescription lazy_pr_mstar("LazyPRMstar", "geometric"); // no declares in code
849  planner_des.push_back(lazy_pr_mstar);
850 
851  OMPLPlannerDescription spars("SPARS", "geometric");
852  spars.addParameter("stretch_factor", "3.0",
853  "roadmap spanner stretch factor. multiplicative upper bound on path "
854  "quality. It does not make sense to make this parameter more than 3. "
855  "default: 3.0");
856  spars.addParameter("sparse_delta_fraction", "0.25",
857  "delta fraction for connection distance. This value represents "
858  "the visibility range of sparse samples. default: 0.25");
859  spars.addParameter("dense_delta_fraction", "0.001", "delta fraction for interface detection. default: 0.001");
860  spars.addParameter("max_failures", "1000", "maximum consecutive failure limit. default: 1000");
861  planner_des.push_back(spars);
862 
863  OMPLPlannerDescription spar_stwo("SPARStwo", "geometric");
864  spar_stwo.addParameter("stretch_factor", "3.0",
865  "roadmap spanner stretch factor. multiplicative upper bound on path "
866  "quality. It does not make sense to make this parameter more than 3. "
867  "default: 3.0");
868  spar_stwo.addParameter("sparse_delta_fraction", "0.25",
869  "delta fraction for connection distance. This value represents "
870  "the visibility range of sparse samples. default: 0.25");
871  spar_stwo.addParameter("dense_delta_fraction", "0.001", "delta fraction for interface detection. default: 0.001");
872  spar_stwo.addParameter("max_failures", "5000", "maximum consecutive failure limit. default: 5000");
873  planner_des.push_back(spar_stwo);
874 
875  OMPLPlannerDescription aitstar("AITstar", "geometric");
876  aitstar.addParameter("use_k_nearest", "1",
877  "whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1");
878  aitstar.addParameter("rewire_factor", "1.001",
879  "rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001");
880  aitstar.addParameter("samples_per_batch", "100", "batch size. Valid values: [1:1:1000]. Default: 100");
881  aitstar.addParameter("use_graph_pruning", "1", "enable graph pruning (1) or not (0). Default: 1");
882  aitstar.addParameter("find_approximate_solutions", "0", "track approximate solutions (1) or not (0). Default: 0");
883  aitstar.addParameter("set_max_num_goals", "1",
884  "maximum number of goals sampled from sampleable goal regions. "
885  "Valid values: [1:1:1000]. Default: 1");
886  planner_des.push_back(aitstar);
887 
888  OMPLPlannerDescription abitstar("ABITstar", "geometric");
889  abitstar.addParameter("use_k_nearest", "1",
890  "whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1");
891  abitstar.addParameter("rewire_factor", "1.001",
892  "rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001");
893  abitstar.addParameter("samples_per_batch", "100", "batch size. Valid values: [1:1:1000]. Default: 100");
894  abitstar.addParameter("use_graph_pruning", "1", "enable graph pruning (1) or not (0). Default: 1");
895  abitstar.addParameter(
896  "prune_threshold_as_fractional_cost_change", "0.1",
897  "fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1");
898  abitstar.addParameter("delay_rewiring_to_first_solution", "0",
899  "delay (1) or not (0) rewiring until a solution is found. Default: 0");
900  abitstar.addParameter("use_just_in_time_sampling", "0",
901  "delay the generation of samples until they are * necessary. Only works with r-disc connection "
902  "and path length minimization. Default: 0");
903  abitstar.addParameter("drop_unconnected_samples_on_prune", "0",
904  "drop unconnected samples when pruning, regardless of their heuristic value. Default: 0");
905  abitstar.addParameter("stop_on_each_solution_improvement", "0",
906  "stop the planner each time a solution improvement is found. Useful for debugging. Default: 0");
907  abitstar.addParameter("use_strict_queue_ordering", "0",
908  "sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0");
909  abitstar.addParameter("find_approximate_solutions", "0", "track approximate solutions (1) or not (0). Default: 0");
910  abitstar.addParameter(
911  "initial_inflation_factor", "1000000",
912  "inflation factor for the initial search. Valid values: [1.0:0.01:1000000.0]. Default: 1000000");
913  abitstar.addParameter(
914  "inflation_scaling_parameter", "10",
915  "scaling parameter for the inflation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0");
916  abitstar.addParameter(
917  "truncation_scaling_parameter", "5.0",
918  "scaling parameter for the truncation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0");
919  planner_des.push_back(abitstar);
920 
921  OMPLPlannerDescription bitstar("BITstar", "geometric");
922  bitstar.addParameter("use_k_nearest", "1",
923  "whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1");
924  bitstar.addParameter("rewire_factor", "1.001",
925  "rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001");
926  bitstar.addParameter("samples_per_batch", "100", "batch size. Valid values: [1:1:1000]. Default: 100");
927  bitstar.addParameter("use_graph_pruning", "1", "enable graph pruning (1) or not (0). Default: 1");
928  bitstar.addParameter(
929  "prune_threshold_as_fractional_cost_change", "0.1",
930  "fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1");
931  bitstar.addParameter("delay_rewiring_to_first_solution", "0",
932  "delay (1) or not (0) rewiring until a solution is found. Default: 0");
933  bitstar.addParameter("use_just_in_time_sampling", "0",
934  "delay the generation of samples until they are * necessary. Only works with r-disc connection "
935  "and path length minimization. Default: 0");
936  bitstar.addParameter("drop_unconnected_samples_on_prune", "0",
937  "drop unconnected samples when pruning, regardless of their heuristic value. Default: 0");
938  bitstar.addParameter("stop_on_each_solution_improvement", "0",
939  "stop the planner each time a solution improvement is found. Useful for debugging. Default: 0");
940  bitstar.addParameter("use_strict_queue_ordering", "0",
941  "sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0");
942  bitstar.addParameter("find_approximate_solutions", "0", "track approximate solutions (1) or not (0). Default: 0");
943  planner_des.push_back(bitstar);
944 
945  return planner_des;
946 }
947 
948 // ******************************************************************************************
949 // Generate simple_moveit_controllers.yaml config file
950 // ******************************************************************************************
951 bool MoveItConfigData::outputSimpleControllersYAML(const std::string& file_path)
952 {
953  YAML::Emitter emitter;
954  emitter << YAML::BeginMap;
955  emitter << YAML::Key << "controller_list";
956  emitter << YAML::Value << YAML::BeginSeq;
957  for (const auto& controller : controller_configs_)
958  {
959  // Only process FollowJointTrajectory types
960  std::string type = controller.type_;
961  if (boost::ends_with(type, "/JointTrajectoryController"))
962  type = "FollowJointTrajectory";
963  if (type == "FollowJointTrajectory" || type == "GripperCommand")
964  {
965  emitter << YAML::BeginMap;
966  emitter << YAML::Key << "name";
967  emitter << YAML::Value << controller.name_;
968  emitter << YAML::Key << "action_ns";
969  emitter << YAML::Value << (type == "FollowJointTrajectory" ? "follow_joint_trajectory" : "gripper_action");
970  emitter << YAML::Key << "type";
971  emitter << YAML::Value << type;
972  emitter << YAML::Key << "default";
973  emitter << YAML::Value << "True";
974 
975  // Write joints
976  emitter << YAML::Key << "joints";
977  emitter << YAML::Value << YAML::BeginSeq;
978  // Iterate through the joints
979  for (const std::string& joint : controller.joints_)
980  emitter << joint;
981  emitter << YAML::EndSeq;
982 
983  emitter << YAML::EndMap;
984  }
985  }
986  emitter << YAML::EndSeq;
987  emitter << YAML::EndMap;
988 
989  std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
990  if (!output_stream.good())
991  {
992  ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
993  return false;
994  }
995  output_stream << emitter.c_str();
996  output_stream.close();
997 
998  return true; // file created successfully
999 }
1000 
1001 // ******************************************************************************************
1002 // Helper function to get the default start pose for moveit_sim_hw_interface
1003 // ******************************************************************************************
1004 srdf::Model::GroupState MoveItConfigData::getDefaultStartPose()
1005 {
1006  if (!srdf_->group_states_.empty())
1007  return srdf_->group_states_[0];
1008  else
1009  return srdf::Model::GroupState{ .name_ = "todo_state_name", .group_ = "todo_group_name", .joint_values_ = {} };
1010 }
1011 
1012 // ******************************************************************************************
1013 // Generate ros_controllers.yaml config file
1014 // ******************************************************************************************
1015 bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path)
1016 {
1017  // Cache the joints' names.
1018  std::vector<std::vector<std::string>> planning_groups;
1019 
1020  // We are going to write the joints names many times.
1021  // Loop through groups to store the joints names in group_joints vector and reuse is.
1022  for (srdf::Model::Group& group : srdf_->groups_)
1023  {
1024  std::vector<std::string> group_joints;
1025  // Get list of associated joints
1026  const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_);
1027  const std::vector<const moveit::core::JointModel*>& joint_models = joint_model_group->getActiveJointModels();
1028  // Iterate through the joints and push into group_joints vector.
1029  for (const moveit::core::JointModel* joint : joint_models)
1030  {
1031  if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == moveit::core::JointModel::FIXED)
1032  continue;
1033  else
1034  group_joints.push_back(joint->getName());
1035  }
1036  // Push all the group joints into planning_groups vector.
1037  planning_groups.push_back(group_joints);
1038  }
1039 
1040  YAML::Emitter emitter;
1041  emitter << YAML::BeginMap;
1042 
1043  {
1044 #if 0 // TODO: This is only for fake ROS controllers, which should go into a separate file
1045  // Also replace moveit_sim_controllers with http://wiki.ros.org/fake_joint
1046  emitter << YAML::Comment("Simulation settings for using moveit_sim_controllers");
1047  emitter << YAML::Key << "moveit_sim_hw_interface" << YAML::Value << YAML::BeginMap;
1048  // MoveIt Simulation Controller settings for setting initial pose
1049  {
1050  // Use the first planning group if initial joint_model_group was not set, else write a default value
1051  emitter << YAML::Key << "joint_model_group";
1052  emitter << YAML::Value << getDefaultStartPose().group_;
1053 
1054  // Use the first robot pose if initial joint_model_group_pose was not set, else write a default value
1055  emitter << YAML::Key << "joint_model_group_pose";
1056  emitter << YAML::Value << getDefaultStartPose().name_;
1057 
1058  emitter << YAML::EndMap;
1059  }
1060  // Settings for ros_control control loop
1061  emitter << YAML::Newline;
1062  emitter << YAML::Comment("Settings for ros_control_boilerplate control loop");
1063  emitter << YAML::Key << "generic_hw_control_loop" << YAML::Value << YAML::BeginMap;
1064  {
1065  emitter << YAML::Key << "loop_hz";
1066  emitter << YAML::Value << "300";
1067  emitter << YAML::Key << "cycle_time_error_threshold";
1068  emitter << YAML::Value << "0.01";
1069  emitter << YAML::EndMap;
1070  }
1071  // Settings for ros_control hardware interface
1072  emitter << YAML::Newline;
1073  emitter << YAML::Comment("Settings for ros_control hardware interface");
1074  emitter << YAML::Key << "hardware_interface" << YAML::Value << YAML::BeginMap;
1075  {
1076  // Get list of all joints for the robot
1077  const std::vector<const moveit::core::JointModel*>& joint_models = getRobotModel()->getJointModels();
1078 
1079  emitter << YAML::Key << "joints";
1080  {
1081  if (joint_models.size() != 1)
1082  {
1083  emitter << YAML::Value << YAML::BeginSeq;
1084  // Iterate through the joints
1085  for (std::vector<const moveit::core::JointModel*>::const_iterator joint_it = joint_models.begin();
1086  joint_it < joint_models.end(); ++joint_it)
1087  {
1088  if ((*joint_it)->isPassive() || (*joint_it)->getMimic() != nullptr ||
1089  (*joint_it)->getType() == moveit::core::JointModel::FIXED)
1090  continue;
1091  else
1092  emitter << (*joint_it)->getName();
1093  }
1094  emitter << YAML::EndSeq;
1095  }
1096  else
1097  {
1098  emitter << YAML::Value << YAML::BeginMap;
1099  emitter << joint_models[0]->getName();
1100  emitter << YAML::EndMap;
1101  }
1102  }
1103  emitter << YAML::Key << "sim_control_mode";
1104  emitter << YAML::Value << "1";
1105  emitter << YAML::Comment("0: position, 1: velocity");
1106  emitter << YAML::Newline;
1107  emitter << YAML::EndMap;
1108  }
1109 #endif
1110  for (const auto& controller : controller_configs_)
1111  {
1112  if (controller.type_ == "FollowJointTrajectory" || controller.type_ == "GripperCommand")
1113  continue; // these are handled by outputSimpleControllersYAML()
1114 
1115  emitter << YAML::Key << controller.name_;
1116  emitter << YAML::Value << YAML::BeginMap;
1117  emitter << YAML::Key << "type";
1118  emitter << YAML::Value << controller.type_;
1119 
1120  // Write joints
1121  emitter << YAML::Key << "joints";
1122  emitter << YAML::Value << YAML::BeginSeq;
1123  // Iterate through the joints
1124  for (const std::string& joint : controller.joints_)
1125  emitter << joint;
1126  emitter << YAML::EndSeq;
1127 
1128  // Write gains as they are required for vel and effort controllers
1129  emitter << YAML::Key << "gains";
1130  emitter << YAML::Value << YAML::BeginMap;
1131  {
1132  // Iterate through the joints
1133  for (const std::string& joint : controller.joints_)
1134  {
1135  emitter << YAML::Key << joint << YAML::Value << YAML::BeginMap;
1136  emitter << YAML::Key << "p";
1137  emitter << YAML::Value << "100";
1138  emitter << YAML::Key << "d";
1139  emitter << YAML::Value << "1";
1140  emitter << YAML::Key << "i";
1141  emitter << YAML::Value << "1";
1142  emitter << YAML::Key << "i_clamp";
1143  emitter << YAML::Value << "1" << YAML::EndMap;
1144  }
1145  emitter << YAML::EndMap;
1146  }
1147  emitter << YAML::EndMap;
1148  }
1149  }
1150 
1151  std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
1152  if (!output_stream.good())
1153  {
1154  ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
1155  return false;
1156  }
1157  output_stream << emitter.c_str();
1158  output_stream.close();
1159 
1160  return true; // file created successfully
1161 }
1162 
1163 // ******************************************************************************************
1164 // Output 3D Sensor configuration file
1165 // ******************************************************************************************
1166 bool MoveItConfigData::output3DSensorPluginYAML(const std::string& file_path)
1167 {
1168  YAML::Emitter emitter;
1169 
1170  emitter << YAML::BeginMap;
1171  emitter << YAML::Key << "sensors";
1172  emitter << YAML::Value << YAML::BeginSeq;
1173 
1174  for (auto& sensors_plugin_config : sensors_plugin_config_parameter_list_)
1175  {
1176  emitter << YAML::BeginMap;
1177 
1178  for (auto& parameter : sensors_plugin_config)
1179  {
1180  emitter << YAML::Key << parameter.first;
1181  emitter << YAML::Value << parameter.second.getValue();
1182  }
1183  emitter << YAML::EndMap;
1184  }
1185 
1186  emitter << YAML::EndSeq;
1187 
1188  emitter << YAML::EndMap;
1189 
1190  std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
1191  if (!output_stream.good())
1192  {
1193  ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
1194  return false;
1195  }
1196 
1197  output_stream << emitter.c_str();
1198  output_stream.close();
1199 
1200  return true; // file created successfully
1201 }
1202 
1203 // ******************************************************************************************
1204 // Output joint limits config files
1205 // ******************************************************************************************
1206 bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path)
1207 {
1208  YAML::Emitter emitter;
1209  emitter << YAML::Comment("joint_limits.yaml allows the dynamics properties specified in the URDF "
1210  "to be overwritten or augmented as needed");
1211  emitter << YAML::Newline;
1212 
1213  emitter << YAML::BeginMap;
1214 
1215  emitter << YAML::Comment("For beginners, we downscale velocity and acceleration limits.") << YAML::Newline;
1216  emitter << YAML::Comment("You can always specify higher scaling factors (<= 1.0) in your motion requests.");
1217  emitter << YAML::Comment("Increase the values below to 1.0 to always move at maximum speed.");
1218  emitter << YAML::Key << "default_velocity_scaling_factor";
1219  emitter << YAML::Value << "0.1";
1220 
1221  emitter << YAML::Key << "default_acceleration_scaling_factor";
1222  emitter << YAML::Value << "0.1";
1223 
1224  emitter << YAML::Newline << YAML::Newline;
1225  emitter << YAML::Comment("Specific joint properties can be changed with the keys "
1226  "[max_position, min_position, max_velocity, max_acceleration]")
1227  << YAML::Newline;
1228  emitter << YAML::Comment("Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]");
1229 
1230  emitter << YAML::Key << "joint_limits";
1231  emitter << YAML::Value << YAML::BeginMap;
1232 
1233  // Union all the joints in groups. Uses a custom comparator to allow the joints to be sorted by name
1234  std::set<const moveit::core::JointModel*, JointModelCompare> joints;
1235 
1236  // Loop through groups
1237  for (srdf::Model::Group& group : srdf_->groups_)
1238  {
1239  // Get list of associated joints
1240  const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_);
1241 
1242  const std::vector<const moveit::core::JointModel*>& joint_models = joint_model_group->getJointModels();
1243 
1244  // Iterate through the joints
1245  for (const moveit::core::JointModel* joint_model : joint_models)
1246  {
1247  // Check that this joint only represents 1 variable.
1248  if (joint_model->getVariableCount() == 1)
1249  joints.insert(joint_model);
1250  }
1251  }
1252 
1253  // Add joints to yaml file, if no more than 1 dof
1254  for (const moveit::core::JointModel* joint : joints)
1255  {
1256  emitter << YAML::Key << joint->getName();
1257  emitter << YAML::Value << YAML::BeginMap;
1258 
1259  const moveit::core::VariableBounds& b = joint->getVariableBounds()[0];
1260 
1261  // Output property
1262  emitter << YAML::Key << "has_velocity_limits";
1263  if (b.velocity_bounded_)
1264  emitter << YAML::Value << "true";
1265  else
1266  emitter << YAML::Value << "false";
1267 
1268  // Output property
1269  emitter << YAML::Key << "max_velocity";
1270  emitter << YAML::Value << std::min(fabs(b.max_velocity_), fabs(b.min_velocity_));
1271 
1272  // Output property
1273  emitter << YAML::Key << "has_acceleration_limits";
1274  if (b.acceleration_bounded_)
1275  emitter << YAML::Value << "true";
1276  else
1277  emitter << YAML::Value << "false";
1278 
1279  // Output property
1280  emitter << YAML::Key << "max_acceleration";
1281  emitter << YAML::Value << std::min(fabs(b.max_acceleration_), fabs(b.min_acceleration_));
1282 
1283  emitter << YAML::EndMap;
1284  }
1285 
1286  emitter << YAML::EndMap;
1287 
1288  std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
1289  if (!output_stream.good())
1290  {
1291  ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
1292  return false;
1293  }
1294  output_stream << emitter.c_str();
1295  output_stream.close();
1296 
1297  return true; // file created successfully
1298 }
1299 
1300 // ******************************************************************************************
1301 // Decide the best two joints to be used for the projection evaluator
1302 // ******************************************************************************************
1303 std::string MoveItConfigData::decideProjectionJoints(const std::string& planning_group)
1304 {
1305  std::string joint_pair = "";
1306 
1307  // Retrieve pointer to the shared kinematic model
1308  const moveit::core::RobotModelConstPtr& model = getRobotModel();
1309 
1310  // Error check
1311  if (!model->hasJointModelGroup(planning_group))
1312  return joint_pair;
1313 
1314  // Get the joint model group
1315  const moveit::core::JointModelGroup* group = model->getJointModelGroup(planning_group);
1316 
1317  // get vector of joint names
1318  const std::vector<std::string>& joints = group->getJointModelNames();
1319 
1320  if (joints.size() >= 2)
1321  {
1322  // Check that the first two joints have only 1 variable
1323  if (group->getJointModel(joints[0])->getVariableCount() == 1 &&
1324  group->getJointModel(joints[1])->getVariableCount() == 1)
1325  {
1326  // Just choose the first two joints.
1327  joint_pair = "joints(" + joints[0] + "," + joints[1] + ")";
1328  }
1329  }
1330 
1331  return joint_pair;
1332 }
1333 
1334 template <typename T>
1335 bool parse(const YAML::Node& node, const std::string& key, T& storage, const T& default_value = T())
1336 {
1337  const YAML::Node& n = node[key];
1338  bool valid = n.IsDefined();
1339  storage = valid ? n.as<T>() : default_value;
1340  return valid;
1341 }
1342 
1343 bool MoveItConfigData::inputOMPLYAML(const std::string& file_path)
1344 {
1345  // Load file
1346  std::ifstream input_stream(file_path.c_str());
1347  if (!input_stream.good())
1348  {
1349  ROS_ERROR_STREAM("Unable to open file for reading " << file_path);
1350  return false;
1351  }
1352 
1353  // Begin parsing
1354  try
1355  {
1356  YAML::Node doc = YAML::Load(input_stream);
1357 
1358  // Loop through all groups
1359  for (YAML::const_iterator group_it = doc.begin(); group_it != doc.end(); ++group_it)
1360  {
1361  // get group name
1362  const std::string group_name = group_it->first.as<std::string>();
1363 
1364  // compare group name found to list of groups in group_meta_data_
1365  std::map<std::string, GroupMetaData>::iterator group_meta_it;
1366  group_meta_it = group_meta_data_.find(group_name);
1367  if (group_meta_it != group_meta_data_.end())
1368  {
1369  std::string planner;
1370  parse(group_it->second, "default_planner_config", planner);
1371  std::size_t pos = planner.find("kConfigDefault");
1372  if (pos != std::string::npos)
1373  {
1374  planner = planner.substr(0, pos);
1375  }
1376  group_meta_data_[group_name].default_planner_ = planner;
1377  }
1378  }
1379  }
1380  catch (YAML::ParserException& e) // Catch errors
1381  {
1382  ROS_ERROR_STREAM(e.what());
1383  return false;
1384  }
1385  return true;
1386 }
1387 
1388 // ******************************************************************************************
1389 // Input kinematics.yaml file
1390 // ******************************************************************************************
1391 bool MoveItConfigData::inputKinematicsYAML(const std::string& file_path)
1392 {
1393  // Load file
1394  std::ifstream input_stream(file_path.c_str());
1395  if (!input_stream.good())
1396  {
1397  ROS_ERROR_STREAM("Unable to open file for reading " << file_path);
1398  return false;
1399  }
1400 
1401  // Begin parsing
1402  try
1403  {
1404  YAML::Node doc = YAML::Load(input_stream);
1405 
1406  // Loop through all groups
1407  for (YAML::const_iterator group_it = doc.begin(); group_it != doc.end(); ++group_it)
1408  {
1409  const std::string& group_name = group_it->first.as<std::string>();
1410  const YAML::Node group = group_it->second;
1411 
1412  // Create new meta data
1413  GroupMetaData meta_data;
1414 
1415  parse(group, "kinematics_solver", meta_data.kinematics_solver_);
1416  parse(group, "kinematics_solver_search_resolution", meta_data.kinematics_solver_search_resolution_,
1417  DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION);
1418  parse(group, "kinematics_solver_timeout", meta_data.kinematics_solver_timeout_, DEFAULT_KIN_SOLVER_TIMEOUT);
1419  parse(group, "goal_joint_tolerance", meta_data.goal_joint_tolerance_,
1420  moveit::planning_interface::MoveGroupInterface::DEFAULT_GOAL_JOINT_TOLERANCE);
1421  parse(group, "goal_position_tolerance", meta_data.goal_position_tolerance_,
1422  moveit::planning_interface::MoveGroupInterface::DEFAULT_GOAL_POSITION_TOLERANCE);
1423  parse(group, "goal_orientation_tolerance", meta_data.goal_orientation_tolerance_,
1424  moveit::planning_interface::MoveGroupInterface::DEFAULT_GOAL_ORIENTATION_TOLERANCE);
1425 
1426  // Assign meta data to vector
1427  group_meta_data_[group_name] = std::move(meta_data);
1428  }
1429  }
1430  catch (YAML::ParserException& e) // Catch errors
1431  {
1432  ROS_ERROR_STREAM(e.what());
1433  return false;
1434  }
1435 
1436  return true; // file created successfully
1437 }
1438 
1439 // ******************************************************************************************
1440 // Input planning_context.launch file
1441 // ******************************************************************************************
1442 bool MoveItConfigData::inputPlanningContextLaunch(const std::string& file_path)
1443 {
1444  TiXmlDocument launch_document(file_path);
1445  if (!launch_document.LoadFile())
1446  {
1447  ROS_ERROR_STREAM("Failed parsing " << file_path);
1448  return false;
1449  }
1450 
1451  // find the kinematics section
1452  TiXmlHandle doc(&launch_document);
1453  TiXmlElement* kinematics_group = doc.FirstChild("launch").FirstChild("group").ToElement();
1454  while (kinematics_group && kinematics_group->Attribute("ns") &&
1455  kinematics_group->Attribute("ns") != std::string("$(arg robot_description)_kinematics"))
1456  {
1457  kinematics_group = kinematics_group->NextSiblingElement("group");
1458  }
1459  if (!kinematics_group)
1460  {
1461  ROS_ERROR("<group ns=\"$(arg robot_description)_kinematics\"> not found");
1462  return false;
1463  }
1464 
1465  // iterate over all <rosparam namespace="group" file="..."/> elements
1466  // and if 'group' matches an existing group, copy the filename
1467  for (TiXmlElement* kinematics_parameter_file = kinematics_group->FirstChildElement("rosparam");
1468  kinematics_parameter_file; kinematics_parameter_file = kinematics_parameter_file->NextSiblingElement("rosparam"))
1469  {
1470  const char* ns = kinematics_parameter_file->Attribute("ns");
1471  if (ns && (group_meta_data_.find(ns) != group_meta_data_.end()))
1472  {
1473  group_meta_data_[ns].kinematics_parameters_file_ = kinematics_parameter_file->Attribute("file");
1474  }
1475  }
1476 
1477  return true;
1478 }
1479 
1480 // ******************************************************************************************
1481 // Helper function for parsing an individual ROSController from ros_controllers yaml file
1482 // ******************************************************************************************
1483 bool MoveItConfigData::parseROSController(const YAML::Node& controller)
1484 {
1485  // Used in parsing ROS controllers
1486  ControllerConfig control_setting;
1487 
1488  if (const YAML::Node& trajectory_controllers = controller)
1489  {
1490  for (const YAML::Node& trajectory_controller : trajectory_controllers)
1491  {
1492  // Controller node
1493  if (const YAML::Node& controller_node = trajectory_controller)
1494  {
1495  if (const YAML::Node& joints = controller_node["joints"])
1496  {
1497  control_setting.joints_.clear();
1498  for (YAML::const_iterator joint_it = joints.begin(); joint_it != joints.end(); ++joint_it)
1499  {
1500  control_setting.joints_.push_back(joint_it->as<std::string>());
1501  }
1502  if (!parse(controller_node, "name", control_setting.name_))
1503  {
1504  ROS_ERROR_STREAM_NAMED("ros_controller.yaml", "Couldn't parse ros_controllers.yaml");
1505  return false;
1506  }
1507  if (!parse(controller_node, "type", control_setting.type_))
1508  {
1509  ROS_ERROR_STREAM_NAMED("ros_controller.yaml", "Couldn't parse ros_controllers.yaml");
1510  return false;
1511  }
1512  // All required fields were parsed correctly
1513  controller_configs_.push_back(control_setting);
1514  }
1515  else
1516  {
1517  ROS_ERROR_STREAM_NAMED("ros_controller.yaml", "Couldn't parse ros_controllers.yaml");
1518  return false;
1519  }
1520  }
1521  }
1522  }
1523  return true;
1524 }
1525 
1526 // ******************************************************************************************
1527 // Helper function for parsing ROSControllers from ros_controllers yaml file
1528 // ******************************************************************************************
1529 bool MoveItConfigData::processROSControllers(std::ifstream& input_stream)
1530 {
1531  // Used in parsing ROS controllers
1532  ControllerConfig control_setting;
1533  YAML::Node controllers = YAML::Load(input_stream);
1534 
1535  // Loop through all controllers
1536  for (YAML::const_iterator controller_it = controllers.begin(); controller_it != controllers.end(); ++controller_it)
1537  {
1538  // Follow Joint Trajectory action controllers
1539  if (controller_it->first.as<std::string>() == "controller_list")
1540  {
1541  if (!parseROSController(controller_it->second))
1542  return false;
1543  }
1544  // Other settings found in the ros_controllers file
1545  else
1546  {
1547  const std::string& controller_name = controller_it->first.as<std::string>();
1548  control_setting.joints_.clear();
1549 
1550  // Push joints if found in the controller
1551  if (const YAML::Node& joints = controller_it->second["joints"])
1552  {
1553  if (joints.IsSequence())
1554  {
1555  for (YAML::const_iterator joint_it = joints.begin(); joint_it != joints.end(); ++joint_it)
1556  {
1557  control_setting.joints_.push_back(joint_it->as<std::string>());
1558  }
1559  }
1560  else
1561  {
1562  control_setting.joints_.push_back(joints.as<std::string>());
1563  }
1564  }
1565 
1566  // If the setting has joints then it is a controller that needs to be parsed
1567  if (!control_setting.joints_.empty())
1568  {
1569  if (const YAML::Node& urdf_node = controller_it->second["type"])
1570  {
1571  control_setting.type_ = controller_it->second["type"].as<std::string>();
1572  control_setting.name_ = controller_name;
1573  controller_configs_.push_back(control_setting);
1574  control_setting.joints_.clear();
1575  }
1576  }
1577  }
1578  }
1579  return true;
1580 }
1581 
1582 // ******************************************************************************************
1583 // Input ros_controllers.yaml file
1584 // ******************************************************************************************
1585 bool MoveItConfigData::inputROSControllersYAML(const std::string& file_path)
1586 {
1587  // Load file
1588  std::ifstream input_stream(file_path.c_str());
1589  if (!input_stream.good())
1590  {
1591  ROS_WARN_STREAM_NAMED("ros_controllers.yaml", "Does not exist " << file_path);
1592  return false;
1593  }
1594 
1595  // Begin parsing
1596  try
1597  {
1598  processROSControllers(input_stream);
1599  }
1600  catch (YAML::ParserException& e) // Catch errors
1601  {
1602  ROS_ERROR_STREAM(e.what());
1603  return false;
1604  }
1605 
1606  return true; // file read successfully
1607 }
1608 
1609 // ******************************************************************************************
1610 // Add a Follow Joint Trajectory action Controller for each Planning Group
1611 // ******************************************************************************************
1612 bool MoveItConfigData::addDefaultControllers(const std::string& controller_type)
1613 {
1614  if (srdf_->srdf_model_->getGroups().empty())
1615  return false;
1616  // Loop through groups
1617  for (const srdf::Model::Group& group_it : srdf_->srdf_model_->getGroups())
1618  {
1619  ControllerConfig group_controller;
1620  // Get list of associated joints
1621  const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group_it.name_);
1622  const std::vector<const moveit::core::JointModel*>& joint_models = joint_model_group->getActiveJointModels();
1623 
1624  // Iterate through the joints
1625  for (const moveit::core::JointModel* joint : joint_models)
1626  {
1627  if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == moveit::core::JointModel::FIXED)
1628  continue;
1629  group_controller.joints_.push_back(joint->getName());
1630  }
1631  if (!group_controller.joints_.empty())
1632  {
1633  group_controller.name_ = group_it.name_ + "_controller";
1634  group_controller.type_ = controller_type;
1635  addController(group_controller);
1636  }
1637  }
1638  return true;
1639 }
1640 
1641 // ******************************************************************************************
1642 // Set package path; try to resolve path from package name if directory does not exist
1643 // ******************************************************************************************
1644 bool MoveItConfigData::setPackagePath(const std::string& pkg_path)
1645 {
1646  std::string full_package_path;
1647 
1648  // check that the folder exists
1649  if (!fs::is_directory(pkg_path))
1650  {
1651  // does not exist, check if its a package
1652  full_package_path = ros::package::getPath(pkg_path);
1653 
1654  // check that the folder exists
1655  if (!fs::is_directory(full_package_path))
1656  {
1657  return false;
1658  }
1659  }
1660  else
1661  {
1662  // they inputted a full path
1663  full_package_path = pkg_path;
1664  }
1665 
1666  config_pkg_path_ = full_package_path;
1667  return true;
1668 }
1669 
1670 // ******************************************************************************************
1671 // Extract the package/stack name from an absolute file path
1672 // Input: path
1673 // Output: package name and relative path
1674 // ******************************************************************************************
1675 bool MoveItConfigData::extractPackageNameFromPath(const std::string& path, std::string& package_name,
1676  std::string& relative_filepath) const
1677 {
1678  fs::path sub_path = path; // holds the directory less one folder
1679  fs::path relative_path; // holds the path after the sub_path
1680 
1681  bool package_found = false;
1682 
1683  // truncate path step by step and check if it contains a package.xml
1684  while (!sub_path.empty())
1685  {
1686  ROS_DEBUG_STREAM("checking in " << sub_path.make_preferred().string());
1687  if (fs::is_regular_file(sub_path / "package.xml"))
1688  {
1689  ROS_DEBUG_STREAM("Found package.xml in " << sub_path.make_preferred().string());
1690  package_found = true;
1691  relative_filepath = relative_path.string();
1692  package_name = sub_path.filename().string();
1693  break;
1694  }
1695  relative_path = sub_path.filename() / relative_path;
1696  sub_path.remove_filename();
1697  }
1698 
1699  // Assign data to moveit_config_data
1700  if (!package_found)
1701  {
1702  // No package name found, we must be outside ROS
1703  return false;
1704  }
1705 
1706  ROS_DEBUG_STREAM("Package name for file \"" << path << "\" is \"" << package_name << "\"");
1707  return true;
1708 }
1709 
1710 // ******************************************************************************************
1711 // Resolve path to .setup_assistant file
1712 // ******************************************************************************************
1713 
1714 bool MoveItConfigData::getSetupAssistantYAMLPath(std::string& path)
1715 {
1716  path = appendPaths(config_pkg_path_, ".setup_assistant");
1717 
1718  // Check if the old package is a setup assistant package
1719  return fs::is_regular_file(path);
1720 }
1721 
1722 // ******************************************************************************************
1723 // Make the full URDF path using the loaded .setup_assistant data
1724 // ******************************************************************************************
1725 bool MoveItConfigData::createFullURDFPath()
1726 {
1727  boost::trim(urdf_pkg_name_);
1728 
1729  // Check if a package name was provided
1730  if (urdf_pkg_name_.empty() || urdf_pkg_name_ == "\"\"")
1731  {
1732  urdf_path_ = urdf_pkg_relative_path_;
1733  urdf_pkg_name_.clear();
1734  }
1735  else
1736  {
1737  // Check that ROS can find the package
1738  std::string robot_desc_pkg_path = ros::package::getPath(urdf_pkg_name_);
1739 
1740  if (robot_desc_pkg_path.empty())
1741  {
1742  urdf_path_.clear();
1743  return false;
1744  }
1745 
1746  // Append the relative URDF url path
1747  urdf_path_ = appendPaths(robot_desc_pkg_path, urdf_pkg_relative_path_);
1748  }
1749 
1750  // Check that this file exits -------------------------------------------------
1751  return fs::is_regular_file(urdf_path_);
1752 }
1753 
1754 // ******************************************************************************************
1755 // Make the full SRDF path using the loaded .setup_assistant data
1756 // ******************************************************************************************
1757 bool MoveItConfigData::createFullSRDFPath(const std::string& package_path)
1758 {
1759  srdf_path_ = appendPaths(package_path, srdf_pkg_relative_path_);
1760 
1761  return fs::is_regular_file(srdf_path_);
1762 }
1763 
1764 // ******************************************************************************************
1765 // Input .setup_assistant file - contains data used for the MoveIt Setup Assistant
1766 // ******************************************************************************************
1767 bool MoveItConfigData::inputSetupAssistantYAML(const std::string& file_path)
1768 {
1769  // Load file
1770  std::ifstream input_stream(file_path.c_str());
1771  if (!input_stream.good())
1772  {
1773  ROS_ERROR_STREAM("Unable to open file for reading " << file_path);
1774  return false;
1775  }
1776 
1777  // Begin parsing
1778  try
1779  {
1780  const YAML::Node& doc = YAML::Load(input_stream);
1781 
1782  // Get title node
1783  if (const YAML::Node& title_node = doc["moveit_setup_assistant_config"])
1784  {
1785  // URDF Properties
1786  if (const YAML::Node& urdf_node = title_node["URDF"])
1787  {
1788  if (!parse(urdf_node, "package", urdf_pkg_name_))
1789  return false; // if we do not find this value we cannot continue
1790 
1791  if (!parse(urdf_node, "relative_path", urdf_pkg_relative_path_))
1792  return false; // if we do not find this value we cannot continue
1793 
1794  parse(urdf_node, "xacro_args", xacro_args_);
1795  }
1796  // SRDF Properties
1797  if (const YAML::Node& srdf_node = title_node["SRDF"])
1798  {
1799  if (!parse(srdf_node, "relative_path", srdf_pkg_relative_path_))
1800  return false; // if we do not find this value we cannot continue
1801  }
1802  // Package generation time
1803  if (const YAML::Node& config_node = title_node["CONFIG"])
1804  {
1805  parse(config_node, "author_name", author_name_);
1806  parse(config_node, "author_email", author_email_);
1807  parse(config_node, "generated_timestamp", config_pkg_generated_timestamp_);
1808  }
1809  return true;
1810  }
1811  }
1812  catch (YAML::ParserException& e) // Catch errors
1813  {
1814  ROS_ERROR_STREAM(e.what());
1815  }
1816 
1817  return false; // if it gets to this point an error has occured
1818 }
1819 
1820 // ******************************************************************************************
1821 // Input sensors_3d yaml file
1822 // ******************************************************************************************
1823 void MoveItConfigData::input3DSensorsYAML(const std::string& file_path)
1824 {
1825  sensors_plugin_config_parameter_list_ = load3DSensorsYAML(file_path);
1826 }
1827 
1828 // ******************************************************************************************
1829 // Load sensors_3d.yaml file
1830 // ******************************************************************************************
1831 std::vector<std::map<std::string, GenericParameter>> MoveItConfigData::load3DSensorsYAML(const std::string& file_path)
1832 {
1833  std::vector<std::map<std::string, GenericParameter>> config;
1834 
1835  // Is there a sensors config in the package?
1836  if (file_path.empty())
1837  return config;
1838 
1839  // Load file
1840  std::ifstream input_stream(file_path.c_str());
1841  if (!input_stream.good())
1842  {
1843  ROS_ERROR_STREAM_NAMED("sensors_3d.yaml", "Unable to open file for reading " << file_path);
1844  return config;
1845  }
1846 
1847  // Begin parsing
1848  try
1849  {
1850  const YAML::Node& doc = YAML::Load(input_stream);
1851  // Get sensors node
1852  const YAML::Node& sensors_node = doc["sensors"];
1853 
1854  // Make sure that the sensors are written as a sequence
1855  if (sensors_node && sensors_node.IsSequence())
1856  {
1857  // Loop over the sensors available in the file
1858  for (const YAML::Node& sensor : sensors_node)
1859  {
1860  std::map<std::string, GenericParameter> sensor_map;
1861  bool empty_node = true;
1862  for (YAML::const_iterator sensor_it = sensor.begin(); sensor_it != sensor.end(); ++sensor_it)
1863  {
1864  empty_node = false;
1865  GenericParameter sensor_param;
1866  sensor_param.setName(sensor_it->first.as<std::string>());
1867  sensor_param.setValue(sensor_it->second.as<std::string>());
1868 
1869  // Set the key as the parameter name to make accessing it easier
1870  sensor_map[sensor_it->first.as<std::string>()] = sensor_param;
1871  }
1872  // Don't push empty nodes
1873  if (!empty_node)
1874  config.push_back(sensor_map);
1875  }
1876  }
1877  }
1878  catch (YAML::ParserException& e) // Catch errors
1879  {
1880  ROS_ERROR_STREAM("Error parsing sensors yaml: " << e.what());
1881  }
1882 
1883  return config;
1884 }
1885 
1886 // ******************************************************************************************
1887 // Helper Function for joining a file path and a file name, or two file paths, etc, in a cross-platform way
1888 // ******************************************************************************************
1889 std::string MoveItConfigData::appendPaths(const std::string& path1, const std::string& path2)
1890 {
1891  fs::path result = path1;
1892  result /= path2;
1893  return result.make_preferred().string();
1894 }
1895 
1896 srdf::Model::Group* MoveItConfigData::findGroupByName(const std::string& name)
1897 {
1898  // Find the group we are editing based on the goup name string
1899  srdf::Model::Group* searched_group = nullptr; // used for holding our search results
1900 
1901  for (srdf::Model::Group& group : srdf_->groups_)
1902  {
1903  if (group.name_ == name) // string match
1904  {
1905  searched_group = &group; // convert to pointer from iterator
1906  break; // we are done searching
1907  }
1908  }
1909 
1910  // Check if subgroup was found
1911  if (searched_group == nullptr) // not found
1912  ROS_FATAL_STREAM("An internal error has occured while searching for groups. Group '" << name
1913  << "' was not found "
1914  "in the SRDF.");
1915 
1916  return searched_group;
1917 }
1918 
1919 // ******************************************************************************************
1920 // Find a controller by name
1921 // ******************************************************************************************
1922 ControllerConfig* MoveItConfigData::findControllerByName(const std::string& controller_name)
1923 {
1924  // Find the controller we are editing based on its name
1925  for (ControllerConfig& controller : controller_configs_)
1926  {
1927  if (controller.name_ == controller_name) // string match
1928  return &controller; // convert to pointer from iterator
1929  }
1930 
1931  return nullptr; // not found
1932 }
1933 
1934 // ******************************************************************************************
1935 // Deletes a controller by name
1936 // ******************************************************************************************
1937 bool MoveItConfigData::deleteController(const std::string& controller_name)
1938 {
1939  for (std::vector<ControllerConfig>::iterator controller_it = controller_configs_.begin();
1940  controller_it != controller_configs_.end(); ++controller_it)
1941  {
1942  if (controller_it->name_ == controller_name) // string match
1943  {
1944  controller_configs_.erase(controller_it);
1945  // we are done searching
1946  return true;
1947  }
1948  }
1949  return false;
1950 }
1951 
1952 // ******************************************************************************************
1953 // Adds a controller to controller_configs_ vector
1954 // ******************************************************************************************
1955 bool MoveItConfigData::addController(const ControllerConfig& new_controller)
1956 {
1957  // Find if there is an existing controller with the same name
1958  ControllerConfig* controller = findControllerByName(new_controller.name_);
1959 
1960  if (controller && controller->type_ == new_controller.type_)
1961  return false;
1962 
1963  controller_configs_.push_back(new_controller);
1964  return true;
1965 }
1966 
1967 // ******************************************************************************************
1968 // Used to add a sensor plugin configuation parameter to the sensor plugin configuration parameter list
1969 // ******************************************************************************************
1970 void MoveItConfigData::addGenericParameterToSensorPluginConfig(const std::string& name, const std::string& value,
1971  const std::string& /*comment*/)
1972 {
1973  // Use index 0 since we only write one plugin
1974  GenericParameter new_parameter;
1975  new_parameter.setName(name);
1976  new_parameter.setValue(value);
1977  sensors_plugin_config_parameter_list_.resize(1);
1978  sensors_plugin_config_parameter_list_[0][name] = new_parameter;
1979 }
1980 
1981 // ******************************************************************************************
1982 // Used to clear sensor plugin configuration parameter list
1983 // ******************************************************************************************
1984 void MoveItConfigData::clearSensorPluginConfig()
1985 {
1986  sensors_plugin_config_parameter_list_.clear();
1987 }
1988 
1989 } // namespace moveit_setup_assistant
moveit_setup_assistant::MoveItConfigData::setPackagePath
bool setPackagePath(const std::string &pkg_path)
Definition: moveit_config_data.cpp:1676
moveit_setup_assistant::MoveItConfigData::srdf_path_
std::string srdf_path_
Full file-system path to srdf.
Definition: moveit_config_data.h:278
moveit::core::JointModelGroup::getActiveJointModels
const std::vector< const JointModel * > & getActiveJointModels() const
collision_detection::AllowedCollisionMatrix::setDefaultEntry
void setDefaultEntry(const std::string &name, bool allowed)
moveit_setup_assistant::MoveItConfigData::inputOMPLYAML
bool inputOMPLYAML(const std::string &file_path)
Definition: moveit_config_data.cpp:1375
moveit_setup_assistant::MoveItConfigData::input3DSensorsYAML
void input3DSensorsYAML(const std::string &file_path)
Load perception sensor config (sensors_3d.yaml) into internal data structure.
Definition: moveit_config_data.cpp:1855
moveit_setup_assistant::MoveItConfigData::deleteController
bool deleteController(const std::string &controller_name)
Definition: moveit_config_data.cpp:1969
ros::package::getPath
ROSLIB_DECL std::string getPath(const std::string &package_name)
moveit_setup_assistant::MoveItConfigData::createFullSRDFPath
bool createFullSRDFPath(const std::string &package_path)
Make the full SRDF path using the loaded .setup_assistant data.
Definition: moveit_config_data.cpp:1789
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
moveit_setup_assistant::parse
bool parse(const YAML::Node &node, const std::string &key, T &storage, const T &default_value=T())
Definition: moveit_config_data.cpp:1367
moveit_setup_assistant::MoveItConfigData::inputKinematicsYAML
bool inputKinematicsYAML(const std::string &file_path)
Definition: moveit_config_data.cpp:1423
moveit::core::VariableBounds
moveit_setup_assistant::MoveItConfigData::loadAllowedCollisionMatrix
void loadAllowedCollisionMatrix(const srdf::SRDFWriter &srdf)
Load the allowed collision matrix from the SRDF's list of link pairs.
Definition: moveit_config_data.cpp:175
moveit_setup_assistant::MoveItConfigData::clearSensorPluginConfig
void clearSensorPluginConfig()
Clear the sensor plugin configuration parameter list.
Definition: moveit_config_data.cpp:2016
moveit_setup_assistant::MoveItConfigData::robot_model_
moveit::core::RobotModelPtr robot_model_
Shared kinematic model.
Definition: moveit_config_data.h:553
ROS_FATAL_STREAM
#define ROS_FATAL_STREAM(args)
srdf::SRDFWriter
moveit_setup_assistant::MoveItConfigData::MoveItConfigData
MoveItConfigData()
Definition: moveit_config_data.cpp:95
moveit_setup_assistant::OmplPlanningParameter::value
std::string value
Definition: moveit_config_data.h:128
moveit_setup_assistant::MoveItConfigData::decideProjectionJoints
std::string decideProjectionJoints(const std::string &planning_group)
Decide the best two joints to be used for the projection evaluator.
Definition: moveit_config_data.cpp:1335
moveit_setup_assistant::MoveItConfigData::outputOMPLPlanningYAML
bool outputOMPLPlanningYAML(const std::string &file_path)
Definition: moveit_config_data.cpp:266
moveit_setup_assistant::MoveItConfigData::inputSetupAssistantYAML
bool inputSetupAssistantYAML(const std::string &file_path)
Definition: moveit_config_data.cpp:1799
moveit_setup_assistant::MoveItConfigData::addDefaultControllers
bool addDefaultControllers(const std::string &controller_type="effort_controllers/JointTrajectoryController")
Add a Follow Joint Trajectory action Controller for each Planning Group.
Definition: moveit_config_data.cpp:1644
moveit_setup_assistant::MoveItConfigData::group_meta_data_
std::map< std::string, GroupMetaData > group_meta_data_
Planning groups extra data not found in srdf but used in config files.
Definition: moveit_config_data.h:291
collision_detection::AllowedCollision::ALWAYS
ALWAYS
moveit_setup_assistant::MoveItConfigData::urdf_pkg_relative_path_
std::string urdf_pkg_relative_path_
Path relative to urdf package (note: this may be same as urdf_path_)
Definition: moveit_config_data.h:256
moveit_setup_assistant::MoveItConfigData::gazebo_urdf_string_
std::string gazebo_urdf_string_
Gazebo URDF robot model string.
Definition: moveit_config_data.h:271
moveit_setup_assistant::MoveItConfigData::sensors_plugin_config_parameter_list_
std::vector< std::map< std::string, GenericParameter > > sensors_plugin_config_parameter_list_
Sensor plugin configuration parameter list, each sensor plugin type is a map.
Definition: moveit_config_data.h:550
moveit::planning_interface::MoveGroupInterface::DEFAULT_GOAL_POSITION_TOLERANCE
static constexpr double DEFAULT_GOAL_POSITION_TOLERANCE
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
moveit_setup_assistant::MoveItConfigData::outputJointLimitsYAML
bool outputJointLimitsYAML(const std::string &file_path)
Definition: moveit_config_data.cpp:1238
default_value
string default_value
moveit_setup_assistant::MoveItConfigData::addController
bool addController(const ControllerConfig &new_controller)
Adds a controller to controller_configs_ vector.
Definition: moveit_config_data.cpp:1987
moveit_setup_assistant::MoveItConfigData::author_email_
std::string author_email_
Email of the author of this config.
Definition: moveit_config_data.h:315
moveit_setup_assistant::MoveItConfigData::extractPackageNameFromPath
bool extractPackageNameFromPath(const std::string &path, std::string &package_name, std::string &relative_filepath) const
Definition: moveit_config_data.cpp:1707
moveit::planning_interface::MoveGroupInterface::DEFAULT_GOAL_ORIENTATION_TOLERANCE
static constexpr double DEFAULT_GOAL_ORIENTATION_TOLERANCE
moveit_setup_assistant::MoveItConfigData::getOMPLPlanners
std::vector< OMPLPlannerDescription > getOMPLPlanners() const
Definition: moveit_config_data.cpp:687
moveit_config_data.h
moveit_setup_assistant::MoveItConfigData::outputSimpleControllersYAML
bool outputSimpleControllersYAML(const std::string &file_path)
Definition: moveit_config_data.cpp:983
moveit_setup_assistant::GenericParameter::setValue
void setValue(std::string value)
Definition: moveit_config_data.h:184
console.h
collision_detection::AllowedCollisionMatrix::setEntry
void setEntry(bool allowed)
moveit::core::VariableBounds::max_velocity_
double max_velocity_
moveit_setup_assistant::MoveItConfigData::getPlanningScene
planning_scene::PlanningScenePtr getPlanningScene()
Provide a shared planning scene.
Definition: moveit_config_data.cpp:159
srdf::Model::GroupState::group_
std::string group_
moveit_setup_assistant::MoveItConfigData::outputSetupAssistantFile
bool outputSetupAssistantFile(const std::string &file_path)
Definition: moveit_config_data.cpp:193
moveit_setup_assistant::MoveItConfigData::parseROSController
bool parseROSController(const YAML::Node &controller)
Definition: moveit_config_data.cpp:1515
moveit_setup_assistant::ControllerConfig::type_
std::string type_
Definition: moveit_config_data.h:118
moveit_setup_assistant::MoveItConfigData::getSetupAssistantYAMLPath
bool getSetupAssistantYAMLPath(std::string &path)
Definition: moveit_config_data.cpp:1746
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
moveit_setup_assistant::GenericParameter
Definition: moveit_config_data.h:172
moveit_setup_assistant::MoveItConfigData::getRobotModel
moveit::core::RobotModelConstPtr getRobotModel()
Provide a shared kinematic model loader.
Definition: moveit_config_data.cpp:128
moveit_setup_assistant::MoveItConfigData::outputROSControllersYAML
bool outputROSControllersYAML(const std::string &file_path)
Definition: moveit_config_data.cpp:1047
ROS_ERROR
#define ROS_ERROR(...)
value
float value
moveit::core::VariableBounds::min_velocity_
double min_velocity_
moveit_setup_assistant::MoveItConfigData::srdf_
srdf::SRDFWriterPtr srdf_
SRDF Data and Writer.
Definition: moveit_config_data.h:284
moveit_setup_assistant::MoveItConfigData::updateRobotModel
void updateRobotModel()
Update the Kinematic Model with latest SRDF modifications.
Definition: moveit_config_data.cpp:142
moveit_setup_assistant::MoveItConfigData::author_name_
std::string author_name_
Name of the author of this config.
Definition: moveit_config_data.h:312
moveit_setup_assistant::MoveItConfigData::createFullURDFPath
bool createFullURDFPath()
Make the full URDF path using the loaded .setup_assistant data.
Definition: moveit_config_data.cpp:1757
moveit_setup_assistant::MoveItConfigData::appendPaths
std::string appendPaths(const std::string &path1, const std::string &path2)
Definition: moveit_config_data.cpp:1921
name
name
moveit::core::JointModelGroup::getJointModels
const std::vector< const JointModel * > & getJointModels() const
moveit_setup_assistant::MoveItConfigData::addGenericParameterToSensorPluginConfig
void addGenericParameterToSensorPluginConfig(const std::string &name, const std::string &value="", const std::string &comment="")
Used for adding a sensor plugin configuation prameter to the sensor plugin configuration parameter li...
Definition: moveit_config_data.cpp:2002
moveit::planning_interface::MoveGroupInterface::DEFAULT_GOAL_JOINT_TOLERANCE
static constexpr double DEFAULT_GOAL_JOINT_TOLERANCE
package.h
moveit_setup_assistant::MoveItConfigData::load3DSensorsYAML
static std::vector< std::map< std::string, GenericParameter > > load3DSensorsYAML(const std::string &file_path)
Load perception sensor config.
Definition: moveit_config_data.cpp:1863
moveit_setup_assistant::MoveItConfigData::getJointHardwareInterface
std::string getJointHardwareInterface(const std::string &joint_name)
Helper function to get the controller that is controlling the joint.
Definition: moveit_config_data.cpp:550
moveit_setup_assistant::MoveItConfigData::inputROSControllersYAML
bool inputROSControllersYAML(const std::string &file_path)
Definition: moveit_config_data.cpp:1617
srdf::Model::GroupState::joint_values_
std::map< std::string, std::vector< double > > joint_values_
moveit_setup_assistant::MoveItConfigData::~MoveItConfigData
~MoveItConfigData()
ROS_WARN_STREAM_NAMED
#define ROS_WARN_STREAM_NAMED(name, args)
moveit_setup_assistant
Definition: compute_default_collisions.h:46
srdf::Model::GroupState
moveit_setup_assistant::MoveItConfigData::getInitialJoints
std::map< std::string, double > getInitialJoints() const
Definition: moveit_config_data.cpp:665
move_group_interface.h
moveit_setup_assistant::MoveItConfigData::outputSTOMPPlanningYAML
bool outputSTOMPPlanningYAML(const std::string &file_path)
Definition: moveit_config_data.cpp:346
moveit_setup_assistant::GenericParameter::setName
void setName(std::string name)
Definition: moveit_config_data.h:180
moveit_setup_assistant::MoveItConfigData::allowed_collision_matrix_
collision_detection::AllowedCollisionMatrix allowed_collision_matrix_
Allowed collision matrix for robot poses.
Definition: moveit_config_data.h:306
collision_detection::AllowedCollisionMatrix::clear
void clear()
moveit_setup_assistant::MoveItConfigData::findGroupByName
srdf::Model::Group * findGroupByName(const std::string &name)
Definition: moveit_config_data.cpp:1928
moveit_setup_assistant::MoveItConfigData::config_pkg_path_
std::string config_pkg_path_
Loaded configuration package path - if an existing package was loaded, holds that path.
Definition: moveit_config_data.h:297
moveit_setup_assistant::ControllerConfig::name_
std::string name_
Definition: moveit_config_data.h:117
moveit_setup_assistant::DEFAULT_KIN_SOLVER_TIMEOUT
static const double DEFAULT_KIN_SOLVER_TIMEOUT
Definition: moveit_config_data.h:91
moveit_setup_assistant::MoveItConfigData::urdf_model_
urdf::ModelSharedPtr urdf_model_
URDF robot model.
Definition: moveit_config_data.h:264
moveit_setup_assistant::MoveItConfigData::urdf_pkg_name_
std::string urdf_pkg_name_
Name of package containig urdf (note: this may be empty b/c user may not have urdf in pkg)
Definition: moveit_config_data.h:253
moveit_setup_assistant::MoveItConfigData::xacro_args_
std::string xacro_args_
xacro arguments
Definition: moveit_config_data.h:261
moveit_setup_assistant::MoveItConfigData::controller_configs_
std::vector< ControllerConfig > controller_configs_
Controllers config data.
Definition: moveit_config_data.h:556
moveit_setup_assistant::MoveItConfigData::planning_scene_
planning_scene::PlanningScenePtr planning_scene_
Shared planning scene.
Definition: moveit_config_data.h:559
moveit::core::VariableBounds::acceleration_bounded_
bool acceleration_bounded_
moveit_setup_assistant::MoveItConfigData::getDefaultStartPose
srdf::Model::GroupState getDefaultStartPose()
Helper function to get the default start pose for moveit_sim_hw_interface.
Definition: moveit_config_data.cpp:1036
moveit_setup_assistant::MoveItConfigData::srdf_pkg_relative_path_
std::string srdf_pkg_relative_path_
Path relative to loaded configuration package.
Definition: moveit_config_data.h:281
srdf
moveit_setup_assistant::OmplPlanningParameter::comment
std::string comment
Definition: moveit_config_data.h:129
moveit::core::JointModelGroup
moveit_setup_assistant::ControllerConfig::joints_
std::vector< std::string > joints_
Definition: moveit_config_data.h:119
moveit::core::VariableBounds::velocity_bounded_
bool velocity_bounded_
moveit_setup_assistant::MoveItConfigData::findControllerByName
ControllerConfig * findControllerByName(const std::string &controller_name)
Definition: moveit_config_data.cpp:1954
srdf::Model::GroupState::name_
std::string name_
find
auto find(Container &pairs, const std::string &link1, const std::string &link2)
Definition: collision_matrix_model.cpp:98
srdf::Model::Group
moveit_setup_assistant::MoveItConfigData::processROSControllers
bool processROSControllers(std::ifstream &input_stream)
Definition: moveit_config_data.cpp:1561
moveit_setup_assistant::ControllerConfig
Definition: moveit_config_data.h:115
moveit_setup_assistant::MoveItConfigData::outputKinematicsYAML
bool outputKinematicsYAML(const std::string &file_path)
Definition: moveit_config_data.cpp:489
ROS_INFO
#define ROS_INFO(...)
moveit_setup_assistant::OmplPlanningParameter
Definition: moveit_config_data.h:125
moveit_setup_assistant::MoveItConfigData::config_pkg_generated_timestamp_
std::time_t config_pkg_generated_timestamp_
Timestamp when configuration package was generated, if it was previously generated.
Definition: moveit_config_data.h:309
moveit::core::JointModel::FIXED
FIXED
moveit_setup_assistant::MoveItConfigData::output3DSensorPluginYAML
bool output3DSensorPluginYAML(const std::string &file_path)
Definition: moveit_config_data.cpp:1198
moveit::core::VariableBounds::max_acceleration_
double max_acceleration_
moveit_setup_assistant::OMPLPlannerDescription
This class describes the OMPL planners by name, type, and parameter list, used to create the ompl_pla...
Definition: moveit_config_data.h:134
config
config
moveit_setup_assistant::MoveItConfigData::setRobotModel
void setRobotModel(const moveit::core::RobotModelPtr &robot_model)
Load a robot model.
Definition: moveit_config_data.cpp:120
moveit_setup_assistant::MoveItConfigData::urdf_path_
std::string urdf_path_
Full file-system path to urdf.
Definition: moveit_config_data.h:250
moveit_setup_assistant::MoveItConfigData::inputPlanningContextLaunch
bool inputPlanningContextLaunch(const std::string &file_path)
Definition: moveit_config_data.cpp:1474
moveit_setup_assistant::OmplPlanningParameter::name
std::string name
Definition: moveit_config_data.h:127
moveit::core::VariableBounds::min_acceleration_
double min_acceleration_
moveit_setup_assistant::MoveItConfigData::outputFakeControllersYAML
bool outputFakeControllersYAML(const std::string &file_path)
Definition: moveit_config_data.cpp:571
moveit::core::JointModel
moveit_setup_assistant::DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION
static const double DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION
Definition: moveit_config_data.h:90
moveit_setup_assistant::MoveItConfigData::outputGazeboURDFFile
bool outputGazeboURDFFile(const std::string &file_path)
Definition: moveit_config_data.cpp:248
group
group

moveit_setup_assistant
Author(s): Dave Coleman
autogenerated on Sat May 3 2025 02:28:04