From 4711b3ee8208ff58c950291aba262a848d6a8682 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Wed, 17 Oct 2018 15:59:34 +0100 Subject: [PATCH 01/13] Fixed interfaces for rosplan2 --- src/RPDocker.cpp | 8 ++++---- src/RPLocaliser.cpp | 7 +++++-- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/RPDocker.cpp b/src/RPDocker.cpp index f8d862c..6b1e05e 100644 --- a/src/RPDocker.cpp +++ b/src/RPDocker.cpp @@ -27,7 +27,7 @@ namespace KCL_rosplan { // dock the kobuki if(0==msg->name.compare("dock")) { - ROS_INFO("KCL: (Docker) action received"); + ROS_INFO("KCL: (Docker) action received - dock"); // Check robot name bool right_robot = false; @@ -55,7 +55,7 @@ namespace KCL_rosplan { if (finished_before_timeout) { actionlib::SimpleClientGoalState state = action_client.getState(); - ROS_INFO("KCL: (Docker) action finished: %s", state.toString().c_str()); + ROS_INFO("KCL: (Docker) action finished: %s - dock", state.toString().c_str()); if(state == actionlib::SimpleClientGoalState::SUCCEEDED) { @@ -113,7 +113,7 @@ namespace KCL_rosplan { // undock the kobuki else if(0==msg->name.compare("undock")) { - ROS_INFO("KCL: (Docker) action recieved"); + ROS_INFO("KCL: (Docker) action recieved - undock"); // publish feedback (enabled) rosplan_dispatch_msgs::ActionFeedback fb; @@ -133,7 +133,7 @@ namespace KCL_rosplan { count++; } - ROS_INFO("KCL: (Localiser) action complete"); + ROS_INFO("KCL: (Docker) action complete - undock"); // add predicate rosplan_knowledge_msgs::KnowledgeUpdateService updatePredSrv; diff --git a/src/RPLocaliser.cpp b/src/RPLocaliser.cpp index 26f830c..a4b040c 100644 --- a/src/RPLocaliser.cpp +++ b/src/RPLocaliser.cpp @@ -33,7 +33,7 @@ namespace KCL_rosplan { // Check robot name bool right_robot = false; for(size_t i=0; iparameters.size(); i++) { - if(0==msg->parameters[i].key.compare("v") && 0==msg->parameters[i].value.compare(name)) { + if((0==msg->parameters[i].key.compare("v") or 0==msg->parameters[i].key.compare("r")) && 0==msg->parameters[i].value.compare(name)) { right_robot = true; } } @@ -116,6 +116,9 @@ namespace KCL_rosplan { // remove old robot_at updatePredSrv.request.update_type = rosplan_knowledge_msgs::KnowledgeUpdateService::Request::REMOVE_KNOWLEDGE; updatePredSrv.request.knowledge.attribute_name = "robot_at"; + pair.key = "w"; + pair.value = ""; + updatePredSrv.request.knowledge.values.push_back(pair); update_knowledge_client.call(updatePredSrv); // predicate robot_at @@ -197,7 +200,7 @@ namespace KCL_rosplan { int main(int argc, char **argv) { - ros::init(argc, argv, "rosplan_interface_localisation"); + ros::init(argc, argv, "rosplan_localiser_interface"); ros::NodeHandle nh("~"); ros::NodeHandle nh2; From 40589b2ef732d19849db1c7e0518004336c07e00 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Wed, 17 Oct 2018 16:56:57 +0100 Subject: [PATCH 02/13] Fixed localiser parameters --- src/RPLocaliser.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/RPLocaliser.cpp b/src/RPLocaliser.cpp index a4b040c..4ad5d59 100644 --- a/src/RPLocaliser.cpp +++ b/src/RPLocaliser.cpp @@ -115,7 +115,11 @@ namespace KCL_rosplan { // remove old robot_at updatePredSrv.request.update_type = rosplan_knowledge_msgs::KnowledgeUpdateService::Request::REMOVE_KNOWLEDGE; + updatePredSrv = rosplan_knowledge_msgs::KnowledgeUpdateService(); updatePredSrv.request.knowledge.attribute_name = "robot_at"; + pair.key = "v"; + pair.value = name; + updatePredSrv.request.knowledge.values.push_back(pair); pair.key = "w"; pair.value = ""; updatePredSrv.request.knowledge.values.push_back(pair); @@ -123,11 +127,14 @@ namespace KCL_rosplan { // predicate robot_at updatePredSrv.request.update_type = rosplan_knowledge_msgs::KnowledgeUpdateService::Request::ADD_KNOWLEDGE; + updatePredSrv = rosplan_knowledge_msgs::KnowledgeUpdateService(); updatePredSrv.request.knowledge.attribute_name = "robot_at"; - diagnostic_msgs::KeyValue pairWP; - pairWP.key = "wp"; - pairWP.value = wpName; - updatePredSrv.request.knowledge.values.push_back(pairWP); + pair.key = "v"; + pair.value = name; + updatePredSrv.request.knowledge.values.push_back(pair); + pair.key = "wp"; + pair.value = wpName; + updatePredSrv.request.knowledge.values.push_back(pair); update_knowledge_client.call(updatePredSrv); } From 135fc7e8009ff571e5fd4397555904e05da526c2 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Wed, 17 Oct 2018 20:15:24 +0100 Subject: [PATCH 03/13] Fixed docking --- src/RPDocker.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/RPDocker.cpp b/src/RPDocker.cpp index 6b1e05e..38197cc 100644 --- a/src/RPDocker.cpp +++ b/src/RPDocker.cpp @@ -32,12 +32,12 @@ namespace KCL_rosplan { // Check robot name bool right_robot = false; for(size_t i=0; iparameters.size(); i++) { - if(0==msg->parameters[i].key.compare("v") && 0==msg->parameters[i].value.compare(name)) { + if(0==msg->parameters[i].value.compare(name)) { right_robot = true; } } if(!right_robot) { - ROS_DEBUG("KCL: (Docker) aborting action dispatch; handling robot %s", name.c_str()); + ROS_WARN("KCL: (Docker) aborting action dispatch; handling robot %s", name.c_str()); return; } @@ -51,7 +51,7 @@ namespace KCL_rosplan { fb.status = "action enabled"; action_feedback_pub.publish(fb); - bool finished_before_timeout = action_client.waitForResult(ros::Duration(5*msg->duration)); + bool finished_before_timeout = action_client.waitForResult(ros::Duration(/*5*msg->duration*/50)); if (finished_before_timeout) { actionlib::SimpleClientGoalState state = action_client.getState(); From ad83edd0d7e8bfbb9468ee5b2245b4c81b9951d8 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Wed, 17 Oct 2018 20:21:05 +0100 Subject: [PATCH 04/13] Fixed localiser --- src/RPLocaliser.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/RPLocaliser.cpp b/src/RPLocaliser.cpp index 4ad5d59..8b791c4 100644 --- a/src/RPLocaliser.cpp +++ b/src/RPLocaliser.cpp @@ -114,8 +114,9 @@ namespace KCL_rosplan { update_knowledge_client.call(updatePredSrv); // remove old robot_at - updatePredSrv.request.update_type = rosplan_knowledge_msgs::KnowledgeUpdateService::Request::REMOVE_KNOWLEDGE; updatePredSrv = rosplan_knowledge_msgs::KnowledgeUpdateService(); + updatePredSrv.request.update_type = rosplan_knowledge_msgs::KnowledgeUpdateService::Request::REMOVE_KNOWLEDGE; + updatePredSrv.request.knowledge.knowledge_type = rosplan_knowledge_msgs::KnowledgeItem::FACT; updatePredSrv.request.knowledge.attribute_name = "robot_at"; pair.key = "v"; pair.value = name; @@ -126,8 +127,9 @@ namespace KCL_rosplan { update_knowledge_client.call(updatePredSrv); // predicate robot_at - updatePredSrv.request.update_type = rosplan_knowledge_msgs::KnowledgeUpdateService::Request::ADD_KNOWLEDGE; updatePredSrv = rosplan_knowledge_msgs::KnowledgeUpdateService(); + updatePredSrv.request.update_type = rosplan_knowledge_msgs::KnowledgeUpdateService::Request::ADD_KNOWLEDGE; + updatePredSrv.request.knowledge.knowledge_type = rosplan_knowledge_msgs::KnowledgeItem::FACT; updatePredSrv.request.knowledge.attribute_name = "robot_at"; pair.key = "v"; pair.value = name; @@ -138,6 +140,7 @@ namespace KCL_rosplan { update_knowledge_client.call(updatePredSrv); } + ROS_INFO("(Localiser) %s", statement.data.c_str()); talker_pub.publish(statement); ros::Rate big_rate(0.5); big_rate.sleep(); From e91e3e67adab9cc8db483aabee9539905f5abf91 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Wed, 31 Oct 2018 11:32:41 +0000 Subject: [PATCH 05/13] Added asker action interface --- CMakeLists.txt | 4 + include/rosplan_interface_turtlebot/RPAsker.h | 37 +++++++++ src/RPAsker.cpp | 80 +++++++++++++++++++ 3 files changed, 121 insertions(+) create mode 100644 include/rosplan_interface_turtlebot/RPAsker.h create mode 100644 src/RPAsker.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 43cc6c8..4d54680 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,7 +47,11 @@ add_dependencies(rplocaliser ${catkin_EXPORTED_TARGETS}) add_executable(rptalker src/RPTalker.cpp) add_dependencies(rptalker ${catkin_EXPORTED_TARGETS}) +add_executable(rpasker src/RPAsker.cpp) +add_dependencies(rpasker ${catkin_EXPORTED_TARGETS}) + ## Specify libraries against which to link a library or executable target target_link_libraries(rpdocker ${catkin_LIBRARIES} ${Boost_LIBRARIES}) target_link_libraries(rplocaliser ${catkin_LIBRARIES} ${Boost_LIBRARIES}) target_link_libraries(rptalker ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(rpasker ${catkin_LIBRARIES} ${Boost_LIBRARIES}) diff --git a/include/rosplan_interface_turtlebot/RPAsker.h b/include/rosplan_interface_turtlebot/RPAsker.h new file mode 100644 index 0000000..470e8f2 --- /dev/null +++ b/include/rosplan_interface_turtlebot/RPAsker.h @@ -0,0 +1,37 @@ +// +// Created by Gerard Canal on 31/10/18. +// + +#ifndef ROSPLAN_INTERFACE_TURTLEBOT2_RPASKER_H +#define ROSPLAN_INTERFACE_TURTLEBOT2_RPASKER_H + +#include "ros/ros.h" +#include "std_msgs/String.h" +#include "rosplan_knowledge_msgs/KnowledgeItem.h" +#include "rosplan_knowledge_msgs/KnowledgeUpdateService.h" +#include "rosplan_dispatch_msgs/ActionDispatch.h" +#include "rosplan_dispatch_msgs/ActionFeedback.h" + +namespace KCL_rosplan { + class RPAsker { + private: + ros::NodeHandle nh_; + + ros::Publisher tts_pub; + ros::Subscriber dispatcher; + ros::Publisher action_feedback_pub; + + + void speak(const std::string &s); + std::string robot_name; + + public: + RPAsker(ros::NodeHandle& nh); + + ~RPAsker() = default; + + void dispatchCallback(const rosplan_dispatch_msgs::ActionDispatch::ConstPtr &msg); + }; +} + +#endif //ROSPLAN_INTERFACE_TURTLEBOT2_RPASKER_H diff --git a/src/RPAsker.cpp b/src/RPAsker.cpp new file mode 100644 index 0000000..f1427f5 --- /dev/null +++ b/src/RPAsker.cpp @@ -0,0 +1,80 @@ +// +// Created by Gerard Canal on 31/10/18. +// + +#include + +#include "rosplan_interface_turtlebot/RPAsker.h" + +namespace KCL_rosplan { + + RPAsker::RPAsker(ros::NodeHandle& nh) : nh_(nh) { + // listen for action dispatch + std::string adt = "default_dispatch_topic"; + nh.getParam("action_dispatch_topic", adt); + dispatcher = nh.subscribe(adt, 1000, &KCL_rosplan::RPAsker::dispatchCallback, this); + nh.param("turtlebot_name", robot_name, std::string("kenny")); + + // create publishers + std::string aft = "default_feedback_topic"; + nh.getParam("action_feedback_topic", aft); + action_feedback_pub = nh.advertise(aft, 10, true); + tts_pub = nh.advertise("/speech", 1, true); + } + + void RPAsker::speak(const std::string &s) { + std_msgs::String msg; + msg.data = s; + tts_pub.publish(msg); + } + + void RPAsker::dispatchCallback(const rosplan_dispatch_msgs::ActionDispatch::ConstPtr &msg) { + std::string to_say; + bool correct_action = false; + + if (msg->name == "ask_load") { + correct_action = true; + to_say = "Please, would you be kind enough to fetch the printed papers and put them on top of me?"; + } + else if (msg->name == "ask_unload") { + correct_action = true; + to_say = "Your printings are ready! Can you get the papers from me?"; + } + + if (correct_action) { + if (msg->parameters[0].value != robot_name) return; + + // publish feedback (enabled) + rosplan_dispatch_msgs::ActionFeedback fb; + fb.action_id = msg->action_id; + fb.status = "action enabled"; + action_feedback_pub.publish(fb); + + speak(to_say); + ros::Duration(0.075*to_say.length()).sleep(); // Duration proportional to the said speech + + // publish feedback (achieved) + fb.action_id = msg->action_id; + fb.status = "action achieved"; + action_feedback_pub.publish(fb); + } + } +} + + +/*-------------*/ +/* Main method */ +/*-------------*/ + +int main(int argc, char **argv) { + + ros::init(argc, argv, "rosplan_interface_asker"); + ros::NodeHandle nh; + + // create PDDL action subscriber + KCL_rosplan::RPAsker rpa(nh); + ROS_INFO("KCL: (Asker) Ready to receive"); + + ros::spin(); + return 0; +} \ No newline at end of file From 4ae8c641a867d40262e11fadfda0223befa05db9 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Wed, 31 Oct 2018 11:48:02 +0000 Subject: [PATCH 06/13] Asker KB add --- include/rosplan_interface_turtlebot/RPAsker.h | 1 + src/RPAsker.cpp | 18 +++++++++++++++++- 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/include/rosplan_interface_turtlebot/RPAsker.h b/include/rosplan_interface_turtlebot/RPAsker.h index 470e8f2..bc4cd05 100644 --- a/include/rosplan_interface_turtlebot/RPAsker.h +++ b/include/rosplan_interface_turtlebot/RPAsker.h @@ -20,6 +20,7 @@ namespace KCL_rosplan { ros::Publisher tts_pub; ros::Subscriber dispatcher; ros::Publisher action_feedback_pub; + ros::ServiceClient update_knowledge_client; void speak(const std::string &s); diff --git a/src/RPAsker.cpp b/src/RPAsker.cpp index f1427f5..b42bcb3 100644 --- a/src/RPAsker.cpp +++ b/src/RPAsker.cpp @@ -20,6 +20,9 @@ namespace KCL_rosplan { nh.getParam("action_feedback_topic", aft); action_feedback_pub = nh.advertise(aft, 10, true); tts_pub = nh.advertise("/speech", 1, true); + + // knowledge interface + update_knowledge_client = nh.serviceClient("/kcl_rosplan/update_knowledge_base"); } void RPAsker::speak(const std::string &s) { @@ -29,15 +32,17 @@ namespace KCL_rosplan { } void RPAsker::dispatchCallback(const rosplan_dispatch_msgs::ActionDispatch::ConstPtr &msg) { - std::string to_say; + std::string to_say, predicate; bool correct_action = false; if (msg->name == "ask_load") { correct_action = true; + predicate = "asked_load"; to_say = "Please, would you be kind enough to fetch the printed papers and put them on top of me?"; } else if (msg->name == "ask_unload") { correct_action = true; + predicate = "asked_unload"; to_say = "Your printings are ready! Can you get the papers from me?"; } @@ -53,6 +58,17 @@ namespace KCL_rosplan { speak(to_say); ros::Duration(0.075*to_say.length()).sleep(); // Duration proportional to the said speech + // add predicate + rosplan_knowledge_msgs::KnowledgeUpdateService updatePredSrv; + updatePredSrv.request.update_type = rosplan_knowledge_msgs::KnowledgeUpdateService::Request::ADD_KNOWLEDGE; + updatePredSrv.request.knowledge.knowledge_type = rosplan_knowledge_msgs::KnowledgeItem::FACT; + updatePredSrv.request.knowledge.attribute_name = predicate; + diagnostic_msgs::KeyValue pair; + pair.key = "r"; + pair.value = robot_name; + updatePredSrv.request.knowledge.values.push_back(pair); + update_knowledge_client.call(updatePredSrv); + // publish feedback (achieved) fb.action_id = msg->action_id; fb.status = "action achieved"; From 682fd85e7ddde3a487a64fcbca22ae11075dcc90 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Wed, 31 Oct 2018 19:18:47 +0000 Subject: [PATCH 07/13] Fixed asker --- CMakeLists.txt | 3 +++ src/RPAsker.cpp | 7 ++++--- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4d54680..0299206 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,9 @@ cmake_minimum_required(VERSION 2.8.3) project(rosplan_interface_turtlebot2) +add_compile_options(-std=c++11) +add_definitions(-Wno-deprecated-declarations) + ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS roscpp diff --git a/src/RPAsker.cpp b/src/RPAsker.cpp index b42bcb3..1d16869 100644 --- a/src/RPAsker.cpp +++ b/src/RPAsker.cpp @@ -47,6 +47,7 @@ namespace KCL_rosplan { } if (correct_action) { + ROS_INFO("(Asker) Received action %s", msg->name.c_str()); if (msg->parameters[0].value != robot_name) return; // publish feedback (enabled) @@ -84,8 +85,8 @@ namespace KCL_rosplan { int main(int argc, char **argv) { - ros::init(argc, argv, "rosplan_interface_asker"); - ros::NodeHandle nh; + ros::init(argc, argv, "rosplan_asker_interface"); + ros::NodeHandle nh("~"); // create PDDL action subscriber KCL_rosplan::RPAsker rpa(nh); @@ -93,4 +94,4 @@ int main(int argc, char **argv) { ros::spin(); return 0; -} \ No newline at end of file +} From 2114d6ff1d7234efbfa7f9ab6cf5f19901f9fd8a Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Tue, 6 Nov 2018 00:20:13 +0000 Subject: [PATCH 08/13] Added wait interface --- CMakeLists.txt | 5 +++++ package.xml | 1 + 2 files changed, 6 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0299206..05c59d2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs rosplan_dispatch_msgs rosplan_knowledge_msgs + rosplan_planning_system actionlib kobuki_msgs tf @@ -53,8 +54,12 @@ add_dependencies(rptalker ${catkin_EXPORTED_TARGETS}) add_executable(rpasker src/RPAsker.cpp) add_dependencies(rpasker ${catkin_EXPORTED_TARGETS}) +add_executable(rpwait src/RPWaitPapers.cpp) +add_dependencies(rpwait ${catkin_EXPORTED_TARGETS}) + ## Specify libraries against which to link a library or executable target target_link_libraries(rpdocker ${catkin_LIBRARIES} ${Boost_LIBRARIES}) target_link_libraries(rplocaliser ${catkin_LIBRARIES} ${Boost_LIBRARIES}) target_link_libraries(rptalker ${catkin_LIBRARIES} ${Boost_LIBRARIES}) target_link_libraries(rpasker ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(rpwait ${catkin_LIBRARIES} ${Boost_LIBRARIES}) diff --git a/package.xml b/package.xml index e6de7fb..7f5d3b1 100644 --- a/package.xml +++ b/package.xml @@ -17,6 +17,7 @@ geometry_msgs rosplan_dispatch_msgs rosplan_knowledge_msgs + rosplan_planning_system actionlib kobuki_msgs tf From db1d53f99078c498874c2ce58514366739eda5be Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Tue, 6 Nov 2018 00:20:43 +0000 Subject: [PATCH 09/13] Added wait interface --- .../RPWaitPapers.h | 49 ++++++++ src/RPWaitPapers.cpp | 115 ++++++++++++++++++ 2 files changed, 164 insertions(+) create mode 100644 include/rosplan_interface_turtlebot/RPWaitPapers.h create mode 100644 src/RPWaitPapers.cpp diff --git a/include/rosplan_interface_turtlebot/RPWaitPapers.h b/include/rosplan_interface_turtlebot/RPWaitPapers.h new file mode 100644 index 0000000..e4a2a59 --- /dev/null +++ b/include/rosplan_interface_turtlebot/RPWaitPapers.h @@ -0,0 +1,49 @@ +// +// Created by Gerard Canal on 02/11/18. +// + +#ifndef ROSPLAN_INTERFACE_TURTLEBOT2_RPWAITPAPERS_H +#define ROSPLAN_INTERFACE_TURTLEBOT2_RPWAITPAPERS_H + +#include +#include + +#include "rosplan_action_interface/RPActionInterface.h" +#include "std_msgs/Bool.h" + +/** + * This file defines an action interface created in tutorial 10. + */ +namespace KCL_rosplan { + + class RPWaitPapers: public RPActionInterface { + + private: + bool interactive; + std::map people_distribution; + std::map busy_distribution; + float timeout; + float busy_wait_timeout; + ros::ServiceClient getPropsClient; + ros::Publisher somebody_pub; + ros::Publisher busy_pub; + ros::Subscriber papers_in; + bool papers_loaded; + + + bool somebody_at(const std::string& loc); + bool busy(const std::string& loc); + void papers_cb(std_msgs::BoolConstPtr b); + public: + + /* constructor */ + RPWaitPapers(ros::NodeHandle &nh); + + /* listen to and process action_dispatch topic */ + bool concreteCallback(const rosplan_dispatch_msgs::ActionDispatch::ConstPtr &msg); + }; +} +#endif + + +#endif //ROSPLAN_INTERFACE_TURTLEBOT2_RPWAITPAPERS_H diff --git a/src/RPWaitPapers.cpp b/src/RPWaitPapers.cpp new file mode 100644 index 0000000..9cab691 --- /dev/null +++ b/src/RPWaitPapers.cpp @@ -0,0 +1,115 @@ +// +// Created by Gerard Canal on 02/11/18. +// + +#include + +#include "random.h" +#include "rosplan_interface_turtlebot/RPWaitPapers.h" +#include "rosplan_knowledge_msgs/GetAttributeService.h" + +/* The implementation of RPTutorial.h */ +namespace KCL_rosplan { + + /* constructor */ + RPWaitPapers::RPWaitPapers(ros::NodeHandle &nh) { + // perform setup + nh.param("interactive", interactive, true); + nh.param("timeout", timeout, 60.0f); + nh.param("busy_timeout", busy_wait_timeout, 75.0f); + nh.getParam("people_distribution", people_distribution); + nh.getParam("busy_distribution", busy_distribution); + ros::ServiceClient getPropsClient = nh.serviceClient("/rosplan_knowledge_base/state/propositions"); + getPropsClient.waitForExistence(ros::Duration(60)); + busy_pub = nh.advertise("/isbusy_mock", 1); + somebody_pub = nh.advertise("/someodyat_mock", 1); + papers_in = nh.subscribe("/papers_loaded", 1, &RPWaitPapers::papers_cb, this); + papers_loaded = false; + } + + /* action dispatch callback */ + bool RPWaitPapers::concreteCallback(const rosplan_dispatch_msgs::ActionDispatch::ConstPtr& msg) { + // Get robot location + rosplan_knowledge_msgs::GetAttributeService attrSrv; + attrSrv.request.predicate_name = "robot_at"; + if (!getPropsClient.call(attrSrv)) { + ROS_ERROR("KCL: (WaitPapers) Failed to call service %s: %s", + getPropsClient.getService().c_str(), attrSrv.request.predicate_name.c_str()); + return false; + } + if (attrSrv.response.attributes.size() == 0) return false; + std::string robot_at = attrSrv.response.attributes[0].values[0].value; + diagnostic_msgs::KeyValue kv; + kv.key = robot_at; + + ros::Time start_t = ros::Time::now(); + ros::Duration timeout_t; + bool printer_busy = busy(robot_at); + if (printer_busy) timeout_t = ros::Duration(busy_wait_timeout); + else timeout_t = ros::Duration(timeout); + + bool found_somebody = printer_busy or somebody_at(robot_at); + if (found_somebody) kv.value = "yes"; + else kv.value = "no"; + somebody_pub.publish(kv); + if (printer_busy) kv.value = "yes"; + else kv.value = "no"; + busy_pub.publish(kv); + + ros::Rate r(10); + while (ros::Time::now()-start_t < timeout_t) { + if (found_somebody && papers_loaded) return true; + ros::spinOnce(); + r.sleep(); + } + // complete the action + ROS_INFO("KCL: (%s) TUTORIAL Action completing.", msg->name.c_str()); + if (ros::Time::now()-start_t > timeout_t) { + ROS_INFO("KCL: (%s) TIMEOUTED! Action failed.", msg->name.c_str()); + return false; + } + + + return true; + } + + bool RPWaitPapers::somebody_at(const std::string &loc) { + if (interactive) { + std::cout << "Is there somebody at " << loc << "? (y/n) "; + char c; + std::cin >> c; + return c == 'y'; + } + return (rand() % 100) <= (100 * people_distribution[loc]); + } + + bool RPWaitPapers::busy(const std::string &loc) { + if (interactive) { + std::cout << "Is the printer at " << loc << " busy? (y/n) "; + char c; + std::cin >> c; + return c == 'y'; + } + return (rand() % 100) <= (100 * busy_distribution[loc]); + } + + void RPWaitPapers::papers_cb(std_msgs::BoolConstPtr b) { + papers_loaded = b->data; + } +} // close namespace + +/*-------------*/ +/* Main method */ +/*-------------*/ + +int main(int argc, char **argv) { + ros::init(argc, argv, "rosplan_wait_papers_action", ros::init_options::AnonymousName); + ros::NodeHandle nh("~"); + + // create PDDL action subscriber + KCL_rosplan::RPWaitPapers rpti(nh); + + rpti.runActionInterface(); + + return 0; +} \ No newline at end of file From c4149af5823f93b6990215ad319d09c35da17dd3 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Wed, 7 Nov 2018 12:42:01 +0000 Subject: [PATCH 10/13] Updated waitpapers interface --- .../RPWaitPapers.h | 4 +- src/RPWaitPapers.cpp | 60 +++++++++++++++++-- 2 files changed, 57 insertions(+), 7 deletions(-) diff --git a/include/rosplan_interface_turtlebot/RPWaitPapers.h b/include/rosplan_interface_turtlebot/RPWaitPapers.h index e4a2a59..3307214 100644 --- a/include/rosplan_interface_turtlebot/RPWaitPapers.h +++ b/include/rosplan_interface_turtlebot/RPWaitPapers.h @@ -27,13 +27,16 @@ namespace KCL_rosplan { ros::ServiceClient getPropsClient; ros::Publisher somebody_pub; ros::Publisher busy_pub; + ros::Publisher speech_pub; ros::Subscriber papers_in; + ros::ServiceClient update_kb; bool papers_loaded; bool somebody_at(const std::string& loc); bool busy(const std::string& loc); void papers_cb(std_msgs::BoolConstPtr b); + void updateKBDistributions(); public: /* constructor */ @@ -43,7 +46,6 @@ namespace KCL_rosplan { bool concreteCallback(const rosplan_dispatch_msgs::ActionDispatch::ConstPtr &msg); }; } -#endif #endif //ROSPLAN_INTERFACE_TURTLEBOT2_RPWAITPAPERS_H diff --git a/src/RPWaitPapers.cpp b/src/RPWaitPapers.cpp index 9cab691..7099cac 100644 --- a/src/RPWaitPapers.cpp +++ b/src/RPWaitPapers.cpp @@ -3,6 +3,7 @@ // #include +#include #include "random.h" #include "rosplan_interface_turtlebot/RPWaitPapers.h" @@ -19,12 +20,16 @@ namespace KCL_rosplan { nh.param("busy_timeout", busy_wait_timeout, 75.0f); nh.getParam("people_distribution", people_distribution); nh.getParam("busy_distribution", busy_distribution); - ros::ServiceClient getPropsClient = nh.serviceClient("/rosplan_knowledge_base/state/propositions"); + getPropsClient = nh.serviceClient("/rosplan_knowledge_base/state/propositions"); getPropsClient.waitForExistence(ros::Duration(60)); busy_pub = nh.advertise("/isbusy_mock", 1); somebody_pub = nh.advertise("/someodyat_mock", 1); papers_in = nh.subscribe("/papers_loaded", 1, &RPWaitPapers::papers_cb, this); + speech_pub = nh.advertise("/speech", 1); papers_loaded = false; + update_kb = nh.serviceClient("/rosplan_knowledge_base/update_array"); + update_kb.waitForExistence(ros::Duration(60)); + updateKBDistributions(); } /* action dispatch callback */ @@ -38,7 +43,13 @@ namespace KCL_rosplan { return false; } if (attrSrv.response.attributes.size() == 0) return false; - std::string robot_at = attrSrv.response.attributes[0].values[0].value; + std::string robot_at; + for (size_t i = 0; i < attrSrv.response.attributes.size(); ++i) { + if (attrSrv.response.attributes[i].is_negative == 0) { + robot_at = attrSrv.response.attributes[i].values[1].value; + break; + } + } diagnostic_msgs::KeyValue kv; kv.key = robot_at; @@ -62,14 +73,18 @@ namespace KCL_rosplan { ros::spinOnce(); r.sleep(); } + std_msgs::String speech; // complete the action - ROS_INFO("KCL: (%s) TUTORIAL Action completing.", msg->name.c_str()); + ROS_INFO("KCL: (%s) Action completing.", msg->name.c_str()); if (ros::Time::now()-start_t > timeout_t) { ROS_INFO("KCL: (%s) TIMEOUTED! Action failed.", msg->name.c_str()); + speech.data = "Oh, it seems there is nobody here"; + speech_pub.publish(speech); return false; } - + speech.data = "Thank you"; + speech_pub.publish(speech); return true; } @@ -96,6 +111,39 @@ namespace KCL_rosplan { void RPWaitPapers::papers_cb(std_msgs::BoolConstPtr b) { papers_loaded = b->data; } + + void RPWaitPapers::updateKBDistributions() { + auto kus = rosplan_knowledge_msgs::KnowledgeUpdateServiceArrayRequest(); + for (auto it = people_distribution.begin(); it != people_distribution.end(); ++it) { + auto ki = rosplan_knowledge_msgs::KnowledgeItem(); + ki.knowledge_type = ki.FUNCTION; + ki.attribute_name = "OCCUPATION_RATE"; + auto kv = diagnostic_msgs::KeyValue(); + kv.key = "w"; + kv.value = it->first; + ki.values.push_back(kv); + ki.function_value = it->second; + kus.knowledge.push_back(ki); + kus.update_type.push_back(kus.ADD_KNOWLEDGE); + } + for (auto it = busy_distribution.begin(); it != busy_distribution.end(); ++it) { + auto ki = rosplan_knowledge_msgs::KnowledgeItem(); + ki.knowledge_type = ki.FUNCTION; + ki.attribute_name = "BUSY_RATE"; + auto kv = diagnostic_msgs::KeyValue(); + kv.key = "w"; + kv.value = it->first; + ki.values.push_back(kv); + ki.function_value = it->second; + kus.knowledge.push_back(ki); + kus.update_type.push_back(kus.ADD_KNOWLEDGE); + } + auto srvcall = rosplan_knowledge_msgs::KnowledgeUpdateServiceArray(); + srvcall.request = kus; + if (not update_kb.call(srvcall)) { + ROS_ERROR("KCL (RPWaitPapers): Failed to call service to update Knowledge Base"); + } + } } // close namespace /*-------------*/ @@ -103,7 +151,7 @@ namespace KCL_rosplan { /*-------------*/ int main(int argc, char **argv) { - ros::init(argc, argv, "rosplan_wait_papers_action", ros::init_options::AnonymousName); + ros::init(argc, argv, "rosplan_wait_papers_interface"); ros::NodeHandle nh("~"); // create PDDL action subscriber @@ -112,4 +160,4 @@ int main(int argc, char **argv) { rpti.runActionInterface(); return 0; -} \ No newline at end of file +} From 1e5609e98d715c7c407e9179d43cb620688512c1 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Sat, 10 Nov 2018 18:09:49 +0000 Subject: [PATCH 11/13] Fixed include --- src/RPWaitPapers.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/RPWaitPapers.cpp b/src/RPWaitPapers.cpp index 7099cac..9fc2dd7 100644 --- a/src/RPWaitPapers.cpp +++ b/src/RPWaitPapers.cpp @@ -5,7 +5,7 @@ #include #include -#include "random.h" +#include #include "rosplan_interface_turtlebot/RPWaitPapers.h" #include "rosplan_knowledge_msgs/GetAttributeService.h" From ebdee59ce19dd18ccdb404b85245f362aef65fb5 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Tue, 4 Dec 2018 11:35:08 +0100 Subject: [PATCH 12/13] Update waitpapers --- src/RPWaitPapers.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/RPWaitPapers.cpp b/src/RPWaitPapers.cpp index 9fc2dd7..339b07f 100644 --- a/src/RPWaitPapers.cpp +++ b/src/RPWaitPapers.cpp @@ -23,7 +23,7 @@ namespace KCL_rosplan { getPropsClient = nh.serviceClient("/rosplan_knowledge_base/state/propositions"); getPropsClient.waitForExistence(ros::Duration(60)); busy_pub = nh.advertise("/isbusy_mock", 1); - somebody_pub = nh.advertise("/someodyat_mock", 1); + somebody_pub = nh.advertise("/somebodyat_mock", 1); papers_in = nh.subscribe("/papers_loaded", 1, &RPWaitPapers::papers_cb, this); speech_pub = nh.advertise("/speech", 1); papers_loaded = false; @@ -59,17 +59,17 @@ namespace KCL_rosplan { if (printer_busy) timeout_t = ros::Duration(busy_wait_timeout); else timeout_t = ros::Duration(timeout); - bool found_somebody = printer_busy or somebody_at(robot_at); + /*bool found_somebody = printer_busy or somebody_at(robot_at); if (found_somebody) kv.value = "yes"; else kv.value = "no"; somebody_pub.publish(kv); if (printer_busy) kv.value = "yes"; else kv.value = "no"; - busy_pub.publish(kv); + busy_pub.publish(kv);*/ ros::Rate r(10); while (ros::Time::now()-start_t < timeout_t) { - if (found_somebody && papers_loaded) return true; + if (/*found_somebody && */papers_loaded) break; ros::spinOnce(); r.sleep(); } @@ -110,6 +110,7 @@ namespace KCL_rosplan { void RPWaitPapers::papers_cb(std_msgs::BoolConstPtr b) { papers_loaded = b->data; + ROS_INFO_STREAM("Got message: " << papers_loaded); } void RPWaitPapers::updateKBDistributions() { From 9810eaa88b57bd58c7347cbcc4a123b2f58b1247 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Fri, 9 Jul 2021 15:02:24 +0100 Subject: [PATCH 13/13] Fix action interfaces --- src/RPAsker.cpp | 4 ++-- src/RPDocker.cpp | 12 ++++++------ src/RPLocaliser.cpp | 4 ++-- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/RPAsker.cpp b/src/RPAsker.cpp index 1d16869..db104a0 100644 --- a/src/RPAsker.cpp +++ b/src/RPAsker.cpp @@ -53,7 +53,7 @@ namespace KCL_rosplan { // publish feedback (enabled) rosplan_dispatch_msgs::ActionFeedback fb; fb.action_id = msg->action_id; - fb.status = "action enabled"; + fb.status = rosplan_dispatch_msgs::ActionFeedback::ACTION_ENABLED; action_feedback_pub.publish(fb); speak(to_say); @@ -72,7 +72,7 @@ namespace KCL_rosplan { // publish feedback (achieved) fb.action_id = msg->action_id; - fb.status = "action achieved"; + fb.status = rosplan_dispatch_msgs::ActionFeedback::ACTION_SUCCEEDED_TO_GOAL_STATE; action_feedback_pub.publish(fb); } } diff --git a/src/RPDocker.cpp b/src/RPDocker.cpp index 38197cc..a98eaa2 100644 --- a/src/RPDocker.cpp +++ b/src/RPDocker.cpp @@ -48,7 +48,7 @@ namespace KCL_rosplan { // publish feedback (enabled) rosplan_dispatch_msgs::ActionFeedback fb; fb.action_id = msg->action_id; - fb.status = "action enabled"; + fb.status = rosplan_dispatch_msgs::ActionFeedback::ACTION_ENABLED; action_feedback_pub.publish(fb); bool finished_before_timeout = action_client.waitForResult(ros::Duration(/*5*msg->duration*/50)); @@ -81,7 +81,7 @@ namespace KCL_rosplan { // publish feedback (achieved) rosplan_dispatch_msgs::ActionFeedback fb; fb.action_id = msg->action_id; - fb.status = "action achieved"; + fb.status = rosplan_dispatch_msgs::ActionFeedback::ACTION_SUCCEEDED_TO_GOAL_STATE; action_feedback_pub.publish(fb); } else { @@ -89,7 +89,7 @@ namespace KCL_rosplan { // publish feedback (failed) rosplan_dispatch_msgs::ActionFeedback fb; fb.action_id = msg->action_id; - fb.status = "action failed"; + fb.status = rosplan_dispatch_msgs::ActionFeedback::ACTION_FAILED; action_feedback_pub.publish(fb); } @@ -101,7 +101,7 @@ namespace KCL_rosplan { // publish feedback (failed) rosplan_dispatch_msgs::ActionFeedback fb; fb.action_id = msg->action_id; - fb.status = "action failed"; + fb.status = rosplan_dispatch_msgs::ActionFeedback::ACTION_FAILED; action_feedback_pub.publish(fb); ROS_INFO("KCL: (Docker) action timed out"); @@ -118,7 +118,7 @@ namespace KCL_rosplan { // publish feedback (enabled) rosplan_dispatch_msgs::ActionFeedback fb; fb.action_id = msg->action_id; - fb.status = "action enabled"; + fb.status = rosplan_dispatch_msgs::ActionFeedback::ACTION_ENABLED; action_feedback_pub.publish(fb); geometry_msgs::Twist base_cmd; @@ -156,7 +156,7 @@ namespace KCL_rosplan { big_rate.sleep(); // publish feedback (achieved) - fb.status = "action achieved"; + fb.status = rosplan_dispatch_msgs::ActionFeedback::ACTION_SUCCEEDED_TO_GOAL_STATE; action_feedback_pub.publish(fb); } } diff --git a/src/RPLocaliser.cpp b/src/RPLocaliser.cpp index 8b791c4..0190cc9 100644 --- a/src/RPLocaliser.cpp +++ b/src/RPLocaliser.cpp @@ -49,7 +49,7 @@ namespace KCL_rosplan { // publish feedback (enabled) rosplan_dispatch_msgs::ActionFeedback fb; fb.action_id = msg->action_id; - fb.status = "action enabled"; + fb.status = rosplan_dispatch_msgs::ActionFeedback::ACTION_ENABLED; action_feedback_pub.publish(fb); geometry_msgs::Twist base_cmd; @@ -149,7 +149,7 @@ namespace KCL_rosplan { big_rate.sleep(); // publish feedback (achieved) - fb.status = "action achieved"; + fb.status = rosplan_dispatch_msgs::ActionFeedback::ACTION_SUCCEEDED_TO_GOAL_STATE; action_feedback_pub.publish(fb); } }