Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include <moveit/motion_planning_rviz_plugin/motion_planning_frame.h>
00033 #include <moveit/motion_planning_rviz_plugin/motion_planning_display.h>
00034
00035 #include <moveit/kinematic_constraints/utils.h>
00036 #include <moveit/robot_state/conversions.h>
00037
00038 #include "ui_motion_planning_rviz_plugin_frame.h"
00039
00040 namespace moveit_rviz_plugin
00041 {
00042
00043 void MotionPlanningFrame::planButtonClicked()
00044 {
00045 planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computePlanButtonClicked, this), "compute plan");
00046 }
00047
00048 void MotionPlanningFrame::executeButtonClicked()
00049 {
00050 ui_->execute_button->setEnabled(false);
00051 planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeExecuteButtonClicked, this), "execute");
00052 }
00053
00054 void MotionPlanningFrame::planAndExecuteButtonClicked()
00055 {
00056 ui_->plan_and_execute_button->setEnabled(false);
00057 ui_->execute_button->setEnabled(false);
00058 planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computePlanAndExecuteButtonClicked, this), "plan and execute");
00059 }
00060
00061 void MotionPlanningFrame::allowReplanningToggled(bool checked)
00062 {
00063 if (move_group_)
00064 move_group_->allowReplanning(checked);
00065 }
00066
00067 void MotionPlanningFrame::allowLookingToggled(bool checked)
00068 {
00069 if (move_group_)
00070 move_group_->allowLooking(checked);
00071 }
00072
00073 void MotionPlanningFrame::pathConstraintsIndexChanged(int index)
00074 {
00075 if (move_group_)
00076 {
00077 if (index > 0)
00078 {
00079 std::string c = ui_->path_constraints_combo_box->itemText(index).toStdString();
00080 if (!move_group_->setPathConstraints(c))
00081 ROS_WARN_STREAM("Unable to set the path constraints: " << c);
00082 }
00083 else
00084 move_group_->clearPathConstraints();
00085 }
00086 }
00087
00088 void MotionPlanningFrame::computePlanButtonClicked()
00089 {
00090 if (!move_group_)
00091 return;
00092 configureForPlanning();
00093 current_plan_.reset(new moveit::planning_interface::MoveGroup::Plan());
00094 if (move_group_->plan(*current_plan_))
00095 ui_->execute_button->setEnabled(true);
00096 else
00097 current_plan_.reset();
00098 }
00099
00100 void MotionPlanningFrame::computeExecuteButtonClicked()
00101 {
00102 if (move_group_ && current_plan_)
00103 move_group_->execute(*current_plan_);
00104 }
00105
00106 void MotionPlanningFrame::computePlanAndExecuteButtonClicked()
00107 {
00108 if (!move_group_)
00109 return;
00110 configureForPlanning();
00111 move_group_->move();
00112 ui_->plan_and_execute_button->setEnabled(true);
00113 }
00114
00115 void MotionPlanningFrame::useStartStateButtonClicked()
00116 {
00117 robot_state::RobotState start = *planning_display_->getQueryStartState();
00118 updateQueryStateHelper(start, ui_->start_state_selection->currentText().toStdString());
00119 planning_display_->setQueryStartState(start);
00120 }
00121
00122 void MotionPlanningFrame::useGoalStateButtonClicked()
00123 {
00124 robot_state::RobotState goal = *planning_display_->getQueryGoalState();
00125 updateQueryStateHelper(goal, ui_->goal_state_selection->currentText().toStdString());
00126 planning_display_->setQueryGoalState(goal);
00127 }
00128
00129 void MotionPlanningFrame::updateQueryStateHelper(robot_state::RobotState &state, const std::string &v)
00130 {
00131 if (v == "<random>")
00132 {
00133 configureWorkspace();
00134 if (robot_state::JointStateGroup *jsg = state.getJointStateGroup(planning_display_->getCurrentPlanningGroup()))
00135 jsg->setToRandomValues();
00136 }
00137 else
00138 if (v == "<current>")
00139 {
00140 const planning_scene_monitor::LockedPlanningSceneRO &ps = planning_display_->getPlanningSceneRO();
00141 if (ps)
00142 state = ps->getCurrentState();
00143 }
00144 else
00145 if (v == "<same as goal>")
00146 {
00147 state = *planning_display_->getQueryGoalState();
00148 }
00149 else
00150 if (v == "<same as start>")
00151 {
00152 state = *planning_display_->getQueryStartState();
00153 }
00154 else
00155 {
00156
00157 if (robot_state::JointStateGroup *jsg = state.getJointStateGroup(planning_display_->getCurrentPlanningGroup()))
00158 jsg->setToDefaultState(v);
00159 }
00160 }
00161
00162 void MotionPlanningFrame::populatePlannersList(const moveit_msgs::PlannerInterfaceDescription &desc)
00163 {
00164 std::string group = planning_display_->getCurrentPlanningGroup();
00165 ui_->planning_algorithm_combo_box->clear();
00166
00167
00168 ui_->library_label->setText(QString::fromStdString(desc.name));
00169 ui_->library_label->setStyleSheet("QLabel { color : green; font: bold }");
00170
00171 bool found_group = false;
00172
00173 if (!group.empty())
00174 for (std::size_t i = 0 ; i < desc.planner_ids.size() ; ++i)
00175 if (desc.planner_ids[i] == group)
00176 found_group = true;
00177 else
00178 if (desc.planner_ids[i].substr(0, group.length()) == group)
00179 {
00180 if (desc.planner_ids[i].size() > group.length() && desc.planner_ids[i][group.length()] == '[')
00181 {
00182 std::string id = desc.planner_ids[i].substr(group.length());
00183 if (id.size() > 2)
00184 {
00185 id.resize(id.length() - 1);
00186 ui_->planning_algorithm_combo_box->addItem(QString::fromStdString(id.substr(1)));
00187 }
00188 }
00189 }
00190 if (ui_->planning_algorithm_combo_box->count() == 0 && !found_group)
00191 for (std::size_t i = 0 ; i < desc.planner_ids.size() ; ++i)
00192 ui_->planning_algorithm_combo_box->addItem(QString::fromStdString(desc.planner_ids[i]));
00193 ui_->planning_algorithm_combo_box->insertItem(0, "<unspecified>");
00194 ui_->planning_algorithm_combo_box->setCurrentIndex(0);
00195 }
00196
00197 void MotionPlanningFrame::populateConstraintsList()
00198 {
00199 if (move_group_)
00200 planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateConstraintsList, this, move_group_->getKnownConstraints()));
00201 }
00202
00203 void MotionPlanningFrame::populateConstraintsList(const std::vector<std::string> &constr)
00204 {
00205 ui_->path_constraints_combo_box->clear();
00206 ui_->path_constraints_combo_box->addItem("None");
00207 for (std::size_t i = 0 ; i < constr.size() ; ++i)
00208 ui_->path_constraints_combo_box->addItem(QString::fromStdString(constr[i]));
00209 }
00210
00211 void MotionPlanningFrame::constructPlanningRequest(moveit_msgs::MotionPlanRequest &mreq)
00212 {
00213 mreq.group_name = planning_display_->getCurrentPlanningGroup();
00214 mreq.num_planning_attempts = 1;
00215 mreq.allowed_planning_time = ui_->planning_time->value();
00216 robot_state::robotStateToRobotStateMsg(*planning_display_->getQueryStartState(), mreq.start_state);
00217 mreq.workspace_parameters.min_corner.x = ui_->wcenter_x->value() - ui_->wsize_x->value() / 2.0;
00218 mreq.workspace_parameters.min_corner.y = ui_->wcenter_y->value() - ui_->wsize_y->value() / 2.0;
00219 mreq.workspace_parameters.min_corner.z = ui_->wcenter_z->value() - ui_->wsize_z->value() / 2.0;
00220 mreq.workspace_parameters.max_corner.x = ui_->wcenter_x->value() + ui_->wsize_x->value() / 2.0;
00221 mreq.workspace_parameters.max_corner.y = ui_->wcenter_y->value() + ui_->wsize_y->value() / 2.0;
00222 mreq.workspace_parameters.max_corner.z = ui_->wcenter_z->value() + ui_->wsize_z->value() / 2.0;
00223 const robot_state::JointStateGroup *jsg = planning_display_->getQueryGoalState()->getJointStateGroup(mreq.group_name);
00224 if (jsg)
00225 {
00226 mreq.goal_constraints.resize(1);
00227 mreq.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(jsg);
00228 }
00229 }
00230
00231 void MotionPlanningFrame::configureWorkspace()
00232 {
00233 robot_model::JointModel::Bounds b(3);
00234 b[0].first = ui_->wcenter_x->value() - ui_->wsize_x->value() / 2.0;
00235 b[0].second = ui_->wcenter_x->value() + ui_->wsize_x->value() / 2.0;
00236 b[1].first = ui_->wcenter_y->value() - ui_->wsize_y->value() / 2.0;
00237 b[1].second = ui_->wcenter_y->value() + ui_->wsize_y->value() / 2.0;
00238 b[2].first = ui_->wcenter_z->value() - ui_->wsize_z->value() / 2.0;
00239 b[2].second = ui_->wcenter_z->value() + ui_->wsize_z->value() / 2.0;
00240
00241 if (move_group_)
00242 move_group_->setWorkspace(b[0].first, b[1].first, b[2].first,
00243 b[0].second, b[1].second, b[2].second);
00244
00245 if (planning_display_->getPlanningSceneMonitor() && planning_display_->getPlanningSceneMonitor()->getRobotModelLoader() &&
00246 planning_display_->getPlanningSceneMonitor()->getRobotModelLoader()->getModel())
00247 {
00248 const robot_model::RobotModelPtr &kmodel = planning_display_->getPlanningSceneMonitor()->getRobotModelLoader()->getModel();
00249 const std::vector<robot_model::JointModel*> &jm = kmodel->getJointModels();
00250 for (std::size_t i = 0 ; i < jm.size() ; ++i)
00251 if (jm[i]->getType() == robot_model::JointModel::PLANAR)
00252 {
00253 jm[i]->setVariableBounds(jm[i]->getName() + "/x", b[0]);
00254 jm[i]->setVariableBounds(jm[i]->getName() + "/y", b[1]);
00255 }
00256 else
00257 if (jm[i]->getType() == robot_model::JointModel::FLOATING)
00258 {
00259 jm[i]->setVariableBounds(jm[i]->getName() + "/trans_x", b[0]);
00260 jm[i]->setVariableBounds(jm[i]->getName() + "/trans_y", b[1]);
00261 jm[i]->setVariableBounds(jm[i]->getName() + "/trans_z", b[2]);
00262 }
00263 }
00264 }
00265
00266 void MotionPlanningFrame::configureForPlanning()
00267 {
00268 move_group_->setStartState(*planning_display_->getQueryStartState());
00269 move_group_->setJointValueTarget(*planning_display_->getQueryGoalState());
00270 move_group_->setPlanningTime(ui_->planning_time->value());
00271 configureWorkspace();
00272 }
00273
00274 }