44 #include <std_srvs/Empty.h>
45 #include <moveit_msgs/RobotState.h>
49 #include "ui_motion_planning_rviz_plugin_frame.h"
61 ui_->execute_button->setEnabled(
false);
69 ui_->plan_and_execute_button->setEnabled(
false);
70 ui_->execute_button->setEnabled(
false);
77 ui_->stop_button->setEnabled(
false);
99 std::string
c =
ui_->path_constraints_combo_box->itemText(
index).toStdString();
112 ui_->clear_octomap_button->setEnabled(
false);
120 std::vector<geometry_msgs::Pose> waypoints;
121 const std::string& link_name =
move_group_->getEndEffectorLink();
131 double cart_step_size = 0.01;
132 bool avoid_collisions =
true;
135 moveit_msgs::RobotTrajectory trajectory;
136 double fraction =
move_group_->computeCartesianPath(waypoints, cart_step_size, trajectory, avoid_collisions);
140 ROS_INFO(
"Achieved %f %% of Cartesian path", fraction * 100.);
145 rt.setRobotTrajectoryMsg(*
move_group_->getCurrentState(), trajectory);
149 ROS_INFO(
"Computing time stamps %s", success ?
"SUCCEDED" :
"FAILED");
152 current_plan_ = std::make_shared<moveit::planning_interface::MoveGroupInterface::Plan>();
162 current_plan_ = std::make_shared<moveit::planning_interface::MoveGroupInterface::Plan>();
172 ui_->result_label->setText(
"Planning...");
176 bool success = (
ui_->use_cartesian_path->isEnabled() &&
ui_->use_cartesian_path->checkState()) ?
186 ui_->execute_button->setEnabled(
true);
187 ui_->result_label->setText(QString(
"Time: ").
append(QString::number(
current_plan_->planning_time_,
'f', 3)));
192 ui_->result_label->setText(
"Failed");
200 moveit::planning_interface::MoveGroupInterfacePtr mgi(
move_group_);
203 ui_->stop_button->setEnabled(
true);
204 bool success = mgi->execute(*
current_plan_) == moveit::core::MoveItErrorCode::SUCCESS;
212 moveit::planning_interface::MoveGroupInterfacePtr mgi(
move_group_);
219 mgi->setStartStateToCurrentState();
220 ui_->stop_button->setEnabled(
true);
221 if (
ui_->use_cartesian_path->isEnabled() &&
ui_->use_cartesian_path->checkState())
228 bool success = mgi->move() == moveit::core::MoveItErrorCode::SUCCESS;
231 ui_->plan_and_execute_button->setEnabled(
true);
244 ui_->result_label->setText(
"Executed");
246 ui_->result_label->setText(!
ui_->stop_button->isEnabled() ?
"Stopped" :
"Failed");
248 ui_->stop_button->setEnabled(
false);
251 if (
ui_->start_state_combo_box->currentText() ==
"<current>")
262 if (
ui_->start_state_combo_box->currentText() ==
"<current>")
267 if (
ui_->goal_state_combo_box->currentText() ==
"<current>")
275 "update start state");
289 "update goal state");
315 if (v ==
"<random valid>")
323 static const int MAX_ATTEMPTS = 100;
324 int attempt_count = 0;
325 while (attempt_count < MAX_ATTEMPTS)
339 if (attempt_count >= MAX_ATTEMPTS)
340 ROS_WARN(
"Unable to find a random collision free configuration after %d attempts", MAX_ATTEMPTS);
349 if (v ==
"<current>")
354 state = ps->getCurrentState();
358 if (v ==
"<same as goal>")
364 if (v ==
"<same as start>")
370 if (v ==
"<previous>")
383 ui_->planning_pipeline_combo_box->clear();
386 size_t default_planner_index = 0;
389 QString item_text(
d.pipeline_id.c_str());
393 if (item_text.isEmpty())
394 item_text = QString::fromStdString(
d.name);
395 default_planner_index =
ui_->planning_pipeline_combo_box->count();
397 ui_->planning_pipeline_combo_box->addItem(item_text);
401 ui_->planning_pipeline_combo_box->setItemData(default_planner_index, font, Qt::FontRole);
405 ui_->planning_pipeline_combo_box->setCurrentIndex(default_planner_index);
411 ui_->planning_algorithm_combo_box->clear();
414 ui_->library_label->setText(QString::fromStdString(desc.name));
415 ui_->library_label->setStyleSheet(
"QLabel { color : green; font: bold }");
417 bool found_group =
false;
421 for (
const std::string& planner_id : desc.planner_ids)
422 if (planner_id ==
group)
424 else if (planner_id.substr(0,
group.length()) ==
group)
426 if (planner_id.size() >
group.length() && planner_id[
group.length()] ==
'[')
428 std::string
id = planner_id.substr(
group.length());
431 id.resize(
id.
length() - 1);
432 ui_->planning_algorithm_combo_box->addItem(QString::fromStdString(
id.substr(1)));
437 if (
ui_->planning_algorithm_combo_box->count() == 0 && !found_group)
438 for (
const std::string& planner_id : desc.planner_ids)
439 ui_->planning_algorithm_combo_box->addItem(QString::fromStdString(planner_id));
441 ui_->planning_algorithm_combo_box->insertItem(0,
"<unspecified>");
444 const std::string& default_planner_config =
move_group_->getDefaultPlannerId(found_group ? group : std::string());
445 int default_index =
ui_->planning_algorithm_combo_box->findText(QString::fromStdString(default_planner_config));
446 if (default_index < 0)
448 ui_->planning_algorithm_combo_box->setCurrentIndex(default_index);
452 ui_->planning_algorithm_combo_box->setItemData(default_index, font, Qt::FontRole);
463 ui_->path_constraints_combo_box->clear();
464 ui_->path_constraints_combo_box->addItem(
"None");
465 for (
const std::string& constraint : constr)
466 ui_->path_constraints_combo_box->addItem(QString::fromStdString(constraint));
472 mreq.num_planning_attempts =
ui_->planning_attempts->value();
473 mreq.allowed_planning_time =
ui_->planning_time->value();
474 mreq.max_velocity_scaling_factor =
ui_->velocity_scaling_factor->value();
475 mreq.max_acceleration_scaling_factor =
ui_->acceleration_scaling_factor->value();
477 mreq.workspace_parameters.min_corner.x =
ui_->wcenter_x->value() -
ui_->wsize_x->value() / 2.0;
478 mreq.workspace_parameters.min_corner.y =
ui_->wcenter_y->value() -
ui_->wsize_y->value() / 2.0;
479 mreq.workspace_parameters.min_corner.z =
ui_->wcenter_z->value() -
ui_->wsize_z->value() / 2.0;
480 mreq.workspace_parameters.max_corner.x =
ui_->wcenter_x->value() +
ui_->wsize_x->value() / 2.0;
481 mreq.workspace_parameters.max_corner.y =
ui_->wcenter_y->value() +
ui_->wsize_y->value() / 2.0;
482 mreq.workspace_parameters.max_corner.z =
ui_->wcenter_z->value() +
ui_->wsize_z->value() / 2.0;
487 mreq.goal_constraints.resize(1);
510 if (psm && psm->getRobotModelLoader() && psm->getRobotModelLoader()->getModel())
512 const moveit::core::RobotModelPtr& robot_model = psm->getRobotModelLoader()->getModel();
513 const std::vector<moveit::core::JointModel*>& jm = robot_model->getJointModels();
517 joint->setVariableBounds(joint->getName() +
"/" + joint->getLocalVariableNames()[0], bx);
518 joint->setVariableBounds(joint->getName() +
"/" + joint->getLocalVariableNames()[1], by);
522 joint->setVariableBounds(joint->getName() +
"/" + joint->getLocalVariableNames()[0], bx);
523 joint->setVariableBounds(joint->getName() +
"/" + joint->getLocalVariableNames()[1], by);
524 joint->setVariableBounds(joint->getName() +
"/" + joint->getLocalVariableNames()[2], bz);
534 move_group_->setNumPlanningAttempts(
ui_->planning_attempts->value());
535 move_group_->setMaxVelocityScalingFactor(
ui_->velocity_scaling_factor->value());
536 move_group_->setMaxAccelerationScalingFactor(
ui_->acceleration_scaling_factor->value());
587 moveit_msgs::RobotState msg_no_attached(*msg);
588 msg_no_attached.attached_collision_objects.clear();
589 msg_no_attached.is_diff =
true;
605 moveit_msgs::RobotState msg_no_attached(*msg);
606 msg_no_attached.attached_collision_objects.clear();
607 msg_no_attached.is_diff =
true;