Final noetic nav version

ws/map.yaml

@@ -0,0 +1,7 @@
+image: map.pgm
+resolution: 0.050000
+origin: [-10.000000, -10.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

ws/src/actions/CMakeLists.txt

@@ -67,14 +67,24 @@ add_service_files(
   CreateGoal.srv
 )
 
+
+# ## Generate actions in the 'action' folder
+
 ## Generate actions in the 'action' folder
+
 #  add_action_files(
 #     DIRECTORY action
 #     FILES 
 #     navServ.action
-#     moveAction.action
+
 #   )
 
+# ## Generate added messages and services with any dependencies listed here
+#  generate_messages(
+#    DEPENDENCIES
+#    actionlib_msgs std_msgs geometry_msg
+#     moveAction.action
+#   )
 ## Generate added messages and services with any dependencies listed here
 #  generate_messages(
 #    DEPENDENCIES
@@ -113,7 +123,9 @@ add_service_files(
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES actionlib_tutorial
+
   CATKIN_DEPENDS actionlib actionlib_msgs move_base_msgs geometry_msgs roscpp rospy std_msgs message_runtime sensor_msgs
+
 #  DEPENDS system_lib
 )
 

ws/src/actions/action/navServ.action

@@ -1,11 +1,11 @@
-#goal definition
-uint8 NAV_MODE = 0
-uint8 FORWARD = 1
-uint8 BACKWARD = 2
-uint8 DOOR_SIGNAL = 3
+# #goal definition
+# uint8 NAV_MODE = 0
+# uint8 FORWARD = 1
+# uint8 BACKWARD = 2
+# uint8 DOOR_SIGNAL = 3
 
-string target_location
-uint8 goal_type
+# string target_location
+# uint8 goal_type
 geometry_msgs/PoseStamped target_pose
 ---
 #result definition

ws/src/actions/package.xml

@@ -52,6 +52,7 @@
   <build_depend>actionlib</build_depend>
   <build_depend>actionlib_msgs</build_depend>
   <build_depend>move_base_msgs</build_depend>
+  <build_depend>frida_navigation_interfaces</build_depend>
   <build_depend>message_generation</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>roscpp</build_depend>
@@ -61,6 +62,7 @@
   <build_export_depend>actionlib</build_export_depend>
   <build_export_depend>actionlib_msgs</build_export_depend>
   <build_export_depend>move_base_msgs</build_export_depend>
+  <build_export_depend>frida_navigation_interfaces</build_export_depend>
   <build_export_depend>geometry_msgs</build_export_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
@@ -69,6 +71,7 @@
   <exec_depend>actionlib</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>move_base_msgs</exec_depend>
+  <exec_depend>frida_navigation_interfaces</exec_depend>
   <exec_depend>actionlib_msgs</exec_depend>
   <exec_depend>message_generation</exec_depend>
   <exec_depend>roscpp</exec_depend>

ws/src/actions/scripts/goalCreatorClient.py

@@ -0,0 +1,54 @@
+#!/usr/bin/python3
+
+import rospy
+from frida_navigation_interfaces.srv import CreateGoal
+from geometry_msgs.msg import PoseStamped
+
+
+class GoalCreator:
+    def __init__(self) -> None:
+        self.goal_creator_service = rospy.ServiceProxy(
+            "/create_goal", CreateGoal
+        )
+        self.goal_creator_service.wait_for_service()
+
+        self.og_pub = rospy.Publisher("/og_goal", PoseStamped, queue_size=10)
+        self.pub = rospy.Publisher("/test_goal", PoseStamped, queue_size=10)
+
+        rospy.loginfo("Goal Creator Service is ready")
+
+    # Receive target pose and offset to goal
+    def call_service(self):
+        target = PoseStamped()
+        target.header.frame_id = "laser_frame"
+        target.pose.position.x = 0.0
+        target.pose.position.y = 0.0
+        target.pose.position.z = 0.0
+        target.pose.orientation.x = 0.0
+        target.pose.orientation.y = 0.0
+        target.pose.orientation.z = 0.0
+        target.pose.orientation.w = 1.0
+
+        self.og_pub.publish(target)
+
+        x_offset = 1.0
+        y_offset = 0.0
+
+        try:
+            response = self.goal_creator_service(target, x_offset, y_offset)
+            print(response)
+        except rospy.ServiceException as e:
+            print(f"Service call failed: {e}")
+            return
+
+        self.pub.publish(response.goal)
+
+if __name__ == "__main__":
+    rospy.init_node("goal_caller", anonymous=False)
+    goalCreator = GoalCreator()
+
+    while not rospy.is_shutdown():
+        goalCreator.call_service()
+        rospy.sleep(1)
+
+

ws/src/actions/scripts/navClient.py

@@ -53,7 +53,7 @@ def createMsg(self):
 
     def nav_goal(self, target):
         class NavGoalScope:
-            target_location = target
+            target_location = "kitchen table"
             result = False
             pose = PoseStamped()
 
@@ -70,7 +70,7 @@ def get_result_callback(state, result):
 
         rospy.loginfo("Sending Nav Goal")
         self.client.send_goal(
-                    navServGoal(target_location = NavGoalScope.target_location, goal_type = navServGoal.BACKWARD,  target_pose = NavGoalScope.pose),
+                    navServGoal(target_location = NavGoalScope.target_location),
                     feedback_cb=nav_goal_feedback,
                     done_cb=get_result_callback)
 

ws/src/actions/scripts/navServer.py

@@ -17,6 +17,7 @@
 import tf2_geometry_msgs
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
 from frida_navigation_interfaces.msg import navServAction, navServGoal, navServResult
+from frida_navigation_interfaces.srv import getLocations
 import tf2_ros
 from sklearn.linear_model import RANSACRegressor
 
@@ -80,8 +81,12 @@ def __init__(self, name):
 
         # Initialize Navigation Action Server
         self._as = actionlib.SimpleActionServer(self._action_name, navServAction, execute_cb=self.execute_cb, auto_start = False)
+        self.info_keeper = actionlib.SimpleActionServer("get_locations", getLocations, execute_cb=self.get_locations, auto_start=False)
         self._as.start()
 
+    def get_locations(self, goal):
+        rospy.loginfo("Getting locations")
+        self.info_keeper.set_succeeded(getLocations(locations=str(self.placesPoses)))
 
     def initPlaces(self):
         # create a dictionary of places and their poses using the json file which has the following format
@@ -109,30 +114,29 @@ def initPlaces(self):
     def execute_cb(self, goal):
         rospy.loginfo("[INFO] Executing goal")
         target = str(goal.target_location)
-        goal_type = goal.goal_type
         goal_pose = self.get_goal(target) if target != "" else goal.target_pose
         cmd_vel = Twist()
-        if (goal_type == goal.NAV_MODE):
+        if (True):
             if goal_pose is None:
                 rospy.loginfo("[ERROR] Invalid target")
                 self._as.set_succeeded(navServResult(result=False))
 
             self.send_goal(goal_pose)
             rospy.loginfo("[INFO] Robot Moving Towards " + target if target != "" else goal.target_pose)
             self._as.set_succeeded(navServResult(result=True))
-        elif (goal_type == goal.FORWARD):
+        elif (False):
             if goal_pose is None:
                 rospy.loginfo("[ERROR] Invalid target")
                 self._as.set_succeeded(navServResult(result=False))
 
             self.move_forward(cmd_vel, goal_pose)
             rospy.loginfo("[INFO] Robot approached " + target)
             self._as.set_succeeded(navServResult(result=self.success))
-        elif (goal_type == goal.BACKWARD):
+        elif (False):
             self.move_backward(cmd_vel)
             rospy.loginfo("[INFO] Robot moved backward")
             self._as.set_succeeded(navServResult(result=self.success))
-        elif (goal_type == goal.DOOR_SIGNAL):
+        elif (False):
             self.door_signal()
             rospy.loginfo("[INFO] Detected door signal")
             self._as.set_succeeded(navServResult(result=self.success))
@@ -142,7 +146,13 @@ def execute_cb(self, goal):
 
 
     def get_goal(self, target : str):
-        keys = target.split(" ")
+        print(f"quiere ir a : {target}")
+        if "living" in target:
+            keys = target.split(" ")
+            keys[0] = "living_room"
+            keys.pop(1)
+        print(keys)
+
 
         if (len(keys) <= 1):
             if (keys[0] in self.placesPoses):
@@ -156,7 +166,8 @@ def get_goal(self, target : str):
         elif (len(keys) <= 2 and keys[0] in self.placesPoses and keys[1] in self.placesPoses[keys[0]]):
             rospy.loginfo(f"Robot Moving Towards Safe Place POSE: {self.placesPoses[keys[0]][keys[1]]}")
             return self.placesPoses[keys[0]][keys[1]]
-
+        else:
+            return self.placesPoses[keys[0]]["safe_place"]
         rospy.loginfo("Invalid target")
         return None
 

ws/src/actions/scripts/roomGetterClient.py

@@ -0,0 +1,29 @@
+#! /usr/bin/env python3
+
+import rospy
+from geometry_msgs.msg import PointStamped
+from frida_navigation_interfaces.srv import RoomGetter
+
+
+def main():
+    rospy.init_node("room_getter_client")
+    rospy.loginfo("Room Getter client is ready")
+    rospy.wait_for_service('room_getter')
+    try:
+        room_getter = rospy.ServiceProxy('room_getter', RoomGetter)
+        goal = PointStamped()
+        x= -6.475536823272705
+        y= 4.268789291381836
+        z= 0.0057659149169921875
+        goal.point.x = x
+        goal.point.y = y
+        goal.point.z = 0
+        goal.header.frame_id = "map"
+        resp1 = room_getter(goal)
+        print(f"Room: {resp1.room}")
+    except rospy.ServiceException as e:
+        print("Service call failed: %s"%e)
+    rospy.spin()
+
+if __name__ == "__main__":
+    main()