From 5fc4b3aa05c223d7b44402c3fb3d83bceba0495e Mon Sep 17 00:00:00 2001 From: olaghattas Date: Sat, 8 Feb 2025 14:19:47 -0500 Subject: [PATCH 1/6] add new high level functions and separate charging and idle --- shr_parameters/params/shr_parameters.yaml | 2 - shr_plan/include/shr_plan/actions.hpp | 176 ++++++++++++++-------- shr_plan/pddl/high_level_domain.pddl | 113 +++++++++++++- 3 files changed, 224 insertions(+), 67 deletions(-) diff --git a/shr_parameters/params/shr_parameters.yaml b/shr_parameters/params/shr_parameters.yaml index 74a1cab..b70c7b9 100644 --- a/shr_parameters/params/shr_parameters.yaml +++ b/shr_parameters/params/shr_parameters.yaml @@ -24,9 +24,7 @@ shr_parameters: } eat_times: { type: string_array, - default_value: [ "08h15m0s/08h45m0s" ], - description: "time that each protocol is triggered", } MedicineProtocols: diff --git a/shr_plan/include/shr_plan/actions.hpp b/shr_plan/include/shr_plan/actions.hpp index a94a48f..4cf7922 100644 --- a/shr_plan/include/shr_plan/actions.hpp +++ b/shr_plan/include/shr_plan/actions.hpp @@ -453,25 +453,8 @@ namespace pddl_lib { class ProtocolActions : public pddl_lib::ActionInterface { public: - // Timeout for now doesnt do anything inrodere for the protocol to be retriggered - BT::NodeStatus high_level_domain_Idle(const InstantiatedAction &action) override { - auto &kb = KnowledgeBase::getInstance(); - kb.clear_unknowns(); - kb.insert_predicate({"abort", {}}); - - // CHECKING IF ROBOT IS CHARGING FIRST - auto [ps, lock] = ProtocolState::getConcurrentInstance(); - lock.Lock(); -// auto params = ps.world_state_converter->get_params(); - - //RCLCPP_INFO(rclcpp::get_logger(std::string("weblog=") + "high_level_domain_Idle" + "started"), "user..."); - RCLCPP_INFO(rclcpp::get_logger(std::string("user=") + "high_level_domain_Idle" + "started"), "user..."); - - // std::string currentDateTime = getCurrentDateTime(); - // std::string log_message = - // std::string("weblog=") + currentDateTime + " high_level_domain_Idle " + " started!"; - // RCLCPP_INFO(ps.world_state_converter->get_logger(), log_message.c_str()); + BT::NodeStatus charge_robot(ProtocolState &ps, const InstantiatedAction &action){ if (!ps.world_state_converter->get_world_state_msg()->robot_charging == 1) { std::cout << "High level claim robot called " << std::endl; auto robot_resource = ps.claimRobot(); @@ -479,36 +462,8 @@ namespace pddl_lib { ps.audio_action_client_->async_cancel_all_goals(); ps.undocking_->async_cancel_all_goals(); ps.docking_->async_cancel_all_goals(); - // ps.localize_->async_cancel_all_goals(); - - - std::cout << "localize " << std::endl; -// RCLCPP_INFO( -// rclcpp::get_logger(std::string("weblog=") + "high_level_domain_Idle" + "localizing started"), -// "user..."); -// -// shr_msgs::action::LocalizeRequest::Goal goal_msg_loc; -// goal_msg_loc.force_localize = false; -// -// -// auto status_loc = send_goal_blocking(goal_msg_loc, action, ps); -// std::cout << "status: " << status_loc << std::endl; -// if (!status_loc) { -// std::cout << "Fail: " << std::endl; -// ps.localize_->async_cancel_all_goals(); -// //lock.UnLock(); -// //return BT::NodeStatus::FAILURE; -// } -// ps.localize_->async_cancel_all_goals(); - std::string currentDateTime = getCurrentDateTime(); - std::string log_message = - std::string("weblog=") + currentDateTime + " high_level_domain_Idle " + " started!"; - RCLCPP_INFO(ps.world_state_converter->get_logger(), log_message.c_str()); - RCLCPP_INFO( - rclcpp::get_logger(std::string("weblog=") + "high_level_domain_Idle" + "Navigation started"), - "user..."); std::cout << "navigate " << std::endl; nav2_msgs::action::NavigateToPose::Goal navigation_goal_; @@ -524,7 +479,7 @@ namespace pddl_lib { std::cout << "status: " << status_nav << std::endl; if (!status_nav) { std::cout << "Fail: " << std::endl; - lock.UnLock(); + // lock.UnLock(); return BT::NodeStatus::FAILURE; } std::cout << "success navigation : " << std::endl; @@ -532,20 +487,20 @@ namespace pddl_lib { std::cout << "dock " << std::endl; // comment in sim - shr_msgs::action::DockingRequest::Goal goal_msg_dock; - RCLCPP_INFO(rclcpp::get_logger(std::string("weblog=") + "high_level_domain_Idle" + "docking started"), - "user..."); - - auto status_dock = send_goal_blocking(goal_msg_dock, action, ps); - std::cout << "status: " << status_dock << std::endl; - if (!status_dock) { - ps.docking_->async_cancel_all_goals(); - std::cout << "Fail: " << std::endl; - lock.UnLock(); - return BT::NodeStatus::FAILURE; - } - ps.docking_->async_cancel_all_goals(); - std::cout << "success: " << std::endl; + shr_msgs::action::DockingRequest::Goal goal_msg_dock; + RCLCPP_INFO(rclcpp::get_logger(std::string("weblog=") + "high_level_domain_Idle" + "docking started"), + "user..."); + + auto status_dock = send_goal_blocking(goal_msg_dock, action, ps); + std::cout << "status: " << status_dock << std::endl; + if (!status_dock) { + ps.docking_->async_cancel_all_goals(); + std::cout << "Fail: " << std::endl; + //lock.UnLock(); + return BT::NodeStatus::FAILURE; + } + ps.docking_->async_cancel_all_goals(); + std::cout << "success: " << std::endl; // comment in sim // // sleep for 60 seconds to deal with the delay from //charging topic @@ -555,7 +510,7 @@ namespace pddl_lib { std::cout << "High level ending " << std::endl; } - + // for safety have it undock so that nav2 doesnt have to move when the robot is sp close to the docking if (ps.world_state_converter->get_world_state_msg()->robot_charging != 1){ std::cout << "Undock " << std::endl; @@ -592,9 +547,37 @@ namespace pddl_lib { } ps.undocking_->async_cancel_all_goals(); } + + return BT::NodeStatus::SUCCESS; + + } + + // Timeout for now doesnt do anything inrodere for the protocol to be retriggered + BT::NodeStatus high_level_domain_Idle(const InstantiatedAction &action) override { + auto &kb = KnowledgeBase::getInstance(); + kb.clear_unknowns(); + kb.insert_predicate({"abort", {}}); + + // CHECKING IF ROBOT IS CHARGING FIRST + auto [ps, lock] = ProtocolState::getConcurrentInstance(); + + RCLCPP_INFO(rclcpp::get_logger(std::string("user=") + "high_level_domain_Idle" + "started"), "user..."); + + std::string currentDateTime = getCurrentDateTime(); + std::string log_message = + std::string("weblog=") + currentDateTime + " high_level_domain_Idle " + " started!"; + RCLCPP_INFO(ps.world_state_converter->get_logger(), log_message.c_str()); + + RCLCPP_INFO( + rclcpp::get_logger(std::string("weblog=") + "high_level_domain_Idle" + "Navigation started"), + "user..."); + + lock.Lock(); + BT::NodeStatus status = charge_robot(ps, action); + ps.active_protocol = {}; lock.UnLock(); - return BT::NodeStatus::SUCCESS; + return status; } void abort(const InstantiatedAction &action) override { @@ -734,6 +717,73 @@ namespace pddl_lib { {t1, from, to}}; return shr_domain_MoveToLandmark(action_inst); } + BT::NodeStatus high_level_domain_Shutdown(const InstantiatedAction &action) override { + std::cout << " ------ Shutdown ----" << std::endl; + auto &kb = KnowledgeBase::getInstance(); +// InstantiatedParameter inst = action.parameters[0]; +// InstantiatedParameter cur = action.parameters[2]; +// InstantiatedParameter dest = action.parameters[3]; +// if (dest.name == cur.name) { +// cur.name = "living_room"; +// } + RCLCPP_INFO(rclcpp::get_logger("@@@@@@@ ########## Shutdown #################"), "Your message here"); + +// std::string currentDateTime = getCurrentDateTime(); + //RCLCPP_INFO(rclcpp::get_logger(std::string("weblog=")+"high_level_domain_StartWanderingProtocol"+"started"), "user..."); +// RCLCPP_INFO(rclcpp::get_logger( +// currentDateTime + std::string("user=") + "StartMoveReminderProtocol" + "started"), +// "user..."); +// auto [ps, lock] = ProtocolState::getConcurrentInstance(); +// lock.Lock(); +// std::string log_message = +// std::string("weblog=") + currentDateTime + " high_level_domain_StartMoveReminderProtocol" + +// " started"; +// RCLCPP_INFO(ps.world_state_converter->get_logger(), log_message.c_str()); + +// instantiate_protocol("move_reminder.pddl"); +// instantiate_protocol("move_reminder.pddl", {{"current_loc", cur.name}, +// {"dest_loc", dest.name}}); +// ps.active_protocol = inst; +// lock.UnLock(); + + // dock the robot if it is not charging + + // Get keyword predicates to load them in next protocol + + // reboot + + return BT::NodeStatus::SUCCESS; + } + + BT::NodeStatus high_level_domain_StartROS(const InstantiatedAction &action) override { + std::cout << " ------ Start ros ----" << std::endl; + auto &kb = KnowledgeBase::getInstance(); + + RCLCPP_INFO(rclcpp::get_logger("########## STARTT #################"), "Your message here"); + + // start actions servers and navigation + +// std::string currentDateTime = getCurrentDateTime(); + //RCLCPP_INFO(rclcpp::get_logger(std::string("weblog=")+"high_level_domain_StartWanderingProtocol"+"started"), "user..."); +// +// RCLCPP_INFO(rclcpp::get_logger( +// currentDateTime + std::string("user=") + "StartMoveReminderProtocol" + "started"), +// "user..."); +// auto [ps, lock] = ProtocolState::getConcurrentInstance(); +// lock.Lock(); +// std::string log_message = +// std::string("weblog=") + currentDateTime + " high_level_domain_StartMoveReminderProtocol" + +// " started"; +// RCLCPP_INFO(ps.world_state_converter->get_logger(), log_message.c_str()); + +// instantiate_protocol("move_reminder.pddl"); +// instantiate_protocol("move_reminder.pddl", {{"current_loc", cur.name}, +// {"dest_loc", dest.name}}); +// ps.active_protocol = inst; +// lock.UnLock(); + return BT::NodeStatus::SUCCESS; + } + BT::NodeStatus shr_domain_MakeCall(const InstantiatedAction &action) override { auto [ps, lock] = ProtocolState::getConcurrentInstance(); diff --git a/shr_plan/pddl/high_level_domain.pddl b/shr_plan/pddl/high_level_domain.pddl index 07910df..be498ea 100644 --- a/shr_plan/pddl/high_level_domain.pddl +++ b/shr_plan/pddl/high_level_domain.pddl @@ -18,6 +18,8 @@ ) (:predicates + (started) + (robot_at ?lmr - Landmark) (person_at ?t - Time ?p - Person ?lmp - Landmark) (person_currently_at ?p - Person ?lmp - Landmark) @@ -71,6 +73,7 @@ :parameters (?from - Landmark ?to - Landmark) :precondition (and (robot_at ?from) + (started) ) :effect (and (robot_at ?to) (not (robot_at ?from)) ) ) @@ -105,6 +108,19 @@ :effect (and (priority_5) (not (priority_4))) ) +;; to start ros and navigation before the protocol +(:action StartROS + :parameters () + :precondition (;;and + ;; will be triggered before it starts a protocol + ;; (priority_2) + ) + :effect (and + ;;(not (priority_2)) + (started) + ) +) + (:action StartMedReminderProtocol :parameters (?m - MedicineProtocol ?lmp - Landmark ?p - Person) :precondition (and @@ -117,8 +133,8 @@ (forall (?med - MedicineProtocol) (not (medicine_reminder_enabled ?med)) ) ;; person in visible area - (visible_location ?lmp) + (started) ) :effect (and (success) @@ -158,7 +174,8 @@ (not (already_ate ?f)) (not (already_called_about_eating ?f)) (forall (?food - FoodProtocol) (not (food_protocol_enabled ?food)) ) - ) + (started) + ) :effect (and (success) (not (priority_2)) @@ -199,6 +216,7 @@ ;; person in visible area (person_currently_at ?p ?lmp) (visible_location ?lmp) + (started) ) :effect (and @@ -241,6 +259,7 @@ ;; person in visible area (person_currently_at ?p ?lmp) (visible_location ?lmp) + (started) ) :effect (and @@ -285,6 +304,7 @@ ;; person in visible area (person_currently_at ?p ?lmp) (visible_location ?lmp) + (started) ) :effect (and @@ -328,6 +348,7 @@ ;; person in visible area (person_currently_at ?p ?lmp) (visible_location ?lmp) + (started) ) :effect (and @@ -374,5 +395,93 @@ ) ) +;; shutdown is supposed to stop ros2 processes +;; it should try to dock if it is not docked +;; triggered when there should be protocol and it has been done + +(:action Shutdown + :parameters () + :precondition + (and + (started) + ;; has to be higher priority than idle + (priority_4) + + ;; CANT SHUTDOWN IF time to do something is true and + ;; all predicates indicating that they it is done are false + ;; give F in such case + + ;; need to add a forall for every protocol objects in problem + ;;; 1 + ;;; forall would give false if one is F + (forall (?med - MedicineProtocol) + (not + (and + (time_to_take_medicine ?med) + (not (already_took_medicine ?med)) + (not (already_reminded_medicine ?med)) + ) + ) + ) + ;;; 2 + (forall (?internal - InternalCheckReminderProtocol) + (not + (and + (time_for_internal_check_reminder ?internal) + (not (already_reminded_internal_check ?internal)) + ) + ) + ) + ;;; 3 + (forall (?mv - MoveReminderProtocol) + (not + (and + (time_for_move_reminder ?mv) + (not (already_reminded_move ?mv)) + ) + ) + ) + ;;; 4 + (forall (?ex - ExerciseReminderProtocol) + (not + (and + (time_for_exercise_reminder ?ex) + (not (already_reminded_exercise ?ex)) + ) + ) + ) + + ;;; 5 + (forall (?practice - PracticeReminderProtocol) + (not + (and + (time_for_practice_reminder ?practice) + (not (already_reminded_practice ?practice)) + ) + ) + ) + + ;;; 6 + (forall (?food - FoodProtocol) + (not + (and + (time_to_eat ?food) + (not (already_ate ?food)) + (not (already_called_about_eating ?food)) + ) + ) + ) + + ) + :effect (and (success) + (not (priority_5)) + ;;(forall (?ask - AskBeforeHelp) (not (askforhelp_protocol_enabled ?ask)) ) + (not (low_level_failed)) + ;;(tried_shutdown) + (not started) + ) +) + + ) From 6fa0d6bcc1e4c541078e4722e348079af5b7d1d5 Mon Sep 17 00:00:00 2001 From: olaghattas Date: Sat, 8 Feb 2025 16:14:56 -0500 Subject: [PATCH 2/6] add logic to read and write predicates to deal with rebooting --- shr_plan/include/shr_plan/actions.hpp | 56 +++++- shr_plan/include/shr_plan/intersection.txt | 0 .../include/shr_plan/intersection_helpers.hpp | 160 ++++++++++++++++++ shr_plan/include/shr_plan/keywords.txt | 8 + shr_plan/src/planning_controller_node.cpp | 31 ++++ 5 files changed, 248 insertions(+), 7 deletions(-) create mode 100644 shr_plan/include/shr_plan/intersection.txt create mode 100644 shr_plan/include/shr_plan/intersection_helpers.hpp create mode 100644 shr_plan/include/shr_plan/keywords.txt diff --git a/shr_plan/include/shr_plan/actions.hpp b/shr_plan/include/shr_plan/actions.hpp index 4cf7922..5be87f0 100644 --- a/shr_plan/include/shr_plan/actions.hpp +++ b/shr_plan/include/shr_plan/actions.hpp @@ -12,6 +12,7 @@ #include "shr_msgs/action/waypoint_request.hpp" #include #include "shr_plan/helpers.hpp" +#include namespace pddl_lib { @@ -546,6 +547,9 @@ namespace pddl_lib { rclcpp::sleep_for(std::chrono::seconds(1)); } ps.undocking_->async_cancel_all_goals(); + + // indicating that robot didnt charge itself and needs ot start again + return BT::NodeStatus::FAILURE; } return BT::NodeStatus::SUCCESS; @@ -726,7 +730,44 @@ namespace pddl_lib { // if (dest.name == cur.name) { // cur.name = "living_room"; // } - RCLCPP_INFO(rclcpp::get_logger("@@@@@@@ ########## Shutdown #################"), "Your message here"); + RCLCPP_INFO(rclcpp::get_logger("@@@@@@@ ########## Shutdown #################"), "Starting shutdown"); + BT::NodeStatus status; + auto [ps, lock] = ProtocolState::getConcurrentInstance(); + + // dock the robot if it is not charging + while (status !=BT::NodeStatus::SUCCESS){ + /// TODO: IF IT RUNS FOR TOO LONG ISSUE MIGHT BE IN THE CHARGER + /// TODO: DISPLAY A WARNING ON THE SCREEN THAT IT NEEDS HELP + lock.Lock(); + status = charge_robot(ps, action); + lock.UnLock(); + } + + // Get keyword predicates to load them in next protocol + std::cout << " RUNNING MATCH " << std::endl; + std::filesystem::path pkg_dir = ament_index_cpp::get_package_share_directory("shr_plan"); + std::filesystem::path domain_file_path = pkg_dir / "pddl" / "problem_high_level.pddl"; + + write_from_problem_file(domain_file_path.c_str()); + + // reboot + std::cout << " RUNNING REBOOT " << std::endl; + + const char* password = std::getenv("robot_pass"); + + if (!password) { + std::cerr << "Environment variable 'robot_pass' not set!" << std::endl; + BT::NodeStatus::FAILURE; + } + + std::string cmd_reboot = "echo '" + std::string(password) + "' | sudo -S reboot"; + std::system(cmd_reboot.c_str()); + + + + + + // std::string currentDateTime = getCurrentDateTime(); //RCLCPP_INFO(rclcpp::get_logger(std::string("weblog=")+"high_level_domain_StartWanderingProtocol"+"started"), "user..."); @@ -746,12 +787,6 @@ namespace pddl_lib { // ps.active_protocol = inst; // lock.UnLock(); - // dock the robot if it is not charging - - // Get keyword predicates to load them in next protocol - - // reboot - return BT::NodeStatus::SUCCESS; } @@ -761,6 +796,13 @@ namespace pddl_lib { RCLCPP_INFO(rclcpp::get_logger("########## STARTT #################"), "Your message here"); + const char* homeDir = std::getenv("HOME"); + + std::string cmd_startros = "."; + cmd_startros += homeDir; + cmd_startros += "/manual_start.sh"; + std::system(cmd_startros.c_str()); + // start actions servers and navigation // std::string currentDateTime = getCurrentDateTime(); diff --git a/shr_plan/include/shr_plan/intersection.txt b/shr_plan/include/shr_plan/intersection.txt new file mode 100644 index 0000000..e69de29 diff --git a/shr_plan/include/shr_plan/intersection_helpers.hpp b/shr_plan/include/shr_plan/intersection_helpers.hpp new file mode 100644 index 0000000..e8a052c --- /dev/null +++ b/shr_plan/include/shr_plan/intersection_helpers.hpp @@ -0,0 +1,160 @@ +// +// Created by olagh48652 on 2/8/25. +// + +#ifndef PLAN_SOLVER_INTERSECTION_HELPERS_HPP +#define PLAN_SOLVER_INTERSECTION_HELPERS_HPP + + +#include +#include +#include +#include +#include + + +std::vector> read_predicates_from_file(){ + std::ifstream file("intersection.txt"); + if (!file.is_open()) { + std::cerr << "Error opening file!" << std::endl; + return {}; + } + + std::vector> predicates; + + std::string line; + while (std::getline(file, line)) { + if (line.empty()) continue; // Skip empty lines + + // Remove surrounding parentheses safely + if (line.front() == '(' && line.back() == ')' && line.size() > 2) { + line = line.substr(1, line.size() - 2); + } + + std::istringstream iss(line); + std::string first, second; + if (iss >> first >> second) { // Ensure two words are extracted + predicates.emplace_back(first, second); + } + } + + file.close(); + return predicates; +} + +std::unordered_set readKeywords(const std::string &filename) { + std::unordered_set keywords; + std::ifstream file(filename); + if (!file.is_open()) { + std::cerr << "Error opening keywords file!" << std::endl; + return keywords; + } + + std::string line; + while (std::getline(file, line)) { + if (!line.empty()) { + keywords.insert(line); + } + } + file.close(); + return keywords; +} + +std::string extractInitBlock(const std::string &content) { + size_t start = content.find("(:init"); + if (start == std::string::npos) { + std::cerr << "No init block found!" << std::endl; + return ""; + } + + std::string initBlock; + int balance = 0; + for (size_t i = start; i < content.size(); i++) { + initBlock += content[i]; + if (content[i] == '(') balance++; + if (content[i] == ')') balance--; + + if (balance == 0) break; // Found matching closing parenthesis + } + + if (balance != 0) { + std::cerr << "Unmatched parentheses in init block!" << std::endl; + return ""; + } + + return initBlock; +} + +std::vector extractMatchingPredicates(const std::string &initBlock, const std::unordered_set &keywords) { + std::vector predicates; + std::istringstream stream(initBlock); + std::string token; + + while (std::getline(stream, token, '(')) { // Split by '(' + size_t end = token.find(')'); + if (end != std::string::npos) { + std::string predicate = token.substr(0, end); + std::istringstream predStream(predicate); + std::string firstWord; + predStream >> firstWord; // Extract first word + + if (keywords.find(firstWord) != keywords.end()) { + predicates.push_back("(" + predicate + ")"); // Restore parentheses + } + } + } + + return predicates; +} + +void writeToFile(const std::string &filename, const std::vector &predicates) { + std::ofstream file(filename); + if (!file.is_open()) { + std::cerr << "Error opening output file!" << std::endl; + return; + } + + for (const auto &predicate : predicates) { + file << predicate << "\n"; + } + + file.close(); +} + +int write_from_problem_file(std::string problemFile) { + std::string keywordsFile = "keywords.txt"; + std::string outputFile = "intersection.txt"; + + // Read keywords + std::unordered_set keywords = readKeywords(keywordsFile); + if (keywords.empty()) { + std::cerr << "NO keywords in file!" << std::endl; + return 0; + } + // Read problem file + std::ifstream file(problemFile); + if (!file.is_open()) { + std::cerr << "Error opening problem file!" << std::endl; + return 0; + } + + std::stringstream buffer; + buffer << file.rdbuf(); + std::string content = buffer.str(); + file.close(); + + // Extract (:init ...) block + std::string initBlock = extractInitBlock(content); + if (initBlock.empty()) return 0; + + // Extract matching predicates + std::vector matchingPredicates = extractMatchingPredicates(initBlock, keywords); + + // Write intersection to output file + writeToFile(outputFile, matchingPredicates); + + std::cout << "Intersection saved to " << outputFile << std::endl; + return 1; +} + +#endif //PLAN_SOLVER_INTERSECTION_HELPERS_HPP diff --git a/shr_plan/include/shr_plan/keywords.txt b/shr_plan/include/shr_plan/keywords.txt new file mode 100644 index 0000000..0998138 --- /dev/null +++ b/shr_plan/include/shr_plan/keywords.txt @@ -0,0 +1,8 @@ +already_took_medicine +already_reminded_medicine +already_reminded_exercise +already_reminded_move +already_reminded_practice +already_reminded_internal_check +already_ate +already_called_about_eating \ No newline at end of file diff --git a/shr_plan/src/planning_controller_node.cpp b/shr_plan/src/planning_controller_node.cpp index 5d4d80b..955c3ab 100644 --- a/shr_plan/src/planning_controller_node.cpp +++ b/shr_plan/src/planning_controller_node.cpp @@ -17,6 +17,7 @@ #include #include +#include #include // for getenv using namespace pddl_lib; @@ -364,6 +365,36 @@ int main(int argc, char **argv) { } ); + // insert predicates from the file + //kb.insert_predicate({"abort", {}}); + // InstantiatedPredicate pred{"already_took_medicine", {ps.active_protocol}}; + // kb.insert_predicate(pred); + const std::unordered_map protocol_type = { + {"am_meds", "MedicineProtocol"}, + {"pm_meds", "MedicineProtocol"}, + {"move_reminder", "MoveReminderProtocol"}, + {"internal_check_reminder", "InternalCheckReminderProtocol"}, + {"practice_reminder", "PracticeReminderProtocol"}, + {"exercise_reminder", "ExerciseReminderProtocol"}, + {"breakfast", "FoodProtocol"} + }; + + auto predicates = read_predicates_from_file(); + + for (const auto& [first, second] : predicates) { + std::cout << "First: " << first << ", Second: " << second << std::endl; + + if (protocol_type.find(second) != protocol_type.end()) { + InstantiatedParameter active_protocol = {second, protocol_type.at(second)}; + InstantiatedPredicate pred{first, {active_protocol}}; + kb.insert_predicate(pred); + } else { + std::cerr << "Error: Protocol type for '" << second << "' not found!" << std::endl; + } + } + + + // run the domains BT::BehaviorTreeFactory factory = create_tree_factory(); From 28d630d6e15a34eb28a18e6cf21cc92e99d8a67a Mon Sep 17 00:00:00 2001 From: olaghattas Date: Sun, 9 Feb 2025 21:59:08 -0500 Subject: [PATCH 3/6] fixed reading and writing and updating problem file --- shr_plan/CMakeLists.txt | 2 +- shr_plan/include/shr_plan/actions.hpp | 66 +++++++------------ .../include/shr_plan/intersection_helpers.hpp | 46 +++++++++---- shr_plan/src/planning_controller_node.cpp | 53 ++++++++------- shr_plan/test/test_writing_to_file.cpp | 66 +++++++++++++++++++ 5 files changed, 155 insertions(+), 78 deletions(-) create mode 100644 shr_plan/test/test_writing_to_file.cpp diff --git a/shr_plan/CMakeLists.txt b/shr_plan/CMakeLists.txt index 9898f6b..6d30192 100644 --- a/shr_plan/CMakeLists.txt +++ b/shr_plan/CMakeLists.txt @@ -75,7 +75,7 @@ install(TARGETS ) install( - DIRECTORY launch pddl + DIRECTORY launch pddl include DESTINATION share/shr_plan ) diff --git a/shr_plan/include/shr_plan/actions.hpp b/shr_plan/include/shr_plan/actions.hpp index 5be87f0..147cd96 100644 --- a/shr_plan/include/shr_plan/actions.hpp +++ b/shr_plan/include/shr_plan/actions.hpp @@ -579,6 +579,8 @@ namespace pddl_lib { lock.Lock(); BT::NodeStatus status = charge_robot(ps, action); + std::cout << "%%%%%%% IDLE %%%%%%% IDLE " << std::endl; + ps.active_protocol = {}; lock.UnLock(); return status; @@ -724,13 +726,7 @@ namespace pddl_lib { BT::NodeStatus high_level_domain_Shutdown(const InstantiatedAction &action) override { std::cout << " ------ Shutdown ----" << std::endl; auto &kb = KnowledgeBase::getInstance(); -// InstantiatedParameter inst = action.parameters[0]; -// InstantiatedParameter cur = action.parameters[2]; -// InstantiatedParameter dest = action.parameters[3]; -// if (dest.name == cur.name) { -// cur.name = "living_room"; -// } - RCLCPP_INFO(rclcpp::get_logger("@@@@@@@ ########## Shutdown #################"), "Starting shutdown"); + BT::NodeStatus status; auto [ps, lock] = ProtocolState::getConcurrentInstance(); @@ -739,53 +735,36 @@ namespace pddl_lib { /// TODO: IF IT RUNS FOR TOO LONG ISSUE MIGHT BE IN THE CHARGER /// TODO: DISPLAY A WARNING ON THE SCREEN THAT IT NEEDS HELP lock.Lock(); - status = charge_robot(ps, action); + BT::NodeStatus status = charge_robot(ps, action); lock.UnLock(); } // Get keyword predicates to load them in next protocol std::cout << " RUNNING MATCH " << std::endl; std::filesystem::path pkg_dir = ament_index_cpp::get_package_share_directory("shr_plan"); - std::filesystem::path domain_file_path = pkg_dir / "pddl" / "problem_high_level.pddl"; - - write_from_problem_file(domain_file_path.c_str()); - - // reboot - std::cout << " RUNNING REBOOT " << std::endl; - - const char* password = std::getenv("robot_pass"); - - if (!password) { - std::cerr << "Environment variable 'robot_pass' not set!" << std::endl; - BT::NodeStatus::FAILURE; - } - - std::string cmd_reboot = "echo '" + std::string(password) + "' | sudo -S reboot"; - std::system(cmd_reboot.c_str()); + std::filesystem::path keywordsFile = pkg_dir / "include" / "shr_plan" / "keywords.txt"; + const char* homeDir = std::getenv("HOME"); + std::filesystem::path outputFile = pkg_dir / "include" / "shr_plan" / "intersection.txt"; + std::string domain_file_path = std::string(homeDir) + "/planner_data/plan_solver/problem.pddl"; + write_from_problem_file(domain_file_path, keywordsFile.c_str(), outputFile.c_str()); + // reboot + std::cout << " RUNNING REBOOT " << std::endl; +// const char* password = std::getenv("robot_pass"); +// +// if (!password) { +// std::cerr << "Environment variable 'robot_pass' not set!" << std::endl; +// BT::NodeStatus::FAILURE; +// } +// std::string cmd_reboot = "echo '" + std::string(password) + "' | sudo -S reboot"; +// std::system(cmd_reboot.c_str()); -// std::string currentDateTime = getCurrentDateTime(); - //RCLCPP_INFO(rclcpp::get_logger(std::string("weblog=")+"high_level_domain_StartWanderingProtocol"+"started"), "user..."); -// RCLCPP_INFO(rclcpp::get_logger( -// currentDateTime + std::string("user=") + "StartMoveReminderProtocol" + "started"), -// "user..."); -// auto [ps, lock] = ProtocolState::getConcurrentInstance(); -// lock.Lock(); -// std::string log_message = -// std::string("weblog=") + currentDateTime + " high_level_domain_StartMoveReminderProtocol" + -// " started"; -// RCLCPP_INFO(ps.world_state_converter->get_logger(), log_message.c_str()); -// instantiate_protocol("move_reminder.pddl"); -// instantiate_protocol("move_reminder.pddl", {{"current_loc", cur.name}, -// {"dest_loc", dest.name}}); -// ps.active_protocol = inst; -// lock.UnLock(); return BT::NodeStatus::SUCCESS; } @@ -797,12 +776,13 @@ namespace pddl_lib { RCLCPP_INFO(rclcpp::get_logger("########## STARTT #################"), "Your message here"); const char* homeDir = std::getenv("HOME"); - std::string cmd_startros = "."; - cmd_startros += homeDir; + cmd_startros += std::string(homeDir); cmd_startros += "/manual_start.sh"; std::system(cmd_startros.c_str()); + std::cout << " ------ finish start ----" << std::endl; + // start actions servers and navigation // std::string currentDateTime = getCurrentDateTime(); @@ -1061,6 +1041,8 @@ namespace pddl_lib { return BT::NodeStatus::FAILURE; } + + BT::NodeStatus shr_domain_MoveToLandmark(const InstantiatedAction &action) override { /// move robot to location RCLCPP_INFO( diff --git a/shr_plan/include/shr_plan/intersection_helpers.hpp b/shr_plan/include/shr_plan/intersection_helpers.hpp index e8a052c..9079418 100644 --- a/shr_plan/include/shr_plan/intersection_helpers.hpp +++ b/shr_plan/include/shr_plan/intersection_helpers.hpp @@ -13,8 +13,8 @@ #include -std::vector> read_predicates_from_file(){ - std::ifstream file("intersection.txt"); +std::vector> read_predicates_from_file(const std::string &outputFile){ + std::ifstream file(outputFile); if (!file.is_open()) { std::cerr << "Error opening file!" << std::endl; return {}; @@ -42,9 +42,9 @@ std::vector> read_predicates_from_file(){ return predicates; } -std::unordered_set readKeywords(const std::string &filename) { +std::unordered_set readKeywords(const std::string &keywords_filename) { std::unordered_set keywords; - std::ifstream file(filename); + std::ifstream file(keywords_filename); if (!file.is_open()) { std::cerr << "Error opening keywords file!" << std::endl; return keywords; @@ -107,23 +107,47 @@ std::vector extractMatchingPredicates(const std::string &initBlock, return predicates; } +// Function to trim leading and trailing spaces +std::string trim(const std::string &str) { + size_t start = str.find_first_not_of(" \t\n\r"); + size_t end = str.find_last_not_of(" \t\n\r"); + return (start == std::string::npos || end == std::string::npos) ? "" : str.substr(start, end - start + 1); +} + +// Function to write unique predicates to a file void writeToFile(const std::string &filename, const std::vector &predicates) { - std::ofstream file(filename); - if (!file.is_open()) { + std::unordered_set existingPredicates; + + // Read existing content from the file and add to the set + std::ifstream file(filename); + std::string line; + while (std::getline(file, line)) { + existingPredicates.insert(trim(line)); // Trim and insert each line from the file + } + file.close(); + + // Open the file again in append mode + std::ofstream outFile(filename, std::ios::app); + if (!outFile.is_open()) { std::cerr << "Error opening output file!" << std::endl; return; } + // Only append unique predicates (after trimming) for (const auto &predicate : predicates) { - file << predicate << "\n"; + std::string trimmedPredicate = trim(predicate); // Trim the current predicate + if (existingPredicates.find(trimmedPredicate) == existingPredicates.end()) { + outFile << predicate << "\n"; // Append if it's not already in the set + existingPredicates.insert(trimmedPredicate); // Mark this predicate as written + } } - file.close(); + outFile.close(); } -int write_from_problem_file(std::string problemFile) { - std::string keywordsFile = "keywords.txt"; - std::string outputFile = "intersection.txt"; +int write_from_problem_file(const std::string &problemFile, + const std::string &keywordsFile, + const std::string &outputFile){ // Read keywords std::unordered_set keywords = readKeywords(keywordsFile); diff --git a/shr_plan/src/planning_controller_node.cpp b/shr_plan/src/planning_controller_node.cpp index 955c3ab..32fda1e 100644 --- a/shr_plan/src/planning_controller_node.cpp +++ b/shr_plan/src/planning_controller_node.cpp @@ -25,6 +25,7 @@ using namespace pddl_lib; Domain load_domain(const std::string &domain_file) { std::string domain_str; std::filesystem::path pkg_dir = ament_index_cpp::get_package_share_directory("shr_plan"); + std::cout << "pkg_dir: "<< pkg_dir << std::endl; std::filesystem::path domain_file_path = pkg_dir / "pddl" / domain_file; std::ifstream domain_file_stream(domain_file_path.c_str()); @@ -310,40 +311,39 @@ int main(int argc, char **argv) { ps.nav_client_ = rclcpp_action::create_client( world_state_converter, "navigate_to_pose"); - while (!ps.nav_client_->wait_for_action_server(std::chrono::seconds(5))) { - RCLCPP_INFO(rclcpp::get_logger("navigate_to_pose"), "Waiting for /navigate_to_pose action server..."); - } - +// while (!ps.nav_client_->wait_for_action_server(std::chrono::seconds(5))) { +// RCLCPP_INFO(rclcpp::get_logger("navigate_to_pose"), "Waiting for /navigate_to_pose action server..."); +// } ps.read_action_client_ = rclcpp_action::create_client( world_state_converter, "read_script"); - while (!ps.read_action_client_->wait_for_action_server(std::chrono::seconds(5))) { - RCLCPP_INFO(rclcpp::get_logger("read_script"), "Waiting for /read_script action server..."); - } +// while (!ps.read_action_client_->wait_for_action_server(std::chrono::seconds(5))) { +// RCLCPP_INFO(rclcpp::get_logger("read_script"), "Waiting for /read_script action server..."); +// } ps.audio_action_client_ = rclcpp_action::create_client( world_state_converter, "play_audio"); - while (!ps.audio_action_client_->wait_for_action_server(std::chrono::seconds(5))) { - RCLCPP_INFO(rclcpp::get_logger("play_audio"), "Waiting for /play_audio action server..."); - } +// while (!ps.audio_action_client_->wait_for_action_server(std::chrono::seconds(5))) { +// RCLCPP_INFO(rclcpp::get_logger("play_audio"), "Waiting for /play_audio action server..."); +// } ps.docking_ = rclcpp_action::create_client( world_state_converter, "docking"); - while (!ps.docking_->wait_for_action_server(std::chrono::seconds(5))) { - RCLCPP_INFO(rclcpp::get_logger("docking"), "Waiting for /docking action server..."); - } +// while (!ps.docking_->wait_for_action_server(std::chrono::seconds(5))) { +// RCLCPP_INFO(rclcpp::get_logger("docking"), "Waiting for /docking action server..."); +// } ps.undocking_ = rclcpp_action::create_client( world_state_converter, "undocking"); - while (!ps.undocking_->wait_for_action_server(std::chrono::seconds(5))) { - RCLCPP_INFO(rclcpp::get_logger("undocking"), "Waiting for /undocking action server..."); - } +// while (!ps.undocking_->wait_for_action_server(std::chrono::seconds(5))) { +// RCLCPP_INFO(rclcpp::get_logger("undocking"), "Waiting for /undocking action server..."); +// } ps.localize_ = rclcpp_action::create_client( world_state_converter, "localize"); - while (!ps.localize_->wait_for_action_server(std::chrono::seconds(5))) { - RCLCPP_INFO(rclcpp::get_logger("localize"), "Waiting for /localize action server..."); - } +// while (!ps.localize_->wait_for_action_server(std::chrono::seconds(5))) { +// RCLCPP_INFO(rclcpp::get_logger("localize"), "Waiting for /localize action server..."); +// } ps.call_client_ = rclcpp_action::create_client( world_state_converter, "make_call"); - while (!ps.call_client_->wait_for_action_server(std::chrono::seconds(5))) { - RCLCPP_INFO(rclcpp::get_logger("make_call"), "Waiting for /make_call action server..."); - } +// while (!ps.call_client_->wait_for_action_server(std::chrono::seconds(5))) { +// RCLCPP_INFO(rclcpp::get_logger("make_call"), "Waiting for /make_call action server..."); +// } lock.UnLock(); } @@ -379,7 +379,11 @@ int main(int argc, char **argv) { {"breakfast", "FoodProtocol"} }; - auto predicates = read_predicates_from_file(); + std::filesystem::path pkg_dir = ament_index_cpp::get_package_share_directory("shr_plan"); + std::filesystem::path outputFile = pkg_dir / "include" / "shr_plan" / "intersection.txt"; + + std::cout << "outputFile: " << outputFile.c_str() << std::endl; + auto predicates = read_predicates_from_file(outputFile.c_str()); for (const auto& [first, second] : predicates) { std::cout << "First: " << first << ", Second: " << second << std::endl; @@ -394,13 +398,14 @@ int main(int argc, char **argv) { } - // run the domains BT::BehaviorTreeFactory factory = create_tree_factory(); while (true) { rclcpp::sleep_for(std::chrono::seconds(1)); + + std::string active_domain; auto protocol = ProtocolState::getActiveProtocol(); diff --git a/shr_plan/test/test_writing_to_file.cpp b/shr_plan/test/test_writing_to_file.cpp new file mode 100644 index 0000000..f7d4a18 --- /dev/null +++ b/shr_plan/test/test_writing_to_file.cpp @@ -0,0 +1,66 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +// Function to trim leading and trailing spaces +std::string trim(const std::string &str) { + size_t start = str.find_first_not_of(" \t\n\r"); + size_t end = str.find_last_not_of(" \t\n\r"); + return (start == std::string::npos || end == std::string::npos) ? "" : str.substr(start, end - start + 1); +} + +// Function to write unique predicates to a file +void writeToFile(const std::string &filename, const std::vector &predicates) { + std::unordered_set existingPredicates; + + // Read existing content from the file and add to the set + std::ifstream file(filename); + std::string line; + while (std::getline(file, line)) { + existingPredicates.insert(trim(line)); // Trim and insert each line from the file + } + file.close(); + + // Open the file again in append mode + std::ofstream outFile(filename, std::ios::app); + if (!outFile.is_open()) { + std::cerr << "Error opening output file!" << std::endl; + return; + } + + // Only append unique predicates (after trimming) + for (const auto &predicate : predicates) { + std::string trimmedPredicate = trim(predicate); // Trim the current predicate + if (existingPredicates.find(trimmedPredicate) == existingPredicates.end()) { + outFile << predicate << "\n"; // Append if it's not already in the set + existingPredicates.insert(trimmedPredicate); // Mark this predicate as written + } + } + + outFile.close(); +} + +int main() { + // Define some sample predicates to write + std::vector predicates = { + "already_took_medicine pm_meds", + "already_took_medicine pm_meds", // Duplicate, will not be written again + "already_reminded_move move_reminder", + "already_reminded_move move_reminder" // Duplicate, will not be written again + }; + + // Output file where predicates will be appended + std::string outputFile = "output.txt"; + + // Call the function to write unique predicates + writeToFile(outputFile, predicates); + + std::cout << "Predicates written to " << outputFile << std::endl; + + return 0; +} From c5c0afbf13a94bfe854afe8bdc1e5ead8ac6c989 Mon Sep 17 00:00:00 2001 From: olaghattas Date: Tue, 18 Feb 2025 00:40:22 -0500 Subject: [PATCH 4/6] fixed shutdown --- .../shr_parameters_py/shr_parameters.py | 36 +++- shr_plan/include/shr_plan/actions.hpp | 75 +++++++- .../include/shr_plan/intersection_helpers.hpp | 179 +++--------------- shr_plan/pddl/high_level_domain.pddl | 4 +- shr_plan/src/planning_controller_node.cpp | 33 +--- shr_plan/test/test_writing_to_file.cpp | 95 +++++----- 6 files changed, 179 insertions(+), 243 deletions(-) diff --git a/shr_parameters_py/shr_parameters_py/shr_parameters.py b/shr_parameters_py/shr_parameters_py/shr_parameters.py index 8a54990..5185ee4 100644 --- a/shr_parameters_py/shr_parameters_py/shr_parameters.py +++ b/shr_parameters_py/shr_parameters_py/shr_parameters.py @@ -28,23 +28,17 @@ class __Instances: LandmarksRobot = ["home", "designated_space"] Persons = ["nathan"] instances = __Instances() - class __Foodprotocols: - instances = ["breakfast", "lunch", "dinner"] - eat_times = ["06h45m0s/08h00m0s", "00h00m00s/00h00m00s", "00h00m00s/00h00m00s"] - eat_locations = ["kitchen", "kitchen", "kitchen"] - check_guide_to_succeeded_times = ["0h1m0s", "0h1m0s", "0h1m0s"] - remind_automated_food_at_times = ["0h10m0s", "0h10m0s", "0h10m0s"] - remind_automated_food_at_2_times = ["0h10m0s", "0h1m0s", "0h10m0s"] + instances = ["breakfast"] + eat_times = ["08h15m0s/08h45m0s"] FoodProtocols = __Foodprotocols() - class __Medicineprotocols: instances = ["am_meds", "pm_meds"] take_medication_times = ["09h00m0s/10h00m0s", "21h00m0s/22h00m0s"] MedicineProtocols = __Medicineprotocols() class __Internalcheckreminderprotocols: instances = ["internal_check_reminder"] - internal_check_reminder_times = ["08h15m0s/08h45m0s"] + internal_check_reminder_times = ["22h00m0s/23h00m0s"] InternalCheckReminderProtocols = __Internalcheckreminderprotocols() class __Practicereminderprotocols: instances = ["practice_reminder"] @@ -114,6 +108,14 @@ def update(self, parameters): updated_params.pddl.instances.Persons = param.value self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + if param.name == self.prefix_ + "pddl.FoodProtocols.instances": + updated_params.pddl.FoodProtocols.instances = param.value + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + + if param.name == self.prefix_ + "pddl.FoodProtocols.eat_times": + updated_params.pddl.FoodProtocols.eat_times = param.value + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + if param.name == self.prefix_ + "pddl.MedicineProtocols.instances": updated_params.pddl.MedicineProtocols.instances = param.value self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) @@ -205,6 +207,16 @@ def declare_params(self): parameter = updated_params.pddl.instances.Persons self.node_.declare_parameter(self.prefix_ + "pddl.instances.Persons", parameter, descriptor) + if not self.node_.has_parameter(self.prefix_ + "pddl.FoodProtocols.instances"): + descriptor = ParameterDescriptor(description="food protocols", read_only = False) + parameter = updated_params.pddl.FoodProtocols.instances + self.node_.declare_parameter(self.prefix_ + "pddl.FoodProtocols.instances", parameter, descriptor) + + if not self.node_.has_parameter(self.prefix_ + "pddl.FoodProtocols.eat_times"): + descriptor = ParameterDescriptor(description="time that each protocol is triggered", read_only = False) + parameter = updated_params.pddl.FoodProtocols.eat_times + self.node_.declare_parameter(self.prefix_ + "pddl.FoodProtocols.eat_times", parameter, descriptor) + if not self.node_.has_parameter(self.prefix_ + "pddl.MedicineProtocols.instances"): descriptor = ParameterDescriptor(description="medicine protocols", read_only = False) parameter = updated_params.pddl.MedicineProtocols.instances @@ -296,6 +308,12 @@ def declare_params(self): param = self.node_.get_parameter(self.prefix_ + "pddl.instances.Persons") self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) updated_params.pddl.instances.Persons = param.value + param = self.node_.get_parameter(self.prefix_ + "pddl.FoodProtocols.instances") + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + updated_params.pddl.FoodProtocols.instances = param.value + param = self.node_.get_parameter(self.prefix_ + "pddl.FoodProtocols.eat_times") + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + updated_params.pddl.FoodProtocols.eat_times = param.value param = self.node_.get_parameter(self.prefix_ + "pddl.MedicineProtocols.instances") self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) updated_params.pddl.MedicineProtocols.instances = param.value diff --git a/shr_plan/include/shr_plan/actions.hpp b/shr_plan/include/shr_plan/actions.hpp index 147cd96..e68efd3 100644 --- a/shr_plan/include/shr_plan/actions.hpp +++ b/shr_plan/include/shr_plan/actions.hpp @@ -735,21 +735,88 @@ namespace pddl_lib { /// TODO: IF IT RUNS FOR TOO LONG ISSUE MIGHT BE IN THE CHARGER /// TODO: DISPLAY A WARNING ON THE SCREEN THAT IT NEEDS HELP lock.Lock(); - BT::NodeStatus status = charge_robot(ps, action); + status = charge_robot(ps, action); lock.UnLock(); } // Get keyword predicates to load them in next protocol std::cout << " RUNNING MATCH " << std::endl; std::filesystem::path pkg_dir = ament_index_cpp::get_package_share_directory("shr_plan"); - std::filesystem::path keywordsFile = pkg_dir / "include" / "shr_plan" / "keywords.txt"; const char* homeDir = std::getenv("HOME"); std::filesystem::path outputFile = pkg_dir / "include" / "shr_plan" / "intersection.txt"; - std::string domain_file_path = std::string(homeDir) + "/planner_data/plan_solver/problem.pddl"; - write_from_problem_file(domain_file_path, keywordsFile.c_str(), outputFile.c_str()); + const std::unordered_map protocol_type_ = { + {"am_meds", "MedicineProtocol"}, + {"pm_meds", "MedicineProtocol"}, + {"move_reminder", "MoveReminderProtocol"}, + {"internal_check_reminder", "InternalCheckReminderProtocol"}, + {"practice_reminder", "PracticeReminderProtocol"}, + {"exercise_reminder", "ExerciseReminderProtocol"}, + {"breakfast", "FoodProtocol"} + }; + + const std::unordered_map> keyword_protocol_ = { + {"already_took_medicine", {"am_meds", "pm_meds"}}, + {"already_reminded_medicine", {"am_meds", "pm_meds"}}, + {"already_reminded_move",{"move_reminder"}}, + {"already_reminded_internal_check",{"internal_check_reminder"}}, + {"already_reminded_practice",{"practice_reminder"}}, + {"already_ate",{"breakfast"}}, + {"already_called_about_eating",{"breakfast"}}, + {"already_reminded_exercise",{"exercise_reminder"}} + }; + + std::ifstream ifs(keywordsFile); + if (!ifs) { + std::cerr << "Failed to open keywords file: " << keywordsFile << std::endl; +// return BT::NodeStatus::FAILURE; + } + + std::vector> keyword_protocol_list; + std::string line; + + while (std::getline(ifs, line)) { + // Here, 'line' is the keyword + // make sure no leading space + // TODO: trim leading space + std::string keyword = line; + + // Check if the keyword exists in keyword_protocol_. + auto keywordIt = keyword_protocol_.find(keyword); + if (keywordIt != keyword_protocol_.end()) { + + // For each protocol name associated with this keyword... + for (const auto& protocolName : keywordIt->second) { + // Look up the protocol type using protocol_type_. + auto typeIt = protocol_type_.find(protocolName); + if (typeIt != protocol_type_.end()) { + // Create an InstantiatedParameter with the protocol name and its type. + InstantiatedParameter active_protocol { protocolName, typeIt->second }; + InstantiatedPredicate pred{keyword, {active_protocol}}; + + // "Find" the predicate in the knowledge base. + if (kb.find_predicate(pred)){ + // add to the list + keyword_protocol_list.emplace_back(keyword, protocolName, typeIt->second); + } + + } else { + std::cerr << "Protocol name '" << protocolName + << "' not found in protocol_type_." << std::endl; + } + } + + + } else { + std::cout << "Keyword '" << keyword << "' not associated with any protocol." << std::endl; + } + } + ifs.close(); + + write_to_intersection(outputFile.c_str(), keyword_protocol_list); + // reboot std::cout << " RUNNING REBOOT " << std::endl; diff --git a/shr_plan/include/shr_plan/intersection_helpers.hpp b/shr_plan/include/shr_plan/intersection_helpers.hpp index 9079418..085f400 100644 --- a/shr_plan/include/shr_plan/intersection_helpers.hpp +++ b/shr_plan/include/shr_plan/intersection_helpers.hpp @@ -13,172 +13,41 @@ #include -std::vector> read_predicates_from_file(const std::string &outputFile){ - std::ifstream file(outputFile); - if (!file.is_open()) { - std::cerr << "Error opening file!" << std::endl; - return {}; - } - - std::vector> predicates; - - std::string line; - while (std::getline(file, line)) { - if (line.empty()) continue; // Skip empty lines - - // Remove surrounding parentheses safely - if (line.front() == '(' && line.back() == ')' && line.size() > 2) { - line = line.substr(1, line.size() - 2); - } - - std::istringstream iss(line); - std::string first, second; - if (iss >> first >> second) { // Ensure two words are extracted - predicates.emplace_back(first, second); - } - } - - file.close(); - return predicates; -} - -std::unordered_set readKeywords(const std::string &keywords_filename) { - std::unordered_set keywords; - std::ifstream file(keywords_filename); - if (!file.is_open()) { - std::cerr << "Error opening keywords file!" << std::endl; - return keywords; +void write_to_intersection(const std::string& filePath, + std::vector> keyword_protocol_list) { + std::ofstream ofs(filePath); + if (!ofs) { + std::cerr << "Failed to open output file: " << filePath << std::endl; + return; } - std::string line; - while (std::getline(file, line)) { - if (!line.empty()) { - keywords.insert(line); - } + // Write each tuple to the file + for (const auto& entry : keyword_protocol_list) { + ofs << std::get<0>(entry) << " " // Keyword + << std::get<1>(entry) << " " // Protocol Name + << std::get<2>(entry) << "\n"; // Protocol Type } - file.close(); - return keywords; + ofs.close(); + std::cout << "Predicates successfully written to " << filePath << std::endl; } -std::string extractInitBlock(const std::string &content) { - size_t start = content.find("(:init"); - if (start == std::string::npos) { - std::cerr << "No init block found!" << std::endl; - return ""; - } - - std::string initBlock; - int balance = 0; - for (size_t i = start; i < content.size(); i++) { - initBlock += content[i]; - if (content[i] == '(') balance++; - if (content[i] == ')') balance--; - - if (balance == 0) break; // Found matching closing parenthesis - } - - if (balance != 0) { - std::cerr << "Unmatched parentheses in init block!" << std::endl; - return ""; +std::vector> read_predicates_from_file(const std::string& filePath) { + std::ifstream ifs(filePath); + if (!ifs) { + std::cerr << "Failed to open file for reading: " << filePath << std::endl; + return {}; } - return initBlock; -} - -std::vector extractMatchingPredicates(const std::string &initBlock, const std::unordered_set &keywords) { - std::vector predicates; - std::istringstream stream(initBlock); - std::string token; + std::vector> predicates; + std::string keyword, protocolName, protocolType; - while (std::getline(stream, token, '(')) { // Split by '(' - size_t end = token.find(')'); - if (end != std::string::npos) { - std::string predicate = token.substr(0, end); - std::istringstream predStream(predicate); - std::string firstWord; - predStream >> firstWord; // Extract first word - - if (keywords.find(firstWord) != keywords.end()) { - predicates.push_back("(" + predicate + ")"); // Restore parentheses - } - } + // Read each line and extract keyword, protocolName, and protocolType + while (ifs >> keyword >> protocolName >> protocolType) { + predicates.emplace_back(keyword, protocolName, protocolType); } + ifs.close(); return predicates; } -// Function to trim leading and trailing spaces -std::string trim(const std::string &str) { - size_t start = str.find_first_not_of(" \t\n\r"); - size_t end = str.find_last_not_of(" \t\n\r"); - return (start == std::string::npos || end == std::string::npos) ? "" : str.substr(start, end - start + 1); -} - -// Function to write unique predicates to a file -void writeToFile(const std::string &filename, const std::vector &predicates) { - std::unordered_set existingPredicates; - - // Read existing content from the file and add to the set - std::ifstream file(filename); - std::string line; - while (std::getline(file, line)) { - existingPredicates.insert(trim(line)); // Trim and insert each line from the file - } - file.close(); - - // Open the file again in append mode - std::ofstream outFile(filename, std::ios::app); - if (!outFile.is_open()) { - std::cerr << "Error opening output file!" << std::endl; - return; - } - - // Only append unique predicates (after trimming) - for (const auto &predicate : predicates) { - std::string trimmedPredicate = trim(predicate); // Trim the current predicate - if (existingPredicates.find(trimmedPredicate) == existingPredicates.end()) { - outFile << predicate << "\n"; // Append if it's not already in the set - existingPredicates.insert(trimmedPredicate); // Mark this predicate as written - } - } - - outFile.close(); -} - -int write_from_problem_file(const std::string &problemFile, - const std::string &keywordsFile, - const std::string &outputFile){ - - // Read keywords - std::unordered_set keywords = readKeywords(keywordsFile); - if (keywords.empty()) { - std::cerr << "NO keywords in file!" << std::endl; - return 0; - } - // Read problem file - std::ifstream file(problemFile); - if (!file.is_open()) { - std::cerr << "Error opening problem file!" << std::endl; - return 0; - } - - std::stringstream buffer; - buffer << file.rdbuf(); - std::string content = buffer.str(); - file.close(); - - // Extract (:init ...) block - std::string initBlock = extractInitBlock(content); - if (initBlock.empty()) return 0; - - // Extract matching predicates - std::vector matchingPredicates = extractMatchingPredicates(initBlock, keywords); - - // Write intersection to output file - writeToFile(outputFile, matchingPredicates); - - std::cout << "Intersection saved to " << outputFile << std::endl; - return 1; -} - #endif //PLAN_SOLVER_INTERSECTION_HELPERS_HPP diff --git a/shr_plan/pddl/high_level_domain.pddl b/shr_plan/pddl/high_level_domain.pddl index be498ea..d7cba55 100644 --- a/shr_plan/pddl/high_level_domain.pddl +++ b/shr_plan/pddl/high_level_domain.pddl @@ -403,9 +403,9 @@ :parameters () :precondition (and - (started) + ;;(started) ;; has to be higher priority than idle - (priority_4) + ;;(priority_4) ;; CANT SHUTDOWN IF time to do something is true and ;; all predicates indicating that they it is done are false diff --git a/shr_plan/src/planning_controller_node.cpp b/shr_plan/src/planning_controller_node.cpp index 32fda1e..a6cd73c 100644 --- a/shr_plan/src/planning_controller_node.cpp +++ b/shr_plan/src/planning_controller_node.cpp @@ -365,47 +365,26 @@ int main(int argc, char **argv) { } ); - // insert predicates from the file - //kb.insert_predicate({"abort", {}}); - // InstantiatedPredicate pred{"already_took_medicine", {ps.active_protocol}}; - // kb.insert_predicate(pred); - const std::unordered_map protocol_type = { - {"am_meds", "MedicineProtocol"}, - {"pm_meds", "MedicineProtocol"}, - {"move_reminder", "MoveReminderProtocol"}, - {"internal_check_reminder", "InternalCheckReminderProtocol"}, - {"practice_reminder", "PracticeReminderProtocol"}, - {"exercise_reminder", "ExerciseReminderProtocol"}, - {"breakfast", "FoodProtocol"} - }; - std::filesystem::path pkg_dir = ament_index_cpp::get_package_share_directory("shr_plan"); std::filesystem::path outputFile = pkg_dir / "include" / "shr_plan" / "intersection.txt"; std::cout << "outputFile: " << outputFile.c_str() << std::endl; auto predicates = read_predicates_from_file(outputFile.c_str()); - for (const auto& [first, second] : predicates) { - std::cout << "First: " << first << ", Second: " << second << std::endl; - - if (protocol_type.find(second) != protocol_type.end()) { - InstantiatedParameter active_protocol = {second, protocol_type.at(second)}; - InstantiatedPredicate pred{first, {active_protocol}}; - kb.insert_predicate(pred); - } else { - std::cerr << "Error: Protocol type for '" << second << "' not found!" << std::endl; - } + // Print the predicates + for (const auto& [first, second, third] : predicates) { + std::cout << "Keyword: " << first << ", ProtocolName: " << second << ", ProtocolType: " << third << std::endl; + InstantiatedParameter active_protocol = {second, third}; + InstantiatedPredicate pred{first, {active_protocol}}; + kb.insert_predicate(pred); } - // run the domains BT::BehaviorTreeFactory factory = create_tree_factory(); while (true) { rclcpp::sleep_for(std::chrono::seconds(1)); - - std::string active_domain; auto protocol = ProtocolState::getActiveProtocol(); diff --git a/shr_plan/test/test_writing_to_file.cpp b/shr_plan/test/test_writing_to_file.cpp index f7d4a18..425c458 100644 --- a/shr_plan/test/test_writing_to_file.cpp +++ b/shr_plan/test/test_writing_to_file.cpp @@ -1,66 +1,69 @@ #include #include -#include #include -#include -#include -#include -#include +#include -// Function to trim leading and trailing spaces -std::string trim(const std::string &str) { - size_t start = str.find_first_not_of(" \t\n\r"); - size_t end = str.find_last_not_of(" \t\n\r"); - return (start == std::string::npos || end == std::string::npos) ? "" : str.substr(start, end - start + 1); -} - -// Function to write unique predicates to a file -void writeToFile(const std::string &filename, const std::vector &predicates) { - std::unordered_set existingPredicates; +// --- Function: write_to_intersection --- +// Writes a list of tuples (keyword, protocolName, type) to a file. +void write_to_intersection(const std::string& filePath, + const std::vector>& keyword_protocol_list) { + std::ofstream ofs(filePath); + if (!ofs) { + std::cerr << "Failed to open output file: " << filePath << std::endl; + return; + } - // Read existing content from the file and add to the set - std::ifstream file(filename); - std::string line; - while (std::getline(file, line)) { - existingPredicates.insert(trim(line)); // Trim and insert each line from the file + // Write each tuple to the file + for (const auto& entry : keyword_protocol_list) { + ofs << std::get<0>(entry) << " " // Keyword + << std::get<1>(entry) << " " // Protocol Name + << std::get<2>(entry) << "\n"; // Protocol Type } - file.close(); + ofs.close(); + std::cout << "Predicates successfully written to " << filePath << std::endl; +} - // Open the file again in append mode - std::ofstream outFile(filename, std::ios::app); - if (!outFile.is_open()) { - std::cerr << "Error opening output file!" << std::endl; - return; +// --- Function: read_from_intersection --- +// Reads from the file and prints each line. +std::vector> read_predicates_from_file(const std::string& filePath) { + std::ifstream ifs(filePath); + if (!ifs) { + std::cerr << "Failed to open file for reading: " << filePath << std::endl; + return {}; } - // Only append unique predicates (after trimming) - for (const auto &predicate : predicates) { - std::string trimmedPredicate = trim(predicate); // Trim the current predicate - if (existingPredicates.find(trimmedPredicate) == existingPredicates.end()) { - outFile << predicate << "\n"; // Append if it's not already in the set - existingPredicates.insert(trimmedPredicate); // Mark this predicate as written - } + std::vector> predicates; + std::string keyword, protocolName, protocolType; + + // Read each line and extract keyword, protocolName, and protocolType + while (ifs >> keyword >> protocolName >> protocolType) { + predicates.emplace_back(keyword, protocolName, protocolType); } - outFile.close(); + ifs.close(); + return predicates; } int main() { - // Define some sample predicates to write - std::vector predicates = { - "already_took_medicine pm_meds", - "already_took_medicine pm_meds", // Duplicate, will not be written again - "already_reminded_move move_reminder", - "already_reminded_move move_reminder" // Duplicate, will not be written again + std::string outputFile = "intersection.txt"; + + // Sample data to write + std::vector> keyword_protocol_list = { + {"already_took_medicine", "am_meds", "MedicineProtocol"}, + {"already_took_medicine", "pm_meds", "MedicineProtocol"}, + {"already_reminded_exercise", "exercise_reminder", "ExerciseReminderProtocol"} }; - // Output file where predicates will be appended - std::string outputFile = "output.txt"; + // Write to file + write_to_intersection(outputFile, keyword_protocol_list); - // Call the function to write unique predicates - writeToFile(outputFile, predicates); + // Read from file + auto predicates = read_predicates_from_file(outputFile); - std::cout << "Predicates written to " << outputFile << std::endl; + // Print the predicates + for (const auto& [first, second, third] : predicates) { + std::cout << "First: " << first << ", Second: " << second << ", Third: " << third << std::endl; + } return 0; -} +} \ No newline at end of file From 33da7dd3b25312b8852b12221295b77e148bc6c8 Mon Sep 17 00:00:00 2001 From: MarzanShuvo Date: Tue, 18 Feb 2025 17:08:40 -0500 Subject: [PATCH 5/6] fixes --- shr_plan/include/shr_plan/actions.hpp | 47 +++++++++++++++++---------- shr_plan/pddl/high_level_domain.pddl | 4 +-- 2 files changed, 31 insertions(+), 20 deletions(-) diff --git a/shr_plan/include/shr_plan/actions.hpp b/shr_plan/include/shr_plan/actions.hpp index e68efd3..d473b10 100644 --- a/shr_plan/include/shr_plan/actions.hpp +++ b/shr_plan/include/shr_plan/actions.hpp @@ -455,8 +455,11 @@ namespace pddl_lib { class ProtocolActions : public pddl_lib::ActionInterface { public: - BT::NodeStatus charge_robot(ProtocolState &ps, const InstantiatedAction &action){ - if (!ps.world_state_converter->get_world_state_msg()->robot_charging == 1) { + BT::NodeStatus charge_robot(ProtocolState &ps, const InstantiatedAction &action, bool pred_started){ + std::cout << "ps.world_state_converter->get_world_state_msg()->robot_charging" << ps.world_state_converter->get_world_state_msg()->robot_charging << std::endl; + std::cout << "pred_started" << pred_started << std::endl; + + if (!ps.world_state_converter->get_world_state_msg()->robot_charging == 1 && pred_started ) { std::cout << "High level claim robot called " << std::endl; auto robot_resource = ps.claimRobot(); ps.read_action_client_->async_cancel_all_goals(); @@ -487,7 +490,7 @@ namespace pddl_lib { std::cout << "dock " << std::endl; -// comment in sim + // comment in sim shr_msgs::action::DockingRequest::Goal goal_msg_dock; RCLCPP_INFO(rclcpp::get_logger(std::string("weblog=") + "high_level_domain_Idle" + "docking started"), "user..."); @@ -502,7 +505,7 @@ namespace pddl_lib { } ps.docking_->async_cancel_all_goals(); std::cout << "success: " << std::endl; -// comment in sim + // comment in sim // // sleep for 60 seconds to deal with the delay from //charging topic std::cout << " waiting " << std::endl; @@ -562,6 +565,9 @@ namespace pddl_lib { kb.clear_unknowns(); kb.insert_predicate({"abort", {}}); + bool pred_started = kb.find_predicate({"started", {}}); + std::cout << "kb.find_predicate " << pred_started << std::endl; + // CHECKING IF ROBOT IS CHARGING FIRST auto [ps, lock] = ProtocolState::getConcurrentInstance(); @@ -577,7 +583,7 @@ namespace pddl_lib { "user..."); lock.Lock(); - BT::NodeStatus status = charge_robot(ps, action); + BT::NodeStatus status = charge_robot(ps, action, pred_started); std::cout << "%%%%%%% IDLE %%%%%%% IDLE " << std::endl; @@ -727,7 +733,7 @@ namespace pddl_lib { std::cout << " ------ Shutdown ----" << std::endl; auto &kb = KnowledgeBase::getInstance(); - BT::NodeStatus status; + BT::NodeStatus status = BT::NodeStatus::FAILURE; auto [ps, lock] = ProtocolState::getConcurrentInstance(); // dock the robot if it is not charging @@ -735,7 +741,7 @@ namespace pddl_lib { /// TODO: IF IT RUNS FOR TOO LONG ISSUE MIGHT BE IN THE CHARGER /// TODO: DISPLAY A WARNING ON THE SCREEN THAT IT NEEDS HELP lock.Lock(); - status = charge_robot(ps, action); + status = charge_robot(ps, action, true); lock.UnLock(); } @@ -817,19 +823,25 @@ namespace pddl_lib { write_to_intersection(outputFile.c_str(), keyword_protocol_list); + + // KILING ROS2 + std::system("python3 /home/hello-robot/kill_ros.py"); + + rclcpp::sleep_for(std::chrono::seconds(120)); + // reboot std::cout << " RUNNING REBOOT " << std::endl; -// const char* password = std::getenv("robot_pass"); -// -// if (!password) { -// std::cerr << "Environment variable 'robot_pass' not set!" << std::endl; -// BT::NodeStatus::FAILURE; -// } + const char* password = std::getenv("robot_pass"); + + if (!password) { + std::cerr << "Environment variable 'robot_pass' not set!" << std::endl; + BT::NodeStatus::FAILURE; + } -// std::string cmd_reboot = "echo '" + std::string(password) + "' | sudo -S reboot"; -// std::system(cmd_reboot.c_str()); + std::string cmd_reboot = "echo '" + std::string(password) + "' | sudo -S reboot"; + std::system(cmd_reboot.c_str()); @@ -843,9 +855,8 @@ namespace pddl_lib { RCLCPP_INFO(rclcpp::get_logger("########## STARTT #################"), "Your message here"); const char* homeDir = std::getenv("HOME"); - std::string cmd_startros = "."; - cmd_startros += std::string(homeDir); - cmd_startros += "/manual_start.sh"; + std::string cmd_startros = std::string(homeDir); + cmd_startros += "/start_nav.sh"; std::system(cmd_startros.c_str()); std::cout << " ------ finish start ----" << std::endl; diff --git a/shr_plan/pddl/high_level_domain.pddl b/shr_plan/pddl/high_level_domain.pddl index d7cba55..be498ea 100644 --- a/shr_plan/pddl/high_level_domain.pddl +++ b/shr_plan/pddl/high_level_domain.pddl @@ -403,9 +403,9 @@ :parameters () :precondition (and - ;;(started) + (started) ;; has to be higher priority than idle - ;;(priority_4) + (priority_4) ;; CANT SHUTDOWN IF time to do something is true and ;; all predicates indicating that they it is done are false From 7de5a866348439e3779d527fabb9c6dc2624e800 Mon Sep 17 00:00:00 2001 From: olaghattas <98136555+olaghattas@users.noreply.github.com> Date: Thu, 20 Feb 2025 22:35:35 -0500 Subject: [PATCH 6/6] Update README.md --- README.md | 256 ++++++++++-------------------------------------------- 1 file changed, 45 insertions(+), 211 deletions(-) diff --git a/README.md b/README.md index 38185b2..26a338c 100644 --- a/README.md +++ b/README.md @@ -1,33 +1,3 @@ - -## Simulator keyboard controls -`shift+O` : open door - -`shift+M` : move to door - -`shift+B` : move to bedroom - -`shift+C` : move to couch - -`shift+K` : toggle move to kitchen position 1/2 - -`shift+V` : take medicine - -`shift+R` : go outside - -`shift+S` : move to dining - -`shift+J` : eat - -'arrows' : nagivate camera - -'shift+arrows' : move person around - -`right-click and drag` : pan camera - -`mouse wheel` : zoom in - - - ## Managing source package dependencies A source package must be build in order to use it. It is recommended to manage these packages using a .repos file. In this file, git repositories are listed in a .yaml file format and can be downloaded using vcs. To install vcs you can run: ``` @@ -58,7 +28,6 @@ Note, the above command must be run at the root of the ROS workspace. Use Blender to view and edit mesh files. When exporting .obj files, make sure that Z is "up" and Y is "forward". The values can be chosen from the export options. - # Smart Home Robot The project is about i) designing a smart home equipped with a socially assistive robot (SAR) and serval internet of things (IoT) devices and ii) evaluating the feasibility of using such a smart home to provide care-giving service for @@ -75,207 +44,72 @@ elderly occupant and the safety of the home. [[pdf]](http://cs.unh.edu/~tg1034/publication/shr_sajay.pdf) -# Install: -**Speech module** -Better TTS voices: -Download the new voice from [here](https://universitysystemnh-my.sharepoint.com/:u:/g/personal/pac48_usnh_edu/ERrsvRkJHx1Fve_Uv4RBRQ0BOGGKMvEZCGmGE4-R7GwuyQ?e=9730gE) then extract and copy it into `/usr/share/festival/voices/english` - -Additional voices can be found here: http://www.festvox.org/packed/festival/2.5/voices/ - -**ffmpeg** -ffmpeg - -**face module package** -`sudo apt-get install ros-kinetic-people-msgs` -`sudo apt-get install ros-kinetic-jsk-rviz-plugin` -`sudo pip2 install face_recognition` -`sudo pip2 install opencv-python` - -**Speech module** -`sudo apt-get install ros-kinetic-sound-play` -Better TTS voice: https://ubuntuforums.org/archive/index.php/t-751169.html -Download the new voice from [here](https://universitysystemnh-my.sharepoint.com/:u:/g/personal/pac48_usnh_edu/ERrsvRkJHx1Fve_Uv4RBRQ0BOGGKMvEZCGmGE4-R7GwuyQ?e=9730gE) then extract and copy it into `/usr/share/festival/voices/english` - -**primesense camera drive** -`sudo apt install libopenni2-dev` -`sudo apt install ros-kinetic-openni2-launch` -`sudo apt install ros-humble-depth-image-proc` -The primesense camera has to be connect to usb2.0 port - -**Aria package(for rosaria)** -`sudo apt install libaria-dev` - -**ROSPlan** -`sudo apt install ros-kinetic-mongodb-store` -ROSPlan: https://github.com/KCL-Planning/ROSPlan **Pull and build SHR** ```bash -mkdir -p ~/catkin_ws/src -cd ~/catkin_ws/src +mkdir -p ~/smart-home/src +cd ~/smart-home/src git clone git@github.com:AssistiveRoboticsUNH/smart-home.git -cd ~/catkin_ws -catkin build -# if you are using youcompleteme so need a compile database: -# catkin build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=1 -``` - -# Other pre-configuration: -**Grant usb port read permission** -`sudo usermod -a -G dialout $USER` -Have to reboot after. - -**Laser Scan Ethernet Config** -On onboard labtop, set ethernet ip as 192.168.0.10 - -The default set for lms500 is 192.168.0.1, and we don't have to change this. The labtop ethernet have to be in the same ip domain, so anything similar to 192.168.0.x will work. Here we use 192.168.0.10. - This is a [reference artical in chinese](https://blog.csdn.net/zhuoyueljl/article/details/75244563) about the LMS500 laser. - -**Set up for remote control** - -Add this to .bashr or .zshrc: -* on board labtop -```bash -# need to add this to onboard labtop that runs roscore -export ROS_IP=0.0.0.0 - -# set to localhost for onboard labtop that runs roscore -export ROS_MASTER_URI=http://localhost:11311 +cd ~/smart-home +colcon build --symlink-install -# IP of onboard labtop that runs roscore -export ROS_HOSTNAME=10.21.152.74 -``` - -* remote control pc or labtop -```bash -# IP of onboard labtop that runs roscore -export ROS_MASTER_URI=http://10.21.152.74:11311 - -# IP of remote pc -export ROS_HOSTNAME=10.21.98,194 +# if you are using youcompleteme so need a compile database: +colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=1 ``` -**Set up for audio and video resource** -`export ROS_WORKSPACE=/path/to/your/catkin_ws` - -# Usage: - -## Simulator: -**Bring up simulator:** -`roscore && roslaunch pioneer_shr pioneer_gazebo.launch` - -**Mapping in gazebo:** -`roslaunch pioneer_shr pioneer_gazebo_mapping.launch` - -**Autonomous navigation in gazebo:** -`roslaunch pioneer_shr auto_navigation_gazebo.launch` - -**Visualize trajectory** -`roslaunch pioneer_shr trajectory_vis.launch` -(you may want to visualize the trajectory after mannually localize the robot) - -## Real Robot mapping: -**ssh into onboard labtop** - -**Bring up pioneer from onboard labtop:** -`roscore && roslaunch pioneer_shr real_mapping.launch` - -**Start teleop keyboard from onboard labtop:** -`roslaunch pioneer_shr keyboard_ctrl.launch` - -**Visualize from remote PC:** -`roslaunch pioneer_shr remote_vis.launch` - -## Real Robot auto-navigation: -**ssh into onboard labtop** - -**Bring up pioneer from onboard labtop via ssh:** -`roscore && roslaunch pioneer_shr auto_navigation_real_world.launch` -**Start teleop keyboard from onboard labtop via ssh:** -`roslaunch pioneer_shr keyboard_ctrl.launch` +# Setup planner to start and shutdown -**Visualize from remote PC:** -`roslaunch pioneer_shr remote_vis.launch` +Step 1: +add a cron job to clear out the intersection.txt file that include the predicates that need to be set true in the knowledge -## Auto-navigation trigger by sensor (obselet): -**Do all steps in auto-navigation** +run ''' crontab -e ''' +and add +1 0 * * * > /path_to_shr_plan/include/shr_plan/intersection.txt -**Bring up simple_navigation_goal ros node** -`roslaunch pioneer_shr sensor_trigger_move2goal_real.launch` (real world) -`roslaunch pioneer_shr sensor_trigger_move2goal_gazebo.launch` (gazebo) +Step2: +in The keyword.txt add all the predicates that indicate that a low level protocol is successful +for example Medicine protolc is sucessful if either predicates (already_took_medicine) (already_reminded_medicine) are true so they should be added to the keyword.txt -## Face detection in gazebo: -**Do Autonomous navigation in gazebo** +Step3: -**start person sim** -`roslaunch person_sim init_standing_person.launch` +you need to set the two dictionaries protocol_type_ and keyword_protocol_ in action.hpp in the high_level_domain_Shutdown function. -**keyboard control for the person** -`roslaunch person_sim move_person_standing.launch` +this one should include all the protocols in :objects in the problem_high_level.pddl -**run face detection** -`roslaunch pioneer_shr face_detection_gazebo.launch` +const std::unordered_map protocol_type_ = { + {"protocol name ", "Type"} + }; + for example -## Face detection in real world: -**Do Autonomous navigation in real world** - -**run face detection** -`roslaunch pioneer_shr face_detection_real.launch` - -## Face recognition in real world: -**Do Autonomous navigation in real world** - -**run camera on robot labtop** -`roslaunch pioneer_shr camera_real.launch` -(not needed if have face_detection running first) - -**start face recognition on remote labtop** -`roslaunch pioneer_shr face_recognition_real.launch` - -## Medcial Protocal in simulation: -**Do face detection and face recogniton in gazebo** - -**run approach person service** -`roslaunch pioneer_shr action_service_gazebo.launch` - -**run executive** -`rosrun pioneer_shr executive` - -## Medcial Protocal in real world: -**Do face detection and face recogniton in real world** - -**run approach person service** -`roslaunch pioneer_shr action_service_real.launch` - -**run executive** -`rosrun pioneer_shr executive` - -## Mid Night Protocal in real world: - -**launch robot and run all service** -`roslaunch pioneer_shr shr_real.launch` - -**launch face recognition on robot labtop** -`roslaunch pioneer_shr face_recognition_real.launch` - -**run executive** -`rosrun pioneer_shr executive p2` + problem_high_level.pddl if looks like this + (:objects + living_room kitchen home outside dining_room bedroom bathroom - Landmark + am_meds pm_meds - MedicineProtocol + breakfast - FoodProtocol + ) + + then protocol_type_ should look like + const std::unordered_map protocol_type_ = { + {"am_meds", "MedicineProtocol"}, + {"pm_meds", "MedicineProtocol"}, + {"breakfast", "FoodProtocol"} + }; -## rosplan simple demo: +As for the keyword_protocol_ it maps the success keyword to the its protocol. +for example: -**launch robot and run all service** -`roslaunch pioneer_shr shr_real.launch` +(already_took_medicine) (already_reminded_medicine) are success keywords for am_meds and pm_meds +and (already_reminded_move) is a success for move_reminder -**launch planner** -`roslaunch rosplan_shr shr.launchp` +the keyword_protocol will look like this: -**run executive** -`rosrun pioneer_shr executive pddl` + const std::unordered_map> keyword_protocol_ = { + {"already_took_medicine", {"am_meds", "pm_meds"}}, + {"already_reminded_medicine", {"am_meds", "pm_meds"}}, + {"already_reminded_move",{"move_reminder"}}, + }; -## rosplan dry run on contigent-FF: -cd to catkin_ws/src/rosplan_shr -`rosrun rosplan_planning_system Contingent-FF -o ./common/domain_shr_conditional.pddl -f ./common/problem_shr_conditional.pddl`