Skip to content

Commit

Permalink
scenarios - applied recent HuBeRo upgrades to `aws_hospital_norma…
Browse files Browse the repository at this point in the history 
…l` and `lab_012_dynamic` scenario nodes

- related to rayvburn/hubero#60
  • Loading branch information
@rayvburn
rayvburn committed Nov 18, 2023
1 parent 582f66e commit cc43ce0
Show file tree
Hide file tree
Showing 5 changed files with 66 additions and 200 deletions.
15 changes: 4 additions & 11 deletions tiago_social_scenarios_sim/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,22 +17,15 @@ catkin_package(
CATKIN_DEPENDS hubero_ros
)

add_library(scenario_utils
include/${PROJECT_NAME}/utils.h
src/utils.cpp
)
target_link_libraries(scenario_utils
${hubero_ros_LIBRARIES}
${catkin_LIBRARIES}
)

## executables
add_executable(aws_hospital_normal_hubero src/aws_hospital_normal_hubero.cpp)
target_link_libraries(aws_hospital_normal_hubero
scenario_utils
${hubero_ros_LIBRARIES}
${catkin_LIBRARIES}
)

add_executable(lab_012_dynamic_hubero src/lab_012_dynamic_hubero.cpp)
target_link_libraries(lab_012_dynamic_hubero
scenario_utils
${hubero_ros_LIBRARIES}
${catkin_LIBRARIES}
)
21 changes: 0 additions & 21 deletions tiago_social_scenarios_sim/include/tiago_social_scenarios_sim/utils.h
View file

This file was deleted.

62 changes: 31 additions & 31 deletions tiago_social_scenarios_sim/src/aws_hospital_normal_hubero.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,7 @@
#include <hubero_common/defines.h>
#include <hubero_ros/task_request_ros_api.h>

#include "tiago_social_scenarios_sim/utils.h"

using namespace hubero;
using namespace tiago_social_scenarios_sim;

// poses config
const std::string TF_FRAME_REF = "world";
Expand All @@ -24,6 +21,8 @@ const double A1_EXIT_YAW = +1.5708; // orientation in the exit: +3.1415;
const Vector3 A2_EXIT_POS(+5.20, +13.50, 0.0);
const double A2_EXIT_YAW = +1.5708; // orientation in the exit: +0.0000;

const auto TASK_TIMEOUT = ros::Duration(100.0);

int main(int argc, char** argv) {
// node initialization
ros::init(argc, argv, "aws_hospital_scenario_hubero_node");
Expand All @@ -42,84 +41,85 @@ int main(int argc, char** argv) {
// abort any pending actions
actor1.stopMovingToGoal();
actor2.stopMovingToGoal();
waitRefreshingRos();

// wait
ROS_INFO("TaskRequestRos APIs fired up, scenario execution will start in %lu seconds", launch_delay);
waitForRosTime(ros::Duration(launch_delay));
hubero::TaskRequestRosApi::waitRosTime(launch_delay);

// =================== 1st stage ========================================

// preparation
std::function<TaskFeedbackType()> actor1_move_feedback_checker = [&actor1]() -> hubero::TaskFeedbackType {
return actor1.getMoveToGoalState();
};
std::function<TaskFeedbackType()> actor2_move_feedback_checker = [&actor2]() -> hubero::TaskFeedbackType {
return actor2.getMoveToGoalState();
};

ROS_INFO("[SCENARIO] Firing up the execution! Actors will approach the nurse desk");

actor1.moveToGoal(A1_NURSE_DESK_POS, A1_NURSE_DESK_YAW, TF_FRAME_REF);
actor2.moveToGoal(A2_NURSE_DESK_POS, A2_NURSE_DESK_YAW, TF_FRAME_REF);
waitRefreshingRos();

std::thread a1_move_to_goal_handler(moveToGoalActorHandler, std::ref(actor1));
std::thread a2_move_to_goal_handler(moveToGoalActorHandler, std::ref(actor2));
actor1.startThreadedExecution(std::cref(actor1_move_feedback_checker), "moveToGoal", TASK_TIMEOUT);
actor2.startThreadedExecution(std::cref(actor2_move_feedback_checker), "moveToGoal", TASK_TIMEOUT);

// check until one of the actors finishes his task
while (!a1_move_to_goal_handler.joinable() && !a2_move_to_goal_handler.joinable()) {
waitRefreshingRos();
ROS_INFO("Waiting until one of the actors will reach its goal pose");
while (actor1.isThreadExecuting() && actor2.isThreadExecuting()) {
hubero::TaskRequestRosApi::wait();
}

ROS_INFO("[SCENARIO] 1st stage completed!");

// =================== 2nd stage ========================================
// one of the actors approached his goal
if (a1_move_to_goal_handler.joinable()) {
a1_move_to_goal_handler.join();
if (!actor1.isThreadExecuting()) {
actor1.join();
ROS_INFO("actor1 approached the goal, he is going to start talking to a nurse");
// talk, maintaining recently approached pose
actor1.talk(A1_NURSE_DESK_POS, A1_NURSE_DESK_YAW, TF_FRAME_REF);
// wait for actor2 to arrive
waitRefreshingRos(std::bind(&hubero::TaskRequestRosApi::getMoveToGoalState, std::ref(actor2)), TASK_FEEDBACK_ACTIVE);
a2_move_to_goal_handler.join();
actor2.join();
ROS_INFO("actor2 also approached the goal, he is going to start talking to a nurse");
// start talking
actor2.talk(A2_NURSE_DESK_POS, A2_NURSE_DESK_YAW, TF_FRAME_REF);
waitRefreshingRos();
} else if (a2_move_to_goal_handler.joinable()) {
a2_move_to_goal_handler.join();
} else if (!actor2.isThreadExecuting()) {
actor2.join();
ROS_INFO("actor2 approached the goal, he is going to start talking to a nurse");
actor2.talk(A2_NURSE_DESK_POS, A2_NURSE_DESK_YAW, TF_FRAME_REF);
// wait for actor1 to arrive
waitRefreshingRos(std::bind(&hubero::TaskRequestRosApi::getMoveToGoalState, std::ref(actor1)), TASK_FEEDBACK_ACTIVE);
a1_move_to_goal_handler.join();
actor1.join();
ROS_INFO("actor1 also approached the goal, he is going to start talking to a nurse");
// start talking
actor1.talk(A1_NURSE_DESK_POS, A1_NURSE_DESK_YAW, TF_FRAME_REF);
waitRefreshingRos();
} else {
throw std::runtime_error("Thread should've been finished but neither of 2 is joinable");
throw std::runtime_error("Any thread should've been finished but neither of 2 started is executing");
}

ROS_INFO("[SCENARIO] 2nd stage completed!");

// =================== 3rd stage ========================================
// actor that approached later talks for a few seconds
waitForRosTime(ros::Duration(5.0));
waitRefreshingRos();
hubero::TaskRequestRosApi::waitRosTime(5.0);

// stop talking
actor1.stopTalking();
actor2.stopTalking();
waitRefreshingRos();

// wait a second and process pending callbacks
waitForRosTime(ros::Duration(1.0));
waitRefreshingRos();
hubero::TaskRequestRosApi::waitRosTime(1.0);

ROS_INFO("Both actors will start moving to the exit");
actor1.moveToGoal(A1_EXIT_POS, A1_EXIT_YAW, TF_FRAME_REF);
actor2.moveToGoal(A2_EXIT_POS, A2_EXIT_YAW, TF_FRAME_REF);
waitRefreshingRos();

// renew threads
a1_move_to_goal_handler = std::thread(moveToGoalActorHandler, std::ref(actor1));
a2_move_to_goal_handler = std::thread(moveToGoalActorHandler, std::ref(actor2));
actor1.startThreadedExecution(std::cref(actor1_move_feedback_checker), "moveToGoal", TASK_TIMEOUT);
actor2.startThreadedExecution(std::cref(actor2_move_feedback_checker), "moveToGoal", TASK_TIMEOUT);

a1_move_to_goal_handler.join();
a2_move_to_goal_handler.join();
actor1.join();
actor2.join();

ROS_INFO("[SCENARIO] 3rd stage completed!");

Expand Down
95 changes: 31 additions & 64 deletions tiago_social_scenarios_sim/src/lab_012_dynamic_hubero.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,7 @@
#include <hubero_common/defines.h>
#include <hubero_ros/task_request_ros_api.h>

#include "tiago_social_scenarios_sim/utils.h"

using namespace hubero;
using namespace tiago_social_scenarios_sim;

// poses config
const std::string TF_FRAME_REF = "world";
Expand All @@ -25,56 +22,38 @@ const double A1_FINISH_YAW = 1.41 - IGN_PI_2;
const Vector3 A2_FINISH_POS(0.9, -4.1, 0.0);
const double A2_FINISH_YAW = -2.00 - IGN_PI_2;

const auto TASK_TIMEOUT = ros::Duration(60.0);

void actorWaypointFollowingThread(
hubero::TaskRequestRosApi& actor,
const Vector3& pos_finish,
const double& yaw_finish
) {
std::function<TaskFeedbackType()> feedback_checker = [&actor]() -> hubero::TaskFeedbackType {
return actor.getMoveToGoalState();
};

actor.moveToGoal(Ax_WP1_POS, Ax_WP1_YAW, TF_FRAME_REF);
waitRefreshingRos();
std::thread actor_move_to_goal_handler(moveToGoalActorHandler, std::cref(actor));
while (!actor_move_to_goal_handler.joinable()) {
waitRefreshingRos();
}
actor_move_to_goal_handler.join();
actor.startThreadedExecution(feedback_checker, "moveToGoal", TASK_TIMEOUT);
actor.join();

actor.moveToGoal(Ax_WP2_POS, Ax_WP2_YAW, TF_FRAME_REF);
waitRefreshingRos();
actor_move_to_goal_handler = std::thread(moveToGoalActorHandler, std::cref(actor));
while (!actor_move_to_goal_handler.joinable()) {
waitRefreshingRos();
}
actor_move_to_goal_handler.join();
actor.startThreadedExecution(feedback_checker, "moveToGoal", TASK_TIMEOUT);
actor.join();

actor.moveToGoal(pos_finish, yaw_finish, TF_FRAME_REF);
waitRefreshingRos();
actor_move_to_goal_handler = std::thread(moveToGoalActorHandler, std::cref(actor));
while (!actor_move_to_goal_handler.joinable()) {
waitRefreshingRos();
}
actor_move_to_goal_handler.join();
actor.startThreadedExecution(feedback_checker, "moveToGoal", TASK_TIMEOUT);
actor.join();
}

void actorFollowingThread(hubero::TaskRequestRosApi& actor, const std::string& obj_name) {
std::function<TaskFeedbackType()> feedback_checker = [&actor]() -> hubero::TaskFeedbackType {
return actor.getFollowObjectState();
};

actor.followObject(obj_name);
waitRefreshingRos();
while (actor.getFollowObjectState() != TASK_FEEDBACK_ACTIVE) {
if (!ros::ok()) {
throw std::runtime_error("Node stopped working!");
}
ROS_INFO(
"Waiting for %s task to become active. Current state %d...",
actor.getName().c_str(),
actor.getFollowObjectState()
);
waitRefreshingRos();
}
while (actor.getFollowObjectState() == TASK_FEEDBACK_ACTIVE) {
if (!ros::ok()) {
throw std::runtime_error("Node stopped working!");
}
waitRefreshingRos();
}
actor.startThreadedExecution(feedback_checker, "FollowObject");
actor.join();
}

int main(int argc, char** argv) {
Expand All @@ -95,47 +74,35 @@ int main(int argc, char** argv) {
// abort any pending actions
actor1.stopMovingToGoal();
actor2.stopMovingToGoal();
waitRefreshingRos();

// wait
ROS_INFO("TaskRequestRos APIs fired up, scenario execution will start in %lu seconds", launch_delay);
waitForRosTime(ros::Duration(launch_delay));
hubero::TaskRequestRosApi::waitRosTime(launch_delay);

ROS_INFO("[SCENARIO] Firing up the execution! Actors will approach the goal poses");

// moving through waypoints
std::thread a2_move_to_goal_handler(actorWaypointFollowingThread, std::ref(actor2), A2_FINISH_POS, A2_FINISH_YAW);
std::thread a1_follow_handler(actorFollowingThread, std::ref(actor1), "actor2");

// check until both first actor finish task
while (!a2_move_to_goal_handler.joinable()) {
waitRefreshingRos();
}
ROS_INFO("[SCENARIO] Waiting until the Actor2 approaches the goal pose");
a2_move_to_goal_handler.join();

// let's stop following - move to goal instead
ROS_INFO("[SCENARIO] Actor1 will stop following the Actor2");
actor1.stopFollowingObject();
while (!a1_follow_handler.joinable()) {
waitRefreshingRos();
}
a1_follow_handler.join();

// move to goal pose
std::thread a1_move_to_goal_handler(
[&actor1]() {
actor1.moveToGoal(A1_FINISH_POS, A1_FINISH_YAW, TF_FRAME_REF);
waitRefreshingRos();
std::thread actor_move_to_goal_handler(moveToGoalActorHandler, std::cref(actor1));
while (!actor_move_to_goal_handler.joinable()) {
waitRefreshingRos();
}
actor_move_to_goal_handler.join();
}
);
while (!a1_move_to_goal_handler.joinable()) {
waitRefreshingRos();
}
a1_move_to_goal_handler.join();
// move to the goal pose
ROS_INFO("[SCENARIO] Actor1 will move to the finish pose");
std::function<TaskFeedbackType()> a1_feedback_checker = [&actor1]() -> hubero::TaskFeedbackType {
return actor1.getMoveToGoalState();
};
actor1.moveToGoal(A1_FINISH_POS, A1_FINISH_YAW, TF_FRAME_REF);
actor1.startThreadedExecution(std::cref(a1_feedback_checker), "moveToGoal", TASK_TIMEOUT);

actor1.join();
actor2.join();

ROS_INFO("Scenario operation finished!");
return 0;
Expand Down
Loading

0 comments on commit cc43ce0

Please sign in to comment.