44#include <std_srvs/srv/empty.hpp>
45#include <moveit_msgs/msg/robot_state.hpp>
46#include <tf2_eigen/tf2_eigen.hpp>
49#include "ui_motion_planning_rviz_plugin_frame.h"
54void MotionPlanningFrame::planButtonClicked()
56 publishSceneIfNeeded();
57 planning_display_->addBackgroundJob([
this] { computePlanButtonClicked(); },
"compute plan");
60void MotionPlanningFrame::executeButtonClicked()
62 ui_->execute_button->setEnabled(
false);
67void MotionPlanningFrame::planAndExecuteButtonClicked()
69 publishSceneIfNeeded();
70 ui_->plan_and_execute_button->setEnabled(
false);
71 ui_->execute_button->setEnabled(
false);
73 planning_display_->spawnBackgroundJob([
this] { computePlanAndExecuteButtonClicked(); });
76void MotionPlanningFrame::stopButtonClicked()
78 ui_->stop_button->setEnabled(
false);
79 planning_display_->addBackgroundJob([
this] { computeStopButtonClicked(); },
"stop");
82void MotionPlanningFrame::allowReplanningToggled(
bool checked)
88void MotionPlanningFrame::allowLookingToggled(
bool checked)
94void MotionPlanningFrame::pathConstraintsIndexChanged(
int index)
100 std::string c =
ui_->path_constraints_combo_box->itemText(index).toStdString();
103 RCLCPP_WARN_STREAM(logger_,
"Unable to set the path constraints: " << c);
113void MotionPlanningFrame::onClearOctomapClicked()
115 auto req = std::make_shared<std_srvs::srv::Empty::Request>();
116 auto result = clear_octomap_service_client_->async_send_request(req);
118 if (result.wait_for(std::chrono::seconds(0)) != std::future_status::ready)
120 RCLCPP_ERROR(logger_,
"Failed to call clear_octomap_service");
122 ui_->clear_octomap_button->setEnabled(
false);
125bool MotionPlanningFrame::computeCartesianPlan()
127 rclcpp::Time start = rclcpp::Clock().now();
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);
135 RCLCPP_ERROR_STREAM(logger_,
"Failed to determine unique end-effector link: " << link_name);
141 double cart_step_size = 0.01;
142 bool avoid_collisions =
true;
145 moveit_msgs::msg::RobotTrajectory trajectory;
146 double fraction =
move_group_->computeCartesianPath(waypoints, cart_step_size, trajectory, avoid_collisions);
150 RCLCPP_INFO(logger_,
"Achieved %f %% of Cartesian path", fraction * 100.);
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");
162 current_plan_ = std::make_shared<moveit::planning_interface::MoveGroupInterface::Plan>();
164 current_plan_->planning_time = (rclcpp::Clock().now() - start).seconds();
170bool MotionPlanningFrame::computeJointSpacePlan()
172 current_plan_ = std::make_shared<moveit::planning_interface::MoveGroupInterface::Plan>();
176void MotionPlanningFrame::computePlanButtonClicked()
182 ui_->result_label->setText(
"Planning...");
184 configureForPlanning();
186 bool success = (
ui_->use_cartesian_path->isEnabled() &&
ui_->use_cartesian_path->checkState()) ?
187 computeCartesianPlan() :
188 computeJointSpacePlan();
192 ui_->execute_button->setEnabled(
true);
193 ui_->result_label->setText(QString(
"Time: ").
append(QString::number(
current_plan_->planning_time,
'f', 3)));
198 ui_->result_label->setText(
"Failed");
203void MotionPlanningFrame::computeExecuteButtonClicked()
206 moveit::planning_interface::MoveGroupInterfacePtr mgi(
move_group_);
209 ui_->stop_button->setEnabled(
true);
210 bool success = mgi->execute(*
current_plan_) == moveit::core::MoveItErrorCode::SUCCESS;
211 onFinishedExecution(success);
215void MotionPlanningFrame::computePlanAndExecuteButtonClicked()
218 moveit::planning_interface::MoveGroupInterfacePtr mgi(
move_group_);
221 configureForPlanning();
225 mgi->setStartStateToCurrentState();
226 ui_->stop_button->setEnabled(
true);
227 if (
ui_->use_cartesian_path->isEnabled() &&
ui_->use_cartesian_path->checkState())
229 if (computeCartesianPlan())
230 computeExecuteButtonClicked();
234 bool success = mgi->move() == moveit::core::MoveItErrorCode::SUCCESS;
235 onFinishedExecution(success);
237 ui_->plan_and_execute_button->setEnabled(
true);
240void MotionPlanningFrame::computeStopButtonClicked()
246void MotionPlanningFrame::onFinishedExecution(
bool success)
251 ui_->result_label->setText(
"Executed");
255 ui_->result_label->setText(!
ui_->stop_button->isEnabled() ?
"Stopped" :
"Failed");
258 ui_->stop_button->setEnabled(
false);
261 if (
ui_->start_state_combo_box->currentText() ==
"<current>")
262 startStateTextChanged(
ui_->start_state_combo_box->currentText());
266 goalStateTextChanged(
ui_->goal_state_combo_box->currentText());
269void MotionPlanningFrame::onNewPlanningSceneState()
271 moveit::core::RobotState current(
planning_display_->getPlanningSceneRO()->getCurrentState());
272 if (
ui_->start_state_combo_box->currentText() ==
"<current>")
277 if (
ui_->goal_state_combo_box->currentText() ==
"<current>")
281void MotionPlanningFrame::startStateTextChanged(
const QString& start_state)
284 planning_display_->addBackgroundJob([
this, state = start_state.toStdString()] { startStateTextChangedExec(state); },
285 "update start state");
288void MotionPlanningFrame::startStateTextChangedExec(
const std::string& start_state)
291 updateQueryStateHelper(start, start_state);
295void MotionPlanningFrame::goalStateTextChanged(
const QString& goal_state)
298 planning_display_->addBackgroundJob([
this, state = goal_state.toStdString()] { goalStateTextChangedExec(state); },
299 "update goal state");
302void MotionPlanningFrame::goalStateTextChangedExec(
const std::string& goal_state)
305 updateQueryStateHelper(goal, goal_state);
309void MotionPlanningFrame::planningGroupTextChanged(
const QString& planning_group)
314void MotionPlanningFrame::updateQueryStateHelper(moveit::core::RobotState& state,
const std::string& v)
318 configureWorkspace();
319 if (
const moveit::core::JointModelGroup* jmg =
325 if (v ==
"<random valid>")
327 configureWorkspace();
329 if (
const moveit::core::JointModelGroup* jmg =
333 static const int MAX_ATTEMPTS = 100;
334 int attempt_count = 0;
335 while (attempt_count < MAX_ATTEMPTS)
349 if (attempt_count >= MAX_ATTEMPTS)
350 RCLCPP_WARN(logger_,
"Unable to find a random collision free configuration after %d attempts", MAX_ATTEMPTS);
354 RCLCPP_WARN_STREAM(logger_,
"Unable to get joint model group " <<
planning_display_->getCurrentPlanningGroup());
359 if (v ==
"<current>")
361 rclcpp::Time t = node_->now();
363 const planning_scene_monitor::LockedPlanningSceneRO& ps =
planning_display_->getPlanningSceneRO();
365 state = ps->getCurrentState();
369 if (v ==
"<same as goal>")
375 if (v ==
"<same as start>")
381 if (v ==
"<previous>")
392void MotionPlanningFrame::populatePlannersList(
const std::vector<moveit_msgs::msg::PlannerInterfaceDescription>& desc)
394 ui_->planning_pipeline_combo_box->clear();
397 size_t default_planner_index = 0;
400 QString item_text(d.pipeline_id.c_str());
404 if (item_text.isEmpty())
405 item_text = QString::fromStdString(d.name);
406 default_planner_index =
ui_->planning_pipeline_combo_box->count();
408 ui_->planning_pipeline_combo_box->addItem(item_text);
412 ui_->planning_pipeline_combo_box->setItemData(default_planner_index, font, Qt::FontRole);
416 ui_->planning_pipeline_combo_box->setCurrentIndex(default_planner_index);
419void MotionPlanningFrame::populatePlannerDescription(
const moveit_msgs::msg::PlannerInterfaceDescription& desc)
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();
427 ui_->library_label->setText(QString::fromStdString(desc.name));
428 ui_->library_label->setStyleSheet(
"QLabel { color : green; font: bold }");
430 bool found_group =
false;
434 for (
const std::string& planner_id : desc.planner_ids)
436 RCLCPP_DEBUG(logger_,
"planner id: %s", planner_id.c_str());
437 if (planner_id == group)
441 else if (planner_id.substr(0, group.length()) == group)
443 if (planner_id.size() > group.length() && planner_id[group.length()] ==
'[')
445 std::string
id = planner_id.substr(group.length());
448 id.resize(
id.length() - 1);
449 ui_->planning_algorithm_combo_box->addItem(QString::fromStdString(
id.substr(1)));
455 if (
ui_->planning_algorithm_combo_box->count() == 0 && !found_group)
457 for (
const std::string& planner_id : desc.planner_ids)
458 ui_->planning_algorithm_combo_box->addItem(QString::fromStdString(planner_id));
461 ui_->planning_algorithm_combo_box->insertItem(0,
"<unspecified>");
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)
468 ui_->planning_algorithm_combo_box->setCurrentIndex(default_index);
472 ui_->planning_algorithm_combo_box->setItemData(default_index, font, Qt::FontRole);
475void MotionPlanningFrame::populateConstraintsList()
481void MotionPlanningFrame::populateConstraintsList(
const std::vector<std::string>& constr)
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));
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();
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;
507 mreq.goal_constraints.resize(1);
512void MotionPlanningFrame::configureWorkspace()
532 if (psm && psm->getRobotModelLoader() && psm->getRobotModelLoader()->getModel())
534 const moveit::core::RobotModelPtr& robot_model = psm->getRobotModelLoader()->getModel();
535 const std::vector<moveit::core::JointModel*>& jm = robot_model->getJointModels();
540 joint->setVariableBounds(joint->getName() +
"/" + joint->getLocalVariableNames()[0], bx);
541 joint->setVariableBounds(joint->getName() +
"/" + joint->getLocalVariableNames()[1], by);
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);
553void MotionPlanningFrame::configureForPlanning()
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();
566void MotionPlanningFrame::remotePlanCallback(
const std_msgs::msg::Empty::ConstSharedPtr& )
571void MotionPlanningFrame::remoteExecuteCallback(
const std_msgs::msg::Empty::ConstSharedPtr& )
573 executeButtonClicked();
576void MotionPlanningFrame::remoteStopCallback(
const std_msgs::msg::Empty::ConstSharedPtr& )
581void MotionPlanningFrame::remoteUpdateStartStateCallback(
const std_msgs::msg::Empty::ConstSharedPtr& )
586 const planning_scene_monitor::LockedPlanningSceneRO& ps =
planning_display_->getPlanningSceneRO();
589 moveit::core::RobotState state = ps->getCurrentState();
595void MotionPlanningFrame::remoteUpdateGoalStateCallback(
const std_msgs::msg::Empty::ConstSharedPtr& )
600 const planning_scene_monitor::LockedPlanningSceneRO& ps =
planning_display_->getPlanningSceneRO();
603 moveit::core::RobotState state = ps->getCurrentState();
609void MotionPlanningFrame::remoteUpdateCustomStartStateCallback(
const moveit_msgs::msg::RobotState::ConstSharedPtr& msg)
611 moveit_msgs::msg::RobotState msg_no_attached(*msg);
612 msg_no_attached.attached_collision_objects.clear();
613 msg_no_attached.is_diff =
true;
617 const planning_scene_monitor::LockedPlanningSceneRO& ps =
planning_display_->getPlanningSceneRO();
620 auto state = std::make_shared<moveit::core::RobotState>(ps->getCurrentState());
627void MotionPlanningFrame::remoteUpdateCustomGoalStateCallback(
const moveit_msgs::msg::RobotState::ConstSharedPtr& msg)
629 moveit_msgs::msg::RobotState msg_no_attached(*msg);
630 msg_no_attached.attached_collision_objects.clear();
631 msg_no_attached.is_diff =
true;
635 const planning_scene_monitor::LockedPlanningSceneRO& ps =
planning_display_->getPlanningSceneRO();
638 auto state = std::make_shared<moveit::core::RobotState>(ps->getCurrentState());
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...
Ui::MotionPlanningUI * ui_
moveit::planning_interface::MoveGroupInterface::PlanPtr current_plan_
std::vector< moveit_msgs::msg::PlannerInterfaceDescription > planner_descriptions_
std::string default_planning_pipeline_
moveit::planning_interface::MoveGroupInterfacePtr move_group_
void constructPlanningRequest(moveit_msgs::msg::MotionPlanRequest &mreq)
MotionPlanningDisplay * planning_display_
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....
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)