motion_planning_frame_planning.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: Ioan Sucan */
36 
40 
43 
44 #include <std_srvs/Empty.h>
45 #include <moveit_msgs/RobotState.h>
46 
47 #include "ui_motion_planning_rviz_plugin_frame.h"
48 
49 namespace moveit_rviz_plugin
50 {
52 {
54  "compute plan");
55 }
56 
58 {
59  ui_->execute_button->setEnabled(false);
60  // execution is done in a separate thread, to not block other background jobs by blocking for synchronous execution
62 }
63 
65 {
66  ui_->plan_and_execute_button->setEnabled(false);
67  ui_->execute_button->setEnabled(false);
68  // execution is done in a separate thread, to not block other background jobs by blocking for synchronous execution
70 }
71 
73 {
74  ui_->stop_button->setEnabled(false); // avoid clicking again
76 }
77 
79 {
80  if (move_group_)
81  move_group_->allowReplanning(checked);
82 }
83 
85 {
86  if (move_group_)
87  move_group_->allowLooking(checked);
88 }
89 
91 {
92  if (move_group_)
93  {
94  if (index > 0)
95  {
96  std::string c = ui_->path_constraints_combo_box->itemText(index).toStdString();
97  if (!move_group_->setPathConstraints(c))
98  ROS_WARN_STREAM("Unable to set the path constraints: " << c);
99  }
100  else
101  move_group_->clearPathConstraints();
102  }
103 }
104 
106 {
107  std_srvs::Empty srv;
109 }
110 
112 {
113  if (!move_group_)
114  return;
115 
116  // Clear status
117  ui_->result_label->setText("Planning...");
118 
121  if (move_group_->plan(*current_plan_))
122  {
123  ui_->execute_button->setEnabled(true);
124 
125  // Success
126  ui_->result_label->setText(QString("Time: ").append(QString::number(current_plan_->planning_time_, 'f', 3)));
127  }
128  else
129  {
130  current_plan_.reset();
131 
132  // Failure
133  ui_->result_label->setText("Failed");
134  }
135  Q_EMIT planningFinished();
136 }
137 
139 {
140  if (move_group_ && current_plan_)
141  {
142  ui_->stop_button->setEnabled(true); // enable stopping
143  bool success = move_group_->execute(*current_plan_) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
144  onFinishedExecution(success);
145  }
146 }
147 
149 {
150  if (!move_group_)
151  return;
153  // move_group::move() on the server side, will always start from the current state
154  // to suppress a warning, we pass an empty state (which encodes "start from current state")
155  move_group_->setStartStateToCurrentState();
156  ui_->stop_button->setEnabled(true);
157  bool success = move_group_->move() == moveit::planning_interface::MoveItErrorCode::SUCCESS;
158  onFinishedExecution(success);
159  ui_->plan_and_execute_button->setEnabled(true);
160 }
161 
163 {
164  if (move_group_)
165  move_group_->stop();
166 }
167 
169 {
170  // visualize result of execution
171  if (success)
172  ui_->result_label->setText("Executed");
173  else
174  ui_->result_label->setText(!ui_->stop_button->isEnabled() ? "Stopped" : "Failed");
175  // disable stop button
176  ui_->stop_button->setEnabled(false);
177 
178  // update query start state to current if neccessary
179  if (ui_->start_state_selection->currentText() == "<current>")
181 }
182 
184 {
185  // use background job: fetching the current state might take up to a second
187  "update start state");
188 }
189 
191 {
192  robot_state::RobotState start = *planning_display_->getQueryStartState();
193  updateQueryStateHelper(start, ui_->start_state_selection->currentText().toStdString());
195 }
196 
198 {
199  // use background job: fetching the current state might take up to a second
201  "update goal state");
202 }
203 
205 {
206  robot_state::RobotState goal = *planning_display_->getQueryGoalState();
207  updateQueryStateHelper(goal, ui_->goal_state_selection->currentText().toStdString());
209 }
210 
211 void MotionPlanningFrame::updateQueryStateHelper(robot_state::RobotState& state, const std::string& v)
212 {
213  if (v == "<random>")
214  {
216  if (const robot_model::JointModelGroup* jmg =
217  state.getJointModelGroup(planning_display_->getCurrentPlanningGroup()))
218  state.setToRandomPositions(jmg);
219  return;
220  }
221 
222  if (v == "<random valid>")
223  {
225 
226  if (const robot_model::JointModelGroup* jmg =
227  state.getJointModelGroup(planning_display_->getCurrentPlanningGroup()))
228  {
229  // Loop until a collision free state is found
230  static const int MAX_ATTEMPTS = 100;
231  int attempt_count = 0; // prevent loop for going forever
232  while (attempt_count < MAX_ATTEMPTS)
233  {
234  // Generate random state
235  state.setToRandomPositions(jmg);
236 
237  state.update(); // prevent dirty transforms
238 
239  // Test for collision
240  if (planning_display_->getPlanningSceneRO()->isStateValid(state, "", false))
241  break;
242 
243  attempt_count++;
244  }
245  // Explain if no valid rand state found
246  if (attempt_count >= MAX_ATTEMPTS)
247  ROS_WARN("Unable to find a random collision free configuration after %d attempts", MAX_ATTEMPTS);
248  }
249  else
250  {
251  ROS_WARN_STREAM("Unable to get joint model group " << planning_display_->getCurrentPlanningGroup());
252  }
253  return;
254  }
255 
256  if (v == "<current>")
257  {
260  if (ps)
261  state = ps->getCurrentState();
262  return;
263  }
264 
265  if (v == "<same as goal>")
266  {
268  return;
269  }
270 
271  if (v == "<same as start>")
272  {
274  return;
275  }
276 
277  // maybe it is a named state
278  if (const robot_model::JointModelGroup* jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup()))
279  state.setToDefaultValues(jmg, v);
280 }
281 
282 void MotionPlanningFrame::populatePlannersList(const moveit_msgs::PlannerInterfaceDescription& desc)
283 {
285  ui_->planning_algorithm_combo_box->clear();
286 
287  // set the label for the planning library
288  ui_->library_label->setText(QString::fromStdString(desc.name));
289  ui_->library_label->setStyleSheet("QLabel { color : green; font: bold }");
290 
291  bool found_group = false;
292  // the name of a planner is either "GROUP[planner_id]" or "planner_id"
293  if (!group.empty())
294  {
295  for (std::size_t i = 0; i < desc.planner_ids.size(); ++i)
296  if (desc.planner_ids[i] == group)
297  found_group = true;
298  else if (desc.planner_ids[i].substr(0, group.length()) == group)
299  {
300  if (desc.planner_ids[i].size() > group.length() && desc.planner_ids[i][group.length()] == '[')
301  {
302  std::string id = desc.planner_ids[i].substr(group.length());
303  if (id.size() > 2)
304  {
305  id.resize(id.length() - 1);
306  ui_->planning_algorithm_combo_box->addItem(QString::fromStdString(id.substr(1)));
307  }
308  }
309  }
310  }
311  if (ui_->planning_algorithm_combo_box->count() == 0 && !found_group)
312  for (std::size_t i = 0; i < desc.planner_ids.size(); ++i)
313  ui_->planning_algorithm_combo_box->addItem(QString::fromStdString(desc.planner_ids[i]));
314  ui_->planning_algorithm_combo_box->insertItem(0, "<unspecified>");
315 
316  // retrieve default planner config from parameter server
317  const std::string& default_planner_config = move_group_->getDefaultPlannerId(found_group ? group : std::string());
318  int defaultIndex = ui_->planning_algorithm_combo_box->findText(QString::fromStdString(default_planner_config));
319  if (defaultIndex < 0)
320  defaultIndex = 0; // 0 is <unspecified> fallback
321  ui_->planning_algorithm_combo_box->setCurrentIndex(defaultIndex);
322 }
323 
325 {
326  if (move_group_)
328  boost::bind(&MotionPlanningFrame::populateConstraintsList, this, move_group_->getKnownConstraints()));
329 }
330 
331 void MotionPlanningFrame::populateConstraintsList(const std::vector<std::string>& constr)
332 {
333  ui_->path_constraints_combo_box->clear();
334  ui_->path_constraints_combo_box->addItem("None");
335  for (std::size_t i = 0; i < constr.size(); ++i)
336  ui_->path_constraints_combo_box->addItem(QString::fromStdString(constr[i]));
337 }
338 
339 void MotionPlanningFrame::constructPlanningRequest(moveit_msgs::MotionPlanRequest& mreq)
340 {
341  mreq.group_name = planning_display_->getCurrentPlanningGroup();
342  mreq.num_planning_attempts = ui_->planning_attempts->value();
343  mreq.allowed_planning_time = ui_->planning_time->value();
344  mreq.max_velocity_scaling_factor = ui_->velocity_scaling_factor->value();
345  mreq.max_acceleration_scaling_factor = ui_->acceleration_scaling_factor->value();
346  robot_state::robotStateToRobotStateMsg(*planning_display_->getQueryStartState(), mreq.start_state);
347  mreq.workspace_parameters.min_corner.x = ui_->wcenter_x->value() - ui_->wsize_x->value() / 2.0;
348  mreq.workspace_parameters.min_corner.y = ui_->wcenter_y->value() - ui_->wsize_y->value() / 2.0;
349  mreq.workspace_parameters.min_corner.z = ui_->wcenter_z->value() - ui_->wsize_z->value() / 2.0;
350  mreq.workspace_parameters.max_corner.x = ui_->wcenter_x->value() + ui_->wsize_x->value() / 2.0;
351  mreq.workspace_parameters.max_corner.y = ui_->wcenter_y->value() + ui_->wsize_y->value() / 2.0;
352  mreq.workspace_parameters.max_corner.z = ui_->wcenter_z->value() + ui_->wsize_z->value() / 2.0;
353  robot_state::RobotStateConstPtr s = planning_display_->getQueryGoalState();
354  const robot_state::JointModelGroup* jmg = s->getJointModelGroup(mreq.group_name);
355  if (jmg)
356  {
357  mreq.goal_constraints.resize(1);
358  mreq.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(*s, jmg);
359  }
360 }
361 
363 {
364  robot_model::VariableBounds bx, by, bz;
365  bx.position_bounded_ = by.position_bounded_ = bz.position_bounded_ = true;
366 
367  robot_model::JointModel::Bounds b(3);
368  bx.min_position_ = ui_->wcenter_x->value() - ui_->wsize_x->value() / 2.0;
369  bx.max_position_ = ui_->wcenter_x->value() + ui_->wsize_x->value() / 2.0;
370  by.min_position_ = ui_->wcenter_y->value() - ui_->wsize_y->value() / 2.0;
371  by.max_position_ = ui_->wcenter_y->value() + ui_->wsize_y->value() / 2.0;
372  bz.min_position_ = ui_->wcenter_z->value() - ui_->wsize_z->value() / 2.0;
373  bz.max_position_ = ui_->wcenter_z->value() + ui_->wsize_z->value() / 2.0;
374 
375  if (move_group_)
376  move_group_->setWorkspace(bx.min_position_, by.min_position_, bz.min_position_, bx.max_position_, by.max_position_,
377  bz.max_position_);
378  planning_scene_monitor::PlanningSceneMonitorPtr psm = planning_display_->getPlanningSceneMonitor();
379  // get non-const access to the kmodel and update planar & floating joints as indicated by the workspace settings
380  if (psm && psm->getRobotModelLoader() && psm->getRobotModelLoader()->getModel())
381  {
382  const robot_model::RobotModelPtr& kmodel = psm->getRobotModelLoader()->getModel();
383  const std::vector<robot_model::JointModel*>& jm = kmodel->getJointModels();
384  for (std::size_t i = 0; i < jm.size(); ++i)
385  if (jm[i]->getType() == robot_model::JointModel::PLANAR)
386  {
387  jm[i]->setVariableBounds(jm[i]->getName() + "/" + jm[i]->getLocalVariableNames()[0], bx);
388  jm[i]->setVariableBounds(jm[i]->getName() + "/" + jm[i]->getLocalVariableNames()[1], by);
389  }
390  else if (jm[i]->getType() == robot_model::JointModel::FLOATING)
391  {
392  jm[i]->setVariableBounds(jm[i]->getName() + "/" + jm[i]->getLocalVariableNames()[0], bx);
393  jm[i]->setVariableBounds(jm[i]->getName() + "/" + jm[i]->getLocalVariableNames()[1], by);
394  jm[i]->setVariableBounds(jm[i]->getName() + "/" + jm[i]->getLocalVariableNames()[2], bz);
395  }
396  }
397 }
398 
400 {
402  move_group_->setJointValueTarget(*planning_display_->getQueryGoalState());
403  move_group_->setPlanningTime(ui_->planning_time->value());
404  move_group_->setNumPlanningAttempts(ui_->planning_attempts->value());
405  move_group_->setMaxVelocityScalingFactor(ui_->velocity_scaling_factor->value());
406  move_group_->setMaxAccelerationScalingFactor(ui_->acceleration_scaling_factor->value());
408  if (static_cast<bool>(planning_display_))
410 }
411 
412 void MotionPlanningFrame::remotePlanCallback(const std_msgs::EmptyConstPtr& msg)
413 {
415 }
416 
417 void MotionPlanningFrame::remoteExecuteCallback(const std_msgs::EmptyConstPtr& msg)
418 {
420 }
421 
422 void MotionPlanningFrame::remoteStopCallback(const std_msgs::EmptyConstPtr& msg)
423 {
425 }
426 
427 void MotionPlanningFrame::remoteUpdateStartStateCallback(const std_msgs::EmptyConstPtr& msg)
428 {
430  {
433  if (ps)
434  {
435  robot_state::RobotState state = ps->getCurrentState();
437  }
438  }
439 }
440 
441 void MotionPlanningFrame::remoteUpdateGoalStateCallback(const std_msgs::EmptyConstPtr& msg)
442 {
444  {
447  if (ps)
448  {
449  robot_state::RobotState state = ps->getCurrentState();
451  }
452  }
453 }
454 
455 void MotionPlanningFrame::remoteUpdateCustomStartStateCallback(const moveit_msgs::RobotStateConstPtr& msg)
456 {
457  moveit_msgs::RobotState msg_no_attached(*msg);
458  msg_no_attached.attached_collision_objects.clear();
459  msg_no_attached.is_diff = true;
461  {
464  if (ps)
465  {
466  robot_state::RobotStatePtr state(new robot_state::RobotState(ps->getCurrentState()));
467  robot_state::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state);
469  }
470  }
471 }
472 
473 void MotionPlanningFrame::remoteUpdateCustomGoalStateCallback(const moveit_msgs::RobotStateConstPtr& msg)
474 {
475  moveit_msgs::RobotState msg_no_attached(*msg);
476  msg_no_attached.attached_collision_objects.clear();
477  msg_no_attached.is_diff = true;
479  {
482  if (ps)
483  {
484  robot_state::RobotStatePtr state(new robot_state::RobotState(ps->getCurrentState()));
485  robot_state::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state);
487  }
488  }
489 }
490 }
const planning_scene_monitor::PlanningSceneMonitorPtr & getPlanningSceneMonitor()
void populatePlannersList(const moveit_msgs::PlannerInterfaceDescription &desc)
void remoteUpdateGoalStateCallback(const std_msgs::EmptyConstPtr &msg)
void remoteUpdateCustomStartStateCallback(const moveit_msgs::RobotStateConstPtr &msg)
moveit::planning_interface::MoveGroupInterface::PlanPtr current_plan_
void remoteUpdateStartStateCallback(const std_msgs::EmptyConstPtr &msg)
XmlRpcServer s
std::size_t size(custom_string const &s)
bool call(MReq &req, MRes &res)
std::string getName(void *handle)
robot_state::RobotStateConstPtr getQueryStartState() const
void constructPlanningRequest(moveit_msgs::MotionPlanRequest &mreq)
void setQueryStartState(const robot_state::RobotState &start)
#define ROS_WARN(...)
void updateQueryStateHelper(robot_state::RobotState &state, const std::string &v)
void setQueryGoalState(const robot_state::RobotState &goal)
void remoteExecuteCallback(const std_msgs::EmptyConstPtr &msg)
group
moveit::planning_interface::MoveGroupInterfacePtr move_group_
void addBackgroundJob(const boost::function< void()> &job, const std::string &name)
moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState &state, const robot_model::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
void remotePlanCallback(const std_msgs::EmptyConstPtr &msg)
#define ROS_WARN_STREAM(args)
bool waitForCurrentRobotState(const ros::Time &t=ros::Time::now())
wait for robot state more recent than t
robot_state::RobotStateConstPtr getQueryGoalState() const
void addMainLoopJob(const boost::function< void()> &job)
queue the execution of this function for the next time the main update() loop gets called ...
void remoteUpdateCustomGoalStateCallback(const moveit_msgs::RobotStateConstPtr &msg)
ROSCPP_DECL std::string append(const std::string &left, const std::string &right)
void remoteStopCallback(const std_msgs::EmptyConstPtr &msg)
void spawnBackgroundJob(const boost::function< void()> &job)
planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO() const
get read-only access to planning scene


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:19:08