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 <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/macros/class_forward.h>
00052 #include <moveit/warehouse/planning_scene_storage.h>
00053 #include <moveit/warehouse/trajectory_constraints_storage.h>
00054 #include <moveit/warehouse/constraints_storage.h>
00055 #include <moveit/warehouse/state_storage.h>
00056
00057 #include <frame_marker.h>
00058 #include <trajectory.h>
00059 #include <job_processing.h>
00060
00061 #include <rviz/render_panel.h>
00062 #include <rviz/visualization_manager.h>
00063 #include <rviz/robot/robot.h>
00064
00065 #include <moveit/rviz_plugin_render_tools/robot_state_visualization.h>
00066 #include <moveit/planning_scene_rviz_plugin/planning_scene_display.h>
00067 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00068 #include <moveit/robot_interaction/robot_interaction.h>
00069 #include <moveit/robot_interaction/interaction_handler.h>
00070
00071 #endif
00072
00073 namespace benchmark_tool
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 private:
00145 const static char* ROBOT_DESCRIPTION_PARAM;
00146 const static char* ROBOT_DESCRIPTION_SEMANTIC_PARAM;
00147 const static unsigned int DEFAULT_WAREHOUSE_PORT;
00148
00149 Ui::MainWindow ui_;
00150 Ui_BenchmarkDialog run_benchmark_ui_;
00151 Ui_RobotLoader load_robot_ui_;
00152 Ui_BoundingBoxGoalsDialog bbox_dialog_ui_;
00153 QDialog *robot_loader_dialog_, *run_benchmark_dialog_, *bbox_dialog_;
00154 boost::shared_ptr<QSettings> settings_;
00155
00156
00157 rviz::RenderPanel* render_panel_;
00158 rviz::VisualizationManager* visualization_manager_;
00159 moveit_rviz_plugin::PlanningSceneDisplay* scene_display_;
00160
00161 bool waitForPlanningSceneMonitor(moveit_rviz_plugin::PlanningSceneDisplay* scene_display);
00162 void scheduleStateUpdate();
00163 void scheduleStateUpdateBackgroundJob();
00164 bool isIKSolutionCollisionFree(robot_state::RobotState* state, const robot_model::JointModelGroup* group,
00165 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 moveit_warehouse::PlanningSceneStoragePtr planning_scene_storage_;
00180 moveit_warehouse::ConstraintsStoragePtr constraints_storage_;
00181 moveit_warehouse::TrajectoryConstraintsStoragePtr trajectory_constraints_storage_;
00182 moveit_warehouse::RobotStateStoragePtr 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 MOVEIT_CLASS_FORWARD(StartState);
00200
00201 class StartState
00202 {
00203 public:
00204 moveit_msgs::RobotState state_msg;
00205 bool selected;
00206
00207 StartState() : selected(false)
00208 {
00209 }
00210 StartState(const moveit_msgs::RobotState& state) : state_msg(state), selected(false)
00211 {
00212 }
00213 StartState(const moveit_msgs::RobotState& state, bool is_selected) : state_msg(state), selected(is_selected)
00214 {
00215 }
00216 };
00217
00218 typedef std::map<std::string, StartStatePtr> StartStateMap;
00219 typedef std::pair<std::string, StartStatePtr> StartStatePair;
00220 StartStateMap start_states_;
00221
00222 void populateGoalPosesList();
00223 void populateStartStatesList();
00224 void populateTrajectoriesList();
00225 void computeGoalPoseDoubleClicked(QListWidgetItem* item);
00226 void switchGoalPoseMarkerSelection(const std::string& marker_name);
00227 typedef std::pair<visualization_msgs::InteractiveMarker, boost::shared_ptr<rviz::InteractiveMarker> > MsgMarkerPair;
00228
00235 bool isGroupCollidingWithWorld(robot_state::RobotState& robot_state, const std::string& group_name);
00236
00237 void checkIfGoalInCollision(const std::string& goal_name);
00238 void checkIfGoalReachable(const std::string& goal_name, bool update_if_reachable = false);
00239 void computeLoadBenchmarkResults(const std::string& file);
00240
00241 void updateGoalPoseMarkers(float wall_dt, float ros_dt);
00242
00243
00244 void switchTrajectorySelection(const std::string& marker_name);
00245 void animateTrajectory(const std::vector<robot_state::RobotStatePtr>& traj);
00246
00247 typedef std::map<std::string, TrajectoryPtr> TrajectoryMap;
00248 typedef std::pair<std::string, TrajectoryPtr> TrajectoryPair;
00249 TrajectoryMap trajectories_;
00250
00251 void createTrajectoryStartMarker(const GripperMarker& marker);
00252
00253
00254 void loadSceneButtonClickedBackgroundJob(void);
00255
00256
00257 const static unsigned int MAIN_LOOP_RATE = 20;
00258 boost::shared_ptr<QTimer> main_loop_jobs_timer_;
00259
00260
00261 typedef enum { STATUS_WARN, STATUS_ERROR, STATUS_INFO } StatusType;
00262 void setStatus(StatusType st, const QString& text)
00263 {
00264 if (st == STATUS_WARN)
00265 {
00266 ROS_WARN_STREAM(text.toStdString());
00267 ui_.status_label->setText(text);
00268 }
00269 else if (st == STATUS_ERROR)
00270 {
00271 ROS_ERROR_STREAM(text.toStdString());
00272 ui_.status_label->setText(text);
00273 }
00274 else if (st == STATUS_INFO)
00275 {
00276 ui_.status_label->setText(text);
00277 }
00278 }
00279
00280 void setStatusFromBackground(StatusType st, const QString& text)
00281 {
00282 JobProcessing::addMainLoopJob(boost::bind(&MainWindow::setStatus, this, st, text));
00283 }
00284 };
00285 }
00286
00287 #endif