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