Fixes to legacy turtlebot2 interfaces

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})

include/rosplan_interface_turtlebot/RPAsker.h

@@ -0,0 +1,38 @@
+//
+// Created by Gerard Canal <gcanal@iri.upc.edu> 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

include/rosplan_interface_turtlebot/RPWaitPapers.h

@@ -0,0 +1,51 @@
+//
+// Created by Gerard Canal <gcanal@iri.upc.edu> on 02/11/18.
+//
+
+#ifndef ROSPLAN_INTERFACE_TURTLEBOT2_RPWAITPAPERS_H
+#define ROSPLAN_INTERFACE_TURTLEBOT2_RPWAITPAPERS_H
+
+#include <ros/ros.h>
+#include <vector>
+
+#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<std::string, float> people_distribution;
+        std::map<std::string, float> 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

package.xml

@@ -17,6 +17,7 @@
   <build_depend>geometry_msgs</build_depend>
   <build_depend>rosplan_dispatch_msgs</build_depend>
   <build_depend>rosplan_knowledge_msgs</build_depend>
+  <build_depend>rosplan_planning_system</build_depend>
   <build_depend>actionlib</build_depend>
   <build_depend>kobuki_msgs</build_depend>
   <build_depend>tf</build_depend>

src/RPAsker.cpp

@@ -0,0 +1,97 @@
+//
+// Created by Gerard Canal <gcanal@iri.upc.edu> on 31/10/18.
+//
+
+#include <rosplan_interface_turtlebot/RPAsker.h>
+
+#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<rosplan_dispatch_msgs::ActionFeedback>(aft, 10, true);
+        tts_pub = nh.advertise<std_msgs::String>("/speech", 1, true);
+
+        // knowledge interface
+        update_knowledge_client = nh.serviceClient<rosplan_knowledge_msgs::KnowledgeUpdateService>("/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;
+}

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; i<msg->parameters.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,15 +81,15 @@ 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 {
 
 					// 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);
 		}
 	}