diff --git a/README.md b/README.md index 38185b20..26a338c4 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` diff --git a/shr_parameters/params/shr_parameters.yaml b/shr_parameters/params/shr_parameters.yaml index 74a1cabb..b70c7b99 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_parameters_py/shr_parameters_py/shr_parameters.py b/shr_parameters_py/shr_parameters_py/shr_parameters.py index 8a549903..5185ee4e 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/CMakeLists.txt b/shr_plan/CMakeLists.txt index 9898f6b2..6d301926 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 a94a48f6..d473b106 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 { @@ -453,62 +454,20 @@ 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, 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) { + 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(); 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,29 +483,29 @@ 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; 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; -// 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..."); + + 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 std::cout << " waiting " << std::endl; @@ -555,7 +514,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; @@ -591,10 +550,46 @@ 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; + + } + + // 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", {}}); + + 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(); + + 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, pred_started); + + std::cout << "%%%%%%% IDLE %%%%%%% IDLE " << std::endl; + ps.active_protocol = {}; lock.UnLock(); - return BT::NodeStatus::SUCCESS; + return status; } void abort(const InstantiatedAction &action) override { @@ -734,6 +729,161 @@ 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(); + + BT::NodeStatus status = BT::NodeStatus::FAILURE; + 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, true); + 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"; + 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); + + + // 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; + } + + std::string cmd_reboot = "echo '" + std::string(password) + "' | sudo -S reboot"; + std::system(cmd_reboot.c_str()); + + + + 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"); + + const char* homeDir = std::getenv("HOME"); + std::string cmd_startros = std::string(homeDir); + cmd_startros += "/start_nav.sh"; + std::system(cmd_startros.c_str()); + + std::cout << " ------ finish start ----" << std::endl; + + // 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(); @@ -969,6 +1119,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.txt b/shr_plan/include/shr_plan/intersection.txt new file mode 100644 index 00000000..e69de29b 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 00000000..085f4000 --- /dev/null +++ b/shr_plan/include/shr_plan/intersection_helpers.hpp @@ -0,0 +1,53 @@ +// +// Created by olagh48652 on 2/8/25. +// + +#ifndef PLAN_SOLVER_INTERSECTION_HELPERS_HPP +#define PLAN_SOLVER_INTERSECTION_HELPERS_HPP + + +#include +#include +#include +#include +#include + + +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; + } + + // 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 + } + ofs.close(); + std::cout << "Predicates successfully written to " << filePath << std::endl; +} + +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 {}; + } + + 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); + } + + ifs.close(); + return predicates; +} + +#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 00000000..09981382 --- /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/pddl/high_level_domain.pddl b/shr_plan/pddl/high_level_domain.pddl index 07910df6..be498ea7 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) + ) +) + + ) diff --git a/shr_plan/src/planning_controller_node.cpp b/shr_plan/src/planning_controller_node.cpp index 5d4d80bc..a6cd73c4 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; @@ -24,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()); @@ -309,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(); } @@ -364,6 +365,20 @@ int main(int argc, char **argv) { } ); + 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()); + + // 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(); 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 00000000..425c4586 --- /dev/null +++ b/shr_plan/test/test_writing_to_file.cpp @@ -0,0 +1,69 @@ +#include +#include +#include +#include + +// --- 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; + } + + // 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 + } + ofs.close(); + std::cout << "Predicates successfully written to " << filePath << std::endl; +} + +// --- 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 {}; + } + + 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); + } + + ifs.close(); + return predicates; +} + +int main() { + 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"} + }; + + // Write to file + write_to_intersection(outputFile, keyword_protocol_list); + + // Read from file + auto predicates = read_predicates_from_file(outputFile); + + // 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