Skip to content

Commit

Permalink
hubero_ros_scenarios - parking scenario reimplemented using the thr…
Browse files Browse the repository at this point in the history 
…eaded task execution
  • Loading branch information
@rayvburn
rayvburn committed Oct 3, 2023
1 parent d387346 commit e14fead
Show file tree
Hide file tree
Showing 2 changed files with 36 additions and 75 deletions.
2 changes: 2 additions & 0 deletions hubero_ros_scenarios/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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


Expand Down
109 changes: 34 additions & 75 deletions hubero_ros_scenarios/src/parking_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand All @@ -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<TaskFeedbackType()> actor4_move_feedback_checker = [&actor4]() -> hubero::TaskFeedbackType {
return actor4.getMoveToGoalState();
};
std::function<TaskFeedbackType()> actor3_move_feedback_checker = [&actor3]() -> hubero::TaskFeedbackType {
return actor3.getMoveToGoalState();
};
std::function<TaskFeedbackType()> actor2_move_feedback_checker = [&actor2]() -> hubero::TaskFeedbackType {
return actor2.getMoveToGoalState();
};
std::function<TaskFeedbackType()> actor1_move_feedback_checker = [&actor1]() -> hubero::TaskFeedbackType {
return actor1.getMoveToGoalState();
};

// =================== 1st stage ========================================
/* Actor4 and Actor3 go straight to their cars;
Expand All @@ -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();
Expand All @@ -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!");
Expand All @@ -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!");

Expand Down

0 comments on commit e14fead

Please sign in to comment.