main_window.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Mario Prats, Ioan Sucan */
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   // Goals and states
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   // Trajectories
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   // main loop processing
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   // rviz
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   // robot interaction
00173   robot_interaction::RobotInteractionPtr robot_interaction_;
00174   rviz::Display* int_marker_display_;
00175 
00176   // Warehouse
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   // Goals and start states
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   // Trajectories
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   // Background processing
00254   void loadSceneButtonClickedBackgroundJob(void);
00255 
00256   // Foreground processing
00257   const static unsigned int MAIN_LOOP_RATE = 20;  // calls to executeMainLoopJobs per second
00258   boost::shared_ptr<QTimer> main_loop_jobs_timer_;
00259 
00260   // Status and logging
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


benchmarks_gui
Author(s): Mario Prats
autogenerated on Mon Jul 24 2017 02:22:21