Skip to content
Permalink
Browse files

setup assistant: combo box to choose default planner (#658)

  • Loading branch information...
akgoins authored and rhaschke committed May 26, 2018
1 parent 4fedbd5 commit 6ab2e362c14775c936dea040953a9554e3092c05
@@ -74,6 +74,7 @@ struct GroupMetaData
double kinematics_solver_search_resolution_; // resolution to use with solver
double kinematics_solver_timeout_; // solver timeout
int kinematics_solver_attempts_; // solver attempts
std::string default_planner_; // Name of the default planner to use
};

/**
@@ -247,6 +248,7 @@ class MoveItConfigData
// ******************************************************************************************
// Public Functions for outputting configuration and setting files
// ******************************************************************************************
std::vector<OMPLPlannerDescription> getOMPLPlanners();
bool outputSetupAssistantFile(const std::string& file_path);
bool outputOMPLPlanningYAML(const std::string& file_path);
bool outputKinematicsYAML(const std::string& file_path);
@@ -267,6 +269,13 @@ class MoveItConfigData
*/
std::string decideProjectionJoints(std::string planning_group);

/**
* Input ompl_planning.yaml file for editing its values
* @param file_path path to ompl_planning.yaml in the input package
* @return bool if the file was read correctly
*/
bool inputOMPLYAML(const std::string& file_path);

/**
* Input kinematics.yaml file for editing its values
* @param file_path path to kinematics.yaml in the input package
@@ -206,6 +206,76 @@ bool MoveItConfigData::outputOMPLPlanningYAML(const std::string& file_path)

emitter << YAML::Value << YAML::BeginMap;

std::vector<OMPLPlannerDescription> planner_des = getOMPLPlanners();

// Add Planners with parameter values
std::vector<std::string> pconfigs;
for (std::size_t i = 0; i < planner_des.size(); ++i)
{
std::string defaultconfig = planner_des[i].name_;
emitter << YAML::Key << defaultconfig;
emitter << YAML::Value << YAML::BeginMap;
emitter << YAML::Key << "type" << YAML::Value << "geometric::" + planner_des[i].name_;
for (std::size_t j = 0; j < planner_des[i].parameter_list_.size(); j++)
{
emitter << YAML::Key << planner_des[i].parameter_list_[j].name;
emitter << YAML::Value << planner_des[i].parameter_list_[j].value;
emitter << YAML::Comment(planner_des[i].parameter_list_[j].comment.c_str());
}
emitter << YAML::EndMap;

pconfigs.push_back(defaultconfig);
}

// End of every avail planner
emitter << YAML::EndMap;

// Output every group and the planners it can use ----------------------------------
for (std::vector<srdf::Model::Group>::iterator group_it = srdf_->groups_.begin(); group_it != srdf_->groups_.end();
++group_it)
{
emitter << YAML::Key << group_it->name_;
emitter << YAML::Value << YAML::BeginMap;
// Output associated planners
emitter << YAML::Key << "default_planner_config" << YAML::Value
<< group_meta_data_[group_it->name_].default_planner_ + "kConfigDefault";
emitter << YAML::Key << "planner_configs";
emitter << YAML::Value << YAML::BeginSeq;
for (std::size_t i = 0; i < pconfigs.size(); ++i)
emitter << pconfigs[i] + "kConfigDefault";
emitter << YAML::EndSeq;

// Output projection_evaluator
std::string projection_joints = decideProjectionJoints(group_it->name_);
if (!projection_joints.empty())
{
emitter << YAML::Key << "projection_evaluator";
emitter << YAML::Value << projection_joints;
// OMPL collision checking discretization
emitter << YAML::Key << "longest_valid_segment_fraction";
emitter << YAML::Value << "0.005";
}

emitter << YAML::EndMap;
}

emitter << YAML::EndMap;

std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
if (!output_stream.good())
{
ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
return false;
}

output_stream << emitter.c_str();
output_stream.close();

return true; // file created successfully
}

std::vector<OMPLPlannerDescription> MoveItConfigData::getOMPLPlanners()
{
std::vector<OMPLPlannerDescription> planner_des;

OMPLPlannerDescription SBL("SBL", "geometric");
@@ -386,68 +456,7 @@ bool MoveItConfigData::outputOMPLPlanningYAML(const std::string& file_path)
SPARStwo.addParameter("max_failures", "5000", "maximum consecutive failure limit. default: 5000");
planner_des.push_back(SPARStwo);

// Add Planners with parameter values
std::vector<std::string> pconfigs;
for (std::size_t i = 0; i < planner_des.size(); ++i)
{
std::string defaultconfig = planner_des[i].name_ + "kConfigDefault";
emitter << YAML::Key << defaultconfig;
emitter << YAML::Value << YAML::BeginMap;
emitter << YAML::Key << "type" << YAML::Value << "geometric::" + planner_des[i].name_;
for (std::size_t j = 0; j < planner_des[i].parameter_list_.size(); j++)
{
emitter << YAML::Key << planner_des[i].parameter_list_[j].name;
emitter << YAML::Value << planner_des[i].parameter_list_[j].value;
emitter << YAML::Comment(planner_des[i].parameter_list_[j].comment.c_str());
}
emitter << YAML::EndMap;

pconfigs.push_back(defaultconfig);
}

// End of every avail planner
emitter << YAML::EndMap;

// Output every group and the planners it can use ----------------------------------
for (std::vector<srdf::Model::Group>::iterator group_it = srdf_->groups_.begin(); group_it != srdf_->groups_.end();
++group_it)
{
emitter << YAML::Key << group_it->name_;
emitter << YAML::Value << YAML::BeginMap;
// Output associated planners
emitter << YAML::Key << "planner_configs";
emitter << YAML::Value << YAML::BeginSeq;
for (std::size_t i = 0; i < pconfigs.size(); ++i)
emitter << pconfigs[i];
emitter << YAML::EndSeq;

// Output projection_evaluator
std::string projection_joints = decideProjectionJoints(group_it->name_);
if (!projection_joints.empty())
{
emitter << YAML::Key << "projection_evaluator";
emitter << YAML::Value << projection_joints;
// OMPL collision checking discretization
emitter << YAML::Key << "longest_valid_segment_fraction";
emitter << YAML::Value << "0.005";
}

emitter << YAML::EndMap;
}

emitter << YAML::EndMap;

std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
if (!output_stream.good())
{
ROS_ERROR_STREAM("Unable to open file for writing " << file_path);
return false;
}

output_stream << emitter.c_str();
output_stream.close();

return true; // file created successfully
return planner_des;
}

// ******************************************************************************************
@@ -740,6 +749,51 @@ bool parse(const YAML::Node& node, const std::string& key, T& storage, const T&
return valid;
}

bool MoveItConfigData::inputOMPLYAML(const std::string& file_path)
{
// Load file
std::ifstream input_stream(file_path.c_str());
if (!input_stream.good())
{
ROS_ERROR_STREAM("Unable to open file for reading " << file_path);
return false;
}

// Begin parsing
try
{
YAML::Node doc = YAML::Load(input_stream);

// Loop through all groups
for (YAML::const_iterator group_it = doc.begin(); group_it != doc.end(); ++group_it)
{
// get group name
const std::string group_name = group_it->first.as<std::string>();

// compare group name found to list of groups in group_meta_data_
std::map<std::string, GroupMetaData>::iterator group_meta_it;
group_meta_it = group_meta_data_.find(group_name);
if (group_meta_it != group_meta_data_.end())
{
std::string planner;
parse(group_it->second, "default_planner_config", planner);
int pos = planner.find_last_not_of("kConfigDefault");
if (pos != std::string::npos)
{
planner = planner.substr(0, pos + 1);
}
group_meta_data_[group_name].default_planner_ = planner;
}
}
}
catch (YAML::ParserException& e) // Catch errors
{
ROS_ERROR_STREAM(e.what());
return false;
}
return true;
}

// ******************************************************************************************
// Input kinematics.yaml file
// ******************************************************************************************
@@ -39,6 +39,7 @@
#include <QMessageBox>
#include <QFormLayout>
#include <QString>
#include <QGroupBox>
#include "group_edit_widget.h"
#include <pluginlib/class_loader.hpp> // for loading all avail kinematic planners

@@ -53,13 +54,16 @@ GroupEditWidget::GroupEditWidget(QWidget* parent, moveit_setup_assistant::MoveIt
// Basic widget container
QVBoxLayout* layout = new QVBoxLayout();

QGroupBox* group1 = new QGroupBox("Kinematics");
QGroupBox* group2 = new QGroupBox("OMPL Planning");

// Label ------------------------------------------------
title_ = new QLabel(this); // specify the title from the parent widget
QFont group_title_font(QFont().defaultFamily(), 12, QFont::Bold);
title_->setFont(group_title_font);
layout->addWidget(title_);

// Simple form -------------------------------------------
// Kinematic form -------------------------------------------
QFormLayout* form_layout = new QFormLayout();
form_layout->setContentsMargins(0, 15, 0, 15);

@@ -89,7 +93,24 @@ GroupEditWidget::GroupEditWidget(QWidget* parent, moveit_setup_assistant::MoveIt
kinematics_attempts_field_->setMaximumWidth(400);
form_layout->addRow("Kin. Solver Attempts:", kinematics_attempts_field_);

layout->addLayout(form_layout);
group1->setLayout(form_layout);

// OMPL Planner form --------------------------------------------

QFormLayout* form_layout2 = new QFormLayout();
form_layout2->setContentsMargins(0, 15, 0, 15);

// Kinematic default planner
default_planner_field_ = new QComboBox(this);
default_planner_field_->setEditable(false);
default_planner_field_->setMaximumWidth(400);
form_layout2->addRow("Group Default Planner:", default_planner_field_);

group2->setLayout(form_layout2);

layout->addWidget(group1);
layout->addWidget(group2);

layout->setAlignment(Qt::AlignTop);

// New Group Options ---------------------------------------------------------
@@ -240,7 +261,25 @@ void GroupEditWidget::setSelected(const std::string& group_name)
kinematics_solver_field_->setCurrentIndex(index);
}

// Set default
// Set default planner
std::string default_planner = config_data_->group_meta_data_[group_name].default_planner_;

// If this group doesn't have a solver, reset it to 'None'
if (default_planner.empty())
{
default_planner = "None";
}

index = default_planner_field_->findText(default_planner.c_str());
if (index == -1)
{
QMessageBox::warning(this, "Missing Default Planner",
QString("Unable to find the default planner '%1'").arg(default_planner.c_str()));
}
else
{
default_planner_field_->setCurrentIndex(index);
}
}

// ******************************************************************************************
@@ -256,9 +295,11 @@ void GroupEditWidget::loadKinematicPlannersComboBox()

// Remove all old items
kinematics_solver_field_->clear();
default_planner_field_->clear();

// Add none option, the default
kinematics_solver_field_->addItem("None");
default_planner_field_->addItem("None");

// load all avail kin planners
std::unique_ptr<pluginlib::ClassLoader<kinematics::KinematicsBase>> loader;
@@ -291,6 +332,13 @@ void GroupEditWidget::loadKinematicPlannersComboBox()
{
kinematics_solver_field_->addItem(plugin_it->c_str());
}

std::vector<OMPLPlannerDescription> planners = config_data_->getOMPLPlanners();
for (int i = 0; i < planners.size(); ++i)
{
std::string planner_name = planners[i].name_;
default_planner_field_->addItem(planner_name.c_str());
}
}

} // namespace
@@ -77,6 +77,7 @@ class GroupEditWidget : public QWidget
QLineEdit* kinematics_resolution_field_;
QLineEdit* kinematics_timeout_field_;
QLineEdit* kinematics_attempts_field_;
QComboBox* default_planner_field_;
QPushButton* btn_delete_; // this button is hidden for new groups
QPushButton* btn_save_; // this button is hidden for new groups
QWidget* new_buttons_widget_; // for showing/hiding the new group buttons
@@ -1093,6 +1093,7 @@ bool PlanningGroupsWidget::saveGroupScreen()
// Get a reference to the supplied strings
const std::string& group_name = group_edit_widget_->group_name_field_->text().toStdString();
const std::string& kinematics_solver = group_edit_widget_->kinematics_solver_field_->currentText().toStdString();
const std::string& default_planner = group_edit_widget_->default_planner_field_->currentText().toStdString();
const std::string& kinematics_resolution = group_edit_widget_->kinematics_resolution_field_->text().toStdString();
const std::string& kinematics_timeout = group_edit_widget_->kinematics_timeout_field_->text().toStdString();
const std::string& kinematics_attempts = group_edit_widget_->kinematics_attempts_field_->text().toStdString();
@@ -1266,6 +1267,7 @@ bool PlanningGroupsWidget::saveGroupScreen()
config_data_->group_meta_data_[group_name].kinematics_solver_search_resolution_ = kinematics_resolution_double;
config_data_->group_meta_data_[group_name].kinematics_solver_timeout_ = kinematics_timeout_double;
config_data_->group_meta_data_[group_name].kinematics_solver_attempts_ = kinematics_attempts_int;
config_data_->group_meta_data_[group_name].default_planner_ = default_planner;
config_data_->changes |= MoveItConfigData::GROUP_KINEMATICS;

// Reload main screen table
@@ -431,6 +431,10 @@ bool StartScreenWidget::loadExistingFiles()
.append(kinematics_yaml_path.make_preferred().native().c_str()));
}

fs::path ompl_yaml_path = config_data_->config_pkg_path_;
ompl_yaml_path /= "config/ompl_planning.yaml";
config_data_->inputOMPLYAML(ompl_yaml_path.make_preferred().native().c_str());

// DONE LOADING --------------------------------------------------------------------------

// Call a function that enables navigation

0 comments on commit 6ab2e36

Please sign in to comment.
You can’t perform that action at this time.