moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
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/srv/empty.hpp>
45#include <moveit_msgs/msg/robot_state.hpp>
46#include <tf2_eigen/tf2_eigen.hpp>
48
49#include "ui_motion_planning_rviz_plugin_frame.h"
50
51namespace moveit_rviz_plugin
52{
53
54void MotionPlanningFrame::planButtonClicked()
55{
56 publishSceneIfNeeded();
57 planning_display_->addBackgroundJob([this] { computePlanButtonClicked(); }, "compute plan");
58}
59
60void MotionPlanningFrame::executeButtonClicked()
61{
62 ui_->execute_button->setEnabled(false);
63 // execution is done in a separate thread, to not block other background jobs by blocking for synchronous execution
64 planning_display_->spawnBackgroundJob([this] { computeExecuteButtonClicked(); });
65}
66
67void MotionPlanningFrame::planAndExecuteButtonClicked()
68{
69 publishSceneIfNeeded();
70 ui_->plan_and_execute_button->setEnabled(false);
71 ui_->execute_button->setEnabled(false);
72 // execution is done in a separate thread, to not block other background jobs by blocking for synchronous execution
73 planning_display_->spawnBackgroundJob([this] { computePlanAndExecuteButtonClicked(); });
74}
75
76void MotionPlanningFrame::stopButtonClicked()
77{
78 ui_->stop_button->setEnabled(false); // avoid clicking again
79 planning_display_->addBackgroundJob([this] { computeStopButtonClicked(); }, "stop");
80}
81
82void MotionPlanningFrame::allowReplanningToggled(bool checked)
83{
84 if (move_group_)
85 move_group_->allowReplanning(checked);
86}
87
88void MotionPlanningFrame::allowLookingToggled(bool checked)
89{
90 if (move_group_)
91 move_group_->allowLooking(checked);
92}
93
94void MotionPlanningFrame::pathConstraintsIndexChanged(int index)
95{
96 if (move_group_)
97 {
98 if (index > 0)
99 {
100 std::string c = ui_->path_constraints_combo_box->itemText(index).toStdString();
101 if (!move_group_->setPathConstraints(c))
102 {
103 RCLCPP_WARN_STREAM(logger_, "Unable to set the path constraints: " << c);
104 }
105 }
106 else
107 {
108 move_group_->clearPathConstraints();
109 }
110 }
111}
112
113void MotionPlanningFrame::onClearOctomapClicked()
114{
115 auto req = std::make_shared<std_srvs::srv::Empty::Request>();
116 auto result = clear_octomap_service_client_->async_send_request(req);
117
118 if (result.wait_for(std::chrono::seconds(0)) != std::future_status::ready)
119 {
120 RCLCPP_ERROR(logger_, "Failed to call clear_octomap_service");
121 }
122 ui_->clear_octomap_button->setEnabled(false);
123}
124
125bool MotionPlanningFrame::computeCartesianPlan()
126{
127 rclcpp::Time start = rclcpp::Clock().now();
128 // get goal pose
129 moveit::core::RobotState goal = *planning_display_->getQueryGoalState();
130 std::vector<geometry_msgs::msg::Pose> waypoints;
131 const std::string& link_name = move_group_->getEndEffectorLink();
132 const moveit::core::LinkModel* link = move_group_->getRobotModel()->getLinkModel(link_name);
133 if (!link)
134 {
135 RCLCPP_ERROR_STREAM(logger_, "Failed to determine unique end-effector link: " << link_name);
136 return false;
137 }
138 waypoints.push_back(tf2::toMsg(goal.getGlobalLinkTransform(link)));
139
140 // setup default params
141 double cart_step_size = 0.01;
142 bool avoid_collisions = true;
143
144 // compute trajectory
145 moveit_msgs::msg::RobotTrajectory trajectory;
146 double fraction = move_group_->computeCartesianPath(waypoints, cart_step_size, trajectory, avoid_collisions);
147
148 if (fraction >= 1.0)
149 {
150 RCLCPP_INFO(logger_, "Achieved %f %% of Cartesian path", fraction * 100.);
151
152 // Compute time parameterization to also provide velocities
153 // https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4
154 robot_trajectory::RobotTrajectory rt(move_group_->getRobotModel(), move_group_->getName());
155 rt.setRobotTrajectoryMsg(*move_group_->getCurrentState(), trajectory);
156 trajectory_processing::TimeOptimalTrajectoryGeneration time_parameterization;
157 bool success = time_parameterization.computeTimeStamps(rt, ui_->velocity_scaling_factor->value(),
158 ui_->acceleration_scaling_factor->value());
159 RCLCPP_INFO(logger_, "Computing time stamps %s", success ? "SUCCEEDED" : "FAILED");
160
161 // Store trajectory in current_plan_
162 current_plan_ = std::make_shared<moveit::planning_interface::MoveGroupInterface::Plan>();
163 rt.getRobotTrajectoryMsg(current_plan_->trajectory);
164 current_plan_->planning_time = (rclcpp::Clock().now() - start).seconds();
165 return success;
166 }
167 return false;
168}
169
170bool MotionPlanningFrame::computeJointSpacePlan()
171{
172 current_plan_ = std::make_shared<moveit::planning_interface::MoveGroupInterface::Plan>();
173 return move_group_->plan(*current_plan_) == moveit::core::MoveItErrorCode::SUCCESS;
174}
175
176void MotionPlanningFrame::computePlanButtonClicked()
177{
178 if (!move_group_)
179 return;
180
181 // Clear status
182 ui_->result_label->setText("Planning...");
183
184 configureForPlanning();
185 planning_display_->rememberPreviousStartState();
186 bool success = (ui_->use_cartesian_path->isEnabled() && ui_->use_cartesian_path->checkState()) ?
187 computeCartesianPlan() :
188 computeJointSpacePlan();
189
190 if (success)
191 {
192 ui_->execute_button->setEnabled(true);
193 ui_->result_label->setText(QString("Time: ").append(QString::number(current_plan_->planning_time, 'f', 3)));
194 }
195 else
196 {
197 current_plan_.reset();
198 ui_->result_label->setText("Failed");
199 }
200 Q_EMIT planningFinished();
201}
202
203void MotionPlanningFrame::computeExecuteButtonClicked()
204{
205 // ensures the MoveGroupInterface is not destroyed while executing
206 moveit::planning_interface::MoveGroupInterfacePtr mgi(move_group_);
207 if (mgi && current_plan_)
208 {
209 ui_->stop_button->setEnabled(true); // enable stopping
210 bool success = mgi->execute(*current_plan_) == moveit::core::MoveItErrorCode::SUCCESS;
211 onFinishedExecution(success);
212 }
213}
214
215void MotionPlanningFrame::computePlanAndExecuteButtonClicked()
216{
217 // ensures the MoveGroupInterface is not destroyed while executing
218 moveit::planning_interface::MoveGroupInterfacePtr mgi(move_group_);
219 if (!mgi)
220 return;
221 configureForPlanning();
222 planning_display_->rememberPreviousStartState();
223 // move_group::move() on the server side, will always start from the current state
224 // to suppress a warning, we pass an empty state (which encodes "start from current state")
225 mgi->setStartStateToCurrentState();
226 ui_->stop_button->setEnabled(true);
227 if (ui_->use_cartesian_path->isEnabled() && ui_->use_cartesian_path->checkState())
228 {
229 if (computeCartesianPlan())
230 computeExecuteButtonClicked();
231 }
232 else
233 {
234 bool success = mgi->move() == moveit::core::MoveItErrorCode::SUCCESS;
235 onFinishedExecution(success);
236 }
237 ui_->plan_and_execute_button->setEnabled(true);
238}
239
240void MotionPlanningFrame::computeStopButtonClicked()
241{
242 if (move_group_)
243 move_group_->stop();
244}
245
246void MotionPlanningFrame::onFinishedExecution(bool success)
247{
248 // visualize result of execution
249 if (success)
250 {
251 ui_->result_label->setText("Executed");
252 }
253 else
254 {
255 ui_->result_label->setText(!ui_->stop_button->isEnabled() ? "Stopped" : "Failed");
256 }
257 // disable stop button
258 ui_->stop_button->setEnabled(false);
259
260 // update query start state to current if necessary
261 if (ui_->start_state_combo_box->currentText() == "<current>")
262 startStateTextChanged(ui_->start_state_combo_box->currentText());
263
264 // update query goal state (from previous or to current)
265 // also ensures that joints tab shows goal state model
266 goalStateTextChanged(ui_->goal_state_combo_box->currentText());
267}
268
269void MotionPlanningFrame::onNewPlanningSceneState()
270{
271 moveit::core::RobotState current(planning_display_->getPlanningSceneRO()->getCurrentState());
272 if (ui_->start_state_combo_box->currentText() == "<current>")
273 {
274 planning_display_->setQueryStartState(current);
275 planning_display_->rememberPreviousStartState();
276 }
277 if (ui_->goal_state_combo_box->currentText() == "<current>")
278 planning_display_->setQueryGoalState(current);
279}
280
281void MotionPlanningFrame::startStateTextChanged(const QString& start_state)
282{
283 // use background job: fetching the current state might take up to a second
284 planning_display_->addBackgroundJob([this, state = start_state.toStdString()] { startStateTextChangedExec(state); },
285 "update start state");
286}
287
288void MotionPlanningFrame::startStateTextChangedExec(const std::string& start_state)
289{
290 moveit::core::RobotState start = *planning_display_->getQueryStartState();
291 updateQueryStateHelper(start, start_state);
292 planning_display_->setQueryStartState(start);
293}
294
295void MotionPlanningFrame::goalStateTextChanged(const QString& goal_state)
296{
297 // use background job: fetching the current state might take up to a second
298 planning_display_->addBackgroundJob([this, state = goal_state.toStdString()] { goalStateTextChangedExec(state); },
299 "update goal state");
300}
301
302void MotionPlanningFrame::goalStateTextChangedExec(const std::string& goal_state)
303{
304 moveit::core::RobotState goal = *planning_display_->getQueryGoalState();
305 updateQueryStateHelper(goal, goal_state);
306 planning_display_->setQueryGoalState(goal);
307}
308
309void MotionPlanningFrame::planningGroupTextChanged(const QString& planning_group)
310{
311 planning_display_->changePlanningGroup(planning_group.toStdString());
312}
313
314void MotionPlanningFrame::updateQueryStateHelper(moveit::core::RobotState& state, const std::string& v)
315{
316 if (v == "<random>")
317 {
318 configureWorkspace();
319 if (const moveit::core::JointModelGroup* jmg =
320 state.getJointModelGroup(planning_display_->getCurrentPlanningGroup()))
321 state.setToRandomPositions(jmg);
322 return;
323 }
324
325 if (v == "<random valid>")
326 {
327 configureWorkspace();
328
329 if (const moveit::core::JointModelGroup* jmg =
330 state.getJointModelGroup(planning_display_->getCurrentPlanningGroup()))
331 {
332 // Loop until a collision free state is found
333 static const int MAX_ATTEMPTS = 100;
334 int attempt_count = 0; // prevent loop for going forever
335 while (attempt_count < MAX_ATTEMPTS)
336 {
337 // Generate random state
338 state.setToRandomPositions(jmg);
339
340 state.update(); // prevent dirty transforms
341
342 // Test for collision
343 if (planning_display_->getPlanningSceneRO()->isStateValid(state, "", false))
344 break;
345
346 attempt_count++;
347 }
348 // Explain if no valid rand state found
349 if (attempt_count >= MAX_ATTEMPTS)
350 RCLCPP_WARN(logger_, "Unable to find a random collision free configuration after %d attempts", MAX_ATTEMPTS);
351 }
352 else
353 {
354 RCLCPP_WARN_STREAM(logger_, "Unable to get joint model group " << planning_display_->getCurrentPlanningGroup());
355 }
356 return;
357 }
358
359 if (v == "<current>")
360 {
361 rclcpp::Time t = node_->now();
362 planning_display_->waitForCurrentRobotState(t);
363 const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
364 if (ps)
365 state = ps->getCurrentState();
366 return;
367 }
368
369 if (v == "<same as goal>")
370 {
371 state = *planning_display_->getQueryGoalState();
372 return;
373 }
374
375 if (v == "<same as start>")
376 {
377 state = *planning_display_->getQueryStartState();
378 return;
379 }
380
381 if (v == "<previous>")
382 {
383 state = planning_display_->getPreviousState();
384 return;
385 }
386
387 // maybe it is a named state
388 if (const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup()))
389 state.setToDefaultValues(jmg, v);
390}
391
392void MotionPlanningFrame::populatePlannersList(const std::vector<moveit_msgs::msg::PlannerInterfaceDescription>& desc)
393{
394 ui_->planning_pipeline_combo_box->clear();
395
397 size_t default_planner_index = 0;
398 for (auto& d : planner_descriptions_)
399 {
400 QString item_text(d.pipeline_id.c_str());
401 // Check for default planning pipeline
402 if (d.pipeline_id == default_planning_pipeline_)
403 {
404 if (item_text.isEmpty())
405 item_text = QString::fromStdString(d.name);
406 default_planner_index = ui_->planning_pipeline_combo_box->count();
407 }
408 ui_->planning_pipeline_combo_box->addItem(item_text);
409 }
410 QFont font;
411 font.setBold(true);
412 ui_->planning_pipeline_combo_box->setItemData(default_planner_index, font, Qt::FontRole);
413
414 // Select default pipeline - triggers populatePlannerDescription() via callback
415 if (!planner_descriptions_.empty())
416 ui_->planning_pipeline_combo_box->setCurrentIndex(default_planner_index);
417}
418
419void MotionPlanningFrame::populatePlannerDescription(const moveit_msgs::msg::PlannerInterfaceDescription& desc)
420{
421 std::string group = planning_display_->getCurrentPlanningGroup();
422 RCLCPP_DEBUG(logger_, "Found %zu planners for group '%s' and pipeline '%s'", desc.planner_ids.size(), group.c_str(),
423 desc.pipeline_id.c_str());
424 ui_->planning_algorithm_combo_box->clear();
425
426 // set the label for the planning library
427 ui_->library_label->setText(QString::fromStdString(desc.name));
428 ui_->library_label->setStyleSheet("QLabel { color : green; font: bold }");
429
430 bool found_group = false;
431 // the name of a planner is either "GROUP[planner_id]" or "planner_id"
432 if (!group.empty())
433 {
434 for (const std::string& planner_id : desc.planner_ids)
435 {
436 RCLCPP_DEBUG(logger_, "planner id: %s", planner_id.c_str());
437 if (planner_id == group)
438 {
439 found_group = true;
440 }
441 else if (planner_id.substr(0, group.length()) == group)
442 {
443 if (planner_id.size() > group.length() && planner_id[group.length()] == '[')
444 {
445 std::string id = planner_id.substr(group.length());
446 if (id.size() > 2)
447 {
448 id.resize(id.length() - 1);
449 ui_->planning_algorithm_combo_box->addItem(QString::fromStdString(id.substr(1)));
450 }
451 }
452 }
453 }
454 }
455 if (ui_->planning_algorithm_combo_box->count() == 0 && !found_group)
456 {
457 for (const std::string& planner_id : desc.planner_ids)
458 ui_->planning_algorithm_combo_box->addItem(QString::fromStdString(planner_id));
459 }
460
461 ui_->planning_algorithm_combo_box->insertItem(0, "<unspecified>");
462
463 // retrieve default planner config from parameter server
464 const std::string& default_planner_config = move_group_->getDefaultPlannerId(found_group ? group : std::string());
465 int default_index = ui_->planning_algorithm_combo_box->findText(QString::fromStdString(default_planner_config));
466 if (default_index < 0)
467 default_index = 0; // 0 is <unspecified> fallback
468 ui_->planning_algorithm_combo_box->setCurrentIndex(default_index);
469
470 QFont font;
471 font.setBold(true);
472 ui_->planning_algorithm_combo_box->setItemData(default_index, font, Qt::FontRole);
473}
474
475void MotionPlanningFrame::populateConstraintsList()
476{
477 if (move_group_)
478 planning_display_->addMainLoopJob([this]() { populateConstraintsList(move_group_->getKnownConstraints()); });
479}
480
481void MotionPlanningFrame::populateConstraintsList(const std::vector<std::string>& constr)
482{
483 ui_->path_constraints_combo_box->clear();
484 ui_->path_constraints_combo_box->addItem("None");
485 for (const std::string& constraint : constr)
486 ui_->path_constraints_combo_box->addItem(QString::fromStdString(constraint));
487}
488
489void MotionPlanningFrame::constructPlanningRequest(moveit_msgs::msg::MotionPlanRequest& mreq)
490{
491 mreq.group_name = planning_display_->getCurrentPlanningGroup();
492 mreq.num_planning_attempts = ui_->planning_attempts->value();
493 mreq.allowed_planning_time = ui_->planning_time->value();
494 mreq.max_velocity_scaling_factor = ui_->velocity_scaling_factor->value();
495 mreq.max_acceleration_scaling_factor = ui_->acceleration_scaling_factor->value();
496 moveit::core::robotStateToRobotStateMsg(*planning_display_->getQueryStartState(), mreq.start_state);
497 mreq.workspace_parameters.min_corner.x = ui_->wcenter_x->value() - ui_->wsize_x->value() / 2.0;
498 mreq.workspace_parameters.min_corner.y = ui_->wcenter_y->value() - ui_->wsize_y->value() / 2.0;
499 mreq.workspace_parameters.min_corner.z = ui_->wcenter_z->value() - ui_->wsize_z->value() / 2.0;
500 mreq.workspace_parameters.max_corner.x = ui_->wcenter_x->value() + ui_->wsize_x->value() / 2.0;
501 mreq.workspace_parameters.max_corner.y = ui_->wcenter_y->value() + ui_->wsize_y->value() / 2.0;
502 mreq.workspace_parameters.max_corner.z = ui_->wcenter_z->value() + ui_->wsize_z->value() / 2.0;
503 moveit::core::RobotStateConstPtr s = planning_display_->getQueryGoalState();
504 const moveit::core::JointModelGroup* jmg = s->getJointModelGroup(mreq.group_name);
505 if (jmg)
506 {
507 mreq.goal_constraints.resize(1);
508 mreq.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(*s, jmg);
509 }
510}
511
512void MotionPlanningFrame::configureWorkspace()
513{
516
518 bx.min_position_ = ui_->wcenter_x->value() - ui_->wsize_x->value() / 2.0;
519 bx.max_position_ = ui_->wcenter_x->value() + ui_->wsize_x->value() / 2.0;
520 by.min_position_ = ui_->wcenter_y->value() - ui_->wsize_y->value() / 2.0;
521 by.max_position_ = ui_->wcenter_y->value() + ui_->wsize_y->value() / 2.0;
522 bz.min_position_ = ui_->wcenter_z->value() - ui_->wsize_z->value() / 2.0;
523 bz.max_position_ = ui_->wcenter_z->value() + ui_->wsize_z->value() / 2.0;
524
525 if (move_group_)
526 {
528 bz.max_position_);
529 }
530 planning_scene_monitor::PlanningSceneMonitorPtr psm = planning_display_->getPlanningSceneMonitor();
531 // get non-const access to the robot_model and update planar & floating joints as indicated by the workspace settings
532 if (psm && psm->getRobotModelLoader() && psm->getRobotModelLoader()->getModel())
533 {
534 const moveit::core::RobotModelPtr& robot_model = psm->getRobotModelLoader()->getModel();
535 const std::vector<moveit::core::JointModel*>& jm = robot_model->getJointModels();
536 for (moveit::core::JointModel* joint : jm)
537 {
538 if (joint->getType() == moveit::core::JointModel::PLANAR)
539 {
540 joint->setVariableBounds(joint->getName() + "/" + joint->getLocalVariableNames()[0], bx);
541 joint->setVariableBounds(joint->getName() + "/" + joint->getLocalVariableNames()[1], by);
542 }
543 else if (joint->getType() == moveit::core::JointModel::FLOATING)
544 {
545 joint->setVariableBounds(joint->getName() + "/" + joint->getLocalVariableNames()[0], bx);
546 joint->setVariableBounds(joint->getName() + "/" + joint->getLocalVariableNames()[1], by);
547 joint->setVariableBounds(joint->getName() + "/" + joint->getLocalVariableNames()[2], bz);
548 }
549 }
550 }
551}
552
553void MotionPlanningFrame::configureForPlanning()
554{
555 move_group_->setStartState(*planning_display_->getQueryStartState());
556 move_group_->setJointValueTarget(*planning_display_->getQueryGoalState());
557 move_group_->setPlanningTime(ui_->planning_time->value());
558 move_group_->setNumPlanningAttempts(ui_->planning_attempts->value());
559 move_group_->setMaxVelocityScalingFactor(ui_->velocity_scaling_factor->value());
560 move_group_->setMaxAccelerationScalingFactor(ui_->acceleration_scaling_factor->value());
561 configureWorkspace();
562 if (static_cast<bool>(planning_display_))
563 planning_display_->dropVisualizedTrajectory();
564}
565
566void MotionPlanningFrame::remotePlanCallback(const std_msgs::msg::Empty::ConstSharedPtr& /*msg*/)
567{
568 planButtonClicked();
569}
570
571void MotionPlanningFrame::remoteExecuteCallback(const std_msgs::msg::Empty::ConstSharedPtr& /*msg*/)
572{
573 executeButtonClicked();
574}
575
576void MotionPlanningFrame::remoteStopCallback(const std_msgs::msg::Empty::ConstSharedPtr& /*msg*/)
577{
578 stopButtonClicked();
579}
580
581void MotionPlanningFrame::remoteUpdateStartStateCallback(const std_msgs::msg::Empty::ConstSharedPtr& /*msg*/)
582{
584 {
585 planning_display_->waitForCurrentRobotState(node_->get_clock()->now());
586 const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
587 if (ps)
588 {
589 moveit::core::RobotState state = ps->getCurrentState();
590 planning_display_->setQueryStartState(state);
591 }
592 }
593}
594
595void MotionPlanningFrame::remoteUpdateGoalStateCallback(const std_msgs::msg::Empty::ConstSharedPtr& /*msg*/)
596{
598 {
599 planning_display_->waitForCurrentRobotState(node_->get_clock()->now());
600 const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
601 if (ps)
602 {
603 moveit::core::RobotState state = ps->getCurrentState();
604 planning_display_->setQueryGoalState(state);
605 }
606 }
607}
608
609void MotionPlanningFrame::remoteUpdateCustomStartStateCallback(const moveit_msgs::msg::RobotState::ConstSharedPtr& msg)
610{
611 moveit_msgs::msg::RobotState msg_no_attached(*msg);
612 msg_no_attached.attached_collision_objects.clear();
613 msg_no_attached.is_diff = true;
615 {
616 planning_display_->waitForCurrentRobotState(node_->get_clock()->now());
617 const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
618 if (ps)
619 {
620 auto state = std::make_shared<moveit::core::RobotState>(ps->getCurrentState());
621 moveit::core::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state);
622 planning_display_->setQueryStartState(*state);
623 }
624 }
625}
626
627void MotionPlanningFrame::remoteUpdateCustomGoalStateCallback(const moveit_msgs::msg::RobotState::ConstSharedPtr& msg)
628{
629 moveit_msgs::msg::RobotState msg_no_attached(*msg);
630 msg_no_attached.attached_collision_objects.clear();
631 msg_no_attached.is_diff = true;
633 {
634 planning_display_->waitForCurrentRobotState(node_->get_clock()->now());
635 const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
636 if (ps)
637 {
638 auto state = std::make_shared<moveit::core::RobotState>(ps->getCurrentState());
639 moveit::core::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state);
640 planning_display_->setQueryGoalState(*state);
641 }
642 }
643}
644} // namespace moveit_rviz_plugin
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
std::vector< VariableBounds > Bounds
The datatype for the joint bounds.
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
void update(bool force=false)
Update all transforms.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
moveit::planning_interface::MoveGroupInterface::PlanPtr current_plan_
std::vector< moveit_msgs::msg::PlannerInterfaceDescription > planner_descriptions_
moveit::planning_interface::MoveGroupInterfacePtr move_group_
void constructPlanningRequest(moveit_msgs::msg::MotionPlanRequest &mreq)
const planning_scene_monitor::PlanningSceneMonitorPtr & getPlanningSceneMonitor()
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const override
Compute a trajectory with waypoints spaced equally in time (according to resample_dt_)....
moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
Generates a constraint message intended to be used as a goal constraint for a joint group....
Definition utils.cpp:152
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::msg::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::msg::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state.
std::string append(const std::string &left, const std::string &right)