action server move

navigation/packages/nav_main/CMakeLists.txt

@@ -12,6 +12,13 @@ find_package(rclcpp REQUIRED)
 find_package(rclpy REQUIRED)
 find_package(sensor_msgs REQUIRED)
 
+# Action server move
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "action/Move.action"
+)
+
 
 install(DIRECTORY launch
   DESTINATION share/${PROJECT_NAME})
@@ -41,6 +48,8 @@ if(BUILD_TESTING)
   # a copyright and license is added to all source files
   set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
+
 endif()
 
+
 ament_package()

navigation/packages/nav_main/action/Move.action

@@ -0,0 +1,5 @@
+string location
+---
+bool success
+---
+string status

navigation/packages/nav_main/package.xml

@@ -9,11 +9,20 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_python</buildtool_depend>
+  <!-- Action server move -->
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
   <depend>rclcpp</depend>
   <depend>rclpy</depend>
   <depend>sensor_msgs</depend>
 
+  <!-- Action server move -->
+  <depend>action_msgs</depend>
+
+  <!-- Action server move -->
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <!-- Action server move -->
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

navigation/packages/nav_main/scripts/move_action_client.py

@@ -0,0 +1,40 @@
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.node import Node
+
+from nav_main.action import Move
+
+
+class MoveActionClient(Node):
+
+    def __init__(self):
+        super().__init__('Move_action_client')
+        self._action_client = ActionClient(self, Move, 'move')
+
+    def send_goal(self, location):
+        goal_msg = Move.Goal()
+        goal_msg.location = location
+
+        self.get_logger().info(f'Sending goal to move to: {location}')
+        self._action_client.wait_for_server()
+        future = self._action_client.send_goal_async(goal_msg)
+        future.add_done_callback(self._on_result)
+
+    def _on_result(self, future):
+        result = future.result().result
+        self.get_logger().info(f"Result: {'Success' if result.success else 'Failed'}, Message: '{result.message}'")
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    action_client = MoveActionClient()
+
+    test_location = "Table"  
+
+    action_client.send_goal(test_location)
+
+    rclpy.spin(action_client)
+
+if __name__ == '__main__':
+    main()

navigation/packages/nav_main/scripts/move_action_server.py

@@ -0,0 +1,61 @@
+import rclpy
+from rclpy.action import ActionServer
+from rclpy.node import Node
+import json
+
+from nav_main.action import Move
+
+class MoveActionServer(Node):
+
+    def __init__(self):
+        super().__init__('move_action_server')
+
+         # Cargar locaciones desde un archivo JSON
+        with open('areas.json', 'r') as file:
+            self.locations = json.load(file)
+
+        self._action_server = ActionServer(
+            self,
+            Move,
+            'Move',
+            self.execute_callback)
+
+    def execute_callback(self, goal_handle):
+        self.get_logger().info('Moving...')
+        target_location = goal_handle.request.location
+
+        # Validar si la locación existe en el JSON
+        if not self.is_valid_location(target_location):
+            self.get_logger().info(f"Invalid location: {target_location}")
+            goal_handle.abort()
+            result = Move.Result()
+            result.success = False
+            result.message = f"Location '{target_location}' not available."
+            return result
+
+        goal_handle.succeed()
+
+        result = Move.Result()
+        result.success = True
+        result.message = f"Successfully moved to {target_location}"
+        return result
+
+
+    # Verificar si la locación solicitada existe en el JSON.
+    def is_valid_location(self, location):
+        for area, objects in self.locations.items():
+            if location in objects:
+                return True
+        return False
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    fibonacci_action_server = MoveActionServer()
+
+    rclpy.spin(fibonacci_action_server)
+
+
+if __name__ == '__main__':
+    main()