Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef BT_MAIN_WINDOW_
00033 #define BT_MAIN_WINDOW_
00034
00035 #include <QtGui/QMainWindow>
00036 #include <QTimer>
00037 #include <QSettings>
00038
00039 #include "ui_main_window.h"
00040 #include "ui_run_benchmark_dialog.h"
00041 #include "ui_robot_loader.h"
00042
00043 #ifndef Q_MOC_RUN
00044
00045 #include <moveit/warehouse/planning_scene_storage.h>
00046 #include <moveit/warehouse/trajectory_constraints_storage.h>
00047 #include <moveit/warehouse/constraints_storage.h>
00048 #include <moveit/warehouse/state_storage.h>
00049
00050 #include <frame_marker.h>
00051 #include <trajectory.h>
00052 #include <job_processing.h>
00053
00054 #include <rviz/render_panel.h>
00055 #include <rviz/visualization_manager.h>
00056 #include <rviz/robot/robot.h>
00057
00058 #include <moveit/rviz_plugin_render_tools/robot_state_visualization.h>
00059 #include <moveit/planning_scene_rviz_plugin/planning_scene_display.h>
00060 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00061 #include <moveit/robot_interaction/robot_interaction.h>
00062
00063 #endif
00064
00065 namespace benchmark_tool
00066 {
00067
00068 class MainWindow : public QMainWindow
00069 {
00070 Q_OBJECT
00071
00072 public:
00073 MainWindow(int argc, char **argv, QWidget *parent = 0);
00074
00075 ~MainWindow();
00076 public Q_SLOTS:
00077
00078 void exitActionTriggered(bool);
00079 void openActionTriggered(bool);
00080 void planningGroupChanged(const QString &text);
00081 void dbConnectButtonClicked();
00082 void dbConnectButtonClickedBackgroundJob();
00083 void robotInteractionButtonClicked();
00084
00085 void loadSceneButtonClicked(void);
00086 void loadSceneButtonClicked(QListWidgetItem *item);
00087
00088
00089 void goalPoseFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback);
00090 void createGoalPoseButtonClicked(void);
00091 void removeSelectedGoalsButtonClicked(void);
00092 void removeAllGoalsButtonClicked(void);
00093 void goalPoseSelectionChanged(void);
00094 void switchGoalVisibilityButtonClicked(void);
00095 void goalPoseDoubleClicked(QListWidgetItem *item);
00096 void copySelectedGoalPoses(void);
00097 void visibleAxisChanged(int state);
00098 void checkGoalsInCollision(void);
00099 void checkGoalsReachable(void);
00100 void runBenchmark(void);
00101 void runBenchmarkOKButtonClicked(void);
00102 void benchmarkFolderButtonClicked(void);
00103 void loadBenchmarkResults(void);
00104 void updateMarkerState(GripperMarkerPtr marker, const GripperMarker::GripperMarkerState &state);
00105 void updateGoalMarkerStateFromName(const std::string &name, const GripperMarker::GripperMarkerState &state);
00106 void goalOffsetChanged();
00107
00108 void saveStartStateButtonClicked(void);
00109 void removeSelectedStatesButtonClicked(void);
00110 void removeAllStatesButtonClicked(void);
00111 void startStateItemDoubleClicked(QListWidgetItem * item);
00112
00113 void loadGoalsFromDBButtonClicked(void);
00114 void saveGoalsOnDBButtonClicked(void);
00115 void deleteGoalsOnDBButtonClicked(void);
00116 void loadStatesFromDBButtonClicked(void);
00117 void saveStatesOnDBButtonClicked(void);
00118 void deleteStatesOnDBButtonClicked(void);
00119
00120
00121 void trajectorySelectionChanged(void);
00122 void createTrajectoryButtonClicked(void);
00123 void removeTrajectoryButtonClicked(void);
00124 void loadTrajectoriesFromDBButtonClicked(void);
00125 void saveTrajectoriesOnDBButtonClicked(void);
00126 void trajectoryNWaypointsChanged(int);
00127 void trajectoryExecuteButtonClicked(void);
00128
00129
00130 void MainLoop();
00131
00132
00133 private:
00134 const static char * ROBOT_DESCRIPTION_PARAM;
00135 const static char * ROBOT_DESCRIPTION_SEMANTIC_PARAM;
00136 const static unsigned int DEFAULT_WAREHOUSE_PORT;
00137
00138 Ui::MainWindow ui_;
00139 Ui_BenchmarkDialog run_benchmark_ui_;
00140 Ui_RobotLoader load_robot_ui_;
00141 QDialog *robot_loader_dialog_;
00142 boost::shared_ptr<QSettings> settings_;
00143
00144
00145 rviz::RenderPanel *render_panel_;
00146 rviz::VisualizationManager *visualization_manager_;
00147 moveit_rviz_plugin::PlanningSceneDisplay *scene_display_;
00148
00149 bool waitForPlanningSceneMonitor(moveit_rviz_plugin::PlanningSceneDisplay *scene_display);
00150 void scheduleStateUpdate();
00151 void scheduleStateUpdateBackgroundJob();
00152 bool isIKSolutionCollisionFree(robot_state::JointStateGroup *group, const std::vector<double> &ik_solution);
00153 bool configure();
00154 void loadNewRobot(const std::string &urdf_path, const std::string &srdf_path);
00155 void setItemSelectionInList(const std::string &item_name, bool selection, QListWidget *list);
00156 void selectItemJob(QListWidgetItem *item, bool flag);
00157
00158
00159 robot_interaction::RobotInteractionPtr robot_interaction_;
00160 rviz::Display *int_marker_display_;
00161
00162
00163 boost::shared_ptr<moveit_warehouse::PlanningSceneStorage> planning_scene_storage_;
00164 boost::shared_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
00165 boost::shared_ptr<moveit_warehouse::TrajectoryConstraintsStorage> trajectory_constraints_storage_;
00166 boost::shared_ptr<moveit_warehouse::RobotStateStorage> robot_state_storage_;
00167
00168 void populatePlanningSceneList(void);
00169
00170
00171 robot_interaction::RobotInteraction::InteractionHandlerPtr query_goal_state_;
00172
00173 Eigen::Affine3d goal_offset_;
00174 EigenSTL::map_string_Affine3d goals_initial_pose_;
00175 EigenSTL::map_string_Affine3d goals_dragging_initial_pose_;
00176 bool goal_pose_dragging_;
00177
00178 typedef std::map<std::string, GripperMarkerPtr> GoalPoseMap;
00179 typedef std::pair<std::string, GripperMarkerPtr> GoalPosePair;
00180 GoalPoseMap goal_poses_;
00181
00182 class StartState
00183 {
00184 public:
00185 moveit_msgs::RobotState state_msg;
00186 bool selected;
00187
00188 StartState(): selected(false) {}
00189 StartState(const moveit_msgs::RobotState &state): state_msg(state), selected(false) {}
00190 StartState(const moveit_msgs::RobotState &state, bool is_selected): state_msg(state), selected(is_selected) {}
00191 };
00192 typedef boost::shared_ptr<StartState> StartStatePtr;
00193
00194 typedef std::map<std::string, StartStatePtr> StartStateMap;
00195 typedef std::pair<std::string, StartStatePtr> StartStatePair;
00196 StartStateMap start_states_;
00197
00198 void populateGoalPosesList();
00199 void populateStartStatesList();
00200 void populateTrajectoriesList();
00201 void computeGoalPoseDoubleClicked(QListWidgetItem * item);
00202 void switchGoalPoseMarkerSelection(const std::string &marker_name);
00203 typedef std::pair<visualization_msgs::InteractiveMarker, boost::shared_ptr<rviz::InteractiveMarker> > MsgMarkerPair;
00204
00205 void checkIfGoalInCollision(const std::string & goal_name);
00206 void checkIfGoalReachable(const std::string &goal_name, bool update_if_reachable = false);
00207 void computeLoadBenchmarkResults(const std::string &file);
00208
00209 void updateGoalPoseMarkers(float wall_dt, float ros_dt);
00210
00211
00212 void switchTrajectorySelection(const std::string &marker_name);
00213 void animateTrajectory(const std::vector<boost::shared_ptr<robot_state::RobotState> > &traj);
00214
00215 typedef std::map<std::string, TrajectoryPtr> TrajectoryMap;
00216 typedef std::pair<std::string, TrajectoryPtr> TrajectoryPair;
00217 TrajectoryMap trajectories_;
00218
00219 void createTrajectoryStartMarker(const GripperMarker &marker);
00220
00221
00222 void loadSceneButtonClickedBackgroundJob(void);
00223
00224
00225 const static unsigned int MAIN_LOOP_RATE = 20;
00226 boost::shared_ptr<QTimer> main_loop_jobs_timer_;
00227
00228
00229 typedef enum {STATUS_WARN, STATUS_ERROR, STATUS_INFO} StatusType;
00230 void setStatus(StatusType st, const QString &text)
00231 {
00232 if (st == STATUS_WARN)
00233 {
00234 ROS_WARN_STREAM(text.toStdString());
00235 ui_.status_label->setText(text);
00236 }
00237 else if (st == STATUS_ERROR)
00238 {
00239 ROS_ERROR_STREAM(text.toStdString());
00240 ui_.status_label->setText(text);
00241 }
00242 else if (st == STATUS_INFO)
00243 {
00244 ui_.status_label->setText(text);
00245 }
00246 }
00247
00248 void setStatusFromBackground(StatusType st, const QString &text)
00249 {
00250 JobProcessing::addMainLoopJob(boost::bind(&MainWindow::setStatus, this, st, text));
00251 }
00252
00253 };
00254
00255 }
00256
00257 #endif