motion_planning_frame.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, 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: Ioan Sucan */
00036 
00037 #ifndef MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_FRAME_
00038 #define MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_FRAME_
00039 
00040 #include <QWidget>
00041 #include <QTreeWidgetItem>
00042 #include <QListWidgetItem>
00043 
00044 #ifndef Q_MOC_RUN
00045 #include <moveit/macros/class_forward.h>
00046 #include <moveit/move_group_interface/move_group.h>
00047 #include <moveit/planning_scene_interface/planning_scene_interface.h>
00048 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00049 #include <moveit/robot_interaction/robot_interaction.h>
00050 #include <moveit/robot_interaction/interaction_handler.h>
00051 #include <moveit/semantic_world/semantic_world.h>
00052 #include <interactive_markers/interactive_marker_server.h>
00053 #include <rviz/default_plugin/interactive_markers/interactive_marker.h>
00054 #include <moveit_msgs/MotionPlanRequest.h>
00055 #include <actionlib/client/simple_action_client.h>
00056 #include <object_recognition_msgs/ObjectRecognitionAction.h>
00057 #endif
00058 
00059 #include <std_msgs/Bool.h>
00060 #include <std_msgs/Empty.h>
00061 #include <map>
00062 #include <string>
00063 
00064 namespace rviz
00065 {
00066 class DisplayContext;
00067 }
00068 
00069 namespace Ui
00070 {
00071 class MotionPlanningUI;
00072 }
00073 
00074 namespace moveit_warehouse
00075 {
00076 MOVEIT_CLASS_FORWARD(PlanningSceneStorage);
00077 MOVEIT_CLASS_FORWARD(ConstraintsStorage);
00078 MOVEIT_CLASS_FORWARD(RobotStateStorage);
00079 }
00080 
00081 namespace moveit_rviz_plugin
00082 {
00083 class MotionPlanningDisplay;
00084 
00085 const std::string OBJECT_RECOGNITION_ACTION = "/recognize_objects";
00086 
00087 static const std::string TAB_CONTEXT = "Context";
00088 static const std::string TAB_PLANNING = "Planning";
00089 static const std::string TAB_MANIPULATION = "Manipulation";
00090 static const std::string TAB_OBJECTS = "Scene Objects";
00091 static const std::string TAB_SCENES = "Stored Scenes";
00092 static const std::string TAB_STATES = "Stored States";
00093 static const std::string TAB_STATUS = "Status";
00094 
00095 class MotionPlanningFrame : public QWidget
00096 {
00097   friend class MotionPlanningDisplay;
00098   Q_OBJECT
00099 
00100 public:
00101   MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz::DisplayContext* context, QWidget* parent = 0);
00102   ~MotionPlanningFrame();
00103 
00104   void changePlanningGroup();
00105   void enable();
00106   void disable();
00107   void sceneUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type);
00108 
00109 protected:
00110   static const int ITEM_TYPE_SCENE = 1;
00111   static const int ITEM_TYPE_QUERY = 2;
00112 
00113   void constructPlanningRequest(moveit_msgs::MotionPlanRequest& mreq);
00114 
00115   void updateSceneMarkers(float wall_dt, float ros_dt);
00116 
00117   void updateExternalCommunication();
00118 
00119   MotionPlanningDisplay* planning_display_;
00120   rviz::DisplayContext* context_;
00121   Ui::MotionPlanningUI* ui_;
00122 
00123   moveit::planning_interface::MoveGroupPtr move_group_;
00124   moveit::planning_interface::PlanningSceneInterfacePtr planning_scene_interface_;
00125   moveit::semantic_world::SemanticWorldPtr semantic_world_;
00126 
00127   moveit::planning_interface::MoveGroup::PlanPtr current_plan_;
00128   moveit_warehouse::PlanningSceneStoragePtr planning_scene_storage_;
00129   moveit_warehouse::ConstraintsStoragePtr constraints_storage_;
00130   moveit_warehouse::RobotStateStoragePtr robot_state_storage_;
00131 
00132   boost::shared_ptr<rviz::InteractiveMarker> scene_marker_;
00133 
00134   typedef std::map<std::string, moveit_msgs::RobotState> RobotStateMap;
00135   typedef std::pair<std::string, moveit_msgs::RobotState> RobotStatePair;
00136   RobotStateMap robot_states_;
00137 
00138 Q_SIGNALS:
00139   void planningFinished();
00140 
00141 private Q_SLOTS:
00142 
00143   // Context tab
00144   void databaseConnectButtonClicked();
00145   void publishSceneButtonClicked();
00146   void planningAlgorithmIndexChanged(int index);
00147   void resetDbButtonClicked();
00148   void approximateIKChanged(int state);
00149 
00150   // Planning tab
00151   void planButtonClicked();
00152   void executeButtonClicked();
00153   void planAndExecuteButtonClicked();
00154   void stopButtonClicked();
00155   void allowReplanningToggled(bool checked);
00156   void allowLookingToggled(bool checked);
00157   void allowExternalProgramCommunication(bool enable);
00158   void pathConstraintsIndexChanged(int index);
00159   void useStartStateButtonClicked();
00160   void useGoalStateButtonClicked();
00161   void onClearOctomapClicked();
00162 
00163   // Scene Objects tab
00164   void importFileButtonClicked();
00165   void importUrlButtonClicked();
00166   void clearSceneButtonClicked();
00167   void sceneScaleChanged(int value);
00168   void sceneScaleStartChange();
00169   void sceneScaleEndChange();
00170   void removeObjectButtonClicked();
00171   void selectedCollisionObjectChanged();
00172   void objectPoseValueChanged(double value);
00173   void collisionObjectChanged(QListWidgetItem* item);
00174   void imProcessFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback);
00175   void copySelectedCollisionObject();
00176   void exportAsTextButtonClicked();
00177   void importFromTextButtonClicked();
00178 
00179   // Stored scenes tab
00180   void saveSceneButtonClicked();
00181   void planningSceneItemClicked();
00182   void saveQueryButtonClicked();
00183   void deleteSceneButtonClicked();
00184   void deleteQueryButtonClicked();
00185   void loadSceneButtonClicked();
00186   void loadQueryButtonClicked();
00187   void warehouseItemNameChanged(QTreeWidgetItem* item, int column);
00188 
00189   // States tab
00190   void loadStateButtonClicked();
00191   void saveStartStateButtonClicked();
00192   void saveGoalStateButtonClicked();
00193   void removeStateButtonClicked();
00194   void clearStatesButtonClicked();
00195   void setAsStartStateButtonClicked();
00196   void setAsGoalStateButtonClicked();
00197 
00198   // Pick and place
00199   void detectObjectsButtonClicked();
00200   void pickObjectButtonClicked();
00201   void placeObjectButtonClicked();
00202   void selectedDetectedObjectChanged();
00203   void detectedObjectChanged(QListWidgetItem* item);
00204   void selectedSupportSurfaceChanged();
00205 
00206   // General
00207   void tabChanged(int index);
00208 
00209 private:
00210   // Context tab
00211   void computeDatabaseConnectButtonClicked();
00212   void computeDatabaseConnectButtonClickedHelper(int mode);
00213   void computeResetDbButtonClicked(const std::string& db);
00214   void populatePlannersList(const moveit_msgs::PlannerInterfaceDescription& desc);
00215 
00216   // Planning tab
00217   void computePlanButtonClicked();
00218   void computeExecuteButtonClicked();
00219   void computePlanAndExecuteButtonClicked();
00220   void computePlanAndExecuteButtonClickedDisplayHelper();
00221   void computeStopButtonClicked();
00222   void onFinishedExecution(bool success);
00223   void populateConstraintsList();
00224   void populateConstraintsList(const std::vector<std::string>& constr);
00225   void configureForPlanning();
00226   void configureWorkspace();
00227   void updateQueryStateHelper(robot_state::RobotState& state, const std::string& v);
00228   void fillStateSelectionOptions();
00229 
00230   // Scene objects tab
00231   void addObject(const collision_detection::WorldPtr& world, const std::string& id, const shapes::ShapeConstPtr& shape,
00232                  const Eigen::Affine3d& pose);
00233   void updateCollisionObjectPose(bool update_marker_position);
00234   void createSceneInteractiveMarker();
00235   void renameCollisionObject(QListWidgetItem* item);
00236   void attachDetachCollisionObject(QListWidgetItem* item);
00237   void populateCollisionObjectsList();
00238   void computeImportFromText(const std::string& path);
00239   void computeExportAsText(const std::string& path);
00240 
00241   // Stored scenes tab
00242   void computeSaveSceneButtonClicked();
00243   void computeSaveQueryButtonClicked(const std::string& scene, const std::string& query_name);
00244   void computeLoadSceneButtonClicked();
00245   void computeLoadQueryButtonClicked();
00246   void populatePlanningSceneTreeView();
00247   void computeDeleteSceneButtonClicked();
00248   void computeDeleteQueryButtonClicked();
00249   void computeDeleteQueryButtonClickedHelper(QTreeWidgetItem* s);
00250   void checkPlanningSceneTreeEnabledButtons();
00251 
00252   // States tab
00253   void saveRobotStateButtonClicked(const robot_state::RobotState& state);
00254   void populateRobotStatesList();
00255 
00256   // Pick and place
00257   void processDetectedObjects();
00258   void updateDetectedObjectsList(const std::vector<std::string>& object_ids, const std::vector<std::string>& objects);
00259   void publishTables();
00260   void updateSupportSurfacesList();
00261   ros::Publisher object_recognition_trigger_publisher_;
00262   std::map<std::string, std::string> pick_object_name_;
00263   std::string place_object_name_;
00264   std::vector<geometry_msgs::PoseStamped> place_poses_;
00265   void pickObject();
00266   void placeObject();
00267   void triggerObjectDetection();
00268   void updateTables();
00269   std::string support_surface_name_;
00270   // For coloring
00271   std::string selected_object_name_;
00272   std::string selected_support_surface_name_;
00273 
00274   boost::scoped_ptr<actionlib::SimpleActionClient<object_recognition_msgs::ObjectRecognitionAction> >
00275       object_recognition_client_;
00276   template <typename T>
00277   void waitForAction(const T& action, const ros::NodeHandle& node_handle, const ros::Duration& wait_for_server,
00278                      const std::string& name);
00279   void listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr& msg);
00280   ros::Subscriber object_recognition_subscriber_;
00281 
00282   ros::Subscriber plan_subscriber_;
00283   ros::Subscriber execute_subscriber_;
00284   ros::Subscriber update_start_state_subscriber_;
00285   ros::Subscriber update_goal_state_subscriber_;
00286   // General
00287   void changePlanningGroupHelper();
00288   void importResource(const std::string& path);
00289   void loadStoredStates(const std::string& pattern);
00290 
00291   void remotePlanCallback(const std_msgs::EmptyConstPtr& msg);
00292   void remoteExecuteCallback(const std_msgs::EmptyConstPtr& msg);
00293   void remoteUpdateStartStateCallback(const std_msgs::EmptyConstPtr& msg);
00294   void remoteUpdateGoalStateCallback(const std_msgs::EmptyConstPtr& msg);
00295 
00296   /* Selects or unselects a item in a list by the item name */
00297   void setItemSelectionInList(const std::string& item_name, bool selection, QListWidget* list);
00298 
00299   ros::NodeHandle nh_;
00300   ros::Publisher planning_scene_publisher_;
00301   ros::Publisher planning_scene_world_publisher_;
00302 
00303   collision_detection::CollisionWorld::ObjectConstPtr scaled_object_;
00304 
00305   std::vector<std::pair<std::string, bool> > known_collision_objects_;
00306   long unsigned int known_collision_objects_version_;
00307   bool first_time_;
00308   ros::ServiceClient clear_octomap_service_client_;
00309 };
00310 
00311 // \todo THIS IS REALLY BAD. NEED TO MOVE THIS AND RELATED FUNCTIONALITY OUT OF HERE
00312 template <typename T>
00313 void MotionPlanningFrame::waitForAction(const T& action, const ros::NodeHandle& node_handle,
00314                                         const ros::Duration& wait_for_server, const std::string& name)
00315 {
00316   ROS_DEBUG("Waiting for MoveGroup action server (%s)...", name.c_str());
00317 
00318   // in case ROS time is published, wait for the time data to arrive
00319   ros::Time start_time = ros::Time::now();
00320   while (start_time == ros::Time::now())
00321   {
00322     ros::WallDuration(0.01).sleep();
00323     ros::spinOnce();
00324   }
00325 
00326   // wait for the server (and spin as needed)
00327   if (wait_for_server == ros::Duration(0, 0))
00328   {
00329     // wait forever until action server connects
00330     while (node_handle.ok() && !action->isServerConnected())
00331     {
00332       ros::WallDuration(0.02).sleep();
00333       ros::spinOnce();
00334     }
00335   }
00336   else
00337   {
00338     // wait for a limited amount of non-simulated time
00339     ros::WallTime final_time = ros::WallTime::now() + ros::WallDuration(wait_for_server.toSec());
00340     while (node_handle.ok() && !action->isServerConnected() && final_time > ros::WallTime::now())
00341     {
00342       ros::WallDuration(0.02).sleep();
00343       ros::spinOnce();
00344     }
00345   }
00346 
00347   if (!action->isServerConnected())
00348     throw std::runtime_error("Unable to connect to move_group action server within allotted time");
00349   else
00350     ROS_DEBUG("Connected to '%s'", name.c_str());
00351 };
00352 }
00353 
00354 #endif


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:22:13