arm_navigation_tests: arm_navigation_tests: planning_scene_warehouse

00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the <ORGANIZATION> nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // Author: E. Gil Jones
00031 #ifndef PLANNING_SCENE_WAREHOUSE_H
00032 #define PLANNING_SCENE_WAREHOUSE_H
00033 
00034 #include <termios.h>
00035 #include <signal.h>
00036 #include <cstdio>
00037 #include <cstdlib>
00038 #include <unistd.h>
00039 #include <math.h>
00040 
00041 #include <ros/ros.h>
00042 
00043 
00044 #include <move_arm_warehouse/move_arm_utils.h>
00045 
00046 #include <qt4/QtGui/qwidget.h>
00047 #include <qt4/QtGui/qmenubar.h>
00048 #include <qt4/QtGui/qmenu.h>
00049 #include <qt4/QtGui/qprogressbar.h>
00050 #include <qt4/QtGui/qdialog.h>
00051 #include <qt4/QtGui/qtablewidget.h>
00052 #include <qt4/QtGui/qlayout.h>
00053 #include <qt4/QtGui/qpushbutton.h>
00054 #include <qt4/Qt/qthread.h>
00055 #include <qt4/QtGui/qslider.h>
00056 #include <qt4/QtGui/qspinbox.h>
00057 #include <qt4/QtGui/qlabel.h>
00058 #include <qt4/QtGui/qgroupbox.h>
00059 #include <qt4/QtGui/qcombobox.h>
00060 #include <qt4/QtGui/qcheckbox.h>
00061 #include <qt4/QtGui/qcolordialog.h>
00062 #include <qt4/QtGui/qtreewidget.h>
00063 #include <qt4/QtGui/qpalette.h>
00064 #include <qt4/QtGui/qformlayout.h>
00065 #include <qt4/QtGui/qlineedit.h>
00066 #include <qt4/QtGui/qmessagebox.h>
00067 #include <QDateTime>
00068 
00069 
00070 static const std::string VIS_TOPIC_NAME = "planning_scene_visualizer";
00071 
00072 //in 100 hz ticks
00073 static const unsigned int CONTROL_SPEED = 10;
00074 
00075 static const double BASE_TRANS_SPEED = .3;
00076 static const double BASE_ROT_SPEED = .15;
00077 
00078 static const double HAND_TRANS_SPEED = .05;
00079 static const double HAND_ROT_SPEED = .15;
00080 
00081 static const std::string LEFT_IK_NAME = "/pr2_left_arm_kinematics/get_constraint_aware_ik";
00082 static const std::string RIGHT_IK_NAME = "/pr2_right_arm_kinematics/get_constraint_aware_ik";
00083 
00084 static const std::string NON_COLL_LEFT_IK_NAME = "/pr2_left_arm_kinematics/get_ik";
00085 static const std::string NON_COLL_RIGHT_IK_NAME = "/pr2_right_arm_kinematics/get_ik";
00086 
00087 static const std::string RIGHT_ARM_GROUP = "right_arm";
00088 static const std::string LEFT_ARM_GROUP = "left_arm";
00089 
00090 static const std::string RIGHT_ARM_REDUNDANCY = "r_upper_arm_roll_joint";
00091 static const std::string LEFT_ARM_REDUNDANCY = "l_upper_arm_roll_joint";
00092 
00093 static const std::string LEFT_IK_LINK = "l_wrist_roll_link";
00094 static const std::string RIGHT_IK_LINK = "r_wrist_roll_link";
00095 
00096 static const std::string PLANNER_SERVICE_NAME = "/ompl_planning/plan_kinematic_path";
00097 static const std::string LEFT_INTERPOLATE_SERVICE_NAME = "/l_interpolated_ik_motion_plan";
00098 static const std::string RIGHT_INTERPOLATE_SERVICE_NAME = "/r_interpolated_ik_motion_plan";
00099 static const std::string TRAJECTORY_FILTER_SERVICE_NAME = "/trajectory_filter/filter_trajectory_with_constraints";
00100 static const std::string PROXIMITY_SPACE_SERVICE_NAME = "/collision_proximity_server_test/get_distance_aware_plan";
00101 static const std::string PROXIMITY_SPACE_VALIDITY_NAME = "/collision_proximity_server_test/get_state_validity";
00102 static const std::string PROXIMITY_SPACE_PLANNER_NAME = "/collision_proximity_planner/plan";
00103 
00104 static const ros::Duration PLANNING_DURATION = ros::Duration(5.0);
00105 
00106 static const std::string SET_PLANNING_SCENE_DIFF_NAME = "environment_server/set_planning_scene_diff";
00107 
00108 class ParameterDialog : public QDialog
00109 {
00110   public:
00111     planning_scene_utils::PlanningSceneParameters params_;
00112     ParameterDialog(planning_scene_utils::PlanningSceneParameters params, QWidget* parent = NULL) : QDialog(parent)
00113     {
00114       params_  = params;
00115       setMinimumWidth(640);
00116       setup();
00117     }
00118 
00119     void setup()
00120     {
00121       QGroupBox* groupBox = new QGroupBox(this);
00122       groupBox->setTitle("Planning Scene Editor Parameters");
00123       QVBoxLayout* boxLayout = new QVBoxLayout(this);
00124 
00125       layout = new QFormLayout(groupBox);
00126       left_ik_name_ = new QLineEdit(groupBox);
00127       layout->addRow("Left IK Service", left_ik_name_);
00128       left_ik_name_->setText(QString::fromStdString(params_.left_ik_name_));
00129 
00130       right_ik_name_ = new QLineEdit(groupBox);
00131       layout->addRow("Right IK Service", right_ik_name_);
00132       right_ik_name_->setText(QString::fromStdString(params_.right_ik_name_));
00133 
00134       non_coll_left_ik_name_ = new QLineEdit(groupBox);
00135       layout->addRow("Non Collision-Aware Left IK Service", non_coll_left_ik_name_);
00136       non_coll_left_ik_name_->setText(QString::fromStdString(params_.non_coll_left_ik_name_));
00137 
00138       non_coll_right_ik_name_ = new QLineEdit(groupBox);
00139       layout->addRow("Non Collision-Aware Right IK Service", non_coll_right_ik_name_);
00140       non_coll_right_ik_name_->setText(QString::fromStdString(params_.non_coll_right_ik_name_));
00141 
00142       right_arm_group_ = new QLineEdit(groupBox);
00143       layout->addRow("Right Arm Group", right_arm_group_);
00144       right_arm_group_->setText(QString::fromStdString(params_.right_arm_group_));
00145 
00146       left_arm_group_ = new QLineEdit(groupBox);
00147       layout->addRow("Left Arm Group", left_arm_group_);
00148       left_arm_group_->setText(QString::fromStdString(params_.left_arm_group_));
00149 
00150       right_arm_redundancy_ = new QLineEdit(groupBox);
00151       layout->addRow("Right Arm Redundancy DOF", right_arm_redundancy_);
00152       right_arm_redundancy_->setText(QString::fromStdString(params_.right_redundancy_));
00153 
00154       left_arm_redundancy_ = new QLineEdit(groupBox);
00155       layout->addRow("Left Arm Redundancy DOF", left_arm_redundancy_);
00156       left_arm_redundancy_->setText(QString::fromStdString(params_.left_redundancy_));
00157 
00158       left_ik_link_ = new QLineEdit(groupBox);
00159       layout->addRow("Left IK Link", left_ik_link_);
00160       left_ik_link_ ->setText(QString::fromStdString(params_.left_ik_link_));
00161 
00162       right_ik_link_ = new QLineEdit(groupBox);
00163       layout->addRow("Right IK Link", right_ik_link_);
00164       right_ik_link_->setText(QString::fromStdString(params_.right_ik_link_));
00165 
00166       planner_service_name_ = new QLineEdit(groupBox);
00167       layout->addRow("Planner Service", planner_service_name_);
00168       planner_service_name_->setText(QString::fromStdString(params_.planner_service_name_));
00169 
00170       left_interpolate_service_name_ = new QLineEdit(groupBox);
00171       layout->addRow("Left Interpolation Service", left_interpolate_service_name_);
00172       left_interpolate_service_name_->setText(QString::fromStdString(params_.left_interpolate_service_name_));
00173 
00174       right_interpolate_service_name_ = new QLineEdit(groupBox);
00175       layout->addRow("Right Interpolation Service", right_interpolate_service_name_);
00176       right_interpolate_service_name_->setText(QString::fromStdString(params_.right_interpolate_service_name_));
00177 
00178       trajectory_filter_service_name_ = new QLineEdit(groupBox);
00179       layout->addRow("Trajectory Filter Service", trajectory_filter_service_name_);
00180       trajectory_filter_service_name_ ->setText(QString::fromStdString(params_.trajectory_filter_service_name_ ));
00181 
00182       proximity_space_service_name_ = new QLineEdit(groupBox);
00183       layout->addRow("Proximity Space Service", proximity_space_service_name_);
00184       proximity_space_service_name_ ->setText(QString::fromStdString(params_.proximity_space_service_name_));
00185 
00186       proximity_space_validity_name_ = new QLineEdit(groupBox);
00187       layout->addRow("Proximity Space Validity Service", proximity_space_validity_name_);
00188       proximity_space_validity_name_ ->setText(QString::fromStdString(params_.proximity_space_validity_name_));
00189 
00190       proximity_space_planner_name_ = new QLineEdit(groupBox);
00191       layout->addRow("Proximity Space Planner", proximity_space_planner_name_);
00192       proximity_space_planner_name_ ->setText(QString::fromStdString(params_.proximity_space_planner_name_));
00193 
00194       use_robot_data_ = new QCheckBox(groupBox);
00195       use_robot_data_->setText("Use Robot Data");
00196       use_robot_data_->setChecked(false);
00197       layout->addRow("Use Data From Simulated/Real Robot?", use_robot_data_);
00198 
00199       QPushButton* button = new QPushButton(groupBox);
00200       button->setText("Accept");
00201       button->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
00202       connect(button, SIGNAL(clicked()), this, SLOT(accept()));
00203 
00204       boxLayout->addWidget(groupBox);
00205       boxLayout->addWidget(button);
00206       groupBox->setLayout(layout);
00207       setLayout(boxLayout);
00208 
00209     }
00210 
00211     void updateParams()
00212     {
00213       params_.left_ik_name_ = left_ik_name_->text().toStdString();
00214       params_.right_ik_name_ = right_ik_name_->text().toStdString();
00215       params_.non_coll_left_ik_name_ = non_coll_left_ik_name_ ->text().toStdString();
00216       params_.non_coll_right_ik_name_ = non_coll_right_ik_name_ ->text().toStdString();
00217       params_.right_arm_group_ = right_arm_group_->text().toStdString();
00218       params_.left_arm_group_ = left_arm_group_ ->text().toStdString();
00219       params_.right_redundancy_ = right_arm_redundancy_->text().toStdString();
00220       params_.left_redundancy_ = left_arm_redundancy_->text().toStdString();
00221       params_.left_ik_link_= left_ik_link_->text().toStdString();
00222       params_.right_ik_link_= right_ik_link_->text().toStdString();
00223       params_.planner_service_name_= planner_service_name_->text().toStdString();
00224       params_.left_interpolate_service_name_= left_interpolate_service_name_->text().toStdString();
00225       params_.right_interpolate_service_name_= right_interpolate_service_name_->text().toStdString();
00226       params_.trajectory_filter_service_name_= trajectory_filter_service_name_->text().toStdString();
00227       params_.proximity_space_service_name_= proximity_space_service_name_->text().toStdString();
00228       params_.proximity_space_validity_name_= proximity_space_validity_name_->text().toStdString();
00229       params_.proximity_space_planner_name_= proximity_space_planner_name_->text().toStdString();
00230       params_.use_robot_data_ = use_robot_data_->isChecked();
00231     }
00232 
00233   private:
00234     QFormLayout* layout;
00235     QLineEdit* left_ik_name_;
00236     QLineEdit* right_ik_name_;
00237     QLineEdit* non_coll_left_ik_name_;
00238     QLineEdit* non_coll_right_ik_name_;
00239     QLineEdit* right_arm_group_;
00240     QLineEdit* left_arm_group_;
00241     QLineEdit* right_arm_redundancy_;
00242     QLineEdit* left_arm_redundancy_;
00243     QLineEdit* left_ik_link_;
00244     QLineEdit* right_ik_link_;
00245     QLineEdit* planner_service_name_;
00246     QLineEdit* left_interpolate_service_name_;
00247     QLineEdit* right_interpolate_service_name_;
00248     QLineEdit* trajectory_filter_service_name_;
00249     QLineEdit* proximity_space_service_name_;
00250     QLineEdit* proximity_space_validity_name_;
00251     QLineEdit* proximity_space_planner_name_;
00252     QCheckBox* use_robot_data_;
00253 
00254 
00255 };
00256 
00257 class PlanningSceneVisualizer : public QWidget, public planning_scene_utils::PlanningSceneEditor
00258 {
00259     Q_OBJECT
00260   public:
00261 
00262     bool quit_threads_;
00263 
00264     class TableLoadThread : public QThread
00265     {
00266       public:
00267         PlanningSceneVisualizer* visualizer_;
00268         TableLoadThread(PlanningSceneVisualizer* visualizer) : QThread(visualizer), visualizer_(visualizer)
00269         {
00270         }
00271 
00272         void run()
00273         {
00274           visualizer_->createPlanningSceneTable();
00275         }
00276     };
00277 
00278     PlanningSceneVisualizer(QWidget* parent, planning_scene_utils::PlanningSceneParameters& params);
00279 
00280     ~PlanningSceneVisualizer();
00281 
00282     void initQtWidgets();
00283     void setupPlanningSceneDialog();
00284     void createPlanningSceneTable();
00285     void createTrajectoryTable();
00286     void createMotionPlanTable();
00287     void createNewObjectDialog();
00288     void createRequestDialog();
00289     void updateState();
00290     void onPlanningSceneLoaded(int scene, int numScenes);
00291     signals:
00292       void changeProgress(int progress);
00293       void updateTables();
00294 
00295   public slots:
00296     void quit();
00297     void popupLoadPlanningScenes();
00298     void progressChanged(int progress) { load_scene_progress_->setValue(progress);}
00299     void loadButtonPressed();
00300     void refreshButtonPressed();
00301     void trajectoryTableSelection();
00302     void motionPlanTableSelection();
00303     void playButtonPressed();
00304     void filterButtonPressed();
00305     void sliderDragged();
00306     void replanButtonPressed();
00307     void trajectoryEditChanged();
00308     void collisionDisplayChanged(const QString& mode);
00309     void createNewPlanningScenePressed();
00310     void saveCurrentPlanningScene();
00311     void createNewMotionPlanRequest(std::string group_name, std::string end_effector_name);
00312     void motionPlanStartColorButtonClicked();
00313     void motionPlanEndColorButtonClicked();
00314     void motionPlanStartVisibleButtonClicked(bool checked);
00315     void motionPlanEndVisibleButtonClicked(bool checked);
00316     void trajectoryColorButtonClicked();
00317     void trajectoryVisibleButtonClicked(bool checked);
00318     void createNewMotionPlanPressed();
00319     void createNewObjectPressed();
00320     void createObjectConfirmedPressed();
00321     void createRequestPressed();
00322     void motionPlanCollisionVisibleButtonClicked(bool checked);
00323     void trajectoryCollisionsVisibleButtonClicked(bool checked);
00324     void selectMotionPlan(std::string ID);
00325     void selectTrajectory(std::string ID);
00326     void deleteSelectedMotionPlan();
00327     void deleteSelectedTrajectory();
00328     void updateStateTriggered();
00329     void executeButtonPressed();
00330 
00331   protected:
00332 
00333     QLabel* selected_trajectory_label_;
00334     QLabel* selected_request_label_;
00335     QMenuBar* menu_bar_;
00336     QMenu* file_menu_;
00337     QMenu* collision_object_menu_;
00338     QAction* new_object_action_;
00339     QDialog* load_planning_scene_dialog_;
00340     QDialog* new_object_dialog_;
00341     QDialog* new_request_dialog_;
00342     QProgressBar* load_scene_progress_;
00343     QAction* new_planning_scene_action_;
00344     QAction* new_motion_plan_action_;
00345     QAction* load_planning_scene_action_;
00346     QAction* save_planning_scene_action_;
00347     QAction* quit_action_;
00348     QTableWidget* planning_scene_table_;
00349     QTreeWidget* motion_plan_tree_;
00350     QTreeWidget* trajectory_tree_;
00351     QSlider* trajectory_slider_;
00352     QPushButton* play_button_;
00353     QPushButton* filter_button_;
00354     QPushButton* replan_button_;
00355     QPushButton* execute_button_;
00356 
00357     QPushButton* load_planning_scene_button_;
00358     QPushButton* refresh_planning_scene_button_;
00359     QComboBox* collision_display_box_;
00360 
00361     QSpinBox* trajectory_point_edit_;
00362 
00363     TableLoadThread* table_load_thread_;
00364 
00365     QComboBox* collision_object_type_box_;
00366     QComboBox* request_group_name_box_;
00367     QSpinBox* collision_object_scale_x_box_;
00368     QSpinBox* collision_object_scale_y_box_;
00369     QSpinBox* collision_object_scale_z_box_;
00370     QSpinBox* collision_object_pos_x_box_;
00371     QSpinBox* collision_object_pos_y_box_;
00372     QSpinBox* collision_object_pos_z_box_;
00373     QPushButton* make_object_button_;
00374 
00375     QCheckBox* load_motion_plan_requests_box_;
00376     QCheckBox* load_trajectories_box_;
00377     QCheckBox* create_request_from_robot_box_;
00378 
00379 };
00380 #endif
planning_scene_warehouse_viewer.h
