add files

.gitmodules

@@ -0,0 +1,6 @@
+[submodule "src/px4_ros_com"]
+	path = src/px4_ros_com
+	url = https://github.com/PX4/px4_ros_com.git
+[submodule "src/px4_msgs"]
+	path = src/px4_msgs
+	url = https://github.com/PX4/px4_msgs.git

src/fase_1/fase_1/fase1_script.py

@@ -0,0 +1,179 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand, VehicleLocalPosition, VehicleStatus
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
+import cv2
+
+class OffboardControl(Node):
+    """Node for controlling a vehicle in offboard mode."""
+
+    def __init__(self) -> None:
+        super().__init__('offboard_control_takeoff_and_setpoints')
+
+        # Configure QoS profile for publishing and subscribing
+        qos_profile = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=1
+        )
+
+        # Create publishers
+        self.offboard_control_mode_publisher = self.create_publisher(
+            OffboardControlMode, '/fmu/in/offboard_control_mode', qos_profile)
+        self.trajectory_setpoint_publisher = self.create_publisher(
+            TrajectorySetpoint, '/fmu/in/trajectory_setpoint', qos_profile)
+        self.vehicle_command_publisher = self.create_publisher(
+            VehicleCommand, '/fmu/in/vehicle_command', qos_profile)
+
+        # Create subscribers
+        self.vehicle_local_position_subscriber = self.create_subscription(
+            VehicleLocalPosition, '/fmu/out/vehicle_local_position', self.vehicle_local_position_callback, qos_profile)
+        self.vehicle_status_subscriber = self.create_subscription(
+            VehicleStatus, '/fmu/out/vehicle_status', self.vehicle_status_callback, qos_profile)
+
+        # Subscriber for camera image
+        self.camera_subscription = self.create_subscription(
+            Image, '/x500_mono_cam/camera/image_raw', self.camera_callback, 10)
+
+        self.bridge = CvBridge()
+
+        # Initialize variables
+        self.offboard_setpoint_counter = 0
+        self.vehicle_local_position = VehicleLocalPosition()
+        self.vehicle_status = VehicleStatus()
+        self.takeoff_height = -5.0
+        self.setpoints = [
+            (0.0, 0.0, self.takeoff_height),  # Start at the origin
+            (-4.0, 0.0, self.takeoff_height),  # Move to (4, 0)
+            (-4.0, 4.0, self.takeoff_height),  # Move to (4, 4)
+            (0.0, -4.0, self.takeoff_height),  # Move to (0, 4)
+            (0.0, 0.0, self.takeoff_height)   # Return to the origin
+        ]
+        self.current_setpoint_index = 0
+
+        # Create a timer to publish control commands
+        self.timer = self.create_timer(0.1, self.timer_callback)
+
+    def vehicle_local_position_callback(self, vehicle_local_position):
+        """Callback function for vehicle_local_position topic subscriber."""
+        self.vehicle_local_position = vehicle_local_position
+        self.get_logger().info(f'Current position: x={vehicle_local_position.x}, y={vehicle_local_position.y}, z={vehicle_local_position.z}')
+
+    def vehicle_status_callback(self, vehicle_status):
+        """Callback function for vehicle_status topic subscriber."""
+        self.vehicle_status = vehicle_status
+
+    def camera_callback(self, msg):
+        """Callback function for camera image topic subscriber."""
+        cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+        cv2.imshow("Drone Camera", cv_image)
+        cv2.waitKey(1)
+
+    def arm(self):
+        """Send an arm command to the vehicle."""
+        self.publish_vehicle_command(
+            VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=1.0, param2=21196.0)
+        self.get_logger().info('Arm command sent')
+
+    def disarm(self):
+        """Send a disarm command to the vehicle."""
+        self.publish_vehicle_command(
+            VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=0.0)
+        self.get_logger().info('Disarm command sent')
+
+    def engage_offboard_mode(self):
+        """Switch to offboard mode."""
+        self.publish_vehicle_command(
+            VehicleCommand.VEHICLE_CMD_DO_SET_MODE, param1=1.0, param2=6.0)
+        self.get_logger().info("Switching to offboard mode")
+
+    def land(self):
+        """Switch to land mode."""
+        self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_LAND)
+        self.get_logger().info("Switching to land mode")
+
+    def publish_offboard_control_heartbeat_signal(self):
+        """Publish the offboard control mode."""
+        msg = OffboardControlMode()
+        msg.position = True
+        msg.velocity = False
+        msg.acceleration = False
+        msg.attitude = False
+        msg.body_rate = False
+        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
+        self.offboard_control_mode_publisher.publish(msg)
+
+    def publish_position_setpoint(self, x: float, y: float, z: float):
+        """Publish the trajectory setpoint."""
+        msg = TrajectorySetpoint()
+        msg.position = [x, y, z]
+        msg.yaw = 1.57079  # (90 degree)
+        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
+        self.trajectory_setpoint_publisher.publish(msg)
+        self.get_logger().info(f"Publishing position setpoint: {[x, y, z]}")
+
+    def publish_vehicle_command(self, command, **params) -> None:
+        """Publish a vehicle command."""
+        msg = VehicleCommand()
+        msg.command = command
+        msg.param1 = params.get("param1", 0.0)
+        msg.param2 = params.get("param2", 0.0)
+        msg.param3 = params.get("param3", 0.0)
+        msg.param4 = params.get("param4", 0.0)
+        msg.param5 = params.get("param5", 0.0)
+        msg.param6 = params.get("param6", 0.0)
+        msg.param7 = params.get("param7", 0.0)
+        msg.target_system = 1
+        msg.target_component = 1
+        msg.source_system = 1
+        msg.source_component = 1
+        msg.from_external = True
+        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
+        self.vehicle_command_publisher.publish(msg)
+
+    def timer_callback(self) -> None:
+        """Callback function for the timer."""
+        self.publish_offboard_control_heartbeat_signal()
+
+        if self.offboard_setpoint_counter == 10:
+            self.engage_offboard_mode()
+            self.arm()
+
+        if self.offboard_setpoint_counter > 10:
+            if self.current_setpoint_index < len(self.setpoints):
+                sp = self.setpoints[self.current_setpoint_index]
+                self.publish_position_setpoint(sp[0], sp[1], sp[2])
+                if (abs(self.vehicle_local_position.x - sp[0]) < 0.5 and
+                        abs(self.vehicle_local_position.y - sp[1]) < 0.5 and
+                        abs(self.vehicle_local_position.z - sp[2]) < 0.5):
+                    self.current_setpoint_index += 1
+                    self.get_logger().info(f'Moved to setpoint: {sp}')
+            else:
+                self.land()
+                self.disarm()
+                rclpy.shutdown()
+
+        if self.offboard_setpoint_counter < 11:
+            self.offboard_setpoint_counter += 1
+
+
+def main(args=None) -> None:
+    print('Starting offboard control node...')
+    rclpy.init(args=args)
+    offboard_control = OffboardControl()
+    rclpy.spin(offboard_control)
+    offboard_control.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    try:
+        main()
+    except Exception as e:
+        print(e)
+

src/fase_1/fase_1/imagem.py

@@ -0,0 +1,78 @@
+# Import the necessary libraries
+import rclpy  # Python library for ROS 2
+from rclpy.node import Node  # Handles the creation of nodes
+from sensor_msgs.msg import Image  # Image is the message type
+from cv_bridge import CvBridge  # Package to convert between ROS and OpenCV Images
+import cv2  # OpenCV library
+from ultralytics import YOLO  # YOLO library
+
+class ImageSubscriber(Node):
+    """
+    Create an ImageSubscriber class, which is a subclass of the Node class.
+    """
+    def __init__(self):
+        """
+        Class constructor to set up the node
+        """
+        # Initiate the Node class's constructor and give it a name
+        super().__init__('image_subscriber')
+
+        # Create the subscriber. This subscriber will receive an Image
+        # from the camera topic. The queue size is 10 messages.
+        self.subscription = self.create_subscription(
+            Image, 
+            'camera', 
+            self.listener_callback, 
+            10)
+        self.subscription  # prevent unused variable warning
+
+        # Used to convert between ROS and OpenCV images
+        self.br = CvBridge()
+
+        # Load the YOLO model
+        self.model = YOLO("colab_model.pt")
+
+    def listener_callback(self, data):
+        """
+        Callback function.
+        """
+        # Display the message on the console
+        self.get_logger().info('Receiving video frame')
+
+        # Convert ROS Image message to OpenCV image
+        current_frame = self.br.imgmsg_to_cv2(data, desired_encoding="bgr8")
+
+        # Resize the frame
+        resized_frame = cv2.resize(current_frame, (880, 680))  # Adjust the size as needed
+
+        # Run YOLO model on the frame
+        results = self.model(resized_frame, conf=0.9)
+
+        # Visualize the results on the frame
+        annotated_frame = results[0].plot()  # YOLOv8 automatically annotates the frame
+
+        # Show Results
+        cv2.imshow('Camera Feed', annotated_frame)
+        cv2.waitKey(1)
+
+def main(args=None):
+    # Initialize the rclpy library
+    rclpy.init(args=args)
+
+    # Create the node
+    image_subscriber = ImageSubscriber()
+
+    # Spin the node so the callback function is called.
+    rclpy.spin(image_subscriber)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    image_subscriber.destroy_node()
+
+    # Shutdown the ROS client library for Python
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
+

src/fase_1/fase_1/yolov8_colab.py

@@ -0,0 +1,6 @@
+from ultralytics import YOLO
+
+
+model = YOLO("/home/pedro/Fase1_CBR/colab_model.pt")
+
+results = model(source=0, show=True, conf=0.6, save=True)

src/fase_1/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>fase_1</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">matheus</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>px4_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/fase_1/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/fase_1
+[install]
+install_scripts=$base/lib/fase_1

src/fase_1/setup.py

@@ -0,0 +1,25 @@
+from setuptools import setup
+
+package_name = 'fase_1'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools', 'opencv-python', 'cv_bridge'],
+    zip_safe=True,
+    maintainer='Seu Nome',
+    maintainer_email='[email protected]',
+    description='Descrição do seu pacote fase_1',
+    license='Apache License 2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'fase1_script = fase_1.fase1_script:main',
+        ],
+    },
+)

src/fase_1/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/fase_1/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)