diff --git a/CMakeLists.txt b/CMakeLists.txt index 43cc6c8..05c59d2 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 @@ -11,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs rosplan_dispatch_msgs rosplan_knowledge_msgs + rosplan_planning_system actionlib kobuki_msgs tf @@ -47,7 +51,15 @@ 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}) + +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/include/rosplan_interface_turtlebot/RPAsker.h b/include/rosplan_interface_turtlebot/RPAsker.h new file mode 100644 index 0000000..bc4cd05 --- /dev/null +++ b/include/rosplan_interface_turtlebot/RPAsker.h @@ -0,0 +1,38 @@ +// +// 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; + ros::ServiceClient update_knowledge_client; + + + 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/include/rosplan_interface_turtlebot/RPWaitPapers.h b/include/rosplan_interface_turtlebot/RPWaitPapers.h new file mode 100644 index 0000000..3307214 --- /dev/null +++ b/include/rosplan_interface_turtlebot/RPWaitPapers.h @@ -0,0 +1,51 @@ +// +// 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::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 */ + RPWaitPapers(ros::NodeHandle &nh); + + /* listen to and process action_dispatch topic */ + bool concreteCallback(const rosplan_dispatch_msgs::ActionDispatch::ConstPtr &msg); + }; +} + + +#endif //ROSPLAN_INTERFACE_TURTLEBOT2_RPWAITPAPERS_H 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 diff --git a/src/RPAsker.cpp b/src/RPAsker.cpp new file mode 100644 index 0000000..db104a0 --- /dev/null +++ b/src/RPAsker.cpp @@ -0,0 +1,97 @@ +// +// 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); + + // knowledge interface + update_knowledge_client = nh.serviceClient("/kcl_rosplan/update_knowledge_base"); + } + + 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, 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?"; + } + + if (correct_action) { + ROS_INFO("(Asker) Received action %s", msg->name.c_str()); + if (msg->parameters[0].value != robot_name) return; + + // publish feedback (enabled) + rosplan_dispatch_msgs::ActionFeedback fb; + fb.action_id = msg->action_id; + fb.status = rosplan_dispatch_msgs::ActionFeedback::ACTION_ENABLED; + action_feedback_pub.publish(fb); + + 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 = rosplan_dispatch_msgs::ActionFeedback::ACTION_SUCCEEDED_TO_GOAL_STATE; + action_feedback_pub.publish(fb); + } + } +} + + +/*-------------*/ +/* Main method */ +/*-------------*/ + +int main(int argc, char **argv) { + + ros::init(argc, argv, "rosplan_asker_interface"); + ros::NodeHandle nh("~"); + + // create PDDL action subscriber + KCL_rosplan::RPAsker rpa(nh); + ROS_INFO("KCL: (Asker) Ready to receive"); + + ros::spin(); + return 0; +} diff --git a/src/RPDocker.cpp b/src/RPDocker.cpp index f8d862c..a98eaa2 100644 --- a/src/RPDocker.cpp +++ b/src/RPDocker.cpp @@ -27,17 +27,17 @@ 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; 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; } @@ -48,14 +48,14 @@ 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)); + bool finished_before_timeout = action_client.waitForResult(ros::Duration(/*5*msg->duration*/50)); 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) { @@ -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"); @@ -113,12 +113,12 @@ 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; 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; @@ -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; @@ -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 26f830c..0190cc9 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; } } @@ -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; @@ -114,20 +114,33 @@ 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; + updatePredSrv.request.knowledge.values.push_back(pair); + pair.key = "w"; + pair.value = ""; + updatePredSrv.request.knowledge.values.push_back(pair); 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"; - 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); } + ROS_INFO("(Localiser) %s", statement.data.c_str()); talker_pub.publish(statement); ros::Rate big_rate(0.5); big_rate.sleep(); @@ -136,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); } } @@ -197,7 +210,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; diff --git a/src/RPWaitPapers.cpp b/src/RPWaitPapers.cpp new file mode 100644 index 0000000..339b07f --- /dev/null +++ b/src/RPWaitPapers.cpp @@ -0,0 +1,164 @@ +// +// Created by Gerard Canal on 02/11/18. +// + +#include +#include + +#include +#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); + getPropsClient = nh.serviceClient("/rosplan_knowledge_base/state/propositions"); + getPropsClient.waitForExistence(ros::Duration(60)); + busy_pub = nh.advertise("/isbusy_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; + update_kb = nh.serviceClient("/rosplan_knowledge_base/update_array"); + update_kb.waitForExistence(ros::Duration(60)); + updateKBDistributions(); + } + + /* 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; + 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; + + 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) break; + ros::spinOnce(); + r.sleep(); + } + std_msgs::String speech; + // complete the action + 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; + } + + 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; + ROS_INFO_STREAM("Got message: " << papers_loaded); + } + + 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 + +/*-------------*/ +/* Main method */ +/*-------------*/ + +int main(int argc, char **argv) { + ros::init(argc, argv, "rosplan_wait_papers_interface"); + ros::NodeHandle nh("~"); + + // create PDDL action subscriber + KCL_rosplan::RPWaitPapers rpti(nh); + + rpti.runActionInterface(); + + return 0; +}