46 #include <std_srvs/Empty.h> 48 #include <QMessageBox> 49 #include <QInputDialog> 52 #include "ui_motion_planning_rviz_plugin_frame.h" 59 , planning_display_(pdisplay)
61 , ui_(new
Ui::MotionPlanningUI())
87 connect(
ui_->planning_algorithm_combo_box, SIGNAL(currentIndexChanged(
int)),
this,
89 connect(
ui_->planning_algorithm_combo_box, SIGNAL(currentIndexChanged(
int)),
this,
106 connect(
ui_->collision_objects_list, SIGNAL(itemChanged(QListWidgetItem*)),
this,
108 connect(
ui_->path_constraints_combo_box, SIGNAL(currentIndexChanged(
int)),
this,
111 connect(
ui_->planning_scene_tree, SIGNAL(itemChanged(QTreeWidgetItem*,
int)),
this,
129 connect(
ui_->detected_objects_list, SIGNAL(itemChanged(QListWidgetItem*)),
this,
133 connect(
ui_->tabWidget, SIGNAL(currentChanged(
int)),
this, SLOT(
tabChanged(
int)));
135 QShortcut* copy_object_shortcut =
new QShortcut(QKeySequence(Qt::CTRL + Qt::Key_C),
ui_->collision_objects_list);
138 ui_->reset_db_button->hide();
139 ui_->background_job_progress->hide();
140 ui_->background_job_progress->setMaximum(0);
142 ui_->tabWidget->setCurrentIndex(0);
161 catch (std::exception& ex)
171 catch (std::exception& ex)
190 catch (std::exception& ex)
207 QList<QListWidgetItem*> found_items = list->findItems(QString(item_name.c_str()), Qt::MatchExactly);
208 for (
int i = 0; i < found_items.size(); ++i)
209 found_items[i]->setSelected(selection);
250 ui_->start_state_selection->clear();
251 ui_->goal_state_selection->clear();
260 const robot_model::JointModelGroup* jmg = kmodel->getJointModelGroup(group);
263 ui_->start_state_selection->addItem(QString(
"<random valid>"));
264 ui_->start_state_selection->addItem(QString(
"<random>"));
265 ui_->start_state_selection->addItem(QString(
"<current>"));
266 ui_->start_state_selection->addItem(QString(
"<same as goal>"));
268 ui_->goal_state_selection->addItem(QString(
"<random valid>"));
269 ui_->goal_state_selection->addItem(QString(
"<random>"));
270 ui_->goal_state_selection->addItem(QString(
"<current>"));
271 ui_->goal_state_selection->addItem(QString(
"<same as start>"));
273 const std::vector<std::string>& known_states = jmg->getDefaultStateNames();
274 if (!known_states.empty())
276 ui_->start_state_selection->insertSeparator(
ui_->start_state_selection->count());
277 ui_->goal_state_selection->insertSeparator(
ui_->goal_state_selection->count());
278 for (std::size_t i = 0; i < known_states.size(); ++i)
280 ui_->start_state_selection->addItem(QString::fromStdString(known_states[i]));
281 ui_->goal_state_selection->addItem(QString::fromStdString(known_states[i]));
284 ui_->start_state_selection->setCurrentIndex(2);
285 ui_->goal_state_selection->setCurrentIndex(0);
303 if (!group.empty() && kmodel)
307 ROS_INFO(
"Constructing new MoveGroup connection for group '%s' in namespace '%s'", group.c_str(),
310 opt.robot_model_ = kmodel;
311 opt.robot_description_.clear();
318 move_group_->setConstraintsDatabase(
ui_->database_host->text().toStdString(),
ui_->database_port->value());
320 catch (std::exception& ex)
330 moveit_msgs::PlannerInterfaceDescription
desc;
334 "populateConstraintsList");
347 if (
ui_->allow_external_program->isChecked())
358 "Frame::changePlanningGroup");
374 std::size_t slash = path.find_last_of(
"/\\");
375 std::string
name = path.substr(slash + 1);
377 Eigen::Affine3d pose;
382 QMessageBox::warning(
this, QString(
"Duplicate names"), QString(
"An attached object named '")
384 .
append(
"' already exists. Please rename the " 385 "attached object before importing."));
393 msgBox.setText(
"There exists another object with the same name.");
394 msgBox.setInformativeText(
"Would you like to overwrite it?");
395 msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
396 msgBox.setDefaultButton(QMessageBox::No);
397 int ret = msgBox.exec();
401 case QMessageBox::Yes:
407 ps->getWorldNonConst()->removeObject(name);
408 addObject(ps->getWorldNonConst(), name, shape, pose);
412 case QMessageBox::No:
416 QString
text = QInputDialog::getText(
417 this, tr(
"Choose a new name"), tr(
"Import the new object under the name:"), QLineEdit::Normal,
418 QString::fromStdString(name +
"-" + boost::lexical_cast<std::string>(
425 name = text.toStdString();
429 if (ps->getWorld()->hasObject(name))
430 QMessageBox::warning(
431 this,
"Name already exists",
432 QString(
"The name '").append(name.c_str()).
append(
"' already exists. Not importing object."));
434 addObject(ps->getWorldNonConst(), name, shape, pose);
438 QMessageBox::warning(
this,
"Object not imported",
"Cannot use an empty name for an imported object");
451 addObject(ps->getWorldNonConst(), name, shape, pose);
456 QMessageBox::warning(
this, QString(
"Import error"), QString(
"Unable to import scene"));
464 ui_->planning_algorithm_combo_box->clear();
465 ui_->library_label->setText(
"NO PLANNING LIBRARY LOADED");
466 ui_->library_label->setStyleSheet(
"QLabel { color : red; font: bold }");
467 ui_->object_status->setText(
"");
470 parentWidget()->show();
476 parentWidget()->hide();
483 else if (
ui_->tabWidget->tabText(index).toStdString() ==
TAB_OBJECTS)
495 if (
ui_->allow_external_program->isChecked())
void listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr &msg)
ros::Subscriber update_goal_state_subscriber_
void publishSceneButtonClicked()
const planning_scene_monitor::PlanningSceneMonitorPtr & getPlanningSceneMonitor()
void saveStartStateButtonClicked()
void populatePlannersList(const moveit_msgs::PlannerInterfaceDescription &desc)
long unsigned int known_collision_objects_version_
void toggleSelectPlanningGroupSubscription(bool enable)
planning_scene_monitor::LockedPlanningSceneRW getPlanningSceneRW()
get write access to planning scene
void sceneScaleStartChange()
void waitForAction(const T &action, const ros::NodeHandle &node_handle, const ros::Duration &wait_for_server, const std::string &name)
void remoteUpdateGoalStateCallback(const std_msgs::EmptyConstPtr &msg)
void collisionObjectChanged(QListWidgetItem *item)
void loadStateButtonClicked()
void remoteUpdateCustomStartStateCallback(const moveit_msgs::RobotStateConstPtr &msg)
void populateConstraintsList()
void remoteUpdateStartStateCallback(const std_msgs::EmptyConstPtr &msg)
const std::string getMoveGroupNS() const
void executeButtonClicked()
void saveQueryButtonClicked()
ServiceClient serviceClient(ros::NodeHandle n, std::string name)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void selectedCollisionObjectChanged()
ros::Subscriber update_custom_goal_state_subscriber_
ros::Subscriber plan_subscriber_
void sceneScaleEndChange()
void changePlanningGroupHelper()
static const std::string TAB_OBJECTS
void saveSceneButtonClicked()
void sceneScaleChanged(int value)
void updateExternalCommunication()
void changePlanningGroup()
void planAndExecuteButtonClicked()
ros::Subscriber execute_subscriber_
void setQueryStartState(const robot_state::RobotState &start)
void loadSceneButtonClicked()
void saveGoalStateButtonClicked()
ros::Subscriber object_recognition_subscriber_
void allowExternalProgramCommunication(bool enable)
void removeObjectButtonClicked()
void sceneUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
void removeStateButtonClicked()
void setQueryGoalState(const robot_state::RobotState &goal)
void detectObjectsButtonClicked()
ros::Subscriber update_start_state_subscriber_
void remoteExecuteCallback(const std_msgs::EmptyConstPtr &msg)
void onClearOctomapClicked()
void placeObjectButtonClicked()
void detectedObjectChanged(QListWidgetItem *item)
moveit::planning_interface::MoveGroupInterfacePtr move_group_
void objectPoseValueChanged(double value)
void addBackgroundJob(const boost::function< void()> &job, const std::string &name)
void clearStatesButtonClicked()
void selectedDetectedObjectChanged()
void setItemSelectionInList(const std::string &item_name, bool selection, QListWidget *list)
const boost::shared_ptr< tf::TransformListener > & getTFClientPtr()
void importFromTextButtonClicked()
void useStartStateButtonClicked()
ros::Subscriber update_custom_start_state_subscriber_
void importResource(const std::string &path)
std::string getCurrentPlanningGroup() const
void remotePlanCallback(const std_msgs::EmptyConstPtr &msg)
void importUrlButtonClicked()
void pathConstraintsIndexChanged(int index)
virtual FrameManager * getFrameManager() const =0
ros::Subscriber stop_subscriber_
const robot_interaction::RobotInteractionPtr & getRobotInteraction() const
void exportAsTextButtonClicked()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void fillStateSelectionOptions()
void databaseConnectButtonClicked()
void clearSceneButtonClicked()
MotionPlanningDisplay * planning_display_
void setAsStartStateButtonClicked()
void importFileButtonClicked()
void resetDbButtonClicked()
Ui::MotionPlanningUI * ui_
const std::string OBJECT_RECOGNITION_ACTION
void addMainLoopJob(const boost::function< void()> &job)
queue the execution of this function for the next time the main update() loop gets called ...
void warehouseItemNameChanged(QTreeWidgetItem *item, int column)
void useGoalStateButtonClicked()
const robot_model::RobotModelConstPtr & getRobotModel() const
MotionPlanningFrame(MotionPlanningDisplay *pdisplay, rviz::DisplayContext *context, QWidget *parent=0)
void remoteUpdateCustomGoalStateCallback(const moveit_msgs::RobotStateConstPtr &msg)
Mesh * createMeshFromResource(const std::string &resource)
std::shared_ptr< rviz::InteractiveMarker > scene_marker_
void allowLookingToggled(bool checked)
ROSCPP_DECL std::string append(const std::string &left, const std::string &right)
void pickObjectButtonClicked()
void setAsGoalStateButtonClicked()
ros::Publisher planning_scene_world_publisher_
rviz::DisplayContext * context_
void copySelectedCollisionObject()
void remoteStopCallback(const std_msgs::EmptyConstPtr &msg)
void deleteSceneButtonClicked()
void updateSceneMarkers(float wall_dt, float ros_dt)
void approximateIKChanged(int state)
void planningAlgorithmIndexChanged(int index)
void loadQueryButtonClicked()
void allowReplanningToggled(bool checked)
void populateCollisionObjectsList()
moveit_warehouse::PlanningSceneStoragePtr planning_scene_storage_
void useApproximateIK(bool flag)
void deleteQueryButtonClicked()
std::unique_ptr< actionlib::SimpleActionClient< object_recognition_msgs::ObjectRecognitionAction > > object_recognition_client_
void selectedSupportSurfaceChanged()
ros::Publisher planning_scene_publisher_
moveit::planning_interface::PlanningSceneInterfacePtr planning_scene_interface_
void addObject(const collision_detection::WorldPtr &world, const std::string &id, const shapes::ShapeConstPtr &shape, const Eigen::Affine3d &pose)
planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO() const
get read-only access to planning scene
void tabChanged(int index)
moveit::semantic_world::SemanticWorldPtr semantic_world_
std::shared_ptr< const Shape > ShapeConstPtr
void planningSceneItemClicked()