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 #include <tf2_eigen/tf2_eigen.h>
48 
49 #include "ui_motion_planning_rviz_plugin_frame.h"
50 
51 namespace moveit_rviz_plugin
52 {
54 {
56  planning_display_->addBackgroundJob([this] { computePlanButtonClicked(); }, "compute plan");
57 }
58 
60 {
61  ui_->execute_button->setEnabled(false);
62  // execution is done in a separate thread, to not block other background jobs by blocking for synchronous execution
64 }
65 
67 {
69  ui_->plan_and_execute_button->setEnabled(false);
70  ui_->execute_button->setEnabled(false);
71  // execution is done in a separate thread, to not block other background jobs by blocking for synchronous execution
73 }
74 
76 {
77  ui_->stop_button->setEnabled(false); // avoid clicking again
79 }
80 
82 {
83  if (move_group_)
84  move_group_->allowReplanning(checked);
85 }
86 
88 {
89  if (move_group_)
90  move_group_->allowLooking(checked);
91 }
92 
94 {
95  if (move_group_)
96  {
97  if (index > 0)
98  {
99  std::string c = ui_->path_constraints_combo_box->itemText(index).toStdString();
100  if (!move_group_->setPathConstraints(c))
101  ROS_WARN_STREAM("Unable to set the path constraints: " << c);
102  }
103  else
104  move_group_->clearPathConstraints();
105  }
106 }
107 
109 {
110  std_srvs::Empty srv;
112  ui_->clear_octomap_button->setEnabled(false);
113 }
114 
116 {
118  // get goal pose
120  std::vector<geometry_msgs::Pose> waypoints;
121  const std::string& link_name = move_group_->getEndEffectorLink();
122  const moveit::core::LinkModel* link = move_group_->getRobotModel()->getLinkModel(link_name);
123  if (!link)
124  {
125  ROS_ERROR_STREAM("Failed to determine unique end-effector link: " << link_name);
126  return false;
127  }
128  waypoints.push_back(tf2::toMsg(goal.getGlobalLinkTransform(link)));
129 
130  // setup default params
131  double cart_step_size = 0.01;
132  bool avoid_collisions = true;
133 
134  // compute trajectory
135  moveit_msgs::RobotTrajectory trajectory;
136  double fraction = move_group_->computeCartesianPath(waypoints, cart_step_size, trajectory, avoid_collisions);
137 
138  if (fraction >= 1.0)
139  {
140  ROS_INFO("Achieved %f %% of Cartesian path", fraction * 100.);
141 
142  // Compute time parameterization to also provide velocities
143  // https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4
144  robot_trajectory::RobotTrajectory rt(move_group_->getRobotModel(), move_group_->getName());
145  rt.setRobotTrajectoryMsg(*move_group_->getCurrentState(), trajectory);
147  bool success =
148  iptp.computeTimeStamps(rt, ui_->velocity_scaling_factor->value(), ui_->acceleration_scaling_factor->value());
149  ROS_INFO("Computing time stamps %s", success ? "SUCCEDED" : "FAILED");
150 
151  // Store trajectory in current_plan_
152  current_plan_ = std::make_shared<moveit::planning_interface::MoveGroupInterface::Plan>();
153  rt.getRobotTrajectoryMsg(current_plan_->trajectory_);
154  current_plan_->planning_time_ = (ros::WallTime::now() - start).toSec();
155  return success;
156  }
157  return false;
158 }
159 
161 {
162  current_plan_ = std::make_shared<moveit::planning_interface::MoveGroupInterface::Plan>();
163  return move_group_->plan(*current_plan_) == moveit::core::MoveItErrorCode::SUCCESS;
164 }
165 
167 {
168  if (!move_group_)
169  return;
170 
171  // Clear status
172  ui_->result_label->setText("Planning...");
173 
176  bool success = (ui_->use_cartesian_path->isEnabled() && ui_->use_cartesian_path->checkState()) ?
179 
180  if (success)
181  {
183  moveit::core::RobotState current_state(planning_display_->getPlanningSceneRO()->getCurrentState());
184  moveit::core::robotStateMsgToRobotState(current_plan_->start_state_, start_state, true);
185  if (moveit::core::haveSameAttachedObjects(start_state, current_state))
186  ui_->execute_button->setEnabled(true);
187  ui_->result_label->setText(QString("Time: ").append(QString::number(current_plan_->planning_time_, 'f', 3)));
188  }
189  else
190  {
191  current_plan_.reset();
192  ui_->result_label->setText("Failed");
193  }
194  Q_EMIT planningFinished();
195 }
196 
198 {
199  // ensures the MoveGroupInterface is not destroyed while executing
200  moveit::planning_interface::MoveGroupInterfacePtr mgi(move_group_);
201  if (mgi && current_plan_)
202  {
203  ui_->stop_button->setEnabled(true); // enable stopping
204  bool success = mgi->execute(*current_plan_) == moveit::core::MoveItErrorCode::SUCCESS;
205  onFinishedExecution(success);
206  }
207 }
208 
210 {
211  // ensures the MoveGroupInterface is not destroyed while executing
212  moveit::planning_interface::MoveGroupInterfacePtr mgi(move_group_);
213  if (!mgi)
214  return;
217  // move_group::move() on the server side, will always start from the current state
218  // to suppress a warning, we pass an empty state (which encodes "start from current state")
219  mgi->setStartStateToCurrentState();
220  ui_->stop_button->setEnabled(true);
221  if (ui_->use_cartesian_path->isEnabled() && ui_->use_cartesian_path->checkState())
222  {
223  if (computeCartesianPlan())
225  }
226  else
227  {
228  bool success = mgi->move() == moveit::core::MoveItErrorCode::SUCCESS;
230  }
231  ui_->plan_and_execute_button->setEnabled(true);
232 }
233 
235 {
236  if (move_group_)
237  move_group_->stop();
238 }
239 
241 {
242  // visualize result of execution
243  if (success)
244  ui_->result_label->setText("Executed");
245  else
246  ui_->result_label->setText(!ui_->stop_button->isEnabled() ? "Stopped" : "Failed");
247  // disable stop button
248  ui_->stop_button->setEnabled(false);
249 
250  // update query start state to current if neccessary
251  if (ui_->start_state_combo_box->currentText() == "<current>")
252  startStateTextChanged(ui_->start_state_combo_box->currentText());
253 
254  // update query goal state (from previous or to current)
255  // also ensures that joints tab shows goal state model
256  goalStateTextChanged(ui_->goal_state_combo_box->currentText());
257 }
258 
260 {
261  moveit::core::RobotState current(planning_display_->getPlanningSceneRO()->getCurrentState());
262  if (ui_->start_state_combo_box->currentText() == "<current>")
263  {
266  }
267  if (ui_->goal_state_combo_box->currentText() == "<current>")
269 }
270 
271 void MotionPlanningFrame::startStateTextChanged(const QString& start_state)
272 {
273  // use background job: fetching the current state might take up to a second
274  planning_display_->addBackgroundJob([this, state = start_state.toStdString()] { startStateTextChangedExec(state); },
275  "update start state");
276 }
277 
278 void MotionPlanningFrame::startStateTextChangedExec(const std::string& start_state)
279 {
281  updateQueryStateHelper(start, start_state);
283 }
284 
285 void MotionPlanningFrame::goalStateTextChanged(const QString& goal_state)
286 {
287  // use background job: fetching the current state might take up to a second
288  planning_display_->addBackgroundJob([this, state = goal_state.toStdString()] { goalStateTextChangedExec(state); },
289  "update goal state");
290 }
291 
292 void MotionPlanningFrame::goalStateTextChangedExec(const std::string& goal_state)
293 {
295  updateQueryStateHelper(goal, goal_state);
297 }
298 
299 void MotionPlanningFrame::planningGroupTextChanged(const QString& planning_group)
300 {
301  planning_display_->changePlanningGroup(planning_group.toStdString());
302 }
303 
305 {
306  if (v == "<random>")
307  {
309  if (const moveit::core::JointModelGroup* jmg =
311  state.setToRandomPositions(jmg);
312  return;
313  }
314 
315  if (v == "<random valid>")
316  {
318 
319  if (const moveit::core::JointModelGroup* jmg =
321  {
322  // Loop until a collision free state is found
323  static const int MAX_ATTEMPTS = 100;
324  int attempt_count = 0; // prevent loop for going forever
325  while (attempt_count < MAX_ATTEMPTS)
326  {
327  // Generate random state
328  state.setToRandomPositions(jmg);
329 
330  state.update(); // prevent dirty transforms
331 
332  // Test for collision
333  if (planning_display_->getPlanningSceneRO()->isStateValid(state, "", false))
334  break;
335 
336  attempt_count++;
337  }
338  // Explain if no valid rand state found
339  if (attempt_count >= MAX_ATTEMPTS)
340  ROS_WARN("Unable to find a random collision free configuration after %d attempts", MAX_ATTEMPTS);
341  }
342  else
343  {
344  ROS_WARN_STREAM("Unable to get joint model group " << planning_display_->getCurrentPlanningGroup());
345  }
346  return;
347  }
348 
349  if (v == "<current>")
350  {
353  if (ps)
354  state = ps->getCurrentState();
355  return;
356  }
357 
358  if (v == "<same as goal>")
359  {
361  return;
362  }
363 
364  if (v == "<same as start>")
365  {
367  return;
368  }
369 
370  if (v == "<previous>")
371  {
373  return;
374  }
375 
376  // maybe it is a named state
378  state.setToDefaultValues(jmg, v);
379 }
380 
381 void MotionPlanningFrame::populatePlannersList(const std::vector<moveit_msgs::PlannerInterfaceDescription>& desc)
382 {
383  ui_->planning_pipeline_combo_box->clear();
384 
385  planner_descriptions_ = desc;
386  size_t default_planner_index = 0;
387  for (auto& d : planner_descriptions_)
388  {
389  QString item_text(d.pipeline_id.c_str());
390  // Check for default planning pipeline
391  if (d.pipeline_id == default_planning_pipeline_)
392  {
393  if (item_text.isEmpty())
394  item_text = QString::fromStdString(d.name);
395  default_planner_index = ui_->planning_pipeline_combo_box->count();
396  }
397  ui_->planning_pipeline_combo_box->addItem(item_text);
398  }
399  QFont font;
400  font.setBold(true);
401  ui_->planning_pipeline_combo_box->setItemData(default_planner_index, font, Qt::FontRole);
402 
403  // Select default pipeline - triggers populatePlannerDescription() via callback
404  if (!planner_descriptions_.empty())
405  ui_->planning_pipeline_combo_box->setCurrentIndex(default_planner_index);
406 }
407 
408 void MotionPlanningFrame::populatePlannerDescription(const moveit_msgs::PlannerInterfaceDescription& desc)
409 {
411  ui_->planning_algorithm_combo_box->clear();
412 
413  // set the label for the planning library
414  ui_->library_label->setText(QString::fromStdString(desc.name));
415  ui_->library_label->setStyleSheet("QLabel { color : green; font: bold }");
416 
417  bool found_group = false;
418  // the name of a planner is either "GROUP[planner_id]" or "planner_id"
419  if (!group.empty())
420  {
421  for (const std::string& planner_id : desc.planner_ids)
422  if (planner_id == group)
423  found_group = true;
424  else if (planner_id.substr(0, group.length()) == group)
425  {
426  if (planner_id.size() > group.length() && planner_id[group.length()] == '[')
427  {
428  std::string id = planner_id.substr(group.length());
429  if (id.size() > 2)
430  {
431  id.resize(id.length() - 1);
432  ui_->planning_algorithm_combo_box->addItem(QString::fromStdString(id.substr(1)));
433  }
434  }
435  }
436  }
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));
440 
441  ui_->planning_algorithm_combo_box->insertItem(0, "<unspecified>");
442 
443  // retrieve default planner config from parameter server
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)
447  default_index = 0; // 0 is <unspecified> fallback
448  ui_->planning_algorithm_combo_box->setCurrentIndex(default_index);
449 
450  QFont font;
451  font.setBold(true);
452  ui_->planning_algorithm_combo_box->setItemData(default_index, font, Qt::FontRole);
453 }
454 
456 {
457  if (move_group_)
458  planning_display_->addMainLoopJob([this]() { populateConstraintsList(move_group_->getKnownConstraints()); });
459 }
460 
461 void MotionPlanningFrame::populateConstraintsList(const std::vector<std::string>& constr)
462 {
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));
467 }
468 
469 void MotionPlanningFrame::constructPlanningRequest(moveit_msgs::MotionPlanRequest& mreq)
470 {
471  mreq.group_name = planning_display_->getCurrentPlanningGroup();
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;
483  moveit::core::RobotStateConstPtr s = planning_display_->getQueryGoalState();
484  const moveit::core::JointModelGroup* jmg = s->getJointModelGroup(mreq.group_name);
485  if (jmg)
486  {
487  mreq.goal_constraints.resize(1);
488  mreq.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(*s, jmg);
489  }
490 }
491 
493 {
494  moveit::core::VariableBounds bx, by, bz;
496 
498  bx.min_position_ = ui_->wcenter_x->value() - ui_->wsize_x->value() / 2.0;
499  bx.max_position_ = ui_->wcenter_x->value() + ui_->wsize_x->value() / 2.0;
500  by.min_position_ = ui_->wcenter_y->value() - ui_->wsize_y->value() / 2.0;
501  by.max_position_ = ui_->wcenter_y->value() + ui_->wsize_y->value() / 2.0;
502  bz.min_position_ = ui_->wcenter_z->value() - ui_->wsize_z->value() / 2.0;
503  bz.max_position_ = ui_->wcenter_z->value() + ui_->wsize_z->value() / 2.0;
504 
505  if (move_group_)
507  bz.max_position_);
508  planning_scene_monitor::PlanningSceneMonitorPtr psm = planning_display_->getPlanningSceneMonitor();
509  // get non-const access to the robot_model and update planar & floating joints as indicated by the workspace settings
510  if (psm && psm->getRobotModelLoader() && psm->getRobotModelLoader()->getModel())
511  {
512  const moveit::core::RobotModelPtr& robot_model = psm->getRobotModelLoader()->getModel();
513  const std::vector<moveit::core::JointModel*>& jm = robot_model->getJointModels();
514  for (moveit::core::JointModel* joint : jm)
515  if (joint->getType() == moveit::core::JointModel::PLANAR)
516  {
517  joint->setVariableBounds(joint->getName() + "/" + joint->getLocalVariableNames()[0], bx);
518  joint->setVariableBounds(joint->getName() + "/" + joint->getLocalVariableNames()[1], by);
519  }
520  else if (joint->getType() == moveit::core::JointModel::FLOATING)
521  {
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);
525  }
526  }
527 }
528 
530 {
532  move_group_->setJointValueTarget(*planning_display_->getQueryGoalState());
533  move_group_->setPlanningTime(ui_->planning_time->value());
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());
538  if (static_cast<bool>(planning_display_))
540 }
541 
542 void MotionPlanningFrame::remotePlanCallback(const std_msgs::EmptyConstPtr& /*msg*/)
543 {
545 }
546 
547 void MotionPlanningFrame::remoteExecuteCallback(const std_msgs::EmptyConstPtr& /*msg*/)
548 {
550 }
551 
552 void MotionPlanningFrame::remoteStopCallback(const std_msgs::EmptyConstPtr& /*msg*/)
553 {
555 }
556 
557 void MotionPlanningFrame::remoteUpdateStartStateCallback(const std_msgs::EmptyConstPtr& /*msg*/)
558 {
560  {
563  if (ps)
564  {
565  moveit::core::RobotState state = ps->getCurrentState();
567  }
568  }
569 }
570 
571 void MotionPlanningFrame::remoteUpdateGoalStateCallback(const std_msgs::EmptyConstPtr& /*msg*/)
572 {
574  {
577  if (ps)
578  {
579  moveit::core::RobotState state = ps->getCurrentState();
581  }
582  }
583 }
584 
585 void MotionPlanningFrame::remoteUpdateCustomStartStateCallback(const moveit_msgs::RobotStateConstPtr& msg)
586 {
587  moveit_msgs::RobotState msg_no_attached(*msg);
588  msg_no_attached.attached_collision_objects.clear();
589  msg_no_attached.is_diff = true;
591  {
594  if (ps)
595  {
596  moveit::core::RobotStatePtr state(new moveit::core::RobotState(ps->getCurrentState()));
597  moveit::core::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state);
599  }
600  }
601 }
602 
603 void MotionPlanningFrame::remoteUpdateCustomGoalStateCallback(const moveit_msgs::RobotStateConstPtr& msg)
604 {
605  moveit_msgs::RobotState msg_no_attached(*msg);
606  msg_no_attached.attached_collision_objects.clear();
607  msg_no_attached.is_diff = true;
609  {
612  if (ps)
613  {
614  moveit::core::RobotStatePtr state(new moveit::core::RobotState(ps->getCurrentState()));
615  moveit::core::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state);
617  }
618  }
619 }
620 } // namespace moveit_rviz_plugin
moveit::core::LinkModel
moveit_rviz_plugin::MotionPlanningFrame::planningGroupTextChanged
void planningGroupTextChanged(const QString &planning_group)
Definition: motion_planning_frame_planning.cpp:331
moveit_rviz_plugin::MotionPlanningDisplay::getPreviousState
const moveit::core::RobotState & getPreviousState() const
Definition: motion_planning_display.h:109
moveit_rviz_plugin::MotionPlanningFrame::remoteUpdateCustomGoalStateCallback
void remoteUpdateCustomGoalStateCallback(const moveit_msgs::RobotStateConstPtr &msg)
Definition: motion_planning_frame_planning.cpp:635
moveit_rviz_plugin::MotionPlanningFrame::populatePlannersList
void populatePlannersList(const std::vector< moveit_msgs::PlannerInterfaceDescription > &desc)
Definition: motion_planning_frame_planning.cpp:413
moveit_rviz_plugin::MotionPlanningFrame::remotePlanCallback
void remotePlanCallback(const std_msgs::EmptyConstPtr &msg)
Definition: motion_planning_frame_planning.cpp:574
moveit_rviz_plugin::MotionPlanningFrame::computeExecuteButtonClicked
void computeExecuteButtonClicked()
Definition: motion_planning_frame_planning.cpp:229
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
moveit::core::JointModel::PLANAR
PLANAR
moveit_rviz_plugin::MotionPlanningDisplay::rememberPreviousStartState
void rememberPreviousStartState()
Definition: motion_planning_display.cpp:969
moveit_rviz_plugin::MotionPlanningFrame::remoteUpdateCustomStartStateCallback
void remoteUpdateCustomStartStateCallback(const moveit_msgs::RobotStateConstPtr &msg)
Definition: motion_planning_frame_planning.cpp:617
moveit::core::VariableBounds
motion_planning_display.h
moveit::core::VariableBounds::max_position_
double max_position_
tf2_eigen.h
moveit_rviz_plugin::PlanningSceneDisplay::waitForCurrentRobotState
bool waitForCurrentRobotState(const ros::Time &t=ros::Time::now())
wait for robot state more recent than t
Definition: planning_scene_display.cpp:321
utils.h
moveit_rviz_plugin::MotionPlanningFrame::current_plan_
moveit::planning_interface::MoveGroupInterface::PlanPtr current_plan_
Definition: motion_planning_frame.h:133
s
XmlRpcServer s
moveit_rviz_plugin::MotionPlanningFrame::clear_octomap_service_client_
ros::ServiceClient clear_octomap_service_client_
Definition: motion_planning_frame.h:334
moveit_rviz_plugin::MotionPlanningFrame::remoteUpdateGoalStateCallback
void remoteUpdateGoalStateCallback(const std_msgs::EmptyConstPtr &msg)
Definition: motion_planning_frame_planning.cpp:603
trajectory_processing::IterativeParabolicTimeParameterization
kinematic_constraints::constructGoalConstraints
moveit_msgs::Constraints constructGoalConstraints(const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg, double tolerance=std::numeric_limits< double >::epsilon())
moveit_rviz_plugin::MotionPlanningFrame::executeButtonClicked
void executeButtonClicked()
Definition: motion_planning_frame_planning.cpp:91
moveit_rviz_plugin::MotionPlanningFrame::configureWorkspace
void configureWorkspace()
Definition: motion_planning_frame_planning.cpp:524
moveit_rviz_plugin::MotionPlanningFrame::startStateTextChangedExec
void startStateTextChangedExec(const std::string &start_state)
Definition: motion_planning_frame_planning.cpp:310
moveit_rviz_plugin::PlanningSceneDisplay::getPlanningSceneMonitor
const planning_scene_monitor::PlanningSceneMonitorPtr & getPlanningSceneMonitor()
Definition: planning_scene_display.cpp:300
moveit::core::RobotState
moveit::core::RobotState::getGlobalLinkTransform
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
moveit::core::RobotState::setToRandomPositions
void setToRandomPositions()
moveit::core::VariableBounds::min_position_
double min_position_
robot_state.h
moveit_rviz_plugin::MotionPlanningFrame::planAndExecuteButtonClicked
void planAndExecuteButtonClicked()
Definition: motion_planning_frame_planning.cpp:98
moveit_rviz_plugin::PlanningSceneDisplay::addMainLoopJob
void addMainLoopJob(const boost::function< void()> &job)
queue the execution of this function for the next time the main update() loop gets called
Definition: planning_scene_display.cpp:265
robot_trajectory::RobotTrajectory
moveit_rviz_plugin::MotionPlanningFrame::onNewPlanningSceneState
void onNewPlanningSceneState()
Definition: motion_planning_frame_planning.cpp:291
moveit_rviz_plugin::MotionPlanningDisplay::changePlanningGroup
void changePlanningGroup(const std::string &group)
Definition: motion_planning_display.cpp:1041
moveit_rviz_plugin::MotionPlanningDisplay::dropVisualizedTrajectory
void dropVisualizedTrajectory()
Definition: motion_planning_display.h:129
moveit_rviz_plugin::MotionPlanningFrame::planButtonClicked
void planButtonClicked()
Definition: motion_planning_frame_planning.cpp:85
moveit_rviz_plugin::MotionPlanningFrame::computeCartesianPlan
bool computeCartesianPlan()
Definition: motion_planning_frame_planning.cpp:147
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
moveit_rviz_plugin::PlanningSceneDisplay::getPlanningSceneRO
planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO() const
get read-only access to planning scene
Definition: planning_scene_display.cpp:328
moveit::core::RobotState::update
void update(bool force=false)
moveit_rviz_plugin::MotionPlanningFrame::ui_
Ui::MotionPlanningUI * ui_
Definition: motion_planning_frame.h:127
moveit_rviz_plugin::MotionPlanningFrame::planningFinished
void planningFinished()
moveit::core::JointModel::Bounds
std::vector< VariableBounds > Bounds
moveit_rviz_plugin::MotionPlanningFrame::onClearOctomapClicked
void onClearOctomapClicked()
Definition: motion_planning_frame_planning.cpp:140
c
c
moveit_rviz_plugin::MotionPlanningFrame::stopButtonClicked
void stopButtonClicked()
Definition: motion_planning_frame_planning.cpp:107
ros::WallTime::now
static WallTime now()
moveit_rviz_plugin::MotionPlanningFrame::computePlanAndExecuteButtonClicked
void computePlanAndExecuteButtonClicked()
Definition: motion_planning_frame_planning.cpp:241
moveit_rviz_plugin::MotionPlanningFrame::computePlanButtonClicked
void computePlanButtonClicked()
Definition: motion_planning_frame_planning.cpp:198
moveit::core::RobotState::setToDefaultValues
void setToDefaultValues()
moveit_rviz_plugin::MotionPlanningFrame::computeJointSpacePlan
bool computeJointSpacePlan()
Definition: motion_planning_frame_planning.cpp:192
ROS_WARN
#define ROS_WARN(...)
setup.d
d
Definition: setup.py:4
moveit_rviz_plugin::PlanningSceneDisplay::spawnBackgroundJob
void spawnBackgroundJob(const boost::function< void()> &job)
Definition: planning_scene_display.cpp:260
moveit_rviz_plugin::MotionPlanningDisplay::getQueryStartState
moveit::core::RobotStateConstPtr getQueryStartState() const
Definition: motion_planning_display.h:99
moveit_rviz_plugin::MotionPlanningFrame::allowLookingToggled
void allowLookingToggled(bool checked)
Definition: motion_planning_frame_planning.cpp:119
moveit_rviz_plugin::PlanningSceneDisplay::addBackgroundJob
void addBackgroundJob(const boost::function< void()> &job, const std::string &name)
Definition: planning_scene_display.cpp:255
ros::WallTime
moveit_rviz_plugin::MotionPlanningFrame::goalStateTextChangedExec
void goalStateTextChangedExec(const std::string &goal_state)
Definition: motion_planning_frame_planning.cpp:324
moveit_rviz_plugin::MotionPlanningFrame::remoteStopCallback
void remoteStopCallback(const std_msgs::EmptyConstPtr &msg)
Definition: motion_planning_frame_planning.cpp:584
moveit_rviz_plugin::MotionPlanningFrame::populatePlannerDescription
void populatePlannerDescription(const moveit_msgs::PlannerInterfaceDescription &desc)
Definition: motion_planning_frame_planning.cpp:440
iterative_time_parameterization.h
moveit_rviz_plugin::MotionPlanningFrame::planner_descriptions_
std::vector< moveit_msgs::PlannerInterfaceDescription > planner_descriptions_
Definition: motion_planning_frame.h:144
moveit_rviz_plugin::MotionPlanningFrame::allowReplanningToggled
void allowReplanningToggled(bool checked)
Definition: motion_planning_frame_planning.cpp:113
moveit_rviz_plugin
Definition: motion_planning_display.h:80
start
ROSCPP_DECL void start()
moveit_rviz_plugin::MotionPlanningDisplay::setQueryStartState
void setQueryStartState(const moveit::core::RobotState &start)
Definition: motion_planning_display.cpp:974
append
ROSCPP_DECL std::string append(const std::string &left, const std::string &right)
trajectory_processing::IterativeParabolicTimeParameterization::computeTimeStamps
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const override
planning_scene_monitor::LockedPlanningSceneRO
moveit_rviz_plugin::MotionPlanningFrame::updateQueryStateHelper
void updateQueryStateHelper(moveit::core::RobotState &state, const std::string &v)
Definition: motion_planning_frame_planning.cpp:336
length
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
moveit_rviz_plugin::MotionPlanningFrame::remoteUpdateStartStateCallback
void remoteUpdateStartStateCallback(const std_msgs::EmptyConstPtr &msg)
Definition: motion_planning_frame_planning.cpp:589
moveit_rviz_plugin::MotionPlanningFrame::constructPlanningRequest
void constructPlanningRequest(moveit_msgs::MotionPlanRequest &mreq)
Definition: motion_planning_frame_planning.cpp:501
moveit_rviz_plugin::MotionPlanningFrame::populateConstraintsList
void populateConstraintsList()
Definition: motion_planning_frame_planning.cpp:487
ros::ServiceClient::call
bool call(const MReq &req, MRes &resp, const std::string &service_md5sum)
moveit_rviz_plugin::MotionPlanningFrame::configureForPlanning
void configureForPlanning()
Definition: motion_planning_frame_planning.cpp:561
moveit::core::JointModelGroup
index
unsigned int index
moveit_rviz_plugin::PlanningSceneDisplay::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
Definition: planning_scene_display.cpp:310
moveit_rviz_plugin::MotionPlanningDisplay::getCurrentPlanningGroup
std::string getCurrentPlanningGroup() const
Definition: motion_planning_display.cpp:1083
v
v
moveit_rviz_plugin::MotionPlanningFrame::default_planning_pipeline_
std::string default_planning_pipeline_
Definition: motion_planning_frame.h:143
tf2::toMsg
B toMsg(const A &a)
moveit_rviz_plugin::MotionPlanningFrame::publishSceneIfNeeded
void publishSceneIfNeeded()
Definition: motion_planning_frame_objects.cpp:128
conversions.h
moveit_rviz_plugin::MotionPlanningFrame::onFinishedExecution
void onFinishedExecution(bool success)
Definition: motion_planning_frame_planning.cpp:272
moveit_rviz_plugin::MotionPlanningFrame::planning_display_
MotionPlanningDisplay * planning_display_
Definition: motion_planning_frame.h:125
moveit_rviz_plugin::MotionPlanningFrame::remoteExecuteCallback
void remoteExecuteCallback(const std_msgs::EmptyConstPtr &msg)
Definition: motion_planning_frame_planning.cpp:579
moveit::core::JointModel::FLOATING
FLOATING
moveit::core::VariableBounds::position_bounded_
bool position_bounded_
moveit::core::RobotState::getJointModelGroup
const JointModelGroup * getJointModelGroup(const std::string &group) const
moveit_rviz_plugin::MotionPlanningDisplay::setQueryGoalState
void setQueryGoalState(const moveit::core::RobotState &goal)
Definition: motion_planning_display.cpp:980
ROS_INFO
#define ROS_INFO(...)
moveit::core::haveSameAttachedObjects
bool haveSameAttachedObjects(const RobotState &left, const RobotState &right, const std::string &logprefix="")
motion_planning_frame.h
moveit_rviz_plugin::MotionPlanningFrame::startStateTextChanged
void startStateTextChanged(const QString &start_state)
Definition: motion_planning_frame_planning.cpp:303
moveit::core::robotStateToRobotStateMsg
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies=true)
moveit::core::JointModel
moveit_rviz_plugin::MotionPlanningFrame::move_group_
moveit::planning_interface::MoveGroupInterfacePtr move_group_
Definition: motion_planning_frame.h:130
moveit_rviz_plugin::MotionPlanningFrame::goalStateTextChanged
void goalStateTextChanged(const QString &goal_state)
Definition: motion_planning_frame_planning.cpp:317
moveit_rviz_plugin::MotionPlanningFrame::pathConstraintsIndexChanged
void pathConstraintsIndexChanged(int index)
Definition: motion_planning_frame_planning.cpp:125
moveit_rviz_plugin::MotionPlanningFrame::computeStopButtonClicked
void computeStopButtonClicked()
Definition: motion_planning_frame_planning.cpp:266
moveit::core::robotStateMsgToRobotState
bool robotStateMsgToRobotState(const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
moveit_rviz_plugin::MotionPlanningDisplay::getQueryGoalState
moveit::core::RobotStateConstPtr getQueryGoalState() const
Definition: motion_planning_display.h:104
group
group


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sat Mar 15 2025 02:27:25