Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added combo box to setup assistant for default planner #658

Merged
@@ -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

This comment has been minimized.

Copy link
@rhaschke

rhaschke May 10, 2018

Collaborator

I think, instead of saving a copy of the default planner here, you should store a reference into the YAML document structure, which then allows to access all details.

};

/**
@@ -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();

This comment has been minimized.

Copy link
@rhaschke

rhaschke Oct 18, 2017

Collaborator

Why did you moved this code block around? This makes it harder to see the actual change.

This comment has been minimized.

Copy link
@akgoins

akgoins Nov 8, 2017

Author Contributor

I did not move this block, but rather I took the list of OMPL planners out and made a separate function to generate the list. I guess Git thought this was the right diff to show rather than showing an even larger block of OMPL planners being copied and pasted elsewhere.

I created the getOMPLPlanners() function so that I could get the list of planners to populate the default planner combo box. I looked everywhere, but this was the only location that contained the OMPL planner list, and the function outputOMPLPlanningYAML() did not return the list, but wrote it to a YAML file.


// 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");

This comment has been minimized.

Copy link
@rhaschke

rhaschke May 10, 2018

Collaborator

I suggest not to cut this kConfigDefault suffix. Usually it's this name, but people may change it.

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.h> // 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
ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.