From e14fead30e418c3c203aab383d94bf15e0249492 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Wed, 4 Oct 2023 00:10:18 +0200 Subject: [PATCH] `hubero_ros_scenarios` - parking scenario reimplemented using the threaded task execution --- hubero_ros_scenarios/README.md | 2 + hubero_ros_scenarios/src/parking_node.cpp | 109 +++++++--------------- 2 files changed, 36 insertions(+), 75 deletions(-) diff --git a/hubero_ros_scenarios/README.md b/hubero_ros_scenarios/README.md index 0bf5b784..6f2f97f3 100644 --- a/hubero_ros_scenarios/README.md +++ b/hubero_ros_scenarios/README.md @@ -24,6 +24,8 @@ In the second terminal, run scenario execution: rosrun hubero_ros_scenarios parking_node 3 ``` +The `parking` scenario is implemented relying on tasks execution in separate threads. + ### `living room` scenario diff --git a/hubero_ros_scenarios/src/parking_node.cpp b/hubero_ros_scenarios/src/parking_node.cpp index ad207116..075123d1 100644 --- a/hubero_ros_scenarios/src/parking_node.cpp +++ b/hubero_ros_scenarios/src/parking_node.cpp @@ -11,12 +11,6 @@ using namespace hubero; const std::string TF_FRAME_REF = "world"; -/// Allows to process any ROS callbacks -void waitRefreshingRos() { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - ros::spinOnce(); -} - int main(int argc, char** argv) { // node initialization ros::init(argc, argv, "parking_scenario_node"); @@ -36,7 +30,21 @@ int main(int argc, char** argv) { // wait ROS_INFO("TaskRequestRos APIs fired up, scenario execution will start in %lu seconds", launch_delay / 1000); - std::this_thread::sleep_for(std::chrono::milliseconds(launch_delay)); + hubero::TaskRequestRosApi::waitRosTime(launch_delay / 1000); + + // preparation + std::function actor4_move_feedback_checker = [&actor4]() -> hubero::TaskFeedbackType { + return actor4.getMoveToGoalState(); + }; + std::function actor3_move_feedback_checker = [&actor3]() -> hubero::TaskFeedbackType { + return actor3.getMoveToGoalState(); + }; + std::function actor2_move_feedback_checker = [&actor2]() -> hubero::TaskFeedbackType { + return actor2.getMoveToGoalState(); + }; + std::function actor1_move_feedback_checker = [&actor1]() -> hubero::TaskFeedbackType { + return actor1.getMoveToGoalState(); + }; // =================== 1st stage ======================================== /* Actor4 and Actor3 go straight to their cars; @@ -48,38 +56,24 @@ int main(int argc, char** argv) { actor2.moveToGoal(Vector3(+7.0, +2.9, 0.0), 0.0, TF_FRAME_REF); actor1.moveToGoal(Vector3(+1.2, -5.7, 0.0), IGN_PI, TF_FRAME_REF); + actor4.startThreadedExecution(std::cref(actor4_move_feedback_checker), "moveToGoal"); + actor3.startThreadedExecution(std::cref(actor3_move_feedback_checker), "moveToGoal"); + actor2.startThreadedExecution(std::cref(actor2_move_feedback_checker), "moveToGoal"); + actor1.startThreadedExecution(std::cref(actor1_move_feedback_checker), "moveToGoal"); + ROS_INFO("[SCENARIO] 1st stage completed!"); // =================== 2nd stage ======================================== - // we will be waiting for actor2's task finish, so we must also know if the task request was processed - if (actor2.getMoveToGoalState() != TASK_FEEDBACK_ACTIVE) { - while (actor2.getMoveToGoalState() != TASK_FEEDBACK_ACTIVE) { - if (!ros::ok()) { - ROS_INFO("Node stopped!"); - return (0); - } - ROS_INFO("Waiting for Actor2 task to become active. Current state %d...", actor2.getMoveToGoalState()); - waitRefreshingRos(); - } - } - ROS_INFO("Task of Actor2 is active! Waiting until he finishes 'moveToGoal'"); - while (actor2.getMoveToGoalState() == TASK_FEEDBACK_ACTIVE) { - if (!ros::ok()) { - ROS_INFO("Node stopped!"); - return (0); - } - waitRefreshingRos(); - } + ROS_INFO("Waiting until Actor2 finishes 'moveToGoal'"); + actor2.join(); ROS_INFO("[SCENARIO] Firing up the 2nd stage!"); /* Actor2 talks for few seconds */ ROS_INFO("Actor2 got to the place! He will start talking to the person nearby!"); actor2.talk(Vector3(+7.0, +2.9, 0.0), IGN_DTOR(90), TF_FRAME_REF); - - std::this_thread::sleep_for(std::chrono::milliseconds(5000)); - ros::spinOnce(); + hubero::TaskRequestRosApi::wait(std::chrono::milliseconds(5000)); ROS_INFO("Actor2 finishes talking to the person nearby!"); actor2.stopTalking(); @@ -88,26 +82,9 @@ int main(int argc, char** argv) { // =================== 3rd stage ======================================== - // actor1 will surely go for a longer time than actor2 - if (actor1.getMoveToGoalState() != TASK_FEEDBACK_ACTIVE) { - while (actor1.getMoveToGoalState() != TASK_FEEDBACK_ACTIVE) { - if (!ros::ok()) { - ROS_INFO("Node stopped!"); - return (0); - } - ROS_INFO("Waiting for Actor1 task to become active. Current state %d...", actor2.getMoveToGoalState()); - waitRefreshingRos(); - } - } - - ROS_INFO("Task of Actor1 is active! Waiting until he finishes 'moveToGoal'"); - while (actor1.getMoveToGoalState() == TASK_FEEDBACK_ACTIVE) { - if ( !ros::ok() ) { - ROS_INFO("Node stopped!"); - return (0); - } - waitRefreshingRos(); - } + // actor1 will most likely go for a longer time than actor2 + ROS_INFO("Waiting until Actor1 finishes 'moveToGoal'"); + actor1.join(); ROS_INFO("Actor1 threw the rubbish away to the dumpster!"); ROS_INFO("[SCENARIO] Firing up the 3rd stage!"); @@ -116,34 +93,16 @@ int main(int argc, char** argv) { ROS_INFO("Actor1 and Actor2 start moving towards their cars!"); actor2.moveToGoal(Vector3(-3.5, +10.5, 0.0), IGN_PI, TF_FRAME_REF); actor1.moveToGoal(Vector3(-0.5, +14.8, 0.0), IGN_PI, TF_FRAME_REF); + actor2.startThreadedExecution(actor2_move_feedback_checker, "moveToGoal"); + actor1.startThreadedExecution(actor1_move_feedback_checker, "moveToGoal"); - // wait until request processed - if (actor1.getMoveToGoalState() != TASK_FEEDBACK_ACTIVE || actor2.getMoveToGoalState() != TASK_FEEDBACK_ACTIVE) { - while (actor1.getMoveToGoalState() != TASK_FEEDBACK_ACTIVE || actor2.getMoveToGoalState() != TASK_FEEDBACK_ACTIVE) { - if (!ros::ok()) { - ROS_INFO("Node stopped!"); - return (0); - } - ROS_INFO( - "Waiting for Actor1 or Actor2 task to become active. Current state: A1 %d, A2 %d...", - actor1.getMoveToGoalState(), - actor2.getMoveToGoalState() - ); - waitRefreshingRos(); - } - } - + // the requests are processed is separate threads ROS_INFO("Actor1 and Actor2 are moving towards cars! Waiting until they reach their goal"); - while ( - actor1.getMoveToGoalState() == TASK_FEEDBACK_ACTIVE - || actor2.getMoveToGoalState() == TASK_FEEDBACK_ACTIVE - ) { - if ( !ros::ok() ) { - ROS_INFO("Node stopped!"); - return (0); - } - waitRefreshingRos(); - } + actor1.join(); + actor2.join(); + + actor3.join(); + actor4.join(); ROS_INFO("[SCENARIO] 3rd stage completed!");