Merge pull request #220 from AustinDeric/update/simple-gui-automation

Simple gui automation
ros-industrial-attic · Mar 30, 2017 · 34f55e6 · 34f55e6
2 parents b3f949b + 322f264
commit 34f55e6
Show file tree

Hide file tree

Showing 10 changed files with 103 additions and 62 deletions.
diff --git a/godel_simple_gui/CMakeLists.txt b/godel_simple_gui/CMakeLists.txt
@@ -37,6 +37,7 @@ set(godel_simple_gui_SRCS
  src/states/wait_to_execute_state.cpp
  src/states/executing_state.cpp
  src/states/error_state.cpp
+ src/states/select_all_surface_state.cpp
 # options
  src/options_submenu.cpp
  src/parameter_window_base.cpp
@@ -61,6 +62,7 @@ set(godel_simple_gui_HDRS
   include/godel_simple_gui/states/wait_to_execute_state.h
   include/godel_simple_gui/states/executing_state.h
   include/godel_simple_gui/states/error_state.h
+  include/godel_simple_gui/states/select_all_surface_state.h
   # options
   include/godel_simple_gui/options_submenu.h
   include/godel_simple_gui/parameter_window_base.h
@@ -87,7 +89,6 @@ set(godel_simple_gui_INCLUDE_DIRECTORIES
 )
 
 add_definitions(-DQT_NO_KEYWORDS)
-
 qt5_wrap_cpp(godel_simple_gui_MOCS ${godel_simple_gui_HDRS})
 qt5_wrap_ui(godel_simple_gui_UIS_H ${godel_simple_gui_UIS})
 

diff --git a/godel_simple_gui/include/godel_simple_gui/blending_widget.h b/godel_simple_gui/include/godel_simple_gui/blending_widget.h
@@ -46,8 +46,9 @@ class BlendingWidget : public QWidget
   void addPlans(const std::vector<std::string>& plan_names);
   void setLabelText(const std::string& txt);
   std::vector<std::string> getPlanNames();
-  void sendGoal(godel_msgs::SelectMotionPlanActionGoal goal);
-
+  void sendGoal(const godel_msgs::SelectMotionPlanActionGoal& goal);
+  void sendGoalAndWait(const godel_msgs::SelectMotionPlanActionGoal& goal);
+  bool planSelectionEmpty();
 
   ros::NodeHandle& nodeHandle() { return nh_; }
 

diff --git a/godel_simple_gui/include/godel_simple_gui/states/executing_state.h b/godel_simple_gui/include/godel_simple_gui/states/executing_state.h
@@ -24,8 +24,8 @@ class ExecutingState : public GuiState
   virtual void onReset(BlendingWidget& gui);
 
 protected:
+  void executeOne(const std::string &plan, BlendingWidget& gui);
   void executeAll(BlendingWidget& gui);
-  void executeOne(const std::string& plan, BlendingWidget& gui);
 
 private:
   std::vector<std::string> plan_names_;

diff --git a/godel_simple_gui/include/godel_simple_gui/states/select_all_surface_state.h b/godel_simple_gui/include/godel_simple_gui/states/select_all_surface_state.h
@@ -0,0 +1,23 @@
+#ifndef SELECT_ALL_SURFACE_STATE_H
+#define SELECT_ALL_SURFACE_STATE_H
+
+#include "godel_simple_gui/gui_state.h"
+
+namespace godel_simple_gui
+{
+
+class SelectAllSurfaceState : public GuiState
+{
+public:
+  // Entry and exit classes
+  virtual void onStart(BlendingWidget& gui);
+  virtual void onExit(BlendingWidget& gui);
+
+  // Handlers for the fixed buttons
+  virtual void onNext(BlendingWidget& gui);
+  virtual void onBack(BlendingWidget& gui);
+  virtual void onReset(BlendingWidget& gui);
+};
+}
+
+#endif
diff --git a/godel_simple_gui/src/blending_widget.cpp b/godel_simple_gui/src/blending_widget.cpp
@@ -159,13 +159,22 @@ void godel_simple_gui::BlendingWidget::setLabelText(const std::string& txt)
 std::vector<std::string> godel_simple_gui::BlendingWidget::getPlanNames()
 {
   std::vector<std::string> selected_plans;
-  if (ui_->plan_list_widget->currentItem() == NULL)
-    ROS_WARN_STREAM("No plan slected, TODO QERRORMESSAGE");
   selected_plans.push_back(ui_->plan_list_widget->currentItem()->text().toStdString());
   return selected_plans;
 }
 
-void godel_simple_gui::BlendingWidget::sendGoal(godel_msgs::SelectMotionPlanActionGoal goal)
+void godel_simple_gui::BlendingWidget::sendGoal(const godel_msgs::SelectMotionPlanActionGoal& goal)
 {
   select_motion_plan_action_client_.sendGoal(goal.goal);
 }
+
+void godel_simple_gui::BlendingWidget::sendGoalAndWait(const godel_msgs::SelectMotionPlanActionGoal& goal)
+{
+  ros::Duration timeout = ros::Duration(60);
+  select_motion_plan_action_client_.sendGoalAndWait(goal.goal, timeout, timeout);
+}
+
+bool godel_simple_gui::BlendingWidget::planSelectionEmpty()
+{
+  return (ui_->plan_list_widget->currentItem() == NULL);
+}
diff --git a/godel_simple_gui/src/states/executing_state.cpp b/godel_simple_gui/src/states/executing_state.cpp
@@ -1,16 +1,11 @@
+#include <ros/console.h>
+#include <QtConcurrent/QtConcurrentRun>
+#include "godel_msgs/SelectMotionPlan.h"
 #include "godel_simple_gui/states/executing_state.h"
-// prev
 #include "godel_simple_gui/states/wait_to_execute_state.h"
-// There is no next!
-// error
 #include "godel_simple_gui/states/error_state.h"
-
-#include <ros/console.h>
 #include "godel_simple_gui/blending_widget.h"
-
-#include "godel_msgs/SelectMotionPlan.h"
-
-#include <QtConcurrent/QtConcurrentRun>
+#include "godel_simple_gui/states/scan_teach_state.h"
 
 const static std::string SELECT_MOTION_PLAN_SERVICE = "select_motion_plan";
 
@@ -37,7 +32,6 @@ void godel_simple_gui::ExecutingState::onStart(BlendingWidget& gui)
 
 void godel_simple_gui::ExecutingState::onExit(BlendingWidget& gui) { gui.setButtonsEnabled(true); }
 
-// Handlers for the fixed buttons
 void godel_simple_gui::ExecutingState::onNext(BlendingWidget& gui) {}
 
 void godel_simple_gui::ExecutingState::onBack(BlendingWidget& gui) {}
@@ -53,7 +47,7 @@ void godel_simple_gui::ExecutingState::executeAll(BlendingWidget& gui)
       executeOne(plan_names_[i], gui);
     }
 
-    Q_EMIT newStateAvailable(new WaitToExecuteState(plan_names_));
+    Q_EMIT newStateAvailable(new ScanTeachState());
   }
   catch (const BadExecutionError& err)
   {
@@ -64,23 +58,11 @@ void godel_simple_gui::ExecutingState::executeAll(BlendingWidget& gui)
 
 void godel_simple_gui::ExecutingState::executeOne(const std::string& plan, BlendingWidget& gui)
 {
-  /*
-  ROS_DEBUG_STREAM("Executing " << plan);
-  godel_msgs::SelectMotionPlan srv;
-  srv.request.name = plan;
-  srv.request.simulate = false;
-  srv.request.wait_for_execution = true;
-  if (!real_client_.call(srv))
-  {
-    std::ostringstream ss;
-    ss << "Failed to execute plan: " << plan << ". Please see logs for more details.";
-    throw BadExecutionError(ss.str());
-  }
-  */
-  ROS_INFO_STREAM("In select motion plan");
   godel_msgs::SelectMotionPlanActionGoal goal;
   goal.goal.name = plan;
   goal.goal.simulate = false;
   goal.goal.wait_for_execution = true;
-  gui.sendGoal(goal);
+  gui.sendGoalAndWait(goal);
 }
+
+
diff --git a/godel_simple_gui/src/states/planning_state.cpp b/godel_simple_gui/src/states/planning_state.cpp
@@ -1,9 +1,10 @@
+#include <ros/console.h>
+#include <QtConcurrent/QtConcurrentRun>
 #include "godel_simple_gui/blending_widget.h"
 #include "godel_simple_gui/states/planning_state.h"  // previous
 #include "godel_simple_gui/states/surface_select_state.h"  // next if fail
 #include "godel_simple_gui/states/select_plans_state.h" // next is success
-#include <QtConcurrent/QtConcurrentRun>
-#include <ros/console.h>
+#include "godel_simple_gui/states/select_all_surface_state.h"
 
 const static std::string PROCESS_PATH_SERVICE = "process_path";
 
@@ -55,7 +56,7 @@ void godel_simple_gui::PlanningState::processPlanningDoneCallback(
   if(result->succeeded)
     Q_EMIT newStateAvailable(new SelectPlansState());
   else
-    Q_EMIT newStateAvailable(new SurfaceSelectState());
+    Q_EMIT newStateAvailable(new SelectAllSurfaceState());
 }
 
 void godel_simple_gui::PlanningState::processPlanningActiveCallback()

diff --git a/godel_simple_gui/src/states/select_all_surface_state.cpp b/godel_simple_gui/src/states/select_all_surface_state.cpp
@@ -0,0 +1,34 @@
+#include <ros/console.h>
+#include "godel_msgs/SelectSurface.h"
+#include "godel_simple_gui/blending_widget.h"
+#include "godel_simple_gui/states/select_all_surface_state.h"
+#include "godel_simple_gui/states/scan_teach_state.h"
+#include "godel_simple_gui/states/planning_state.h"
+
+void godel_simple_gui::SelectAllSurfaceState::onStart(BlendingWidget& gui)
+{
+  const std::string SELECT_SURFACE_SERVICE = "select_surface";
+  ros::ServiceClient client = gui.nodeHandle().serviceClient<godel_msgs::SelectSurface>(
+      SELECT_SURFACE_SERVICE);
+  godel_msgs::SelectSurface msg;
+  msg.request.action = msg.request.SELECT_ALL;
+  client.call(msg.request, msg.response);
+  gui.setText("Press Next to select all surfaces.\nPress Back to select individual surfaces.");
+}
+
+void godel_simple_gui::SelectAllSurfaceState::onExit(BlendingWidget& gui) {}
+
+void godel_simple_gui::SelectAllSurfaceState::onNext(BlendingWidget& gui)
+{
+  Q_EMIT newStateAvailable(new PlanningState());
+}
+
+void godel_simple_gui::SelectAllSurfaceState::onBack(BlendingWidget& gui)
+{
+  Q_EMIT newStateAvailable(new ScanTeachState());
+}
+
+void godel_simple_gui::SelectAllSurfaceState::onReset(BlendingWidget& gui)
+{
+  Q_EMIT newStateAvailable(new ScanTeachState());
+}
diff --git a/godel_simple_gui/src/states/select_plans_state.cpp b/godel_simple_gui/src/states/select_plans_state.cpp
@@ -1,3 +1,5 @@
+#include <ros/console.h>
+#include <QtConcurrent/QtConcurrentRun>
 #include "godel_simple_gui/states/select_plans_state.h"
 // previous state
 #include "godel_simple_gui/states/surface_select_state.h"
@@ -7,13 +9,9 @@
 #include "godel_simple_gui/states/scan_teach_state.h"
 // error
 #include "godel_simple_gui/states/error_state.h"
-
-#include <ros/console.h>
 #include "godel_simple_gui/blending_widget.h"
-
 #include "godel_msgs/GetAvailableMotionPlans.h"
-
-#include <QtConcurrent/QtConcurrentRun>
+#include "godel_simple_gui/states/select_all_surface_state.h"
 
 const static std::string GET_AVAILABLE_MOTION_PLANS_SERVICE = "get_available_motion_plans";
 
@@ -34,15 +32,14 @@ void godel_simple_gui::SelectPlansState::onStart(BlendingWidget& gui)
 
   if (plan_names_.empty())
   {
-    gui.setLabelText("System Status:");
-    Q_EMIT newStateAvailable(new ErrorState("No motion plans available. "
-                                            "Please check surface selections and try again",
-                                            new SurfaceSelectState()));
+    Q_EMIT newStateAvailable(new SelectAllSurfaceState());
+  }
+  else
+  {
+    gui.showPlanListWidget();
+    gui.setLabelText("Select Plan:");
+    gui.addPlans(plan_names_);
   }
-
-  gui.showPlanListWidget();
-  gui.setLabelText("Select Plan:");
-  gui.addPlans(plan_names_);
 
 }
 
@@ -53,7 +50,10 @@ void godel_simple_gui::SelectPlansState::onNext(BlendingWidget& gui)
 {
   gui.setLabelText("System Status:");
   gui.showStatusWindow();
-  Q_EMIT newStateAvailable(new SimulatingState(gui.getPlanNames()));
+  if (gui.planSelectionEmpty())
+    Q_EMIT newStateAvailable(new SimulatingState(plan_names_));
+  else
+    Q_EMIT newStateAvailable(new SimulatingState(gui.getPlanNames()));
 }
 
 void godel_simple_gui::SelectPlansState::onBack(BlendingWidget& gui)

diff --git a/godel_simple_gui/src/states/simulating_state.cpp b/godel_simple_gui/src/states/simulating_state.cpp
@@ -4,6 +4,7 @@
 #include "godel_simple_gui/blending_widget.h"
 #include "godel_simple_gui/states/simulating_state.h"
 #include "godel_simple_gui/states/wait_to_execute_state.h"
+#include <iostream>
 
 const static std::string SELECT_MOTION_PLAN_SERVICE = "select_motion_plan";
 
@@ -43,20 +44,9 @@ void godel_simple_gui::SimulatingState::simulateAll(BlendingWidget& gui)
 
 void godel_simple_gui::SimulatingState::simulateOne(const std::string& plan, BlendingWidget& gui)
 {
-  /*
-  godel_msgs::SelectMotionPlan srv;
-  srv.request.name = plan;
-  srv.request.simulate = true;
-  srv.request.wait_for_execution = true;
-  if (!sim_client_.call(srv))
-  {
-    ROS_WARN_STREAM("Client simulation request returned false");
-  }
-  */
-  ROS_INFO_STREAM("In select motion plan");
   godel_msgs::SelectMotionPlanActionGoal goal;
   goal.goal.name = plan;
   goal.goal.simulate = true;
   goal.goal.wait_for_execution = true;
-  gui.sendGoal(goal);
+  gui.sendGoalAndWait(goal);
 }