From ed115998afe3f164949297d0fd9bccb3614e9b1d Mon Sep 17 00:00:00 2001 From: adri-mp Date: Fri, 26 Jan 2024 19:46:52 +0100 Subject: [PATCH] Repo limpiado --- .gitignore | 1 + ...s.out.tfevents.1706294254.madport.128292.0 | Bin 0 -> 88 bytes src/drone_driver/CMakeLists.txt | 8 - .../{src => include}/control_functions.py | 0 src/drone_driver/launch/car_launch.py | 109 --------- src/drone_driver/src/carExpertPilot.py | 215 ------------------ src/drone_driver/src/carNeuralPilot.py | 84 ------- src/drone_driver/src/droneExpertPilot.py | 10 +- src/drone_driver/src/image_filter_node.py | 2 +- src/drone_driver/src/recorder.py | 78 ------- .../rosbag2generalDataset.py} | 2 +- src/drone_driver/{include => src}/train.py | 11 +- 12 files changed, 8 insertions(+), 512 deletions(-) create mode 100644 runs/Jan26_19-37-34_madport/events.out.tfevents.1706294254.madport.128292.0 rename src/drone_driver/{src => include}/control_functions.py (100%) delete mode 100644 src/drone_driver/launch/car_launch.py delete mode 100755 src/drone_driver/src/carExpertPilot.py delete mode 100755 src/drone_driver/src/carNeuralPilot.py delete mode 100755 src/drone_driver/src/recorder.py rename src/drone_driver/{include/process_data.py => src/rosbag2generalDataset.py} (98%) rename src/drone_driver/{include => src}/train.py (88%) diff --git a/.gitignore b/.gitignore index 6d8b46b..498a22e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ src/drone_driver/dataset src/drone_driver/prueba +src/drone_driver/src/car diff --git a/runs/Jan26_19-37-34_madport/events.out.tfevents.1706294254.madport.128292.0 b/runs/Jan26_19-37-34_madport/events.out.tfevents.1706294254.madport.128292.0 new file mode 100644 index 0000000000000000000000000000000000000000..ed914ae1ada3ad1975fe2784846df463dba7ceb8 GIT binary patch literal 88 zcmeZZfPjCKJmzw`rfvNFFXyJC6mL>dVrHJ6YguYuiIq{19+yr@YF=@EQBrM*}QKepaQD#YMkzOiDReV}zPHH?vL@aW~YXDh_Ar1fl literal 0 HcmV?d00001 diff --git a/src/drone_driver/CMakeLists.txt b/src/drone_driver/CMakeLists.txt index 9381392..f6e5d3b 100644 --- a/src/drone_driver/CMakeLists.txt +++ b/src/drone_driver/CMakeLists.txt @@ -30,15 +30,7 @@ include_directories( #============= ## Install Python scripts -install( - PROGRAMS src/carExpertPilot.py - DESTINATION lib/${PROJECT_NAME} -) -install( - PROGRAMS src/carNeuralPilot.py - DESTINATION lib/${PROJECT_NAME} -) install( PROGRAMS src/parseWorld.py diff --git a/src/drone_driver/src/control_functions.py b/src/drone_driver/include/control_functions.py similarity index 100% rename from src/drone_driver/src/control_functions.py rename to src/drone_driver/include/control_functions.py diff --git a/src/drone_driver/launch/car_launch.py b/src/drone_driver/launch/car_launch.py deleted file mode 100644 index 018f2bc..0000000 --- a/src/drone_driver/launch/car_launch.py +++ /dev/null @@ -1,109 +0,0 @@ -#!/usr/bin/env python3 - -import os -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess -from launch.conditions import IfCondition, UnlessCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command, LaunchConfiguration, PythonExpression -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'simple_circuit.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - declare_use_simulator_cmd = DeclareLaunchArgument( - name='use_simulator', - default_value='True', - description='Whether to start the simulator') - - declare_world_cmd = DeclareLaunchArgument( - name='world', - default_value=world_path, - description='Full path to the world model file to load') - - # Specify the actions - - # Start Gazebo server - start_gazebo_server_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')), - condition=IfCondition(use_simulator), - launch_arguments={'world': world}.items()) - - # Start Gazebo client - start_gazebo_client_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')), - condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless]))) - - expertDriver = Node( - package='drone_driver', - executable='carExpertPilot.py', - output='screen', - arguments=[], - ) - - filterImage = Node( - package='drone_driver', - executable='image_filter_node.py', - # output='screen' - ) - - neuralDriver = Node( - package='drone_driver', - executable='carNeuralPilot.py', - output='screen' - ) - - bagRecorder = ExecuteProcess( - cmd=['ros2', 'bag', 'record', '/cmd_vel', '/cam_f1_left/image_raw', '-o', 'datasetBag'], - ) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - ld.add_action(start_gazebo_client_cmd) - - #ld.add_action(expertDriver) - #ld.add_action(bagRecorder) - - # ld.add_action(filterImage) - #ld.add_action(neuralDriver) - return ld \ No newline at end of file diff --git a/src/drone_driver/src/carExpertPilot.py b/src/drone_driver/src/carExpertPilot.py deleted file mode 100755 index c918042..0000000 --- a/src/drone_driver/src/carExpertPilot.py +++ /dev/null @@ -1,215 +0,0 @@ -#!/usr/bin/env python3 - -## Links used -# https://github.com/ros2/rosbag2 -# https://pytorch.org/tutorials/beginner/introyt/trainingyt.html - -import rclpy -from rclpy.node import Node - -from std_msgs.msg import String -from sensor_msgs.msg import Image -from geometry_msgs.msg import Twist - -import numpy as np -import cv2 -from cv_bridge import CvBridge - -MIN_PIXEL = -360 -MAX_PIXEL = 360 - -# Limit velocitys -MAX_ANGULAR = 4.5 # 5 Nürburgring line -MAX_LINEAR = 9 # 12 in some maps be fast -MIN_LINEAR = 1 - -# PID controlers -ANG_KP = 1 / 1.5 -ANG_KD = 1.6 / 1.5 -ANG_KI = 0.005 - -LIN_KP = 1 / 1.5 -LIN_KD = 1.4 / 1.5 -LIN_KI = 0.001 - -# Red filter parameters -RED_VALUE = 217 -GREEN_VALUE = 41 -BLUE_VALUE = 41 - -MAX_RANGE = 20 -MIN_RANGE = 10 - -# Trace parameters -TRACE_COLOR = [0, 255, 0] -RADIUS = 2 - -# Distance to search the red line end -LIMIT_UMBRAL = 15 - -class PID: - def __init__(self, min, max): - self.min = min - self.max = max - - self.prev_error = 0 - self.int_error = 0 - # Angular values as default - self.KP = ANG_KP - self.KD = ANG_KD - self.KI = ANG_KI - - def set_pid(self, kp, kd, ki): - self.KP = kp - self.KD = kd - self.KI = ki - - def get_pid(self, vel): - - if (vel <= self.min): - vel = self.min - if (vel >= self.max): - vel = self.max - - self.int_error = self.int_error + vel - dev_error = vel - self.prev_error - self.int_error += (self.int_error + vel) - - # Controls de integral value - if (self.int_error > self.max): - self.int_error = self.max - if self.int_error < self.min: - self.int_error = self.min - - self.prev_error = vel - - out = self.KP * vel + self.KI * self.int_error + self.KD * dev_error - - if (out > self.max): - out = self.max - if out < self.min: - out = self.min - - return out - -class carController(Node): - - def __init__(self): - super().__init__('f1_line_follow') - self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10) - self.filteredPublisher_ = self.create_publisher(Image, '/filtered_img', 100) - - self.imageSubscription = self.create_subscription(Image, '/cam_f1_left/image_raw', self.listener_callback, 10) - - self.angular_pid = PID(-MAX_ANGULAR, MAX_ANGULAR) - self.angular_pid.set_pid(ANG_KP, ANG_KD, ANG_KI) - - self.linear_pid = PID(MIN_LINEAR, MAX_LINEAR) - self.linear_pid.set_pid(LIN_KP, LIN_KD, LIN_KI) - - self.px_rang = MAX_PIXEL - MIN_PIXEL - self.ang_rang = MAX_ANGULAR - (- MAX_ANGULAR) - self.lin_rang = MAX_LINEAR - MIN_LINEAR - - timer_period = 0.1 # seconds - self.timer = self.create_timer(timer_period, self.timer_callback) - self.i = 0 - self.filteredImage = None - - def farest_point(self, image): - img_width = image.shape[1] - height_mid = int(image.shape[0] / 2) - - x = 0 - y = 0 - count = 0 - - for row in range (height_mid, height_mid + LIMIT_UMBRAL): - for col in range (img_width): - - comparison = image[row][col] == np.array([0, 0, 0]) - if not comparison.all(): - y += row - x += col - count += 1 - - if (count == 0): - return (0, 0) - - return [int(x / count), int(y / count)] - - def draw_traces(self, image): - img_width = image.shape[1] - img_height = image.shape[0] - - for row_index in range(img_height): - image[row_index][int(img_width / 2)] = np.array(TRACE_COLOR) - - def listener_callback(self, msg): - - bridge = CvBridge() - cv_image = bridge.imgmsg_to_cv2(msg, "bgr8") - - # Apply a red filter to the image - red_lower = np.array([0, 0, 100]) - red_upper = np.array([100, 100, 255]) - red_mask = cv2.inRange(cv_image, red_lower, red_upper) - self.filteredImage = cv2.bitwise_and(cv_image, cv_image, mask=red_mask) - - - def timer_callback(self): - if np.any(self.filteredImage): - width_center = self.filteredImage.shape[1] / 2 - red_farest = self.farest_point(self.filteredImage) - - # Comentar pruebas en linea recta y robustez añadida al programa - if red_farest[0] != 0 or red_farest[1] != 0: - - distance = width_center - red_farest[0] - # Pixel distance to angular vel transformation - angular = (((distance - MIN_PIXEL) * self.ang_rang) / self.px_rang) + (-MAX_ANGULAR) - angular_vel = self.angular_pid.get_pid(angular) - - # Inverse of angular vel and we convert it to linear velocity - angular_inv = MAX_ANGULAR - abs(angular_vel) # 0 - MAX_ANGULAR - linear = (((angular_inv) * self.lin_rang) / MAX_ANGULAR) + MIN_LINEAR - linear_vel = self.linear_pid.get_pid(linear) - - vel_msg = Twist() - vel_msg.linear.x = float(linear_vel) - vel_msg.linear.y = 0.0 - vel_msg.linear.z = 0.0 - vel_msg.angular.x = 0.0 - vel_msg.angular.y = 0.0 - vel_msg.angular.z = float(angular_vel) - - self.publisher_.publish(vel_msg) - - - img = CvBridge().cv2_to_imgmsg(self.filteredImage, "bgr8") - img.header.frame_id = "your_frame_id" # Set the frame ID here - self.filteredPublisher_.publish(img) - - # Traces and draw image - self.draw_traces(self.filteredImage) - cv2.circle(self.filteredImage, red_farest, RADIUS, TRACE_COLOR, RADIUS) - - cv2.imshow("Filtered image", self.filteredImage) - cv2.waitKey(1) - - - - -def main(args=None): - rclpy.init(args=args) - - minimal_publisher = carController() - - rclpy.spin(minimal_publisher) - - minimal_publisher.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/drone_driver/src/carNeuralPilot.py b/src/drone_driver/src/carNeuralPilot.py deleted file mode 100755 index 31b8c82..0000000 --- a/src/drone_driver/src/carNeuralPilot.py +++ /dev/null @@ -1,84 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node - -from std_msgs.msg import String -from sensor_msgs.msg import Image -from geometry_msgs.msg import Twist - -from cv_bridge import CvBridge -import ament_index_python -import torch -from torch.utils.data import DataLoader -import sys -from torchvision import transforms - - -package_path = ament_index_python.get_package_share_directory("drone_driver") -sys.path.append(package_path + "/include") -from models import pilotNet -from train import DATA_PATH, load_checkpoint -from data import dataset_transforms - - -# Limit velocitys -MAX_ANGULAR = 4.5 -MAX_LINEAR = 9 -MIN_LINEAR = 1 - -class carController(Node): - - def __init__(self): - super().__init__('f1_line_follow') - self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10) - - self.imageSubscription = self.create_subscription(Image, '/cam_f1_left/image_raw', self.listener_callback, 10) - self.img = None - - # Neural network - self.model = pilotNet() - load_checkpoint(self.model) - - self.device = torch.device("cuda:0") - self.model.to(self.device) - - - def listener_callback(self, msg): - bridge = CvBridge() - self.img = bridge.imgmsg_to_cv2(msg, "bgr8") - img_tensor = dataset_transforms(self.img).to(self.device) - img_tensor = img_tensor.unsqueeze(0) - - # Image inference - with torch.no_grad(): - predictions = self.model(img_tensor) - - # Velocity set - vel = predictions[0].tolist() - vel_msg = Twist() - vel_msg.linear.x = float(vel[0]) - vel_msg.linear.y = 0.0 - vel_msg.linear.z = 0.0 - vel_msg.angular.x = 0.0 - vel_msg.angular.y = 0.0 - vel_msg.angular.z = float(vel[1]) - - print(float(vel[0]), float(vel[1])) - - self.publisher_.publish(vel_msg) - - -def main(args=None): - rclpy.init(args=args) - - minimal_publisher = carController() - - rclpy.spin(minimal_publisher) - - minimal_publisher.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/drone_driver/src/droneExpertPilot.py b/src/drone_driver/src/droneExpertPilot.py index d36cf03..175f9da 100755 --- a/src/drone_driver/src/droneExpertPilot.py +++ b/src/drone_driver/src/droneExpertPilot.py @@ -2,22 +2,14 @@ import time import math -import argparse import rclpy from as2_python_api.drone_interface import DroneInterface from as2_msgs.msg._platform_status import PlatformStatus from as2_python_api.modules.motion_reference_handler_module import MotionReferenceHandlerModule -from as2_motion_reference_handlers.speed_motion import SpeedMotion -from geometry_msgs.msg import TwistStamped, PoseStamped -import matplotlib.pyplot as plt -from std_msgs.msg import Float32 - from sensor_msgs.msg import Image -import numpy as np -import cv2 from cv_bridge import CvBridge -from control_functions import PID, band_midpoint, search_top_line, search_bottom_line, save_timestamps, save_profiling +from ..include.control_functions import PID, band_midpoint, search_top_line, search_bottom_line, save_timestamps, save_profiling MIN_PIXEL = -360 MAX_PIXEL = 360 diff --git a/src/drone_driver/src/image_filter_node.py b/src/drone_driver/src/image_filter_node.py index 57d2f30..2abe394 100755 --- a/src/drone_driver/src/image_filter_node.py +++ b/src/drone_driver/src/image_filter_node.py @@ -13,7 +13,7 @@ import cv2 from cv_bridge import CvBridge -from control_functions import band_midpoint, search_top_line, search_bottom_line, save_profiling +from ..include.control_functions import band_midpoint, search_top_line, search_bottom_line, save_profiling from droneExpertPilot import LIMIT_UMBRAL diff --git a/src/drone_driver/src/recorder.py b/src/drone_driver/src/recorder.py deleted file mode 100755 index 95e6358..0000000 --- a/src/drone_driver/src/recorder.py +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSProfile, SensorDataQoS -from rclpy.executors import MultiThreadedExecutor -from sensor_msgs.msg import Image -from geometry_msgs.msg import Twist -from rosbag2_py import write_rosbag2 - - -class SyncAndRecordNode(Node): - def __init__(self): - super().__init__('sync_and_record_node') - - self.twist_subscription = self.create_subscription( - Twist, - '/drone0/motion_reference/twist', - self.twist_callback, - QoSProfile(history=1) - ) - - self.image_subscription = self.create_subscription( - Image, - '/drone0/sensor_measurements/frontal_camera/image_raw', - self.image_callback, - SensorDataQoS() - ) - - self.bag_filename = 'synced_data.db3' - self.bag_writer = write_rosbag2.PyRosbag2() - - def twist_callback(self, msg): - # Aquí puedes procesar o guardar los mensajes Twist según tus necesidades - print(f"Twist timestamp: {msg.header.stamp}") - - def image_callback(self, msg): - # Aquí puedes procesar o guardar los mensajes de imagen según tus necesidades - print(f"Image timestamp: {msg.header.stamp}") - - def save_to_rosbag(self): - # Abre la bolsa ROS para escritura - self.bag_writer.open(self.bag_filename, 'w') - - try: - # Inicia el bucle de eventos - rclpy.spin_once(self) - - except KeyboardInterrupt: - pass - - finally: - # Cierra la bolsa ROS después de procesar todos los mensajes - self.bag_writer.close() - - -def main(args=None): - rclpy.init(args=args) - - node = SyncAndRecordNode() - - executor = MultiThreadedExecutor(num_threads=2) - executor.add_node(node) - - try: - # Inicia el bucle principal - executor.spin() - - finally: - # Cierra todo limpiamente - executor.shutdown() - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() - diff --git a/src/drone_driver/include/process_data.py b/src/drone_driver/src/rosbag2generalDataset.py similarity index 98% rename from src/drone_driver/include/process_data.py rename to src/drone_driver/src/rosbag2generalDataset.py index 855dbcb..4e28fc8 100755 --- a/src/drone_driver/include/process_data.py +++ b/src/drone_driver/src/rosbag2generalDataset.py @@ -106,7 +106,7 @@ def transform_data(self, img_topic, vel_topic): ################################################################# -# Data analysis for training # +# Converts rosbags to general dataset # ################################################################# diff --git a/src/drone_driver/include/train.py b/src/drone_driver/src/train.py similarity index 88% rename from src/drone_driver/include/train.py rename to src/drone_driver/src/train.py index 0460671..360ac99 100755 --- a/src/drone_driver/include/train.py +++ b/src/drone_driver/src/train.py @@ -1,17 +1,11 @@ #!/usr/bin/env python3 -from itertools import islice from torch.utils.tensorboard import SummaryWriter from torch import nn import torch -from torch.serialization import save from torch.utils.data import DataLoader -from torchvision import datasets, transforms import torch.optim as optim import sys -from models import pilotNet -import keyboard -from drone_driver.include.process_data import rosbagDataset, dataset_transforms, DATA_PATH import ament_index_python writer = SummaryWriter() @@ -19,6 +13,9 @@ package_path = ament_index_python.get_package_share_directory("drone_driver") CHECKPOINT_PATH = package_path + "/utils/network.tar" +sys.path.append(package_path) +from ..include.models import pilotNet + def should_resume(): return "--resume" in sys.argv or "-r" in sys.argv @@ -46,7 +43,7 @@ def train(model: pilotNet, optimizer: optim.Optimizer): criterion = nn.MSELoss() - dataset = rosbagDataset(DATA_PATH, dataset_transforms) + # dataset = rosbagDataset(DATA_PATH, dataset_transforms) train_loader = DataLoader(dataset, batch_size=50, shuffle=True) for epoch in range(12):