diff --git a/godel_simple_gui/CMakeLists.txt b/godel_simple_gui/CMakeLists.txt index dd71f027..927e49cb 100644 --- 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 index 8ffd9f13..3cdfa66a 100644 --- 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& plan_names); void setLabelText(const std::string& txt); std::vector 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 index daa7f247..afbedd0b 100644 --- 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 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 new file mode 100644 index 00000000..52c9c7f2 --- /dev/null +++ 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 index 77e2ee6a..49d3ecb1 100644 --- 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 godel_simple_gui::BlendingWidget::getPlanNames() { std::vector 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 index 0cda75f9..4c248642 100644 --- a/godel_simple_gui/src/states/executing_state.cpp +++ b/godel_simple_gui/src/states/executing_state.cpp @@ -1,16 +1,11 @@ +#include +#include +#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 #include "godel_simple_gui/blending_widget.h" - -#include "godel_msgs/SelectMotionPlan.h" - -#include +#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 index 0434394d..c27e00cf 100644 --- a/godel_simple_gui/src/states/planning_state.cpp +++ b/godel_simple_gui/src/states/planning_state.cpp @@ -1,9 +1,10 @@ +#include +#include #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 -#include +#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 new file mode 100644 index 00000000..4ee2d96c --- /dev/null +++ b/godel_simple_gui/src/states/select_all_surface_state.cpp @@ -0,0 +1,34 @@ +#include +#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( + 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 index 322adb15..b0b1de9d 100644 --- 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 +#include #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 #include "godel_simple_gui/blending_widget.h" - #include "godel_msgs/GetAvailableMotionPlans.h" - -#include +#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 index 6135e0fe..1d6d665a 100644 --- 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 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); }