Merge pull request #22 from RoBorregos/9-gpsr-task-manager

Task manager nav interfaces added

ws/src/frida_navigation_interfaces/CMakeLists.txt

@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(frida_navigation_interfaces)
+
+find_package(catkin REQUIRED COMPONENTS
+  actionlib
+  actionlib_msgs
+  geometry_msgs
+  message_generation
+  move_base_msgs
+  roscpp
+  rospy
+  std_msgs
+)
+
+
+## Generate actions in the 'action' folder
+add_action_files(
+  FILES
+  navServ.action
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  actionlib_msgs geometry_msgs move_base_msgs std_msgs
+)
+
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES frida_navigation_interfaces
+  CATKIN_DEPENDS actionlib actionlib_msgs geometry_msgs message_generation move_base_msgs roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+

ws/src/frida_navigation_interfaces/action/navServ.action

@@ -0,0 +1,8 @@
+#goal definition
+string target_location
+---
+#result definition
+bool result
+---
+#feedback
+geometry_msgs/PoseStamped pose

ws/src/frida_navigation_interfaces/package.xml

@@ -0,0 +1,50 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>frida_navigation_interfaces</name>
+  <version>0.0.0</version>
+  <description>The frida_navigation_interfaces package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
+  <maintainer email="[email protected]">ros</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>move_base_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>actionlib</build_export_depend>
+  <build_export_depend>actionlib_msgs</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>move_base_msgs</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>message_generation</build_export_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>actionlib_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>move_base_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

ws/src/frida_task_manager/scripts/hri_tasks.py

@@ -31,7 +31,10 @@ class TasksHRI:
     def __init__(self) -> None:
         self.conversation_client = actionlib.SimpleActionClient(CONVERSATION_SERVER, ConversateAction)
         #self.pub_speak = rospy.Publisher(SPEAK_TOPIC, String, queue_size=10)
-        rospy.wait_for_service(SPEAK_TOPIC)
+        try:
+            rospy.wait_for_service(SPEAK_TOPIC, timeout=5.0)
+        except rospy.ROSException:
+            rospy.logerr("Conversation service not available")
         self.speak_client = rospy.ServiceProxy(SPEAK_TOPIC, Speak)
 
         rospy.loginfo("HRI Task Manager initialized")

ws/src/frida_task_manager/scripts/manipulation_tasks.py

@@ -18,12 +18,20 @@
 
 class TasksManipulation:
 
+    STATE = {
+        "TERMINAL_ERROR": -1,
+        "EXECUTION_ERROR": 0,
+        "EXECUTION_SUCCESS": 1
+    }
+
     AREA_TASKS = ["pick", "place", "grab", "give", "open", "close"]
 
     OBJECTS_DICT = {
         "zucaritas": 1,
-        "cookies": 9,
-        "cookie": 9
+        "cookies": 11,
+        "cookie": 11,
+        "snack": 5,
+        "snacks": 5
     }
 
     def __init__(self) -> None:
@@ -35,40 +43,37 @@ def execute_command(self, command: str, target: str, info: str) -> int:
         """Method to execute each command"""
         rospy.loginfo("Manipulation Command")
         if command == "pick":
-            return self.execute_pick_and_place( TasksManipulation.OBJECTS_DICT[target] )
+            return self.execute_pick( target )
         if command == "place":
             return self.execute_pick_and_place(PLACE_TARGET)
 
         return -1
 
+    def execute_pick(self, target: str) -> int:
+        """Method to execute the pick action"""
+        if target not in TasksManipulation.OBJECTS_DICT:
+            rospy.logerr("Object not found")
+            return TasksManipulation.STATE["EXECUTION_ERROR"]
+        return self.execute_pick_and_place( TasksManipulation.OBJECTS_DICT[target] )
+
     def execute_pick_and_place(self, target: int) -> int:
         """Method to call the pick and place action server"""
-        class ManipulationGoalScope:
-            object_ = target
-            result = False
-            result_received = False
-
+
         def manipulation_goal_feedback(feedback_msg):
             pass
-
-        def get_result_callback(state, result):
-            ManipulationGoalScope.result = result.result
-            ManipulationGoalScope.result_received = True
 
         rospy.loginfo("Sending Manipulation Goal")
 
         self.pick_client.send_goal(
-                    manipulationPickAndPlaceGoal(object_id = ManipulationGoalScope.object_),
+                    manipulationPickAndPlaceGoal(object_id = target),
                     feedback_cb=manipulation_goal_feedback,
-                    done_cb=get_result_callback)
-        
+            )
+
         rospy.loginfo(f"Object ID: {target}")
-
-        while not ManipulationGoalScope.result_received and not rospy.is_shutdown():
-            pass
-
-        return 1 if ManipulationGoalScope.result else -1
-
+        self.pick_client.wait_for_result()
+        result = self.pick_client.get_result()
+        return TasksManipulation.STATE["EXECUTION_SUCCESS"] if result.result else TasksManipulation.STATE["EXECUTION_ERROR"]
+
     def cancel_command(self) -> None:
         """Method to cancel the current command"""
         self.pick_client.cancel_all_goals()
@@ -78,5 +83,4 @@ def cancel_command(self) -> None:
     try:
         TasksManipulation()
     except rospy.ROSInterruptException as e:
-        rospy.logerr("Error: {}".format(e))
-        pass
+        rospy.logerr(f'Error: {e}')

ws/src/frida_task_manager/scripts/nav_tasks.py

@@ -11,89 +11,81 @@
 ### ROS messages
 from std_msgs.msg import String
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from geometry_msgs.msg import Pose
+from frida_navigation_interfaces.msg import navServAction, navServFeedback, navServGoal, navServResult
 
-NAV_SERVER = "/move_base"
-
-LOCATIONS = {
-    "office" : {
-        "table" : [
-            -0.7937252521514893,
-            1.0474066734313965,
-            0.0,
-            0.0,
-            0.0,
-            0.02197488636860073,
-            0.9997585230289798
-        ]
-    }
-}
+NAV_SERVER = "/navServer"
+MOVE_BASE_SERVER = "/move_base"
+LOCATION_TOPIC = "/robot_pose"
 
 class TasksNav:
     """Class to manage the navigation tasks"""
-    STATE_ENUM = {
-        "IDLE": 0,
-        "RECEIVE_COMMANDS": 1,
-        "EXECUTING_COMMANDS": 2,
-        "STOPPING": 3,
-        "ERROR": 4,
-        "SHUTDOWN": 5
+    STATE = {
+        "TERMINAL_ERROR": -1,
+        "EXECUTION_ERROR": 0,
+        "EXECUTION_SUCCESS": 1
     }
 
     AREA_TASKS = ["go", "follow", "stop", "approach", "remember"]
 
     def __init__(self) -> None:
-        self.nav_client = actionlib.SimpleActionClient(NAV_SERVER, MoveBaseAction)
+        self.nav_client = actionlib.SimpleActionClient(NAV_SERVER, navServAction)
+        self.move_base_client = actionlib.SimpleActionClient(MOVE_BASE_SERVER, MoveBaseAction)
         self.past_location = None
 
-        init_result = self.nav_client.wait_for_server(timeout=rospy.Duration(5.0))
-        if init_result:
-            rospy.loginfo("Nav Task Manager initialized")
-        else:
-            rospy.logerr("Go action not initialized")
+        if not self.nav_client.wait_for_server(timeout=rospy.Duration(5.0)):
+            rospy.logerr("Nav server not initialized")
+        if not self.move_base_client.wait_for_server(timeout=rospy.Duration(5.0)):
+            rospy.logerr("Move Base server not initialized")
 
     def execute_command(self, command: str, target: str, info: str) -> int:
         """Method to execute each command"""
         rospy.loginfo("Nav Command")
 
         if command == "go":
             return self.go_place(target)
+        if command == "remember":
+            return self.store_current_location()
 
-        return -1
+        return TasksNav.STATE["EXECUTION_ERROR"]
 
     def go_place(self, target: str) -> int:
         """Action to move the robot to a specific location"""
         if target == "past location":
-            return 1
-
-        split_target = target.split(" ")
-        try:
-            location_pose = None
-            for sublocation in split_target:
-                if location_pose is None:
-                    location_pose = LOCATIONS[sublocation]
-                else:
-                    location_pose = location_pose[sublocation]
-
+            if self.past_location is None:
+                rospy.logerr("No past location stored")
+                return TasksNav.STATE["EXECUTION_ERROR"]
             goal = MoveBaseGoal()
             goal.target_pose.header.frame_id = "map"
             goal.target_pose.header.stamp = rospy.Time.now()
-            goal.target_pose.pose.position.x = location_pose[0]
-            goal.target_pose.pose.position.y = location_pose[1]
-            goal.target_pose.pose.position.z = location_pose[2]
-            goal.target_pose.pose.orientation.x = location_pose[3]
-            goal.target_pose.pose.orientation.y = location_pose[4]
-            goal.target_pose.pose.orientation.z = location_pose[5]
-            goal.target_pose.pose.orientation.w = location_pose[6]
+            goal.target_pose.pose = self.past_location
+
+            self.move_base_client.send_goal(goal)
+            self.move_base_client.wait_for_result()
+            rospy.loginfo("Arrived at past location")
+            return TasksNav.STATE["EXECUTION_SUCCESS"]
+        # Move to room location
+        try:
+            goal = navServGoal()
+            goal.target_location = target
 
             self.nav_client.send_goal(goal)
             self.nav_client.wait_for_result()
-
-            return 1
-
-        except KeyError:
+            return TasksNav.STATE["EXECUTION_SUCCESS"]
+        except rospy.ROSException:
             rospy.logerr("Location not found")
-            return -1
-
+            return TasksNav.STATE["EXECUTION_ERROR"]
+
+    def store_current_location(self) -> int:
+        """Method to retrieve the current location of the robot"""
+        try:
+            self.past_location = rospy.wait_for_message(LOCATION_TOPIC, Pose, timeout=3.0)
+            rospy.loginfo("Current location stored")
+            return TasksNav.STATE["EXECUTION_SUCCESS"]
+        except rospy.ROSException:
+            rospy.logerr("Unable to store current location")
+            return TasksNav.STATE["EXECUTION_ERROR"]
+
     def cancel_command(self) -> None:
         """Method to cancel the current command"""
         self.nav_client.cancel_all_goals()
@@ -103,5 +95,4 @@ def cancel_command(self) -> None:
     try:
         TasksNav()
     except rospy.ROSInterruptException as e:
-        rospy.logerr("Error: {}".format(e))
-        pass
+        rospy.logerr(f'Error: {e}')

ws/src/frida_task_manager/scripts/task_manager_server.py

@@ -24,9 +24,9 @@
 SPEAK_TOPIC = "/speech/speak"
 CONVERSATION_SERVER = "/conversation_as"
 
-NAV_ENABLED = True
+NAV_ENABLED = False
 MANIPULATION_ENABLED = True
-CONVERSATION_ENABLED = True
+CONVERSATION_ENABLED = False
 VISION_ENABLED = False
 
 AREAS = ["nav", "manipulation", "hri", "vision"]
@@ -75,15 +75,11 @@ def __init__(self) -> None:
             #self.subtask_manager["vision"] = TasksVision()
 
         self.current_state = TaskManagerServer.STATE_ENUM["IDLE"]
+        self.current_past_state = None
         self.current_command = None
         self.current_queue = []
-        self.current_thread = None
-        self.current_location = None
-        self.past_location = None
-        self.grabbed_object = ""
         self.perceived_information = ""
 
-        #rospy.spin()
         self.run()
 
     def commands_callback(self, commands_input: CommandList) -> None:
@@ -106,7 +102,6 @@ def commands_callback(self, commands_input: CommandList) -> None:
         self.current_state = TaskManagerServer.STATE_ENUM["RECEIVE_COMMANDS"]
         self.current_queue = commands_input.commands
         self.past_state = self.current_state
-        self.past_location = self.current_location
 
         #self.say("I 'have finished my tasks, I'm going to rest now")
 
@@ -162,5 +157,4 @@ def run(self) -> None:
     try:
         TaskManagerServer()
     except rospy.ROSInterruptException as e:
-        rospy.logerr("Error: {}".format(e))
-        pass
+        rospy.logerr(f'Error: {e}')