diff --git a/navigation/packages/nav_main/CMakeLists.txt b/navigation/packages/nav_main/CMakeLists.txt index 876f0ed..7770a98 100644 --- a/navigation/packages/nav_main/CMakeLists.txt +++ b/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() diff --git a/navigation/packages/nav_main/action/Move.action b/navigation/packages/nav_main/action/Move.action new file mode 100644 index 0000000..fbf24fc --- /dev/null +++ b/navigation/packages/nav_main/action/Move.action @@ -0,0 +1,5 @@ +string location +--- +bool success +--- +string status \ No newline at end of file diff --git a/navigation/packages/nav_main/package.xml b/navigation/packages/nav_main/package.xml index aa62386..ae370ce 100644 --- a/navigation/packages/nav_main/package.xml +++ b/navigation/packages/nav_main/package.xml @@ -9,11 +9,20 @@ ament_cmake ament_cmake_python + + rosidl_default_generators rclcpp rclpy sensor_msgs + + action_msgs + + + rosidl_interface_packages + + ament_lint_auto ament_lint_common diff --git a/navigation/packages/nav_main/scripts/location_action.py b/navigation/packages/nav_main/scripts/location_action.py deleted file mode 100644 index e69de29..0000000 diff --git a/navigation/packages/nav_main/scripts/move_action_client.py b/navigation/packages/nav_main/scripts/move_action_client.py new file mode 100644 index 0000000..e99fc1d --- /dev/null +++ b/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() \ No newline at end of file diff --git a/navigation/packages/nav_main/scripts/move_action_server.py b/navigation/packages/nav_main/scripts/move_action_server.py new file mode 100644 index 0000000..dd2a6f2 --- /dev/null +++ b/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() \ No newline at end of file