diff --git a/README.md b/README.md new file mode 100644 index 0000000..bb49b83 --- /dev/null +++ b/README.md @@ -0,0 +1,26 @@ +# Proyecto Roboracer con Librería Nav2 + +## Recorrido de circuito con Nav2 +El coche puede ser visualizado en rviz y al ejecutar el archivo se definen puntos para que siga una trayectoria. +```bash +#definir ruta del world a utilizar +ros2 launch my_bot launch_sim.launch.py world:=./src/my_bot/worlds/obstacles.world +rviz2 + +#definir la ruta del mapa que se prefiera usar +ros2 launch my_bot localization_launch.py map:=./src/my_bot/maps/levine.yaml use_sim_time:=true +ros2 launch my_bot navigation_launch.py use_sim_time:=true map_subscribe_transient_local:=true +``` +## Mapeo usando SLAM + +```bash +# ejecutar simulacion, definir mundo a usar +ros2 launch my_bot launch_sim.launch.py world:=./src/my_bot/worlds/obstacles.world +rviz2 + +# ejecutar SLAM +ros2 launch slam_toolbox online_async_launch.py params_file:=./src/my_bot/config/mapper_params_online_async.yaml use_sim_time:=true + +# mapear con teleop (eliminar lo que esta despues del 2do keyboard si hay problemas) +ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/diff_cont/cmd_vel_unstamped +``` diff --git a/controllers/controllers/__init__.py b/controllers/controllers/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/controllers/controllers/build/.built_by b/controllers/controllers/build/.built_by deleted file mode 100644 index 06e74ac..0000000 --- a/controllers/controllers/build/.built_by +++ /dev/null @@ -1 +0,0 @@ -colcon diff --git a/controllers/controllers/build/COLCON_IGNORE b/controllers/controllers/build/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/controllers/controllers/gap_node.py b/controllers/controllers/gap_node.py deleted file mode 100644 index 24a49e7..0000000 --- a/controllers/controllers/gap_node.py +++ /dev/null @@ -1,71 +0,0 @@ -import rclpy -from rclpy.node import Node - -import numpy as np -from sensor_msgs.msg import LaserScan -from ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive - -class ReactiveFollowGap(Node): - """ - Implement Wall Following on the car - This is just a template, you are free to implement your own node! - """ - def __init__(self): - super().__init__('reactive_node') - # Topics & Subs, Pubs - lidarscan_topic = '/scan' - drive_topic = '/drive' - - # TODO: Subscribe to LIDAR - # TODO: Publish to drive - - def preprocess_lidar(self, ranges): - """ Preprocess the LiDAR scan array. Expert implementation includes: - 1.Setting each value to the mean over some window - 2.Rejecting high values (eg. > 3m) - """ - proc_ranges = ranges - return proc_ranges - - def find_max_gap(self, free_space_ranges): - """ Return the start index & end index of the max gap in free_space_ranges - """ - return None - - def find_best_point(self, start_i, end_i, ranges): - """Start_i & end_i are start and end indicies of max-gap range, respectively - Return index of best point in ranges - Naive: Choose the furthest point within ranges and go there - """ - return None - - def lidar_callback(self, data): - """ Process each LiDAR scan as per the Follow Gap algorithm & publish an AckermannDriveStamped Message - """ - ranges = data.ranges - proc_ranges = self.preprocess_lidar(ranges) - - # TODO: - #Find closest point to LiDAR - - #Eliminate all points inside 'bubble' (set them to zero) - - #Find max length gap - - #Find the best point in the gap - - #Publish Drive message - - -def main(args=None): - rclpy.init(args=args) - print("WallFollow Initialized") - reactive_node = ReactiveFollowGap() - rclpy.spin(reactive_node) - - reactive_node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/controllers/controllers/graficas.py b/controllers/controllers/graficas.py deleted file mode 100644 index fb636f2..0000000 --- a/controllers/controllers/graficas.py +++ /dev/null @@ -1,124 +0,0 @@ -#!/usr/bin/env python3 -import rclpy -from rclpy.node import Node -from nav_msgs.msg import Odometry -from ackermann_msgs.msg import AckermannDriveStamped -from geometry_msgs.msg import Quaternion -import numpy as np -import matplotlib.pyplot as plt -import csv -import os -from datetime import datetime -import math - -def yaw_from_quat(q: Quaternion) -> float: - # Convierte quaternion a yaw (Z) en radianes - x, y, z, w = q.x, q.y, q.z, q.w - siny_cosp = 2.0 * (w*z + x*y) - cosy_cosp = 1.0 - 2.0 * (y*y + z*z) - return math.atan2(siny_cosp, cosy_cosp) - -class F1Logger(Node): - def __init__(self): - super().__init__('f1_logger') - - # Suscripciones - self.sub_odom = self.create_subscription(Odometry, '/ego_racecar/odom', self.on_odom, 10) - self.sub_drive = self.create_subscription(AckermannDriveStamped, '/drive', self.on_drive, 10) - - - # Buffers - self.t = [] - self.x = [] - self.y = [] - self.yaw = [] - self.v_linear = [] - self.steer = [] - - self.last_drive_msg = None - self.start_time = self.get_clock().now() - - # Parámetro opcional para nombre de salida - stamp = datetime.now().strftime('%Y%m%d_%H%M%S') - self.out_csv = f'f1tenth_run_{stamp}.csv' - self.get_logger().info('F1Logger listo. Grabando /odom y /drive... (Ctrl+C para terminar)') - - def now_secs(self): - return (self.get_clock().now() - self.start_time).nanoseconds * 1e-9 - - def on_odom(self, msg: Odometry): - t = self.now_secs() - p = msg.pose.pose.position - q = msg.pose.pose.orientation - yaw = yaw_from_quat(q) - - # Velocidad lineal aproximada desde odom (vector en base local) - vx = msg.twist.twist.linear.x - vy = msg.twist.twist.linear.y - v = math.hypot(vx, vy) - - # Último steering recibido (si hay) - steer = self.last_drive_msg.drive.steering_angle if self.last_drive_msg else float('nan') - - # Guardar - self.t.append(t) - self.x.append(p.x) - self.y.append(p.y) - self.yaw.append(yaw) - self.v_linear.append(v) - self.steer.append(steer) - - def on_drive(self, msg: AckermannDriveStamped): - self.last_drive_msg = msg - - def destroy_node(self): - # Guardar CSV - try: - with open(self.out_csv, 'w', newline='') as f: - w = csv.writer(f) - w.writerow(['t[s]','x[m]','y[m]','yaw[rad]','v[m/s]','steering[rad]']) - for i in range(len(self.t)): - w.writerow([self.t[i], self.x[i], self.y[i], self.yaw[i], self.v_linear[i], self.steer[i]]) - self.get_logger().info(f'CSV guardado: {os.path.abspath(self.out_csv)}') - except Exception as e: - self.get_logger().error(f'No se pudo guardar CSV: {e}') - - # Graficar - if len(self.t) > 5: - try: - fig1 = plt.figure(figsize=(6,6)) - plt.plot(self.x, self.y) - plt.axis('equal') - plt.xlabel('x [m]'); plt.ylabel('y [m]') - plt.title('Trayectoria (x,y)') - - fig2 = plt.figure(figsize=(7,4)) - plt.plot(self.t, self.steer) - plt.xlabel('tiempo [s]'); plt.ylabel('steering [rad]') - plt.title('Ángulo de dirección vs tiempo') - - fig3 = plt.figure(figsize=(7,4)) - plt.plot(self.t, self.v_linear) - plt.xlabel('tiempo [s]'); plt.ylabel('velocidad [m/s]') - plt.title('Velocidad lineal vs tiempo') - - plt.tight_layout() - plt.show() - except Exception as e: - self.get_logger().error(f'No se pudo graficar: {e}') - - super().destroy_node() - -def main(): - rclpy.init() - node = F1Logger() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/controllers/controllers/install/.colcon_install_layout b/controllers/controllers/install/.colcon_install_layout deleted file mode 100644 index 3aad533..0000000 --- a/controllers/controllers/install/.colcon_install_layout +++ /dev/null @@ -1 +0,0 @@ -isolated diff --git a/controllers/controllers/install/COLCON_IGNORE b/controllers/controllers/install/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/controllers/controllers/install/_local_setup_util_ps1.py b/controllers/controllers/install/_local_setup_util_ps1.py deleted file mode 100644 index 3c6d9e8..0000000 --- a/controllers/controllers/install/_local_setup_util_ps1.py +++ /dev/null @@ -1,407 +0,0 @@ -# Copyright 2016-2019 Dirk Thomas -# Licensed under the Apache License, Version 2.0 - -import argparse -from collections import OrderedDict -import os -from pathlib import Path -import sys - - -FORMAT_STR_COMMENT_LINE = '# {comment}' -FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"' -FORMAT_STR_USE_ENV_VAR = '$env:{name}' -FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' # noqa: E501 -FORMAT_STR_REMOVE_LEADING_SEPARATOR = '' # noqa: E501 -FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' # noqa: E501 - -DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' -DSV_TYPE_SET = 'set' -DSV_TYPE_SET_IF_UNSET = 'set-if-unset' -DSV_TYPE_SOURCE = 'source' - - -def main(argv=sys.argv[1:]): # noqa: D103 - parser = argparse.ArgumentParser( - description='Output shell commands for the packages in topological ' - 'order') - parser.add_argument( - 'primary_extension', - help='The file extension of the primary shell') - parser.add_argument( - 'additional_extension', nargs='?', - help='The additional file extension to be considered') - parser.add_argument( - '--merged-install', action='store_true', - help='All install prefixes are merged into a single location') - args = parser.parse_args(argv) - - packages = get_packages(Path(__file__).parent, args.merged_install) - - ordered_packages = order_packages(packages) - for pkg_name in ordered_packages: - if _include_comments(): - print( - FORMAT_STR_COMMENT_LINE.format_map( - {'comment': 'Package: ' + pkg_name})) - prefix = os.path.abspath(os.path.dirname(__file__)) - if not args.merged_install: - prefix = os.path.join(prefix, pkg_name) - for line in get_commands( - pkg_name, prefix, args.primary_extension, - args.additional_extension - ): - print(line) - - for line in _remove_ending_separators(): - print(line) - - -def get_packages(prefix_path, merged_install): - """ - Find packages based on colcon-specific files created during installation. - - :param Path prefix_path: The install prefix path of all packages - :param bool merged_install: The flag if the packages are all installed - directly in the prefix or if each package is installed in a subdirectory - named after the package - :returns: A mapping from the package name to the set of runtime - dependencies - :rtype: dict - """ - packages = {} - # since importing colcon_core isn't feasible here the following constant - # must match colcon_core.location.get_relative_package_index_path() - subdirectory = 'share/colcon-core/packages' - if merged_install: - # return if workspace is empty - if not (prefix_path / subdirectory).is_dir(): - return packages - # find all files in the subdirectory - for p in (prefix_path / subdirectory).iterdir(): - if not p.is_file(): - continue - if p.name.startswith('.'): - continue - add_package_runtime_dependencies(p, packages) - else: - # for each subdirectory look for the package specific file - for p in prefix_path.iterdir(): - if not p.is_dir(): - continue - if p.name.startswith('.'): - continue - p = p / subdirectory / p.name - if p.is_file(): - add_package_runtime_dependencies(p, packages) - - # remove unknown dependencies - pkg_names = set(packages.keys()) - for k in packages.keys(): - packages[k] = {d for d in packages[k] if d in pkg_names} - - return packages - - -def add_package_runtime_dependencies(path, packages): - """ - Check the path and if it exists extract the packages runtime dependencies. - - :param Path path: The resource file containing the runtime dependencies - :param dict packages: A mapping from package names to the sets of runtime - dependencies to add to - """ - content = path.read_text() - dependencies = set(content.split(os.pathsep) if content else []) - packages[path.name] = dependencies - - -def order_packages(packages): - """ - Order packages topologically. - - :param dict packages: A mapping from package name to the set of runtime - dependencies - :returns: The package names - :rtype: list - """ - # select packages with no dependencies in alphabetical order - to_be_ordered = list(packages.keys()) - ordered = [] - while to_be_ordered: - pkg_names_without_deps = [ - name for name in to_be_ordered if not packages[name]] - if not pkg_names_without_deps: - reduce_cycle_set(packages) - raise RuntimeError( - 'Circular dependency between: ' + ', '.join(sorted(packages))) - pkg_names_without_deps.sort() - pkg_name = pkg_names_without_deps[0] - to_be_ordered.remove(pkg_name) - ordered.append(pkg_name) - # remove item from dependency lists - for k in list(packages.keys()): - if pkg_name in packages[k]: - packages[k].remove(pkg_name) - return ordered - - -def reduce_cycle_set(packages): - """ - Reduce the set of packages to the ones part of the circular dependency. - - :param dict packages: A mapping from package name to the set of runtime - dependencies which is modified in place - """ - last_depended = None - while len(packages) > 0: - # get all remaining dependencies - depended = set() - for pkg_name, dependencies in packages.items(): - depended = depended.union(dependencies) - # remove all packages which are not dependent on - for name in list(packages.keys()): - if name not in depended: - del packages[name] - if last_depended: - # if remaining packages haven't changed return them - if last_depended == depended: - return packages.keys() - # otherwise reduce again - last_depended = depended - - -def _include_comments(): - # skipping comment lines when COLCON_TRACE is not set speeds up the - # processing especially on Windows - return bool(os.environ.get('COLCON_TRACE')) - - -def get_commands(pkg_name, prefix, primary_extension, additional_extension): - commands = [] - package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') - if os.path.exists(package_dsv_path): - commands += process_dsv_file( - package_dsv_path, prefix, primary_extension, additional_extension) - return commands - - -def process_dsv_file( - dsv_path, prefix, primary_extension=None, additional_extension=None -): - commands = [] - if _include_comments(): - commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) - with open(dsv_path, 'r') as h: - content = h.read() - lines = content.splitlines() - - basenames = OrderedDict() - for i, line in enumerate(lines): - # skip over empty or whitespace-only lines - if not line.strip(): - continue - # skip over comments - if line.startswith('#'): - continue - try: - type_, remainder = line.split(';', 1) - except ValueError: - raise RuntimeError( - "Line %d in '%s' doesn't contain a semicolon separating the " - 'type from the arguments' % (i + 1, dsv_path)) - if type_ != DSV_TYPE_SOURCE: - # handle non-source lines - try: - commands += handle_dsv_types_except_source( - type_, remainder, prefix) - except RuntimeError as e: - raise RuntimeError( - "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e - else: - # group remaining source lines by basename - path_without_ext, ext = os.path.splitext(remainder) - if path_without_ext not in basenames: - basenames[path_without_ext] = set() - assert ext.startswith('.') - ext = ext[1:] - if ext in (primary_extension, additional_extension): - basenames[path_without_ext].add(ext) - - # add the dsv extension to each basename if the file exists - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if os.path.exists(basename + '.dsv'): - extensions.add('dsv') - - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if 'dsv' in extensions: - # process dsv files recursively - commands += process_dsv_file( - basename + '.dsv', prefix, primary_extension=primary_extension, - additional_extension=additional_extension) - elif primary_extension in extensions and len(extensions) == 1: - # source primary-only files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + primary_extension})] - elif additional_extension in extensions: - # source non-primary files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + additional_extension})] - - return commands - - -def handle_dsv_types_except_source(type_, remainder, prefix): - commands = [] - if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): - try: - env_name, value = remainder.split(';', 1) - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the value') - try_prefixed_value = os.path.join(prefix, value) if value else prefix - if os.path.exists(try_prefixed_value): - value = try_prefixed_value - if type_ == DSV_TYPE_SET: - commands += _set(env_name, value) - elif type_ == DSV_TYPE_SET_IF_UNSET: - commands += _set_if_unset(env_name, value) - else: - assert False - elif type_ in ( - DSV_TYPE_APPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS - ): - try: - env_name_and_values = remainder.split(';') - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the values') - env_name = env_name_and_values[0] - values = env_name_and_values[1:] - for value in values: - if not value: - value = prefix - elif not os.path.isabs(value): - value = os.path.join(prefix, value) - if ( - type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and - not os.path.exists(value) - ): - comment = f'skip extending {env_name} with not existing ' \ - f'path: {value}' - if _include_comments(): - commands.append( - FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) - elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: - commands += _append_unique_value(env_name, value) - else: - commands += _prepend_unique_value(env_name, value) - else: - raise RuntimeError( - 'contains an unknown environment hook type: ' + type_) - return commands - - -env_state = {} - - -def _append_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # append even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional leading separator - extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': extend + value}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -def _prepend_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # prepend even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional trailing separator - extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value + extend}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -# generate commands for removing prepended underscores -def _remove_ending_separators(): - # do nothing if the shell extension does not implement the logic - if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: - return [] - - global env_state - commands = [] - for name in env_state: - # skip variables that already had values before this script started prepending - if name in os.environ: - continue - commands += [ - FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), - FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] - return commands - - -def _set(name, value): - global env_state - env_state[name] = value - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - return [line] - - -def _set_if_unset(name, value): - global env_state - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - if env_state.get(name, os.environ.get(name)): - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -if __name__ == '__main__': # pragma: no cover - try: - rc = main() - except RuntimeError as e: - print(str(e), file=sys.stderr) - rc = 1 - sys.exit(rc) diff --git a/controllers/controllers/install/_local_setup_util_sh.py b/controllers/controllers/install/_local_setup_util_sh.py deleted file mode 100644 index f67eaa9..0000000 --- a/controllers/controllers/install/_local_setup_util_sh.py +++ /dev/null @@ -1,407 +0,0 @@ -# Copyright 2016-2019 Dirk Thomas -# Licensed under the Apache License, Version 2.0 - -import argparse -from collections import OrderedDict -import os -from pathlib import Path -import sys - - -FORMAT_STR_COMMENT_LINE = '# {comment}' -FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"' -FORMAT_STR_USE_ENV_VAR = '${name}' -FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' # noqa: E501 -FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' # noqa: E501 -FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' # noqa: E501 - -DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' -DSV_TYPE_SET = 'set' -DSV_TYPE_SET_IF_UNSET = 'set-if-unset' -DSV_TYPE_SOURCE = 'source' - - -def main(argv=sys.argv[1:]): # noqa: D103 - parser = argparse.ArgumentParser( - description='Output shell commands for the packages in topological ' - 'order') - parser.add_argument( - 'primary_extension', - help='The file extension of the primary shell') - parser.add_argument( - 'additional_extension', nargs='?', - help='The additional file extension to be considered') - parser.add_argument( - '--merged-install', action='store_true', - help='All install prefixes are merged into a single location') - args = parser.parse_args(argv) - - packages = get_packages(Path(__file__).parent, args.merged_install) - - ordered_packages = order_packages(packages) - for pkg_name in ordered_packages: - if _include_comments(): - print( - FORMAT_STR_COMMENT_LINE.format_map( - {'comment': 'Package: ' + pkg_name})) - prefix = os.path.abspath(os.path.dirname(__file__)) - if not args.merged_install: - prefix = os.path.join(prefix, pkg_name) - for line in get_commands( - pkg_name, prefix, args.primary_extension, - args.additional_extension - ): - print(line) - - for line in _remove_ending_separators(): - print(line) - - -def get_packages(prefix_path, merged_install): - """ - Find packages based on colcon-specific files created during installation. - - :param Path prefix_path: The install prefix path of all packages - :param bool merged_install: The flag if the packages are all installed - directly in the prefix or if each package is installed in a subdirectory - named after the package - :returns: A mapping from the package name to the set of runtime - dependencies - :rtype: dict - """ - packages = {} - # since importing colcon_core isn't feasible here the following constant - # must match colcon_core.location.get_relative_package_index_path() - subdirectory = 'share/colcon-core/packages' - if merged_install: - # return if workspace is empty - if not (prefix_path / subdirectory).is_dir(): - return packages - # find all files in the subdirectory - for p in (prefix_path / subdirectory).iterdir(): - if not p.is_file(): - continue - if p.name.startswith('.'): - continue - add_package_runtime_dependencies(p, packages) - else: - # for each subdirectory look for the package specific file - for p in prefix_path.iterdir(): - if not p.is_dir(): - continue - if p.name.startswith('.'): - continue - p = p / subdirectory / p.name - if p.is_file(): - add_package_runtime_dependencies(p, packages) - - # remove unknown dependencies - pkg_names = set(packages.keys()) - for k in packages.keys(): - packages[k] = {d for d in packages[k] if d in pkg_names} - - return packages - - -def add_package_runtime_dependencies(path, packages): - """ - Check the path and if it exists extract the packages runtime dependencies. - - :param Path path: The resource file containing the runtime dependencies - :param dict packages: A mapping from package names to the sets of runtime - dependencies to add to - """ - content = path.read_text() - dependencies = set(content.split(os.pathsep) if content else []) - packages[path.name] = dependencies - - -def order_packages(packages): - """ - Order packages topologically. - - :param dict packages: A mapping from package name to the set of runtime - dependencies - :returns: The package names - :rtype: list - """ - # select packages with no dependencies in alphabetical order - to_be_ordered = list(packages.keys()) - ordered = [] - while to_be_ordered: - pkg_names_without_deps = [ - name for name in to_be_ordered if not packages[name]] - if not pkg_names_without_deps: - reduce_cycle_set(packages) - raise RuntimeError( - 'Circular dependency between: ' + ', '.join(sorted(packages))) - pkg_names_without_deps.sort() - pkg_name = pkg_names_without_deps[0] - to_be_ordered.remove(pkg_name) - ordered.append(pkg_name) - # remove item from dependency lists - for k in list(packages.keys()): - if pkg_name in packages[k]: - packages[k].remove(pkg_name) - return ordered - - -def reduce_cycle_set(packages): - """ - Reduce the set of packages to the ones part of the circular dependency. - - :param dict packages: A mapping from package name to the set of runtime - dependencies which is modified in place - """ - last_depended = None - while len(packages) > 0: - # get all remaining dependencies - depended = set() - for pkg_name, dependencies in packages.items(): - depended = depended.union(dependencies) - # remove all packages which are not dependent on - for name in list(packages.keys()): - if name not in depended: - del packages[name] - if last_depended: - # if remaining packages haven't changed return them - if last_depended == depended: - return packages.keys() - # otherwise reduce again - last_depended = depended - - -def _include_comments(): - # skipping comment lines when COLCON_TRACE is not set speeds up the - # processing especially on Windows - return bool(os.environ.get('COLCON_TRACE')) - - -def get_commands(pkg_name, prefix, primary_extension, additional_extension): - commands = [] - package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') - if os.path.exists(package_dsv_path): - commands += process_dsv_file( - package_dsv_path, prefix, primary_extension, additional_extension) - return commands - - -def process_dsv_file( - dsv_path, prefix, primary_extension=None, additional_extension=None -): - commands = [] - if _include_comments(): - commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) - with open(dsv_path, 'r') as h: - content = h.read() - lines = content.splitlines() - - basenames = OrderedDict() - for i, line in enumerate(lines): - # skip over empty or whitespace-only lines - if not line.strip(): - continue - # skip over comments - if line.startswith('#'): - continue - try: - type_, remainder = line.split(';', 1) - except ValueError: - raise RuntimeError( - "Line %d in '%s' doesn't contain a semicolon separating the " - 'type from the arguments' % (i + 1, dsv_path)) - if type_ != DSV_TYPE_SOURCE: - # handle non-source lines - try: - commands += handle_dsv_types_except_source( - type_, remainder, prefix) - except RuntimeError as e: - raise RuntimeError( - "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e - else: - # group remaining source lines by basename - path_without_ext, ext = os.path.splitext(remainder) - if path_without_ext not in basenames: - basenames[path_without_ext] = set() - assert ext.startswith('.') - ext = ext[1:] - if ext in (primary_extension, additional_extension): - basenames[path_without_ext].add(ext) - - # add the dsv extension to each basename if the file exists - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if os.path.exists(basename + '.dsv'): - extensions.add('dsv') - - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if 'dsv' in extensions: - # process dsv files recursively - commands += process_dsv_file( - basename + '.dsv', prefix, primary_extension=primary_extension, - additional_extension=additional_extension) - elif primary_extension in extensions and len(extensions) == 1: - # source primary-only files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + primary_extension})] - elif additional_extension in extensions: - # source non-primary files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + additional_extension})] - - return commands - - -def handle_dsv_types_except_source(type_, remainder, prefix): - commands = [] - if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): - try: - env_name, value = remainder.split(';', 1) - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the value') - try_prefixed_value = os.path.join(prefix, value) if value else prefix - if os.path.exists(try_prefixed_value): - value = try_prefixed_value - if type_ == DSV_TYPE_SET: - commands += _set(env_name, value) - elif type_ == DSV_TYPE_SET_IF_UNSET: - commands += _set_if_unset(env_name, value) - else: - assert False - elif type_ in ( - DSV_TYPE_APPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS - ): - try: - env_name_and_values = remainder.split(';') - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the values') - env_name = env_name_and_values[0] - values = env_name_and_values[1:] - for value in values: - if not value: - value = prefix - elif not os.path.isabs(value): - value = os.path.join(prefix, value) - if ( - type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and - not os.path.exists(value) - ): - comment = f'skip extending {env_name} with not existing ' \ - f'path: {value}' - if _include_comments(): - commands.append( - FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) - elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: - commands += _append_unique_value(env_name, value) - else: - commands += _prepend_unique_value(env_name, value) - else: - raise RuntimeError( - 'contains an unknown environment hook type: ' + type_) - return commands - - -env_state = {} - - -def _append_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # append even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional leading separator - extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': extend + value}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -def _prepend_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # prepend even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional trailing separator - extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value + extend}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -# generate commands for removing prepended underscores -def _remove_ending_separators(): - # do nothing if the shell extension does not implement the logic - if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: - return [] - - global env_state - commands = [] - for name in env_state: - # skip variables that already had values before this script started prepending - if name in os.environ: - continue - commands += [ - FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), - FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] - return commands - - -def _set(name, value): - global env_state - env_state[name] = value - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - return [line] - - -def _set_if_unset(name, value): - global env_state - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - if env_state.get(name, os.environ.get(name)): - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -if __name__ == '__main__': # pragma: no cover - try: - rc = main() - except RuntimeError as e: - print(str(e), file=sys.stderr) - rc = 1 - sys.exit(rc) diff --git a/controllers/controllers/install/local_setup.bash b/controllers/controllers/install/local_setup.bash deleted file mode 100644 index 03f0025..0000000 --- a/controllers/controllers/install/local_setup.bash +++ /dev/null @@ -1,121 +0,0 @@ -# generated from colcon_bash/shell/template/prefix.bash.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# a bash script is able to determine its own path if necessary -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" -else - _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prefix_bash_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prefix_bash_prepend_unique_value_IFS="$IFS" - IFS=":" - # start with the new value - _all_values="$_value" - _contained_value="" - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - _contained_value=1 - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - if [ -z "$_contained_value" ]; then - if [ -n "$COLCON_TRACE" ]; then - if [ "$_all_values" = "$_value" ]; then - echo "export $_listname=$_value" - else - echo "export $_listname=$_value:\$$_listname" - fi - fi - fi - unset _contained_value - # restore the field separator - IFS="$_colcon_prefix_bash_prepend_unique_value_IFS" - unset _colcon_prefix_bash_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# add this prefix to the COLCON_PREFIX_PATH -_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX" -unset _colcon_prefix_bash_prepend_unique_value - -# check environment variable for custom Python executable -if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then - if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then - echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" - return 1 - fi - _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" -else - # try the Python executable known at configure time - _colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if [ ! -f "$_colcon_python_executable" ]; then - if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then - echo "error: unable to find python3 executable" - return 1 - fi - _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` - fi -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# get all commands in topological order -_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)" -unset _colcon_python_executable -if [ -n "$COLCON_TRACE" ]; then - echo "$(declare -f _colcon_prefix_sh_source_script)" - echo "# Execute generated script:" - echo "# <<<" - echo "${_colcon_ordered_commands}" - echo "# >>>" - echo "unset _colcon_prefix_sh_source_script" -fi -eval "${_colcon_ordered_commands}" -unset _colcon_ordered_commands - -unset _colcon_prefix_sh_source_script - -unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX diff --git a/controllers/controllers/install/local_setup.ps1 b/controllers/controllers/install/local_setup.ps1 deleted file mode 100644 index 6f68c8d..0000000 --- a/controllers/controllers/install/local_setup.ps1 +++ /dev/null @@ -1,55 +0,0 @@ -# generated from colcon_powershell/shell/template/prefix.ps1.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# check environment variable for custom Python executable -if ($env:COLCON_PYTHON_EXECUTABLE) { - if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) { - echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist" - exit 1 - } - $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE" -} else { - # use the Python executable known at configure time - $_colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) { - if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) { - echo "error: unable to find python3 executable" - exit 1 - } - $_colcon_python_executable="python3" - } -} - -# function to source another script with conditional trace output -# first argument: the path of the script -function _colcon_prefix_powershell_source_script { - param ( - $_colcon_prefix_powershell_source_script_param - ) - # source script with conditional trace output - if (Test-Path $_colcon_prefix_powershell_source_script_param) { - if ($env:COLCON_TRACE) { - echo ". '$_colcon_prefix_powershell_source_script_param'" - } - . "$_colcon_prefix_powershell_source_script_param" - } else { - Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'" - } -} - -# get all commands in topological order -$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1 - -# execute all commands in topological order -if ($env:COLCON_TRACE) { - echo "Execute generated script:" - echo "<<<" - $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output - echo ">>>" -} -if ($_colcon_ordered_commands) { - $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression -} diff --git a/controllers/controllers/install/local_setup.sh b/controllers/controllers/install/local_setup.sh deleted file mode 100644 index 565d0b4..0000000 --- a/controllers/controllers/install/local_setup.sh +++ /dev/null @@ -1,137 +0,0 @@ -# generated from colcon_core/shell/template/prefix.sh.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# since a plain shell script can't determine its own path when being sourced -# either use the provided COLCON_CURRENT_PREFIX -# or fall back to the build time prefix (if it exists) -_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/home/maria/F1Tenth-Repository/src/controllers/controllers/install" -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then - echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 - unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX - return 1 - fi -else - _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prefix_sh_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prefix_sh_prepend_unique_value_IFS="$IFS" - IFS=":" - # start with the new value - _all_values="$_value" - _contained_value="" - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - _contained_value=1 - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - if [ -z "$_contained_value" ]; then - if [ -n "$COLCON_TRACE" ]; then - if [ "$_all_values" = "$_value" ]; then - echo "export $_listname=$_value" - else - echo "export $_listname=$_value:\$$_listname" - fi - fi - fi - unset _contained_value - # restore the field separator - IFS="$_colcon_prefix_sh_prepend_unique_value_IFS" - unset _colcon_prefix_sh_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# add this prefix to the COLCON_PREFIX_PATH -_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" -unset _colcon_prefix_sh_prepend_unique_value - -# check environment variable for custom Python executable -if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then - if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then - echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" - return 1 - fi - _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" -else - # try the Python executable known at configure time - _colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if [ ! -f "$_colcon_python_executable" ]; then - if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then - echo "error: unable to find python3 executable" - return 1 - fi - _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` - fi -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# get all commands in topological order -_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)" -unset _colcon_python_executable -if [ -n "$COLCON_TRACE" ]; then - echo "_colcon_prefix_sh_source_script() { - if [ -f \"\$1\" ]; then - if [ -n \"\$COLCON_TRACE\" ]; then - echo \"# . \\\"\$1\\\"\" - fi - . \"\$1\" - else - echo \"not found: \\\"\$1\\\"\" 1>&2 - fi - }" - echo "# Execute generated script:" - echo "# <<<" - echo "${_colcon_ordered_commands}" - echo "# >>>" - echo "unset _colcon_prefix_sh_source_script" -fi -eval "${_colcon_ordered_commands}" -unset _colcon_ordered_commands - -unset _colcon_prefix_sh_source_script - -unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX diff --git a/controllers/controllers/install/local_setup.zsh b/controllers/controllers/install/local_setup.zsh deleted file mode 100644 index b648710..0000000 --- a/controllers/controllers/install/local_setup.zsh +++ /dev/null @@ -1,134 +0,0 @@ -# generated from colcon_zsh/shell/template/prefix.zsh.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# a zsh script is able to determine its own path if necessary -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" -else - _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to convert array-like strings into arrays -# to workaround SH_WORD_SPLIT not being set -_colcon_prefix_zsh_convert_to_array() { - local _listname=$1 - local _dollar="$" - local _split="{=" - local _to_array="(\"$_dollar$_split$_listname}\")" - eval $_listname=$_to_array -} - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prefix_zsh_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS" - IFS=":" - # start with the new value - _all_values="$_value" - _contained_value="" - # workaround SH_WORD_SPLIT not being set - _colcon_prefix_zsh_convert_to_array _values - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - _contained_value=1 - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - if [ -z "$_contained_value" ]; then - if [ -n "$COLCON_TRACE" ]; then - if [ "$_all_values" = "$_value" ]; then - echo "export $_listname=$_value" - else - echo "export $_listname=$_value:\$$_listname" - fi - fi - fi - unset _contained_value - # restore the field separator - IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS" - unset _colcon_prefix_zsh_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# add this prefix to the COLCON_PREFIX_PATH -_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX" -unset _colcon_prefix_zsh_prepend_unique_value -unset _colcon_prefix_zsh_convert_to_array - -# check environment variable for custom Python executable -if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then - if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then - echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" - return 1 - fi - _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" -else - # try the Python executable known at configure time - _colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if [ ! -f "$_colcon_python_executable" ]; then - if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then - echo "error: unable to find python3 executable" - return 1 - fi - _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` - fi -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# get all commands in topological order -_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)" -unset _colcon_python_executable -if [ -n "$COLCON_TRACE" ]; then - echo "$(declare -f _colcon_prefix_sh_source_script)" - echo "# Execute generated script:" - echo "# <<<" - echo "${_colcon_ordered_commands}" - echo "# >>>" - echo "unset _colcon_prefix_sh_source_script" -fi -eval "${_colcon_ordered_commands}" -unset _colcon_ordered_commands - -unset _colcon_prefix_sh_source_script - -unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX diff --git a/controllers/controllers/install/setup.bash b/controllers/controllers/install/setup.bash deleted file mode 100644 index 10ea0f7..0000000 --- a/controllers/controllers/install/setup.bash +++ /dev/null @@ -1,31 +0,0 @@ -# generated from colcon_bash/shell/template/prefix_chain.bash.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_chain_bash_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" - -# source this prefix -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" - -unset COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_bash_source_script diff --git a/controllers/controllers/install/setup.ps1 b/controllers/controllers/install/setup.ps1 deleted file mode 100644 index 558e9b9..0000000 --- a/controllers/controllers/install/setup.ps1 +++ /dev/null @@ -1,29 +0,0 @@ -# generated from colcon_powershell/shell/template/prefix_chain.ps1.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# function to source another script with conditional trace output -# first argument: the path of the script -function _colcon_prefix_chain_powershell_source_script { - param ( - $_colcon_prefix_chain_powershell_source_script_param - ) - # source script with conditional trace output - if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) { - if ($env:COLCON_TRACE) { - echo ". '$_colcon_prefix_chain_powershell_source_script_param'" - } - . "$_colcon_prefix_chain_powershell_source_script_param" - } else { - Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'" - } -} - -# source chained prefixes -_colcon_prefix_chain_powershell_source_script "/opt/ros/humble\local_setup.ps1" - -# source this prefix -$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) -_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" diff --git a/controllers/controllers/install/setup.sh b/controllers/controllers/install/setup.sh deleted file mode 100644 index 43fd463..0000000 --- a/controllers/controllers/install/setup.sh +++ /dev/null @@ -1,45 +0,0 @@ -# generated from colcon_core/shell/template/prefix_chain.sh.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# since a plain shell script can't determine its own path when being sourced -# either use the provided COLCON_CURRENT_PREFIX -# or fall back to the build time prefix (if it exists) -_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/home/maria/F1Tenth-Repository/src/controllers/controllers/install -if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then - _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then - echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 - unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX - return 1 -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_chain_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - - -# source this prefix -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - -unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_sh_source_script -unset COLCON_CURRENT_PREFIX diff --git a/controllers/controllers/install/setup.zsh b/controllers/controllers/install/setup.zsh deleted file mode 100644 index 54799fd..0000000 --- a/controllers/controllers/install/setup.zsh +++ /dev/null @@ -1,31 +0,0 @@ -# generated from colcon_zsh/shell/template/prefix_chain.zsh.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_chain_zsh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" - -# source this prefix -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" - -unset COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_zsh_source_script diff --git a/controllers/controllers/log/COLCON_IGNORE b/controllers/controllers/log/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/controllers/controllers/log/build_2025-08-07_23-05-56/events.log b/controllers/controllers/log/build_2025-08-07_23-05-56/events.log deleted file mode 100644 index 2cd52d9..0000000 --- a/controllers/controllers/log/build_2025-08-07_23-05-56/events.log +++ /dev/null @@ -1,2 +0,0 @@ -[0.000000] (-) TimerEvent: {} -[0.000662] (-) EventReactorShutdown: {} diff --git a/controllers/controllers/log/build_2025-08-07_23-05-56/logger_all.log b/controllers/controllers/log/build_2025-08-07_23-05-56/logger_all.log deleted file mode 100644 index 3b3bf28..0000000 --- a/controllers/controllers/log/build_2025-08-07_23-05-56/logger_all.log +++ /dev/null @@ -1,62 +0,0 @@ -[0.233s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build'] -[0.233s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=False, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=12, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, mixin_files=None, mixin=None, verb_parser=, verb_extension=, main=>, mixin_verb=('build',)) -[0.393s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters -[0.393s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters -[0.393s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters -[0.393s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters -[0.393s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover -[0.393s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover -[0.393s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/maria/F1Tenth-Repository/src/controllers/controllers' -[0.394s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install'] -[0.394s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore' -[0.394s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install' -[0.394s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg'] -[0.394s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg' -[0.394s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta'] -[0.394s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta' -[0.394s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros'] -[0.394s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros' -[0.401s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python'] -[0.401s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake' -[0.401s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python' -[0.401s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py'] -[0.401s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py' -[0.401s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install'] -[0.401s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore' -[0.401s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored -[0.401s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install'] -[0.401s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore' -[0.401s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored -[0.401s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install'] -[0.401s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore' -[0.401s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored -[0.401s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults -[0.401s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover -[0.401s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults -[0.401s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover -[0.401s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults -[0.424s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) check parameters -[0.424s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) discover -[0.426s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 386 installed packages in /opt/ros/humble -[0.427s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) using defaults -[0.450s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor -[0.451s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete -[0.451s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop -[0.451s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed -[0.451s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0' -[0.451s] DEBUG:colcon.colcon_core.event_reactor:joining thread -[0.456s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems -[0.456s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems -[0.456s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2' -[0.468s] DEBUG:colcon.colcon_core.event_reactor:joined thread -[0.473s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems -[0.474s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/local_setup.ps1' -[0.475s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/_local_setup_util_ps1.py' -[0.477s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/setup.ps1' -[0.479s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/local_setup.sh' -[0.480s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/_local_setup_util_sh.py' -[0.480s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/setup.sh' -[0.482s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/local_setup.bash' -[0.483s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/setup.bash' -[0.485s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/local_setup.zsh' -[0.486s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/setup.zsh' diff --git a/controllers/controllers/log/build_2025-08-07_23-06-01/events.log b/controllers/controllers/log/build_2025-08-07_23-06-01/events.log deleted file mode 100644 index 4eeddaf..0000000 --- a/controllers/controllers/log/build_2025-08-07_23-06-01/events.log +++ /dev/null @@ -1,2 +0,0 @@ -[0.000000] (-) TimerEvent: {} -[0.000107] (-) EventReactorShutdown: {} diff --git a/controllers/controllers/log/build_2025-08-07_23-06-01/logger_all.log b/controllers/controllers/log/build_2025-08-07_23-06-01/logger_all.log deleted file mode 100644 index 478968d..0000000 --- a/controllers/controllers/log/build_2025-08-07_23-06-01/logger_all.log +++ /dev/null @@ -1,63 +0,0 @@ -[0.058s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build'] -[0.058s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=False, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=12, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, mixin_files=None, mixin=None, verb_parser=, verb_extension=, main=>, mixin_verb=('build',)) -[0.149s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters -[0.149s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters -[0.149s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters -[0.149s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters -[0.149s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover -[0.149s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover -[0.149s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/maria/F1Tenth-Repository/src/controllers/controllers' -[0.149s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install'] -[0.149s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore' -[0.149s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install' -[0.149s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg'] -[0.149s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg' -[0.149s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta'] -[0.149s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta' -[0.149s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros'] -[0.149s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros' -[0.155s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python'] -[0.155s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake' -[0.155s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python' -[0.155s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py'] -[0.155s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py' -[0.155s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install'] -[0.155s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore' -[0.155s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored -[0.155s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install'] -[0.155s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore' -[0.155s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored -[0.155s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install'] -[0.155s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore' -[0.155s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored -[0.155s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults -[0.155s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover -[0.155s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults -[0.156s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover -[0.156s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults -[0.174s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) check parameters -[0.174s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) discover -[0.175s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 0 installed packages in /home/maria/F1Tenth-Repository/src/controllers/controllers/install -[0.175s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 386 installed packages in /opt/ros/humble -[0.176s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) using defaults -[0.201s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor -[0.202s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete -[0.202s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop -[0.202s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed -[0.202s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0' -[0.202s] DEBUG:colcon.colcon_core.event_reactor:joining thread -[0.205s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems -[0.205s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems -[0.205s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2' -[0.211s] DEBUG:colcon.colcon_core.event_reactor:joined thread -[0.213s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems -[0.213s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/local_setup.ps1' -[0.214s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/_local_setup_util_ps1.py' -[0.215s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/setup.ps1' -[0.215s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/local_setup.sh' -[0.216s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/_local_setup_util_sh.py' -[0.216s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/setup.sh' -[0.216s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/local_setup.bash' -[0.217s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/setup.bash' -[0.217s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/local_setup.zsh' -[0.217s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/maria/F1Tenth-Repository/src/controllers/controllers/install/setup.zsh' diff --git a/controllers/controllers/log/latest b/controllers/controllers/log/latest deleted file mode 120000 index b57d247..0000000 --- a/controllers/controllers/log/latest +++ /dev/null @@ -1 +0,0 @@ -latest_build \ No newline at end of file diff --git a/controllers/controllers/log/latest_build b/controllers/controllers/log/latest_build deleted file mode 120000 index b15e59a..0000000 --- a/controllers/controllers/log/latest_build +++ /dev/null @@ -1 +0,0 @@ -build_2025-08-07_23-06-01 \ No newline at end of file diff --git a/controllers/controllers/mpc_node.py b/controllers/controllers/mpc_node.py deleted file mode 100644 index 6d30573..0000000 --- a/controllers/controllers/mpc_node.py +++ /dev/null @@ -1,429 +0,0 @@ -#!/usr/bin/env python3 -import math -from dataclasses import dataclass, field - -import cvxpy -import numpy as np -import rclpy -from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped -from geometry_msgs.msg import PoseStamped -from rclpy.node import Node -from scipy.linalg import block_diag -from scipy.sparse import block_diag, csc_matrix, diags -from sensor_msgs.msg import LaserScan -from utils import nearest_point - -# TODO CHECK: include needed ROS msg type headers and libraries - - -@dataclass -class mpc_config: - NXK: int = 4 # length of kinematic state vector: z = [x, y, v, yaw] - NU: int = 2 # length of input vector: u = = [steering speed, acceleration] - TK: int = 8 # finite time horizon length kinematic - - # --------------------------------------------------- - # TODO: you may need to tune the following matrices - Rk: list = field( - default_factory=lambda: np.diag([0.01, 100.0]) - ) # input cost matrix, penalty for inputs - [accel, steering_speed] - Rdk: list = field( - default_factory=lambda: np.diag([0.01, 100.0]) - ) # input difference cost matrix, penalty for change of inputs - [accel, steering_speed] - Qk: list = field( - default_factory=lambda: np.diag([13.5, 13.5, 5.5, 13.0]) - ) # state error cost matrix, for the the next (T) prediction time steps [x, y, delta, v, yaw, yaw-rate, beta] - Qfk: list = field( - default_factory=lambda: np.diag([13.5, 13.5, 5.5, 13.0]) - ) # final state error matrix, penalty for the final state constraints: [x, y, delta, v, yaw, yaw-rate, beta] - # --------------------------------------------------- - - N_IND_SEARCH: int = 20 # Search index number - DTK: float = 0.1 # time step [s] kinematic - dlk: float = 0.03 # dist step [m] kinematic - LENGTH: float = 0.58 # Length of the vehicle [m] - WIDTH: float = 0.31 # Width of the vehicle [m] - WB: float = 0.33 # Wheelbase [m] - MIN_STEER: float = -0.4189 # maximum steering angle [rad] - MAX_STEER: float = 0.4189 # maximum steering angle [rad] - MAX_DSTEER: float = np.deg2rad(180.0) # maximum steering speed [rad/s] - MAX_SPEED: float = 6.0 # maximum speed [m/s] - MIN_SPEED: float = 0.0 # minimum backward speed [m/s] - MAX_ACCEL: float = 3.0 # maximum acceleration [m/ss] - - -@dataclass -class State: - x: float = 0.0 - y: float = 0.0 - delta: float = 0.0 - v: float = 0.0 - yaw: float = 0.0 - yawrate: float = 0.0 - beta: float = 0.0 - -class MPC(Node): - """ - Implement Kinematic MPC on the car - This is just a template, you are free to implement your own node! - """ - def __init__(self): - super().__init__('mpc_node') - # TODO: create ROS subscribers and publishers - # use the MPC as a tracker (similar to pure pursuit) - # TODO: get waypoints here - self.waypoints = None - - self.config = mpc_config() - self.odelta_v = None - self.odelta = None - self.oa = None - self.init_flag = 0 - - # initialize MPC problem - self.mpc_prob_init() - - def pose_callback(self, pose_msg): - pass - # TODO: extract pose from ROS msg - vehicle_state = None - - # TODO: Calculate the next reference trajectory for the next T steps - # with current vehicle pose. - # ref_x, ref_y, ref_yaw, ref_v are columns of self.waypoints - ref_path = self.calc_ref_trajectory(self, vehicle_state, ref_x, ref_y, ref_yaw, ref_v) - x0 = [vehicle_state.x, vehicle_state.y, vehicle_state.v, vehicle_state.yaw] - - # TODO: solve the MPC control problem - ( - self.oa, - self.odelta_v, - ox, - oy, - oyaw, - ov, - state_predict, - ) = self.linear_mpc_control(ref_path, x0, self.oa, self.odelta_v) - - # TODO: publish drive message. - steer_output = self.odelta_v[0] - speed_output = vehicle_state.v + self.oa[0] * self.config.DTK - - def mpc_prob_init(self): - """ - Create MPC quadratic optimization problem using cvxpy, solver: OSQP - Will be solved every iteration for control. - More MPC problem information here: https://osqp.org/docs/examples/mpc.html - More QP example in CVXPY here: https://www.cvxpy.org/examples/basic/quadratic_program.html - """ - # Initialize and create vectors for the optimization problem - # Vehicle State Vector - self.xk = cvxpy.Variable( - (self.config.NXK, self.config.TK + 1) - ) - # Control Input vector - self.uk = cvxpy.Variable( - (self.config.NU, self.config.TK) - ) - objective = 0.0 # Objective value of the optimization problem - constraints = [] # Create constraints array - - # Initialize reference vectors - self.x0k = cvxpy.Parameter((self.config.NXK,)) - self.x0k.value = np.zeros((self.config.NXK,)) - - # Initialize reference trajectory parameter - self.ref_traj_k = cvxpy.Parameter((self.config.NXK, self.config.TK + 1)) - self.ref_traj_k.value = np.zeros((self.config.NXK, self.config.TK + 1)) - - # Initializes block diagonal form of R = [R, R, ..., R] (NU*T, NU*T) - R_block = block_diag(tuple([self.config.Rk] * self.config.TK)) - - # Initializes block diagonal form of Rd = [Rd, ..., Rd] (NU*(T-1), NU*(T-1)) - Rd_block = block_diag(tuple([self.config.Rdk] * (self.config.TK - 1))) - - # Initializes block diagonal form of Q = [Q, Q, ..., Qf] (NX*T, NX*T) - Q_block = [self.config.Qk] * (self.config.TK) - Q_block.append(self.config.Qfk) - Q_block = block_diag(tuple(Q_block)) - - # Formulate and create the finite-horizon optimal control problem (objective function) - # The FTOCP has the horizon of T timesteps - - # -------------------------------------------------------- - # TODO: fill in the objectives here, you should be using cvxpy.quad_form() somehwhere - - # TODO: Objective part 1: Influence of the control inputs: Inputs u multiplied by the penalty R - - # TODO: Objective part 2: Deviation of the vehicle from the reference trajectory weighted by Q, including final Timestep T weighted by Qf - - # TODO: Objective part 3: Difference from one control input to the next control input weighted by Rd - - # -------------------------------------------------------- - - # Constraints 1: Calculate the future vehicle behavior/states based on the vehicle dynamics model matrices - # Evaluate vehicle Dynamics for next T timesteps - A_block = [] - B_block = [] - C_block = [] - # init path to zeros - path_predict = np.zeros((self.config.NXK, self.config.TK + 1)) - for t in range(self.config.TK): - A, B, C = self.get_model_matrix( - path_predict[2, t], path_predict[3, t], 0.0 - ) - A_block.append(A) - B_block.append(B) - C_block.extend(C) - - A_block = block_diag(tuple(A_block)) - B_block = block_diag(tuple(B_block)) - C_block = np.array(C_block) - - # [AA] Sparse matrix to CVX parameter for proper stuffing - # Reference: https://github.com/cvxpy/cvxpy/issues/1159#issuecomment-718925710 - m, n = A_block.shape - self.Annz_k = cvxpy.Parameter(A_block.nnz) - data = np.ones(self.Annz_k.size) - rows = A_block.row * n + A_block.col - cols = np.arange(self.Annz_k.size) - Indexer = csc_matrix((data, (rows, cols)), shape=(m * n, self.Annz_k.size)) - - # Setting sparse matrix data - self.Annz_k.value = A_block.data - - # Now we use this sparse version instead of the old A_ block matrix - self.Ak_ = cvxpy.reshape(Indexer @ self.Annz_k, (m, n), order="C") - - # Same as A - m, n = B_block.shape - self.Bnnz_k = cvxpy.Parameter(B_block.nnz) - data = np.ones(self.Bnnz_k.size) - rows = B_block.row * n + B_block.col - cols = np.arange(self.Bnnz_k.size) - Indexer = csc_matrix((data, (rows, cols)), shape=(m * n, self.Bnnz_k.size)) - self.Bk_ = cvxpy.reshape(Indexer @ self.Bnnz_k, (m, n), order="C") - self.Bnnz_k.value = B_block.data - - # No need for sparse matrices for C as most values are parameters - self.Ck_ = cvxpy.Parameter(C_block.shape) - self.Ck_.value = C_block - - # ------------------------------------------------------------- - # TODO: Constraint part 1: - # Add dynamics constraints to the optimization problem - # This constraint should be based on a few variables: - # self.xk, self.Ak_, self.Bk_, self.uk, and self.Ck_ - - # TODO: Constraint part 2: - # Add constraints on steering, change in steering angle - # cannot exceed steering angle speed limit. Should be based on: - # self.uk, self.config.MAX_DSTEER, self.config.DTK - - # TODO: Constraint part 3: - # Add constraints on upper and lower bounds of states and inputs - # and initial state constraint, should be based on: - # self.xk, self.x0k, self.config.MAX_SPEED, self.config.MIN_SPEED, - # self.uk, self.config.MAX_ACCEL, self.config.MAX_STEER - - # ------------------------------------------------------------- - - # Create the optimization problem in CVXPY and setup the workspace - # Optimization goal: minimize the objective function - self.MPC_prob = cvxpy.Problem(cvxpy.Minimize(objective), constraints) - - def calc_ref_trajectory(self, state, cx, cy, cyaw, sp): - """ - calc referent trajectory ref_traj in T steps: [x, y, v, yaw] - using the current velocity, calc the T points along the reference path - :param cx: Course X-Position - :param cy: Course y-Position - :param cyaw: Course Heading - :param sp: speed profile - :dl: distance step - :pind: Setpoint Index - :return: reference trajectory ref_traj, reference steering angle - """ - - # Create placeholder Arrays for the reference trajectory for T steps - ref_traj = np.zeros((self.config.NXK, self.config.TK + 1)) - ncourse = len(cx) - - # Find nearest index/setpoint from where the trajectories are calculated - _, _, _, ind = nearest_point(np.array([state.x, state.y]), np.array([cx, cy]).T) - - # Load the initial parameters from the setpoint into the trajectory - ref_traj[0, 0] = cx[ind] - ref_traj[1, 0] = cy[ind] - ref_traj[2, 0] = sp[ind] - ref_traj[3, 0] = cyaw[ind] - - # based on current velocity, distance traveled on the ref line between time steps - travel = abs(state.v) * self.config.DTK - dind = travel / self.config.dlk - ind_list = int(ind) + np.insert( - np.cumsum(np.repeat(dind, self.config.TK)), 0, 0 - ).astype(int) - ind_list[ind_list >= ncourse] -= ncourse - ref_traj[0, :] = cx[ind_list] - ref_traj[1, :] = cy[ind_list] - ref_traj[2, :] = sp[ind_list] - cyaw[cyaw - state.yaw > 4.5] = np.abs( - cyaw[cyaw - state.yaw > 4.5] - (2 * np.pi) - ) - cyaw[cyaw - state.yaw < -4.5] = np.abs( - cyaw[cyaw - state.yaw < -4.5] + (2 * np.pi) - ) - ref_traj[3, :] = cyaw[ind_list] - - return ref_traj - - def predict_motion(self, x0, oa, od, xref): - path_predict = xref * 0.0 - for i, _ in enumerate(x0): - path_predict[i, 0] = x0[i] - - state = State(x=x0[0], y=x0[1], yaw=x0[3], v=x0[2]) - for (ai, di, i) in zip(oa, od, range(1, self.config.TK + 1)): - state = self.update_state(state, ai, di) - path_predict[0, i] = state.x - path_predict[1, i] = state.y - path_predict[2, i] = state.v - path_predict[3, i] = state.yaw - - return path_predict - - def update_state(self, state, a, delta): - - # input check - if delta >= self.config.MAX_STEER: - delta = self.config.MAX_STEER - elif delta <= -self.config.MAX_STEER: - delta = -self.config.MAX_STEER - - state.x = state.x + state.v * math.cos(state.yaw) * self.config.DTK - state.y = state.y + state.v * math.sin(state.yaw) * self.config.DTK - state.yaw = ( - state.yaw + (state.v / self.config.WB) * math.tan(delta) * self.config.DTK - ) - state.v = state.v + a * self.config.DTK - - if state.v > self.config.MAX_SPEED: - state.v = self.config.MAX_SPEED - elif state.v < self.config.MIN_SPEED: - state.v = self.config.MIN_SPEED - - return state - - def get_model_matrix(self, v, phi, delta): - """ - Calc linear and discrete time dynamic model-> Explicit discrete time-invariant - Linear System: Xdot = Ax +Bu + C - State vector: x=[x, y, v, yaw] - :param v: speed - :param phi: heading angle of the vehicle - :param delta: steering angle: delta_bar - :return: A, B, C - """ - - # State (or system) matrix A, 4x4 - A = np.zeros((self.config.NXK, self.config.NXK)) - A[0, 0] = 1.0 - A[1, 1] = 1.0 - A[2, 2] = 1.0 - A[3, 3] = 1.0 - A[0, 2] = self.config.DTK * math.cos(phi) - A[0, 3] = -self.config.DTK * v * math.sin(phi) - A[1, 2] = self.config.DTK * math.sin(phi) - A[1, 3] = self.config.DTK * v * math.cos(phi) - A[3, 2] = self.config.DTK * math.tan(delta) / self.config.WB - - # Input Matrix B; 4x2 - B = np.zeros((self.config.NXK, self.config.NU)) - B[2, 0] = self.config.DTK - B[3, 1] = self.config.DTK * v / (self.config.WB * math.cos(delta) ** 2) - - C = np.zeros(self.config.NXK) - C[0] = self.config.DTK * v * math.sin(phi) * phi - C[1] = -self.config.DTK * v * math.cos(phi) * phi - C[3] = -self.config.DTK * v * delta / (self.config.WB * math.cos(delta) ** 2) - - return A, B, C - - def mpc_prob_solve(self, ref_traj, path_predict, x0): - self.x0k.value = x0 - - A_block = [] - B_block = [] - C_block = [] - for t in range(self.config.TK): - A, B, C = self.get_model_matrix( - path_predict[2, t], path_predict[3, t], 0.0 - ) - A_block.append(A) - B_block.append(B) - C_block.extend(C) - - A_block = block_diag(tuple(A_block)) - B_block = block_diag(tuple(B_block)) - C_block = np.array(C_block) - - self.Annz_k.value = A_block.data - self.Bnnz_k.value = B_block.data - self.Ck_.value = C_block - - self.ref_traj_k.value = ref_traj - - # Solve the optimization problem in CVXPY - # Solver selections: cvxpy.OSQP; cvxpy.GUROBI - self.MPC_prob.solve(solver=cvxpy.OSQP, verbose=False, warm_start=True) - - if ( - self.MPC_prob.status == cvxpy.OPTIMAL - or self.MPC_prob.status == cvxpy.OPTIMAL_INACCURATE - ): - ox = np.array(self.xk.value[0, :]).flatten() - oy = np.array(self.xk.value[1, :]).flatten() - ov = np.array(self.xk.value[2, :]).flatten() - oyaw = np.array(self.xk.value[3, :]).flatten() - oa = np.array(self.uk.value[0, :]).flatten() - odelta = np.array(self.uk.value[1, :]).flatten() - - else: - print("Error: Cannot solve mpc..") - oa, odelta, ox, oy, oyaw, ov = None, None, None, None, None, None - - return oa, odelta, ox, oy, oyaw, ov - - def linear_mpc_control(self, ref_path, x0, oa, od): - """ - MPC contorl with updating operational point iteraitvely - :param ref_path: reference trajectory in T steps - :param x0: initial state vector - :param oa: acceleration of T steps of last time - :param od: delta of T steps of last time - """ - - if oa is None or od is None: - oa = [0.0] * self.config.TK - od = [0.0] * self.config.TK - - # Call the Motion Prediction function: Predict the vehicle motion for x-steps - path_predict = self.predict_motion(x0, oa, od, ref_path) - poa, pod = oa[:], od[:] - - # Run the MPC optimization: Create and solve the optimization problem - mpc_a, mpc_delta, mpc_x, mpc_y, mpc_yaw, mpc_v = self.mpc_prob_solve( - ref_path, path_predict, x0 - ) - - return mpc_a, mpc_delta, mpc_x, mpc_y, mpc_yaw, mpc_v, path_predict - -def main(args=None): - rclpy.init(args=args) - print("MPC Initialized") - mpc_node = MPC() - rclpy.spin(mpc_node) - - mpc_node.destroy_node() - rclpy.shutdown() diff --git a/controllers/controllers/obstaculosFTG.py b/controllers/controllers/obstaculosFTG.py deleted file mode 100644 index dea42d2..0000000 --- a/controllers/controllers/obstaculosFTG.py +++ /dev/null @@ -1,264 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -import numpy as np -import time -from sensor_msgs.msg import LaserScan -from ackermann_msgs.msg import AckermannDriveStamped -from nav_msgs.msg import Odometry - -class ReactiveFollowGap(Node): - def __init__(self): - super().__init__('reactive_node') - - # Topics - lidarscan_topic = '/scan' - drive_topic = '/drive' - odom_topic = '/ego_racecar/odom' - - # Parámetros del algoritmo - self.base_speed = 4.0 # Velocidad base para moverse lentamente - self.max_speed = 5.0 # Velocidad máxima - self.min_speed = 3.0 # Velocidad mínima - self.max_lidar_range = 10 - self.bubble_radius_m = 2.0 # Aumentamos el radio de la burbuja de seguridad - self.min_clearance = 1.6 # Mayor clearance para buscar caminos más anchos - self.max_steering_angle = np.radians(30) - self.gap_threshold = 0.3 # Umbral reducido para aceptar gaps más estrechos - self.center_bias = 0.5 - self.speed_filter = self.base_speed - self.speed_filter_alpha = 0.7 - self.steering_filter = 0.0 - self.steering_filter_alpha = 0.8 - self.scan_initialized = False - self.scan_count = 0 - - # Control PID - self.kp_angle = 15 # Ganancia proporcional para el ángulo - self.ki_angle = 0.0 # Ganancia integral para el ángulo - self.kd_angle = 0.1 # Ganancia derivativa para el ángulo - - self.kp_speed = 0.02 # Ganancia proporcional para la velocidad - self.ki_speed = 0.0 # Ganancia integral para la velocidad - self.kd_speed = 0.1 # Ganancia derivativa para la velocidad - - self.last_error_angle = 0.0 - self.integral_angle = 0.0 - self.last_error_speed = 0.0 - self.integral_speed = 0.0 - - # Control de vueltas - self.lap_start_time = time.time() - self.lap_count = 0 - self.prev_in_start_zone = False - self.lap_times = [] - self.has_left_start_zone = False - - # Publishers & Subscribers - self.drive_pub = self.create_publisher(AckermannDriveStamped, drive_topic, 10) - self.scan_sub = self.create_subscription(LaserScan, lidarscan_topic, self.lidar_callback, 10) - self.odom_sub = self.create_subscription(Odometry, odom_topic, self.odom_callback, 10) - - self.get_logger().info("ReactiveFollowGap node initialized.") - - def preprocess_lidar(self, ranges): - #Preprocesa los datos del LiDAR: reemplaza valores inválidos y aplica un filtro - proc = np.array(ranges, dtype=np.float32) - invalid_mask = (proc == 0) | np.isinf(proc) | np.isnan(proc) - proc[invalid_mask] = self.max_lidar_range - proc = np.clip(proc, 0.1, self.max_lidar_range) - kernel = np.ones(3) / 3 - return np.convolve(proc, kernel, mode='same') - - def create_safety_bubble(self, ranges, closest_idx): - #Crea la burbuja de seguridad que ayuda a evitar obstaculos dentro del rango definido - bubble_ranges = np.copy(ranges) - angle_increment = 2 * np.pi / len(ranges) - bubble_radius_idx = int(self.bubble_radius_m / (ranges[closest_idx] * angle_increment)) - bubble_radius_idx = max(5, min(bubble_radius_idx, 20)) - start_idx = max(0, closest_idx - bubble_radius_idx) - end_idx = min(len(ranges) - 1, closest_idx + bubble_radius_idx) - bubble_ranges[start_idx:end_idx + 1] = 0.0 - return bubble_ranges - - def find_gaps(self, ranges): - #Detecta los huecos o gaps que el carrito puede usar para avanzar - free_mask = ranges >= self.min_clearance - gaps = [] - start_idx = None - for i, is_free in enumerate(free_mask): - if is_free and start_idx is None: - start_idx = i - elif not is_free and start_idx is not None: - gap_width_rad = (i - start_idx) * (2 * np.pi / len(ranges)) - gap_width_m = gap_width_rad * np.mean(ranges[start_idx:i]) - if gap_width_m >= self.gap_threshold: # Filtramos por el umbral de gap - gaps.append((start_idx, i - 1)) - start_idx = None - if start_idx is not None: - gap_width_rad = (len(free_mask) - start_idx) * (2 * np.pi / len(ranges)) - gap_width_m = gap_width_rad * np.mean(ranges[start_idx:]) - if gap_width_m >= self.gap_threshold: # Filtramos por el umbral de gap - gaps.append((start_idx, len(free_mask) - 1)) - return gaps - - def select_best_gap(self, gaps, ranges): - #Selecciona el mejor, el mas largo con espacio para cruzar, prioriza el gap mas largo - if not gaps: - return None - best_gap = None - best_score = -1 - center_idx = len(ranges) // 2 - for start_idx, end_idx in gaps: - gap_ranges = ranges[start_idx:end_idx + 1] - avg_distance = np.mean(gap_ranges) - max_distance = np.max(gap_ranges) - gap_width = end_idx - start_idx - gap_center = (start_idx + end_idx) // 2 - center_distance = abs(gap_center - center_idx) - distance_score = min(avg_distance / 5.0, 1.0) - width_score = min(gap_width / 50.0, 1.0) # Aumentamos la importancia del ancho - center_score = 1.0 - (center_distance / center_idx) * self.center_bias - total_score = (distance_score * 0.2 + width_score * 0.6 + center_score * 0.2) # Ajustamos peso - if total_score > best_score: - best_score = total_score - best_gap = (start_idx, end_idx) - return best_gap - - def get_target_point(self, start_idx, end_idx, ranges): - #Define un punto siguiente para que el carrito avance - if start_idx is None or end_idx is None: - return None - gap_ranges = ranges[start_idx:end_idx + 1] - max_dist_idx = np.argmax(gap_ranges) - target_idx = start_idx + max_dist_idx - gap_center = (start_idx + end_idx) // 2 - target_idx = gap_center # Centramos el robot en el gap más ancho - return target_idx - - def calculate_pid(self, error, last_error, integral, kp, ki, kd): - #Calcula el pid - integral += error - derivative = error - last_error - output = kp * error + ki * integral + kd * derivative - return output, integral - - def calculate_steering_angle(self, target_idx, scan_data): - #Obtiene el steering angle para acercarse al punto objetivo definido - if target_idx is None: - return 0.0 - target_angle = scan_data.angle_min + target_idx * scan_data.angle_increment - center_angle = scan_data.angle_min + (len(scan_data.ranges) // 2) * scan_data.angle_increment - error_angle = target_angle - center_angle - - # Aumentamos la ganancia proporcional si se detecta un obstáculo cerca - if abs(error_angle) > np.radians(10): # Si el error angular es grande, aumenta el giro - self.kp_angle = 1.5 # Aumentamos la ganancia proporcional para giros más rápidos - else: - self.kp_angle = 0.8 # Restablecemos la ganancia a su valor original si no hay obstáculo cercano - - # Calcular el control PID para el ángulo - steering_angle, self.integral_angle = self.calculate_pid(error_angle, self.last_error_angle, self.integral_angle, self.kp_angle, self.ki_angle, self.kd_angle) - self.last_error_angle = error_angle - - # Limitar el ángulo de dirección para evitar giros bruscos - return np.clip(steering_angle, -self.max_steering_angle, self.max_steering_angle) - - def calculate_speed(self, ranges, steering_angle): - #Adapta la velocidad adecuada del carrito - center_idx = len(ranges) // 2 - front_range = int(np.radians(30) / (2 * np.pi / len(ranges))) - front_distances = ranges[max(0, center_idx - front_range):min(len(ranges), center_idx + front_range)] - min_front_distance = np.min(front_distances) - avg_front_distance = np.mean(front_distances) - - # Ajuste de la velocidad con PID para evitar retrocesos - speed_error = avg_front_distance - 2.0 # Queremos mantener al menos 2 metros de distancia - speed, self.integral_speed = self.calculate_pid(speed_error, self.last_error_speed, self.integral_speed, self.kp_speed, self.ki_speed, self.kd_speed) - self.last_error_speed = speed_error - - # Limitar la velocidad y asegurarse que no retroceda - speed = max(self.min_speed, speed) # Aseguramos que la velocidad no sea menor a un mínimo - return np.clip(speed, self.min_speed, self.max_speed) - - def lidar_callback(self, data): - #Llama la informacion del lidar - self.scan_count += 1 - if self.scan_count < 5: - return - - ranges = self.preprocess_lidar(data.ranges) - closest_idx = np.argmin(ranges) - closest_distance = ranges[closest_idx] - - # Crear burbuja de seguridad solo si es necesario - bubble_ranges = (self.create_safety_bubble(ranges, closest_idx) - if closest_distance < 1.5 else ranges) - - gaps = self.find_gaps(bubble_ranges) - best_gap = self.select_best_gap(gaps, ranges) - - drive_msg = AckermannDriveStamped() - drive_msg.header.stamp = self.get_clock().now().to_msg() - drive_msg.header.frame_id = "base_link" - - if best_gap is not None: - start_idx, end_idx = best_gap - target_idx = self.get_target_point(start_idx, end_idx, ranges) - - if target_idx is not None: - steering_angle = self.calculate_steering_angle(target_idx, data) - speed = self.calculate_speed(ranges, steering_angle) - else: - speed = self.min_speed - steering_angle = 0.0 - else: - speed = self.min_speed - steering_angle = np.radians(10) - - drive_msg.drive.speed = float(speed) - drive_msg.drive.steering_angle = float(steering_angle) - self.drive_pub.publish(drive_msg) - - def odom_callback(self, msg): - #Llama la informacion de odometria del carrito - x = msg.pose.pose.position.x - y = msg.pose.pose.position.y - - # Zona de inicio - in_start_zone = (abs(x) < 2.0 and abs(y) < 2.0) - - #Contador de vueltas - if in_start_zone and not self.prev_in_start_zone and self.has_left_start_zone: - lap_end_time = time.time() - lap_duration = lap_end_time - self.lap_start_time - self.lap_times.append(lap_duration) - self.lap_count += 1 - - self.get_logger().info(f"🏁 Vuelta {self.lap_count} completada en {lap_duration:.2f} s.") - self.lap_start_time = lap_end_time - - - # Marcar que ya ha salido de la zona al menos una vez - if not in_start_zone: - self.has_left_start_zone = True - - # Guardar estado anterior - self.prev_in_start_zone = in_start_zone - - -def main(args=None): - rclpy.init(args=args) - node = ReactiveFollowGap() - try: - rclpy.spin(node) - except KeyboardInterrupt: - node.get_logger().info("Node stopped cleanly") - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/controllers/controllers/obstaculosWF.py b/controllers/controllers/obstaculosWF.py deleted file mode 100644 index c6d77c7..0000000 --- a/controllers/controllers/obstaculosWF.py +++ /dev/null @@ -1,179 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -import numpy as np -import time -from sensor_msgs.msg import LaserScan -from ackermann_msgs.msg import AckermannDriveStamped -from nav_msgs.msg import Odometry - -class WallFollower(Node): - def __init__(self): - super().__init__('wall_follower_node') - - # Topics - lidarscan_topic = '/scan' - drive_topic = '/drive' - odom_topic = '/ego_racecar/odom' - - # Parámetros del algoritmo de seguimiento de pared - self.max_speed = 2.5 # Velocidad máxima reducida a la mitad - self.min_speed = 1.5 # Velocidad mínima ajustada - self.max_lidar_range = 10.0 - self.wall_distance_ref = 1.5 # Distancia de referencia a la pared aumentada a 2 metros - - # PID para el ángulo - self.steering_pid_kp = 0.7 - self.steering_pid_ki = 0.0 - self.steering_pid_kd = 0.1 - self.last_error_angle = 0.0 - self.integral_angle = 0.0 - - # PID para la velocidad - self.speed_pid_kp = 0.05 - self.speed_pid_ki = 0.0 - self.speed_pid_kd = 0.01 - self.last_error_speed = 0.0 - self.integral_speed = 0.0 - - # Control de vueltas (se mantiene sin cambios) - self.lap_start_time = time.time() - self.lap_count = 0 - self.prev_in_start_zone = False - self.lap_times = [] - self.has_left_start_zone = False - - # Publishers & Subscribers - self.drive_pub = self.create_publisher(AckermannDriveStamped, drive_topic, 10) - self.scan_sub = self.create_subscription(LaserScan, lidarscan_topic, self.lidar_callback, 10) - self.odom_sub = self.create_subscription(Odometry, odom_topic, self.odom_callback, 10) - - self.get_logger().info("WallFollower node initialized.") - - def preprocess_lidar(self, ranges): - #Preprocesa los datos del LiDAR: reemplaza valores inválidos y aplica un filtro - proc = np.array(ranges, dtype=np.float32) - invalid_mask = (proc == 0) | np.isinf(proc) | np.isnan(proc) - proc[invalid_mask] = self.max_lidar_range - proc = np.clip(proc, 0.1, self.max_lidar_range) - kernel = np.ones(3) / 3 - return np.convolve(proc, kernel, mode='same') - - def select_cleanest_wall(self, ranges, angle_increment): - #Selecciona el lado con la pared más 'limpia' (menos protuberancias) - num_ranges = len(ranges) - mid_idx = num_ranges // 2 - - # Rango de ángulos para considerar los lados derecho e izquierdo - angle_range_deg = 45 - points_to_check = int(np.radians(angle_range_deg) / angle_increment) - - right_ranges = ranges[mid_idx - points_to_check : mid_idx] - left_ranges = ranges[mid_idx : mid_idx + points_to_check] - - # Calcular la desviación estándar para cada lado - std_dev_right = np.std(right_ranges) - std_dev_left = np.std(left_ranges) - - # Si el lado derecho es más 'limpio' (menor desviación), lo seguimos. - if std_dev_right < std_dev_left: - return 'right', right_ranges - else: - return 'left', left_ranges - - def calculate_pid(self, error, last_error, integral, kp, ki, kd): - #Calculo de PID - integral += error - derivative = error - last_error - output = kp * error + ki * integral + kd * derivative - return output, integral - - def get_wall_following_angle(self, side_to_follow, side_ranges): - #Obtiene el angulo necesario para seguir la pared con menos obstaculos detectable - avg_dist = np.mean(side_ranges) - error = self.wall_distance_ref - avg_dist - - steering_angle, self.integral_angle = self.calculate_pid( - error, self.last_error_angle, self.integral_angle, - self.steering_pid_kp, self.steering_pid_ki, self.steering_pid_kd - ) - self.last_error_angle = error - - if side_to_follow == 'left': - steering_angle = -steering_angle - - return np.clip(steering_angle, -0.5, 0.5) - - def calculate_speed(self, ranges, steering_angle): - #Ajusta la velocidad en función de la distancia frontal y el ángulo de dirección - # Reducir la velocidad en curvas cerradas - speed = self.max_speed - abs(steering_angle) * 1.5 - - # Usa PID para ajustar la velocidad según la distancia frontal - front_range = int(np.radians(20) / (2 * np.pi / len(ranges))) - front_distances = ranges[len(ranges) // 2 - front_range : len(ranges) // 2 + front_range] - min_front_distance = np.min(front_distances) - - speed_error = min_front_distance - 2.0 - speed_adjustment, self.integral_speed = self.calculate_pid( - speed_error, self.last_error_speed, self.integral_speed, - self.speed_pid_kp, self.speed_pid_ki, self.speed_pid_kd - ) - self.last_error_speed = speed_error - - speed += speed_adjustment - - return np.clip(speed, self.min_speed, self.max_speed) - - def lidar_callback(self, data): - ranges = self.preprocess_lidar(data.ranges) - - # Seleccionar la pared más 'limpia' - side_to_follow, side_ranges = self.select_cleanest_wall(ranges, data.angle_increment) - - # Calcular ángulo y velocidad - steering_angle = self.get_wall_following_angle(side_to_follow, side_ranges) - speed = self.calculate_speed(ranges, steering_angle) - - drive_msg = AckermannDriveStamped() - drive_msg.header.stamp = self.get_clock().now().to_msg() - drive_msg.header.frame_id = "base_link" - drive_msg.drive.speed = float(speed) - drive_msg.drive.steering_angle = float(steering_angle) - - self.drive_pub.publish(drive_msg) - - def odom_callback(self, msg): - x = msg.pose.pose.position.x - y = msg.pose.pose.position.y - - in_start_zone = (abs(x) < 2.0 and abs(y) < 2.0) - - if in_start_zone and not self.prev_in_start_zone and self.has_left_start_zone: - lap_end_time = time.time() - lap_duration = lap_end_time - self.lap_start_time - self.lap_times.append(lap_duration) - self.lap_count += 1 - self.get_logger().info(f"🏁 Vuelta {self.lap_count} completada en {lap_duration:.2f} s.") - self.lap_start_time = lap_end_time - - - if not in_start_zone: - self.has_left_start_zone = True - - self.prev_in_start_zone = in_start_zone - -def main(args=None): - rclpy.init(args=args) - node = WallFollower() - try: - rclpy.spin(node) - except KeyboardInterrupt: - node.get_logger().info("Node stopped cleanly") - finally: - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/controllers/controllers/pid_node.py b/controllers/controllers/pid_node.py deleted file mode 100644 index 3b651d4..0000000 --- a/controllers/controllers/pid_node.py +++ /dev/null @@ -1,108 +0,0 @@ -import rclpy -from rclpy.node import Node - -import numpy as np -from sensor_msgs.msg import LaserScan -from ackermann_msgs.msg import AckermannDriveStamped - -class WallFollow(Node): - """ - Implement Wall Following on the car - """ - def __init__(self): - super().__init__('wall_follow_node') - - lidarscan_topic = '/scan' - drive_topic = '/drive' - - # TODO: create subscribers and publishers - - # TODO: set PID gains - # self.kp = - # self.kd = - # self.ki = - - # TODO: store history - # self.integral = - # self.prev_error = - # self.error = - - # TODO: store any necessary values you think you'll need - - def get_range(self, range_data, angle): - """ - Simple helper to return the corresponding range measurement at a given angle. Make sure you take care of NaNs and infs. - - Args: - range_data: single range array from the LiDAR - angle: between angle_min and angle_max of the LiDAR - - Returns: - range: range measurement in meters at the given angle - - """ - - #TODO: implement - return 0.0 - - def get_error(self, range_data, dist): - """ - Calculates the error to the wall. Follow the wall to the left (going counter clockwise in the Levine loop). You potentially will need to use get_range() - - Args: - range_data: single range array from the LiDAR - dist: desired distance to the wall - - Returns: - error: calculated error - """ - - #TODO:implement - return 0.0 - - def pid_control(self, error, velocity): - """ - Based on the calculated error, publish vehicle control - - Args: - error: calculated error - velocity: desired velocity - - Returns: - None - """ - angle = 0.0 - # TODO: Use kp, ki & kd to implement a PID controller - drive_msg = AckermannDriveStamped() - # TODO: fill in drive message and publish - - def scan_callback(self, msg): - """ - Callback function for LaserScan messages. Calculate the error and publish the drive message in this function. - - Args: - msg: Incoming LaserScan message - - Returns: - None - """ - error = 0.0 # TODO: replace with error calculated by get_error() - velocity = 0.0 # TODO: calculate desired car velocity based on error - self.pid_control(error, velocity) # TODO: actuate the car with PID - - -def main(args=None): - rclpy.init(args=args) - print("WallFollow Initialized") - wall_follow_node = WallFollow() - rclpy.spin(wall_follow_node) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - wall_follow_node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/controllers/controllers/proyectoFTG.py b/controllers/controllers/proyectoFTG.py deleted file mode 100644 index d54c607..0000000 --- a/controllers/controllers/proyectoFTG.py +++ /dev/null @@ -1,261 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -import numpy as np -import time -from sensor_msgs.msg import LaserScan -from ackermann_msgs.msg import AckermannDriveStamped -from nav_msgs.msg import Odometry - -class ReactiveFollowGap(Node): - def __init__(self): - super().__init__('reactive_node') - - # Topics - lidarscan_topic = '/scan' - drive_topic = '/drive' - odom_topic = '/ego_racecar/odom' - - # Parámetros del algoritmo - self.base_speed = 4.0 # Velocidad base para moverse lentamente - self.max_speed = 5.0 # Velocidad máxima - self.min_speed = 3.0 # Velocidad mínima - self.max_lidar_range = 10 - self.bubble_radius_m = 2.0 # Aumentamos el radio de la burbuja de seguridad - self.min_clearance = 1.6 # Mayor clearance para buscar caminos más anchos - self.max_steering_angle = np.radians(30) - self.gap_threshold = 0.3 # Umbral reducido para aceptar gaps más estrechos - self.center_bias = 0.5 - self.speed_filter = self.base_speed - self.speed_filter_alpha = 0.7 - self.steering_filter = 0.0 - self.steering_filter_alpha = 0.8 - self.scan_initialized = False - self.scan_count = 0 - - # Control PID - self.kp_angle = 15 # Ganancia proporcional para el ángulo - self.ki_angle = 0.0 # Ganancia integral para el ángulo - self.kd_angle = 0.1 # Ganancia derivativa para el ángulo - - self.kp_speed = 0.02 # Ganancia proporcional para la velocidad - self.ki_speed = 0.0 # Ganancia integral para la velocidad - self.kd_speed = 0.1 # Ganancia derivativa para la velocidad - - self.last_error_angle = 0.0 - self.integral_angle = 0.0 - self.last_error_speed = 0.0 - self.integral_speed = 0.0 - - # Control de vueltas - self.lap_start_time = time.time() - self.lap_count = 0 - self.prev_in_start_zone = False - self.lap_times = [] - self.has_left_start_zone = False - - # Publishers & Subscribers - self.drive_pub = self.create_publisher(AckermannDriveStamped, drive_topic, 10) - self.scan_sub = self.create_subscription(LaserScan, lidarscan_topic, self.lidar_callback, 10) - self.odom_sub = self.create_subscription(Odometry, odom_topic, self.odom_callback, 10) - - self.get_logger().info("ReactiveFollowGap node initialized.") - - def preprocess_lidar(self, ranges): - proc = np.array(ranges, dtype=np.float32) - invalid_mask = (proc == 0) | np.isinf(proc) | np.isnan(proc) - proc[invalid_mask] = self.max_lidar_range - proc = np.clip(proc, 0.1, self.max_lidar_range) - kernel = np.ones(3) / 3 - return np.convolve(proc, kernel, mode='same') - - def create_safety_bubble(self, ranges, closest_idx): - bubble_ranges = np.copy(ranges) - angle_increment = 2 * np.pi / len(ranges) - bubble_radius_idx = int(self.bubble_radius_m / (ranges[closest_idx] * angle_increment)) - bubble_radius_idx = max(5, min(bubble_radius_idx, 20)) - start_idx = max(0, closest_idx - bubble_radius_idx) - end_idx = min(len(ranges) - 1, closest_idx + bubble_radius_idx) - bubble_ranges[start_idx:end_idx + 1] = 0.0 - return bubble_ranges - - def find_gaps(self, ranges): - free_mask = ranges >= self.min_clearance - gaps = [] - start_idx = None - for i, is_free in enumerate(free_mask): - if is_free and start_idx is None: - start_idx = i - elif not is_free and start_idx is not None: - gap_width_rad = (i - start_idx) * (2 * np.pi / len(ranges)) - gap_width_m = gap_width_rad * np.mean(ranges[start_idx:i]) - if gap_width_m >= self.gap_threshold: # Filtramos por el umbral de gap - gaps.append((start_idx, i - 1)) - start_idx = None - if start_idx is not None: - gap_width_rad = (len(free_mask) - start_idx) * (2 * np.pi / len(ranges)) - gap_width_m = gap_width_rad * np.mean(ranges[start_idx:]) - if gap_width_m >= self.gap_threshold: # Filtramos por el umbral de gap - gaps.append((start_idx, len(free_mask) - 1)) - return gaps - - def select_best_gap(self, gaps, ranges): - if not gaps: - return None - best_gap = None - best_score = -1 - center_idx = len(ranges) // 2 - for start_idx, end_idx in gaps: - gap_ranges = ranges[start_idx:end_idx + 1] - avg_distance = np.mean(gap_ranges) - max_distance = np.max(gap_ranges) - gap_width = end_idx - start_idx - gap_center = (start_idx + end_idx) // 2 - center_distance = abs(gap_center - center_idx) - distance_score = min(avg_distance / 5.0, 1.0) - width_score = min(gap_width / 50.0, 1.0) # Aumentamos la importancia del ancho - center_score = 1.0 - (center_distance / center_idx) * self.center_bias - total_score = (distance_score * 0.2 + width_score * 0.6 + center_score * 0.2) # Ajustamos peso - if total_score > best_score: - best_score = total_score - best_gap = (start_idx, end_idx) - return best_gap - - def get_target_point(self, start_idx, end_idx, ranges): - if start_idx is None or end_idx is None: - return None - gap_ranges = ranges[start_idx:end_idx + 1] - max_dist_idx = np.argmax(gap_ranges) - target_idx = start_idx + max_dist_idx - gap_center = (start_idx + end_idx) // 2 - target_idx = gap_center # Centramos el robot en el gap más ancho - return target_idx - - def calculate_pid(self, error, last_error, integral, kp, ki, kd): - integral += error - derivative = error - last_error - output = kp * error + ki * integral + kd * derivative - return output, integral - - def calculate_steering_angle(self, target_idx, scan_data): - if target_idx is None: - return 0.0 - target_angle = scan_data.angle_min + target_idx * scan_data.angle_increment - center_angle = scan_data.angle_min + (len(scan_data.ranges) // 2) * scan_data.angle_increment - error_angle = target_angle - center_angle - - # Aumentamos la ganancia proporcional si se detecta un obstáculo cerca - if abs(error_angle) > np.radians(10): # Si el error angular es grande, aumentar el giro - self.kp_angle = 1.5 # Aumentamos la ganancia proporcional para giros más rápidos - else: - self.kp_angle = 0.8 # Restablecemos la ganancia a su valor original si no hay obstáculo cercano - - # Calcular el control PID para el ángulo - steering_angle, self.integral_angle = self.calculate_pid(error_angle, self.last_error_angle, self.integral_angle, self.kp_angle, self.ki_angle, self.kd_angle) - self.last_error_angle = error_angle - - # Limitar el ángulo de dirección para evitar giros bruscos - return np.clip(steering_angle, -self.max_steering_angle, self.max_steering_angle) - - def calculate_speed(self, ranges, steering_angle): - center_idx = len(ranges) // 2 - front_range = int(np.radians(30) / (2 * np.pi / len(ranges))) - front_distances = ranges[max(0, center_idx - front_range):min(len(ranges), center_idx + front_range)] - min_front_distance = np.min(front_distances) - avg_front_distance = np.mean(front_distances) - - # Ajuste de la velocidad con PID para evitar retrocesos - speed_error = avg_front_distance - 2.0 # Queremos mantener al menos 2 metros de distancia - speed, self.integral_speed = self.calculate_pid(speed_error, self.last_error_speed, self.integral_speed, self.kp_speed, self.ki_speed, self.kd_speed) - self.last_error_speed = speed_error - - # Limitar la velocidad y asegurarse que no retroceda - speed = max(self.min_speed, speed) # Aseguramos que la velocidad no sea menor a un mínimo - return np.clip(speed, self.min_speed, self.max_speed) - - def lidar_callback(self, data): - self.scan_count += 1 - if self.scan_count < 5: - return - - ranges = self.preprocess_lidar(data.ranges) - closest_idx = np.argmin(ranges) - closest_distance = ranges[closest_idx] - - # Crear burbuja de seguridad solo si es necesario - bubble_ranges = (self.create_safety_bubble(ranges, closest_idx) - if closest_distance < 1.5 else ranges) - - gaps = self.find_gaps(bubble_ranges) - best_gap = self.select_best_gap(gaps, ranges) - - drive_msg = AckermannDriveStamped() - drive_msg.header.stamp = self.get_clock().now().to_msg() - drive_msg.header.frame_id = "base_link" - - if best_gap is not None: - start_idx, end_idx = best_gap - target_idx = self.get_target_point(start_idx, end_idx, ranges) - - if target_idx is not None: - steering_angle = self.calculate_steering_angle(target_idx, data) - speed = self.calculate_speed(ranges, steering_angle) - else: - speed = self.min_speed - steering_angle = 0.0 - else: - speed = self.min_speed - steering_angle = np.radians(10) - - drive_msg.drive.speed = float(speed) - drive_msg.drive.steering_angle = float(steering_angle) - self.drive_pub.publish(drive_msg) - - def odom_callback(self, msg): - x = msg.pose.pose.position.x - y = msg.pose.pose.position.y - - # Zona de inicio - in_start_zone = (abs(x) < 2.0 and abs(y) < 2.0) - - # Solo cuenta vuelta si: - # - Ya salió de la zona alguna vez - # - Y acaba de volver a entrar - if in_start_zone and not self.prev_in_start_zone and self.has_left_start_zone: - lap_end_time = time.time() - lap_duration = lap_end_time - self.lap_start_time - self.lap_times.append(lap_duration) - self.lap_count += 1 - - self.get_logger().info(f"🏁 Vuelta {self.lap_count} completada en {lap_duration:.2f} s.") - self.lap_start_time = lap_end_time - - if self.lap_count == 10: - self.get_logger().info(f"Número de vueltas requerido alcanzado ({self.lap_count})") - shortest = min(self.lap_times) - self.get_logger().info(f"Tiempo de vuelta más corto: {shortest:.2f} segundos") - - # Marcar que ya ha salido de la zona al menos una vez - if not in_start_zone: - self.has_left_start_zone = True - - # Guardar estado anterior - self.prev_in_start_zone = in_start_zone - - -def main(args=None): - rclpy.init(args=args) - node = ReactiveFollowGap() - try: - rclpy.spin(node) - except KeyboardInterrupt: - node.get_logger().info("Node stopped cleanly") - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() - diff --git a/controllers/controllers/proyectoObstaculos.py b/controllers/controllers/proyectoObstaculos.py deleted file mode 100644 index 5f766ef..0000000 --- a/controllers/controllers/proyectoObstaculos.py +++ /dev/null @@ -1,404 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -import numpy as np -import time -from sensor_msgs.msg import LaserScan -from ackermann_msgs.msg import AckermannDriveStamped -from nav_msgs.msg import Odometry - -class ReactiveFollowGap(Node): - def __init__(self): - super().__init__('reactive_node') - - # Topics - lidarscan_topic = '/scan' - drive_topic = '/drive' - odom_topic = '/ego_racecar/odom' - - # Parámetros AGRESIVOS para pasillos estrechos - self.base_speed = 4.0 # Velocidad base más conservadora para control - self.max_speed = 8.0 # Velocidad máxima reducida para mejor control - self.min_speed = 1.0 # Velocidad mínima más baja - self.max_lidar_range = 10.0 - self.bubble_radius_m = 0.3 # Radio muy pequeño - MUY AGRESIVO - self.min_clearance = 0.6 # Clearance mínimo muy reducido - AGRESIVO - self.max_steering_angle = np.radians(45) # Ángulo máximo aumentado - self.gap_threshold = 0.15 # Threshold muy pequeño para gaps estrechos - self.center_bias = 0.1 # Bias mínimo - seguir el gap más grande - - # Filtros más responsivos - self.speed_filter = self.base_speed - self.speed_filter_alpha = 0.3 # MUY responsive - self.steering_filter = 0.0 - self.steering_filter_alpha = 0.4 # MUY responsive - - # Control de escaneado - self.scan_initialized = False - self.scan_count = 0 - - # Variables para evitar bucles - self.stuck_counter = 0 - self.position_history = [] - self.last_positions = [] - self.stuck_threshold = 20 # Número de scans para detectar si está atascado - self.escape_mode = False - self.escape_direction = 1 # 1 para derecha, -1 para izquierda - self.escape_counter = 0 - - # Variables para detección de progreso - self.prev_position = None - self.movement_threshold = 0.1 # Movimiento mínimo para no considerarse atascado - - # Control de vueltas - self.lap_start_time = time.time() - self.lap_count = 0 - self.prev_in_start_zone = False - self.lap_times = [] - self.has_left_start_zone = False - - # Publishers & Subscribers - self.drive_pub = self.create_publisher(AckermannDriveStamped, drive_topic, 10) - self.scan_sub = self.create_subscription(LaserScan, lidarscan_topic, self.lidar_callback, 10) - self.odom_sub = self.create_subscription(Odometry, odom_topic, self.odom_callback, 10) - - self.get_logger().info("AGGRESSIVE ReactiveFollowGap initialized for narrow passages.") - - def preprocess_lidar(self, ranges): - """Preprocesamiento mínimo para mantener detalles en pasillos estrechos""" - proc = np.array(ranges, dtype=np.float32) - - # Manejo de valores inválidos - invalid_mask = (proc == 0) | np.isinf(proc) | np.isnan(proc) - proc[invalid_mask] = self.max_lidar_range - proc = np.clip(proc, 0.05, self.max_lidar_range) # Permitir lecturas muy cercanas - - # Filtrado MUY suave para preservar detalles - kernel = np.array([0.1, 0.8, 0.1]) # Filtro muy suave - proc = np.convolve(proc, kernel, mode='same') - - return proc - - def create_safety_bubble(self, ranges, closest_idx): - """Burbuja de seguridad MÍNIMA - solo para obstáculos muy cercanos""" - bubble_ranges = np.copy(ranges) - - if closest_idx is None or closest_idx >= len(ranges): - return bubble_ranges - - closest_distance = ranges[closest_idx] - - # Solo crear burbuja si está MUY cerca - if closest_distance > 0.4: - return bubble_ranges - - # Burbuja muy pequeña - angle_increment = 2 * np.pi / len(ranges) - bubble_radius_idx = max(1, int(self.bubble_radius_m / (closest_distance * angle_increment + 0.01))) - bubble_radius_idx = min(bubble_radius_idx, 5) # Máximo 5 puntos - - start_idx = max(0, closest_idx - bubble_radius_idx) - end_idx = min(len(ranges) - 1, closest_idx + bubble_radius_idx) - - # Burbuja sin degradado - directa - bubble_ranges[start_idx:end_idx + 1] = 0.0 - - return bubble_ranges - - def find_gaps(self, ranges): - """Detección de gaps ULTRA AGRESIVA""" - # Clearance muy pequeño - free_mask = ranges >= self.min_clearance - gaps = [] - start_idx = None - - for i, is_free in enumerate(free_mask): - if is_free and start_idx is None: - start_idx = i - elif not is_free and start_idx is not None: - # Cualquier gap, por pequeño que sea - gaps.append((start_idx, i - 1)) - start_idx = None - - # Manejar gap que termina al final del array - if start_idx is not None: - gaps.append((start_idx, len(free_mask) - 1)) - - return gaps - - def select_best_gap(self, ranges): - """Selección de gap SIMPLIFICADA - el más grande y más lejano""" - gaps = self.find_gaps(ranges) - - if not gaps: - return None - - best_gap = None - best_score = -1 - - for start_idx, end_idx in gaps: - gap_width = end_idx - start_idx + 1 - gap_ranges = ranges[start_idx:end_idx + 1] - avg_distance = np.mean(gap_ranges) - - # Score simple: tamaño del gap * distancia promedio - score = gap_width * avg_distance - - if score > best_score: - best_score = score - best_gap = (start_idx, end_idx) - - return best_gap - - def get_target_point(self, start_idx, end_idx, ranges): - """Punto objetivo en el CENTRO del gap más profundo""" - if start_idx is None or end_idx is None: - return None - - gap_ranges = ranges[start_idx:end_idx + 1] - - # Encontrar el punto más lejano en el gap - max_dist_idx = np.argmax(gap_ranges) - target_idx = start_idx + max_dist_idx - - return target_idx - - def calculate_steering_angle(self, target_idx, scan_data): - """Cálculo de ángulo DIRECTO y AGRESIVO""" - if target_idx is None: - return 0.0 - - target_angle = scan_data.angle_min + target_idx * scan_data.angle_increment - center_angle = scan_data.angle_min + (len(scan_data.ranges) // 2) * scan_data.angle_increment - - # Ángulo directo sin mucho filtrado - steering_angle = (target_angle - center_angle) * 1.2 # Factor agresivo - - # Filtrado mínimo - steering_angle = (self.steering_filter_alpha * self.steering_filter + - (1 - self.steering_filter_alpha) * steering_angle) - self.steering_filter = steering_angle - - return np.clip(steering_angle, -self.max_steering_angle, self.max_steering_angle) - - def check_if_stuck(self, current_position): - """Detecta si el vehículo está atascado en un bucle""" - if current_position is None: - return False - - # Agregar posición actual al historial - self.position_history.append(current_position) - - # Mantener solo las últimas posiciones - if len(self.position_history) > self.stuck_threshold: - self.position_history.pop(0) - - # Si no tenemos suficiente historial, no está atascado - if len(self.position_history) < self.stuck_threshold: - return False - - # Calcular la varianza de posiciones - positions = np.array(self.position_history) - x_variance = np.var(positions[:, 0]) - y_variance = np.var(positions[:, 1]) - - # Si la varianza es muy pequeña, está atascado - total_variance = x_variance + y_variance - stuck = total_variance < 0.5 # Threshold para considerar que está atascado - - if stuck: - self.stuck_counter += 1 - else: - self.stuck_counter = 0 - - return self.stuck_counter > 5 - - def escape_behavior(self, ranges): - """Comportamiento de escape cuando está atascado""" - center_idx = len(ranges) // 2 - - # Analizar espacios a los lados - left_quarter = ranges[:center_idx//2] - right_quarter = ranges[-center_idx//2:] - - left_space = np.mean(left_quarter[left_quarter > 0.1]) - right_space = np.mean(right_quarter[right_quarter > 0.1]) - - # Elegir la dirección con más espacio - if left_space > right_space: - self.escape_direction = 1 # Izquierda - steering_angle = np.radians(35) - else: - self.escape_direction = -1 # Derecha - steering_angle = np.radians(-35) - - # Velocidad baja pero constante para el escape - speed = 2.0 - - self.get_logger().info(f"ESCAPE MODE: Going {'LEFT' if self.escape_direction == 1 else 'RIGHT'}") - - return speed, steering_angle - - def calculate_speed(self, ranges, steering_angle): - """Cálculo de velocidad AGRESIVO para pasillos estrechos""" - center_idx = len(ranges) // 2 - front_range = int(np.radians(20) / (2 * np.pi / len(ranges))) - - front_distances = ranges[max(0, center_idx - front_range): - min(len(ranges), center_idx + front_range)] - - min_front = np.min(front_distances) - avg_front = np.mean(front_distances) - - # Velocidad agresiva basada en distancia frontal - if min_front < 0.3: - target_speed = 0.5 # Muy cerca - casi parar - elif min_front < 0.6: - target_speed = 1.5 # Cerca - velocidad baja - elif min_front < 1.2: - target_speed = 2.5 # Media distancia - elif avg_front > 2.0: - target_speed = self.max_speed # Camino libre - acelerar - else: - target_speed = self.base_speed - - # Reducir velocidad menos por ángulo de dirección - angle_penalty = (abs(steering_angle) / self.max_steering_angle) * 0.2 - target_speed *= (1.0 - angle_penalty) - - target_speed = np.clip(target_speed, self.min_speed, self.max_speed) - - # Filtrado muy responsive - self.speed_filter = (self.speed_filter_alpha * self.speed_filter + - (1 - self.speed_filter_alpha) * target_speed) - - return self.speed_filter - - def lidar_callback(self, data): - """Callback principal con manejo de situaciones de atasco""" - self.scan_count += 1 - if self.scan_count < 2: # Inicialización rápida - return - - ranges = self.preprocess_lidar(data.ranges) - - # Preparar mensaje de drive - drive_msg = AckermannDriveStamped() - drive_msg.header.stamp = self.get_clock().now().to_msg() - drive_msg.header.frame_id = "base_link" - - # Verificar si está en modo escape - if self.escape_mode: - speed, steering_angle = self.escape_behavior(ranges) - self.escape_counter += 1 - - # Salir del modo escape después de un tiempo - if self.escape_counter > 30: # ~3 segundos - self.escape_mode = False - self.escape_counter = 0 - self.stuck_counter = 0 - self.position_history.clear() - self.get_logger().info("Exiting ESCAPE MODE") - - else: - # Comportamiento normal - closest_idx = np.argmin(ranges) - closest_distance = ranges[closest_idx] - - # Burbuja de seguridad mínima - if closest_distance < 0.3: - bubble_ranges = self.create_safety_bubble(ranges, closest_idx) - else: - bubble_ranges = ranges - - # Encontrar el mejor gap - best_gap = self.select_best_gap(bubble_ranges) - - if best_gap is not None: - start_idx, end_idx = best_gap - target_idx = self.get_target_point(start_idx, end_idx, ranges) - - if target_idx is not None: - steering_angle = self.calculate_steering_angle(target_idx, data) - speed = self.calculate_speed(ranges, steering_angle) - else: - speed = self.min_speed - steering_angle = 0.0 - else: - # No hay gaps - buscar el lado con más espacio - center_idx = len(ranges) // 2 - left_space = np.mean(ranges[:center_idx]) - right_space = np.mean(ranges[center_idx:]) - - speed = 1.5 - if left_space > right_space: - steering_angle = np.radians(25) - else: - steering_angle = np.radians(-25) - - self.get_logger().info(f"No gaps found. Turning {'LEFT' if left_space > right_space else 'RIGHT'}") - - # Verificar frenado de emergencia - center_idx = len(ranges) // 2 - emergency_range = int(np.radians(15) / (2 * np.pi / len(ranges))) - emergency_distances = ranges[max(0, center_idx - emergency_range): - min(len(ranges), center_idx + emergency_range)] - - if np.min(emergency_distances) < 0.2: # Obstáculo muy muy cerca - speed = 0.0 - self.get_logger().warn("EMERGENCY BRAKE!") - - drive_msg.drive.speed = float(speed) - drive_msg.drive.steering_angle = float(steering_angle) - self.drive_pub.publish(drive_msg) - - def odom_callback(self, msg): - """Callback de odometría con detección de atasco""" - x = msg.pose.pose.position.x - y = msg.pose.pose.position.y - current_position = (x, y) - - # Verificar si está atascado - if self.check_if_stuck(current_position) and not self.escape_mode: - self.escape_mode = True - self.escape_counter = 0 - self.get_logger().warn("STUCK DETECTED! Activating ESCAPE MODE") - - # Control de vueltas (zona de inicio ajustada) - in_start_zone = (abs(x) < 2.0 and abs(y) < 2.0) - - if in_start_zone and not self.prev_in_start_zone and self.has_left_start_zone: - lap_end_time = time.time() - lap_duration = lap_end_time - self.lap_start_time - self.lap_times.append(lap_duration) - self.lap_count += 1 - - self.get_logger().info(f"🏁 Vuelta {self.lap_count} completada en {lap_duration:.2f} s.") - - if self.lap_times: - best_time = min(self.lap_times) - self.get_logger().info(f"Mejor tiempo: {best_time:.2f}s") - - self.lap_start_time = lap_end_time - - if not in_start_zone: - self.has_left_start_zone = True - - self.prev_in_start_zone = in_start_zone - - -def main(args=None): - rclpy.init(args=args) - node = ReactiveFollowGap() - try: - rclpy.spin(node) - except KeyboardInterrupt: - node.get_logger().info("Node stopped cleanly") - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/controllers/controllers/proyectoRob.py b/controllers/controllers/proyectoRob.py deleted file mode 100644 index ce1f7c5..0000000 --- a/controllers/controllers/proyectoRob.py +++ /dev/null @@ -1,147 +0,0 @@ -import rclpy -from rclpy.node import Node -import numpy as np -import math -from sensor_msgs.msg import LaserScan -from ackermann_msgs.msg import AckermannDriveStamped -from std_msgs.msg import String -from nav_msgs.msg import Odometry - -class FollowGapCarrera(Node): - def __init__(self): - super().__init__('follow_gap_carrera_node') - - self.VELOCIDAD_MAXIMA = 8.2 - self.VELOCIDAD_MINIMA = 3.5 - self.FACTOR_REDUCCION_CURVA = 1.2 - self.DISTANCIA_MAX_LIDAR = 3.5 - self.RADIO_BURBUJA = 0.25 - self.ANGULO_MAX_DIRECCION = np.radians(20) - self.PUNTO_LEJANO_BIAS = 0.8 - self.SUAVIZADO_DIRECCION = 0.9 - self.ultimo_angulo_direccion = 0.0 - - self.FINISH_LINE_X = 0.00 - self.FINISH_LINE_Y = 0.00 - self.FINISH_LINE_Y_RANGE = 5.0 - self.MIN_DISTANCE_TO_ARM = 0.2 - - self.last_pose = None - self.can_count_lap = True - self.race_started = True - - self.lap_publisher = self.create_publisher(String, '/lap_trigger', 10) - self.odom_sub = self.create_subscription(Odometry, '/ego_racecar/odom', self.odom_callback, 10) - msg_out = String() - msg_out.data = "Nueva vuelta" - self.lap_publisher.publish(msg_out) - - lidarscan_topic = '/scan' - drive_topic = '/drive' - self.drive_pub = self.create_publisher(AckermannDriveStamped, drive_topic, 10) - self.scan_sub = self.create_subscription(LaserScan, lidarscan_topic, self.lidar_callback, 10) - - - def odom_callback(self, msg: Odometry): - current_pose = msg.pose.pose - - if self.last_pose is None: - self.last_pose = current_pose - return - - last_x = self.last_pose.position.x - current_x = current_pose.position.x - current_y = current_pose.position.y - - crossed_finish_line = (last_x < self.FINISH_LINE_X and current_x >= self.FINISH_LINE_X) - on_finish_straight = abs(current_y - self.FINISH_LINE_Y) < self.FINISH_LINE_Y_RANGE - - if crossed_finish_line and on_finish_straight and self.can_count_lap: - msg_out = String() - msg_out.data = "Nueva vuelta" - self.lap_publisher.publish(msg_out) - - self.can_count_lap = False - - if not self.can_count_lap: - dist_from_finish = math.sqrt((current_pose.position.x - self.FINISH_LINE_X)**2) - if dist_from_finish > self.MIN_DISTANCE_TO_ARM: - self.can_count_lap = True - - self.last_pose = current_pose - - - def lidar_callback(self, msg): - ranges = np.array(msg.ranges, dtype=np.float32) - ranges[np.isinf(ranges) | np.isnan(ranges)] = self.DISTANCIA_MAX_LIDAR - ranges = np.clip(ranges, 0.0, self.DISTANCIA_MAX_LIDAR) - - idx_closest = np.argmin(ranges) - dist_closest = ranges[idx_closest] - - if dist_closest > 0.1: - angle_span = 2 * np.arctan2(self.RADIO_BURBUJA, dist_closest) - bubble_radius_idx = int(angle_span / msg.angle_increment) - else: - bubble_radius_idx = 30 - start_idx = max(0, idx_closest - bubble_radius_idx) - end_idx = min(len(ranges) - 1, idx_closest + bubble_radius_idx) - ranges[start_idx : end_idx + 1] = 0.0 - - max_len = 0 - best_start_idx = 0 - best_end_idx = 0 - current_len = 0 - current_start_idx = 0 - for i in range(len(ranges)): - if ranges[i] > 0.1: - if current_len == 0: - current_start_idx = i - current_len += 1 - else: - if current_len > max_len: - max_len = current_len - best_start_idx = current_start_idx - best_end_idx = i - 1 - current_len = 0 - if current_len > max_len: - max_len = current_len - best_start_idx = current_start_idx - best_end_idx = len(ranges) - 1 - - drive_msg = AckermannDriveStamped() - - if max_len > 0: - gap_ranges = ranges[best_start_idx : best_end_idx + 1] - idx_punto_lejano_relativo = np.argmax(gap_ranges) - idx_punto_lejano_absoluto = best_start_idx + idx_punto_lejano_relativo - idx_centro_gap = (best_start_idx + best_end_idx) // 2 - target_idx = int(self.PUNTO_LEJANO_BIAS * idx_punto_lejano_absoluto + (1 - self.PUNTO_LEJANO_BIAS) * idx_centro_gap) - target_angle = msg.angle_min + target_idx * msg.angle_increment - - steering_angle = self.SUAVIZADO_DIRECCION * self.ultimo_angulo_direccion + (1 - self.SUAVIZADO_DIRECCION) * target_angle - self.ultimo_angulo_direccion = steering_angle - steering_angle = np.clip(steering_angle, -self.ANGULO_MAX_DIRECCION, self.ANGULO_MAX_DIRECCION) - - severidad_curva = abs(target_angle) / self.ANGULO_MAX_DIRECCION - - severidad_curva = np.clip(severidad_curva, 0.0, 1.0) - - factor_reduccion = (1.0 - severidad_curva) ** self.FACTOR_REDUCCION_CURVA - speed = self.VELOCIDAD_MINIMA + (self.VELOCIDAD_MAXIMA - self.VELOCIDAD_MINIMA) * factor_reduccion - - drive_msg.drive.steering_angle = float(steering_angle) - drive_msg.drive.speed = float(speed) - else: - drive_msg.drive.steering_angle = 0.0 - drive_msg.drive.speed = 0.0 - self.ultimo_angulo_direccion = 0.0 - - self.drive_pub.publish(drive_msg) - -def main(args=None): - rclpy.init(args=args) - node = FollowGapCarrera() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() \ No newline at end of file diff --git a/controllers/controllers/proyectoWF.py b/controllers/controllers/proyectoWF.py deleted file mode 100644 index e650f00..0000000 --- a/controllers/controllers/proyectoWF.py +++ /dev/null @@ -1,182 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -import numpy as np -import time -from sensor_msgs.msg import LaserScan -from ackermann_msgs.msg import AckermannDriveStamped -from nav_msgs.msg import Odometry - -class WallFollower(Node): - def __init__(self): - super().__init__('wall_follower_node') - - # Topics - lidarscan_topic = '/scan' - drive_topic = '/drive' - odom_topic = '/ego_racecar/odom' - - # Parámetros del algoritmo de seguimiento de pared - self.max_speed = 2.5 # Velocidad máxima reducida a la mitad - self.min_speed = 1.5 # Velocidad mínima ajustada - self.max_lidar_range = 10.0 - self.wall_distance_ref = 1.20 # Distancia de referencia a la pared aumentada a 2 metros - - # PID para el ángulo - self.steering_pid_kp = 1.10 - self.steering_pid_ki = 0.00 - self.steering_pid_kd = 0.01 - self.last_error_angle = 0.0 - self.integral_angle = 0.0 - - # PID para la velocidad - self.speed_pid_kp = 0.5 - self.speed_pid_ki = 0.0 - self.speed_pid_kd = 0.01 - self.last_error_speed = 0.0 - self.integral_speed = 0.0 - - # Control de vueltas (se mantiene sin cambios) - self.lap_start_time = time.time() - self.lap_count = 0 - self.prev_in_start_zone = False - self.lap_times = [] - self.has_left_start_zone = False - - # Ángulo de detección - self.angle_range_deg = 45 # Ángulo configurable, 45 grados por defecto - - # Publishers & Subscribers - self.drive_pub = self.create_publisher(AckermannDriveStamped, drive_topic, 10) - self.scan_sub = self.create_subscription(LaserScan, lidarscan_topic, self.lidar_callback, 10) - self.odom_sub = self.create_subscription(Odometry, odom_topic, self.odom_callback, 10) - - self.get_logger().info("WallFollower node initialized.") - - def preprocess_lidar(self, ranges): - # Preprocesa los datos del LiDAR: reemplaza valores inválidos y aplica un filtro - proc = np.array(ranges, dtype=np.float32) - invalid_mask = (proc == 0) | np.isinf(proc) | np.isnan(proc) - proc[invalid_mask] = self.max_lidar_range - proc = np.clip(proc, 0.1, self.max_lidar_range) - kernel = np.ones(3) / 3 - return np.convolve(proc, kernel, mode='same') - - def select_cleanest_wall(self, ranges, angle_increment): - # Selecciona el lado con la pared más 'limpia' (menos protuberancias) - num_ranges = len(ranges) - mid_idx = num_ranges // 2 - - # Rango de ángulos para considerar los lados derecho e izquierdo - points_to_check = int(np.radians(self.angle_range_deg) / angle_increment) - - right_ranges = ranges[mid_idx - points_to_check : mid_idx] - left_ranges = ranges[mid_idx : mid_idx + points_to_check] - - # Calcular la desviación estándar para cada lado - std_dev_right = np.std(right_ranges) - std_dev_left = np.std(left_ranges) - - # Si el lado derecho es más 'limpio' (menor desviación), lo seguimos. - if std_dev_right < std_dev_left: - return 'right', right_ranges - else: - return 'left', left_ranges - - def calculate_pid(self, error, last_error, integral, kp, ki, kd): - # Cálculo de PID - integral += error - derivative = error - last_error - output = kp * error + ki * integral + kd * derivative - return output, integral - - def get_wall_following_angle(self, side_to_follow, side_ranges): - # Obtiene el ángulo necesario para seguir la pared con menos obstáculos detectables - avg_dist = np.mean(side_ranges) - error = self.wall_distance_ref - avg_dist - - steering_angle, self.integral_angle = self.calculate_pid( - error, self.last_error_angle, self.integral_angle, - self.steering_pid_kp, self.steering_pid_ki, self.steering_pid_kd - ) - self.last_error_angle = error - - if side_to_follow == 'left': - steering_angle = -steering_angle - - return np.clip(steering_angle, -0.20, 0.20) - - def calculate_speed(self, ranges, steering_angle): - # Ajusta la velocidad en función de la distancia frontal y el ángulo de dirección - # Reducir la velocidad en curvas cerradas - speed = self.max_speed - abs(steering_angle) * 1.5 - - # Usa PID para ajustar la velocidad según la distancia frontal - front_range = int(np.radians(20) / (2 * np.pi / len(ranges))) - front_distances = ranges[len(ranges) // 2 - front_range : len(ranges) // 2 + front_range] - min_front_distance = np.min(front_distances) - - speed_error = min_front_distance - 2.0 - speed_adjustment, self.integral_speed = self.calculate_pid( - speed_error, self.last_error_speed, self.integral_speed, - self.speed_pid_kp, self.speed_pid_ki, self.speed_pid_kd - ) - self.last_error_speed = speed_error - - speed += speed_adjustment - - return np.clip(speed, self.min_speed, self.max_speed) - - def lidar_callback(self, data): - ranges = self.preprocess_lidar(data.ranges) - - # Seleccionar la pared más 'limpia' - side_to_follow, side_ranges = self.select_cleanest_wall(ranges, data.angle_increment) - - # Calcular ángulo y velocidad - steering_angle = self.get_wall_following_angle(side_to_follow, side_ranges) - speed = self.calculate_speed(ranges, steering_angle) - - drive_msg = AckermannDriveStamped() - drive_msg.header.stamp = self.get_clock().now().to_msg() - drive_msg.header.frame_id = "base_link" - drive_msg.drive.speed = float(speed) - drive_msg.drive.steering_angle = float(steering_angle) - - self.drive_pub.publish(drive_msg) - - def odom_callback(self, msg): - x = msg.pose.pose.position.x - y = msg.pose.pose.position.y - - in_start_zone = (abs(x) < 2.0 and abs(y) < 2.0) - - if in_start_zone and not self.prev_in_start_zone and self.has_left_start_zone: - lap_end_time = time.time() - lap_duration = lap_end_time - self.lap_start_time - self.lap_times.append(lap_duration) - self.lap_count += 1 - self.get_logger().info(f"🏁 Vuelta {self.lap_count} completada en {lap_duration:.2f} s.") - self.lap_start_time = lap_end_time - - - if not in_start_zone: - self.has_left_start_zone = True - - self.prev_in_start_zone = in_start_zone - -def main(args=None): - rclpy.init(args=args) - node = WallFollower() - try: - rclpy.spin(node) - except KeyboardInterrupt: - node.get_logger().info("Node stopped cleanly") - finally: - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() - diff --git a/controllers/controllers/purepursuit_node.py b/controllers/controllers/purepursuit_node.py deleted file mode 100644 index 25def76..0000000 --- a/controllers/controllers/purepursuit_node.py +++ /dev/null @@ -1,40 +0,0 @@ -#!/usr/bin/env python3 -import rclpy -from rclpy.node import Node - -import numpy as np -from sensor_msgs.msg import LaserScan -from ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive -# TODO CHECK: include needed ROS msg type headers and libraries - -class PurePursuit(Node): - """ - Implement Pure Pursuit on the car - This is just a template, you are free to implement your own node! - """ - def __init__(self): - super().__init__('pure_pursuit_node') - # TODO: create ROS subscribers and publishers - - def pose_callback(self, pose_msg): - pass - # TODO: find the current waypoint to track using methods mentioned in lecture - - # TODO: transform goal point to vehicle frame of reference - - # TODO: calculate curvature/steering angle - - # TODO: publish drive message, don't forget to limit the steering angle. - -def main(args=None): - rclpy.init(args=args) - print("PurePursuit Initialized") - pure_pursuit_node = PurePursuit() - rclpy.spin(pure_pursuit_node) - - pure_pursuit_node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/controllers/controllers/rrt_node.py b/controllers/controllers/rrt_node.py deleted file mode 100644 index 91c9e48..0000000 --- a/controllers/controllers/rrt_node.py +++ /dev/null @@ -1,218 +0,0 @@ -""" -This file contains the class definition for tree nodes and RRT -Before you start, please read: https://arxiv.org/pdf/1105.1186.pdf -""" -import numpy as np -from numpy import linalg as LA -import math - -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import LaserScan -from geometry_msgs.msg import PoseStamped -from geometry_msgs.msg import PointStamped -from geometry_msgs.msg import Pose -from geometry_msgs.msg import Point -from nav_msgs.msg import Odometry -from ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive -from nav_msgs.msg import OccupancyGrid - -# TODO: import as you need - -# class def for tree nodes -# It's up to you if you want to use this -class Node(object): - def __init__(self): - self.x = None - self.y = None - self.parent = None - self.cost = None # only used in RRT* - self.is_root = False - -# class def for RRT -class RRT(Node): - def __init__(self): - # topics, not saved as attributes - # TODO: grab topics from param file, you'll need to change the yaml file - pose_topic = "ego_racecar/odom" - scan_topic = "/scan" - - # you could add your own parameters to the rrt_params.yaml file, - # and get them here as class attributes as shown above. - - # TODO: create subscribers - self.pose_sub_ = self.create_subscription( - PoseStamped, - pose_topic, - self.pose_callback, - 1) - self.pose_sub_ - - self.scan_sub_ = self.create_subscription( - LaserScan, - scan_topic, - self.scan_callback, - 1) - self.scan_sub_ - - # publishers - # TODO: create a drive message publisher, and other publishers that you might need - - # class attributes - # TODO: maybe create your occupancy grid here - - def scan_callback(self, scan_msg): - """ - LaserScan callback, you should update your occupancy grid here - - Args: - scan_msg (LaserScan): incoming message from subscribed topic - Returns: - - """ - - def pose_callback(self, pose_msg): - """ - The pose callback when subscribed to particle filter's inferred pose - Here is where the main RRT loop happens - - Args: - pose_msg (PoseStamped): incoming message from subscribed topic - Returns: - - """ - - return None - - def sample(self): - """ - This method should randomly sample the free space, and returns a viable point - - Args: - Returns: - (x, y) (float float): a tuple representing the sampled point - - """ - x = None - y = None - return (x, y) - - def nearest(self, tree, sampled_point): - """ - This method should return the nearest node on the tree to the sampled point - - Args: - tree ([]): the current RRT tree - sampled_point (tuple of (float, float)): point sampled in free space - Returns: - nearest_node (int): index of neareset node on the tree - """ - nearest_node = 0 - return nearest_node - - def steer(self, nearest_node, sampled_point): - """ - This method should return a point in the viable set such that it is closer - to the nearest_node than sampled_point is. - - Args: - nearest_node (Node): nearest node on the tree to the sampled point - sampled_point (tuple of (float, float)): sampled point - Returns: - new_node (Node): new node created from steering - """ - new_node = None - return new_node - - def check_collision(self, nearest_node, new_node): - """ - This method should return whether the path between nearest and new_node is - collision free. - - Args: - nearest (Node): nearest node on the tree - new_node (Node): new node from steering - Returns: - collision (bool): whether the path between the two nodes are in collision - with the occupancy grid - """ - return True - - def is_goal(self, latest_added_node, goal_x, goal_y): - """ - This method should return whether the latest added node is close enough - to the goal. - - Args: - latest_added_node (Node): latest added node on the tree - goal_x (double): x coordinate of the current goal - goal_y (double): y coordinate of the current goal - Returns: - close_enough (bool): true if node is close enoughg to the goal - """ - return False - - def find_path(self, tree, latest_added_node): - """ - This method returns a path as a list of Nodes connecting the starting point to - the goal once the latest added node is close enough to the goal - - Args: - tree ([]): current tree as a list of Nodes - latest_added_node (Node): latest added node in the tree - Returns: - path ([]): valid path as a list of Nodes - """ - path = [] - return path - - - - # The following methods are needed for RRT* and not RRT - def cost(self, tree, node): - """ - This method should return the cost of a node - - Args: - node (Node): the current node the cost is calculated for - Returns: - cost (float): the cost value of the node - """ - return 0 - - def line_cost(self, n1, n2): - """ - This method should return the cost of the straight line between n1 and n2 - - Args: - n1 (Node): node at one end of the straight line - n2 (Node): node at the other end of the straint line - Returns: - cost (float): the cost value of the line - """ - return 0 - - def near(self, tree, node): - """ - This method should return the neighborhood of nodes around the given node - - Args: - tree ([]): current tree as a list of Nodes - node (Node): current node we're finding neighbors for - Returns: - neighborhood ([]): neighborhood of nodes as a list of Nodes - """ - neighborhood = [] - return neighborhood - -def main(args=None): - rclpy.init(args=args) - print("RRT Initialized") - rrt_node = RRT() - rclpy.spin(rrt_node) - - rrt_node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/controllers/controllers/temporizador.py b/controllers/controllers/temporizador.py deleted file mode 100644 index e4afe42..0000000 --- a/controllers/controllers/temporizador.py +++ /dev/null @@ -1,52 +0,0 @@ -import rclpy -from rclpy.node import Node -from std_msgs.msg import String - - -class LapTimeLogger(Node): - def __init__(self): - super().__init__('timer_node') - - self.lap_count = 0 - self.best_lap_time_s = 9999.0 - self.last_trigger_time = None - self.clock = self.get_clock() - - self.subscription = self.create_subscription(String,'/lap_trigger',self.lap_callback,10) - - - def lap_callback(self, msg: String): - current_time = self.clock.now() - - if self.last_trigger_time is None: - self.last_trigger_time = current_time - self.get_logger().info("Carrera iniciada") - return - - lap_duration = current_time - self.last_trigger_time - lap_time_s = lap_duration.nanoseconds / 1e9 - - self.lap_count += 1 - - if lap_time_s < self.best_lap_time_s: - self.best_lap_time_s = lap_time_s - best_lap_str = f"Vuelta completada en menor tiempo" - else: - best_lap_str = "" - - self.get_logger().info(f"Vuelta {self.lap_count} completada en" f": {lap_time_s:.4f}s | {best_lap_str}") - - if self.lap_count ==10: - self.get_logger().info(f"5 vueltas completadas. Tiempo mejor vuelta:{self.best_lap_time_s:.2f}s") - - self.last_trigger_time = current_time - -def main(args=None): - rclpy.init(args=args) - lap_logger_node = LapTimeLogger() - rclpy.spin(lap_logger_node) - lap_logger_node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/controllers/controllers/ttc_node.py b/controllers/controllers/ttc_node.py deleted file mode 100644 index 0af5665..0000000 --- a/controllers/controllers/ttc_node.py +++ /dev/null @@ -1,54 +0,0 @@ -#!/usr/bin/env python3 -import rclpy -from rclpy.node import Node - -import numpy as np -# TODO: include needed ROS msg type headers and libraries -from sensor_msgs.msg import LaserScan -from nav_msgs.msg import Odometry -from ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive - - -class SafetyNode(Node): - """ - The class that handles emergency braking. - """ - def __init__(self): - super().__init__('safety_node') - """ - One publisher should publish to the /drive topic with a AckermannDriveStamped drive message. - - You should also subscribe to the /scan topic to get the LaserScan messages and - the /ego_racecar/odom topic to get the current speed of the vehicle. - - The subscribers should use the provided odom_callback and scan_callback as callback methods - - NOTE that the x component of the linear velocity in odom is the speed - """ - self.speed = 0. - # TODO: create ROS subscribers and publishers. - - def odom_callback(self, odom_msg): - # TODO: update current speed - self.speed = 0. - - def scan_callback(self, scan_msg): - # TODO: calculate TTC - - # TODO: publish command to brake - pass - -def main(args=None): - rclpy.init(args=args) - safety_node = SafetyNode() - rclpy.spin(safety_node) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - safety_node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/controllers/package.xml b/controllers/package.xml deleted file mode 100644 index 6c0c8fd..0000000 --- a/controllers/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - controllers - 0.0.0 - TODO: Package description - israel - TODO: License declaration - - rclpy - std_msgs - ackermann_msgs - std_srvs - sensor_msgs - nav_msgs - geometry_msgs - visualization_msgs - - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/controllers/resource/controllers b/controllers/resource/controllers deleted file mode 100644 index e69de29..0000000 diff --git a/controllers/setup.cfg b/controllers/setup.cfg deleted file mode 100644 index 83202a3..0000000 --- a/controllers/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/controllers -[install] -install_scripts=$base/lib/controllers diff --git a/controllers/setup.py b/controllers/setup.py deleted file mode 100644 index 9564a7b..0000000 --- a/controllers/setup.py +++ /dev/null @@ -1,39 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'controllers' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='israel', - maintainer_email='israel@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'ttc_node = controllers.ttc_node:main', - 'pid_node = controllers.pid_node:main', - 'gap_node = controllers.gap_node:main', - 'purepursuit_node = controllers.purepursuit_node:main', - 'rrt_node = controllers.rrt_node:main', - 'mpc_node = controllers.mpc_node:main', - 'proyectoFTG = controllers.proyectoFTG:main', - 'temporizador = controllers.temporizador:main', - 'proyectoRob = controllers.proyectoRob:main', - 'proyectoObstaculos = controllers.proyectoObstaculos:main', - 'proyectoWF = controllers.proyectoWF:main', - 'obstaculosWF=controllers.obstaculosWF:main', - 'obstaculosFTG=controllers.obstaculosFTG:main', - 'graficas=controllers.graficas:main', - ], - }, -) diff --git a/controllers/test/test_copyright.py b/controllers/test/test_copyright.py deleted file mode 100644 index 97a3919..0000000 --- a/controllers/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# 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' diff --git a/controllers/test/test_flake8.py b/controllers/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/controllers/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# 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) diff --git a/controllers/test/test_pep257.py b/controllers/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/controllers/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# 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_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/f1tenth_gym_ros/LICENSE b/f1tenth_gym_ros/LICENSE deleted file mode 100644 index cd28e87..0000000 --- a/f1tenth_gym_ros/LICENSE +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) 2020 Hongrui Zheng - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. diff --git a/f1tenth_gym_ros/config/ackermann_controllers.yaml b/f1tenth_gym_ros/config/ackermann_controllers.yaml deleted file mode 100644 index 65f16b1..0000000 --- a/f1tenth_gym_ros/config/ackermann_controllers.yaml +++ /dev/null @@ -1,76 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 100 # Hz - - # Lista de controladores - ackermann_steering_controller: - type: ackermann_steering_controller/AckermannSteeringController - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - -# Configuración del controlador de dirección Ackermann -ackermann_steering_controller: - ros__parameters: - # Ruedas de tracción (delanteras) - front_wheels_names: ["front_left_hinge_to_wheel", "front_right_hinge_to_wheel"] - - # Ruedas traseras (solo para odometría) - rear_wheels_names: ["base_to_back_left_wheel", "base_to_back_right_wheel"] - - # Juntas de dirección - front_steering_names: ["base_to_front_left_hinge", "base_to_front_right_hinge"] - - # Parámetros del vehículo (basados en tu XACRO) - wheelbase: 0.24 # Distancia entre ejes - front_wheel_track: 0.20 # Distancia entre ruedas delanteras (wheel_length + width = 0.04 + 0.16) - rear_wheel_track: 0.20 # Distancia entre ruedas traseras - wheel_radius: 0.04 # Radio de las ruedas - - # Configuración de tópicos - reference_timeout: 2.0 - front_steering: true - - # Publicar odometría - publish_rate: 50.0 - odom_frame_id: odom - base_frame_id: ego_racecar/base_link - pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - - # Habilitar odometría - enable_odom_tf: true - - # Comandos de velocidad - linear: - x: - has_velocity_limits: true - has_acceleration_limits: true - max_velocity: 3.0 # m/s (reducido para F1Tenth) - min_velocity: -3.0 # m/s - max_acceleration: 1.5 # m/s^2 - min_acceleration: -1.5 # m/s^2 - - angular: - z: - has_velocity_limits: true - has_acceleration_limits: true - max_velocity: 2.0 # rad/s - min_velocity: -2.0 # rad/s - max_acceleration: 1.0 # rad/s^2 - min_acceleration: -1.0 # rad/s^2 - -# Configuración del publicador de estados de juntas -joint_state_broadcaster: - ros__parameters: - joints: - - front_left_hinge_to_wheel - - front_right_hinge_to_wheel - - base_to_back_left_wheel - - base_to_back_right_wheel - - base_to_front_left_hinge - - base_to_front_right_hinge - - interfaces: - - position - - velocity \ No newline at end of file diff --git a/f1tenth_gym_ros/config/gazebo_params.yaml b/f1tenth_gym_ros/config/gazebo_params.yaml deleted file mode 100644 index 5c61215..0000000 --- a/f1tenth_gym_ros/config/gazebo_params.yaml +++ /dev/null @@ -1,16 +0,0 @@ -gazebo: - ros__parameters: - publish_rate: 400.0 - -physics: - ros__parameters: - ode: - solver: - type: "quick" - iters: 50 - sor: 1.4 - constraints: - cfm: 0.0 - erp: 0.2 - contact_max_correcting_vel: 0.1 - contact_surface_layer: 0.0 \ No newline at end of file diff --git a/f1tenth_gym_ros/config/sim.yaml b/f1tenth_gym_ros/config/sim.yaml deleted file mode 100644 index 193a9ac..0000000 --- a/f1tenth_gym_ros/config/sim.yaml +++ /dev/null @@ -1,62 +0,0 @@ -# MIT License - -# Copyright (c) 2020 Hongrui Zheng - -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: - -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. - -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. - -bridge: - ros__parameters: - # topics and namespaces - ego_namespace: 'ego_racecar' - ego_scan_topic: 'scan' - ego_odom_topic: 'odom' - ego_opp_odom_topic: 'opp_odom' - ego_drive_topic: 'drive' - opp_namespace: 'opp_racecar' - opp_scan_topic: 'opp_scan' - opp_odom_topic: 'odom' - opp_ego_odom_topic: 'opp_odom' - opp_drive_topic: 'opp_drive' - - # transform related - scan_distance_to_base_link: 0.0 - - # laserscan parameters - scan_fov: 4.7 - scan_beams: 1080 - - # map parameters - map_path: '/home/ivan/F1Tenth-Repository/src/f1tenth_gym_ros/maps/levineB' - map_img_ext: '.png' - - # opponent parameters - num_agent: 1 - - # ego starting pose on map - sx: 0.0 - sy: 0.0 - stheta: 3.420 - - # opp starting pose on map - sx1: 9.5 - sy1: 8.5 - stheta1: 3.14 - - # teleop - kb_teleop: True diff --git a/f1tenth_gym_ros/f1tenth_gym_ros/__init__.py b/f1tenth_gym_ros/f1tenth_gym_ros/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/f1tenth_gym_ros/f1tenth_gym_ros/gym_bridge.py b/f1tenth_gym_ros/f1tenth_gym_ros/gym_bridge.py deleted file mode 100644 index 5869c50..0000000 --- a/f1tenth_gym_ros/f1tenth_gym_ros/gym_bridge.py +++ /dev/null @@ -1,420 +0,0 @@ -# MIT License - -# Copyright (c) 2020 Hongrui Zheng - -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: - -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. - -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. - -import rclpy -from rclpy.node import Node - -from sensor_msgs.msg import LaserScan -from nav_msgs.msg import Odometry -from geometry_msgs.msg import PoseStamped -from geometry_msgs.msg import PoseWithCovarianceStamped -from geometry_msgs.msg import Twist -from geometry_msgs.msg import TransformStamped -from geometry_msgs.msg import Transform -from geometry_msgs.msg import Quaternion -from ackermann_msgs.msg import AckermannDriveStamped -from tf2_ros import TransformBroadcaster - -import gym -import numpy as np -from transforms3d import euler - -class GymBridge(Node): - def __init__(self): - super().__init__('gym_bridge') - - self.declare_parameter('ego_namespace', '') # Tipo 'str' con valor predeterminado vacío - self.declare_parameter('ego_odom_topic', '') # Tipo 'str' con valor predeterminado vacío - self.declare_parameter('ego_opp_odom_topic', '') # Tipo 'str' con valor predeterminado vacío - self.declare_parameter('ego_scan_topic', '') # Tipo 'str' con valor predeterminado vacío - self.declare_parameter('ego_drive_topic', '') # Tipo 'str' con valor predeterminado vacío - - self.declare_parameter('opp_namespace', '') # Tipo 'str' con valor predeterminado vacío - self.declare_parameter('opp_odom_topic', '') # Tipo 'str' con valor predeterminado vacío - self.declare_parameter('opp_ego_odom_topic', '') # Tipo 'str' con valor predeterminado vacío - self.declare_parameter('opp_scan_topic', '') # Tipo 'str' con valor predeterminado vacío - self.declare_parameter('opp_drive_topic', '') # Tipo 'str' con valor predeterminado vacío - - self.declare_parameter('scan_distance_to_base_link', 0.0) # Tipo 'float' con valor predeterminado - self.declare_parameter('scan_fov', 0.0) # Tipo 'float' con valor predeterminado - self.declare_parameter('scan_beams', 0) # Tipo 'int' con valor predeterminado - - self.declare_parameter('map_path', '') # Tipo 'str' con valor predeterminado vacío - self.declare_parameter('map_img_ext', '') # Tipo 'str' con valor predeterminado vacío - - self.declare_parameter('num_agent', 0) # Tipo 'int' con valor predeterminado - - self.declare_parameter('sx', 0.0) # Tipo 'float' con valor predeterminado - self.declare_parameter('sy', 0.0) # Tipo 'float' con valor predeterminado - self.declare_parameter('stheta', 0.0) # Tipo 'float' con valor predeterminado - - self.declare_parameter('sx1', 0.0) # Tipo 'float' con valor predeterminado - self.declare_parameter('sy1', 0.0) # Tipo 'float' con valor predeterminado - self.declare_parameter('stheta1', 0.0) # Tipo 'float' con valor predeterminado - - self.declare_parameter('kb_teleop', False) # Tipo 'bool' con valor predeterminado - - - - # check num_agents - num_agents = self.get_parameter('num_agent').value - if num_agents < 1 or num_agents > 2: - raise ValueError('num_agents should be either 1 or 2.') - elif type(num_agents) != int: - raise ValueError('num_agents should be an int.') - - # env backend - self.env = gym.make('f110_gym:f110-v0', - map=self.get_parameter('map_path').value, - map_ext=self.get_parameter('map_img_ext').value, - num_agents=num_agents) - - sx = self.get_parameter('sx').value - sy = self.get_parameter('sy').value - stheta = self.get_parameter('stheta').value - self.ego_pose = [sx, sy, stheta] - self.ego_speed = [0.0, 0.0, 0.0] - self.ego_requested_speed = 0.0 - self.ego_steer = 0.0 - self.ego_collision = False - ego_scan_topic = self.get_parameter('ego_scan_topic').value - ego_drive_topic = self.get_parameter('ego_drive_topic').value - scan_fov = self.get_parameter('scan_fov').value - scan_beams = self.get_parameter('scan_beams').value - self.angle_min = -scan_fov / 2. - self.angle_max = scan_fov / 2. - self.angle_inc = scan_fov / scan_beams - self.ego_namespace = self.get_parameter('ego_namespace').value - ego_odom_topic = self.ego_namespace + '/' + self.get_parameter('ego_odom_topic').value - self.scan_distance_to_base_link = self.get_parameter('scan_distance_to_base_link').value - - if num_agents == 2: - self.has_opp = True - self.opp_namespace = self.get_parameter('opp_namespace').value - sx1 = self.get_parameter('sx1').value - sy1 = self.get_parameter('sy1').value - stheta1 = self.get_parameter('stheta1').value - self.opp_pose = [sx1, sy1, stheta1] - self.opp_speed = [0.0, 0.0, 0.0] - self.opp_requested_speed = 0.0 - self.opp_steer = 0.0 - self.opp_collision = False - self.obs, _ , self.done, _ = self.env.reset(np.array([[sx, sy, stheta], [sx1, sy1, stheta1]])) - self.ego_scan = list(self.obs['scans'][0]) - self.opp_scan = list(self.obs['scans'][1]) - - opp_scan_topic = self.get_parameter('opp_scan_topic').value - opp_odom_topic = self.opp_namespace + '/' + self.get_parameter('opp_odom_topic').value - opp_drive_topic = self.get_parameter('opp_drive_topic').value - - ego_opp_odom_topic = self.ego_namespace + '/' + self.get_parameter('ego_opp_odom_topic').value - opp_ego_odom_topic = self.opp_namespace + '/' + self.get_parameter('opp_ego_odom_topic').value - else: - self.has_opp = False - self.obs, _ , self.done, _ = self.env.reset(np.array([[sx, sy, stheta]])) - self.ego_scan = list(self.obs['scans'][0]) - - # sim physical step timer - self.drive_timer = self.create_timer(0.01, self.drive_timer_callback) - # topic publishing timer - self.timer = self.create_timer(0.004, self.timer_callback) - - # transform broadcaster - self.br = TransformBroadcaster(self) - - # publishers - self.ego_scan_pub = self.create_publisher(LaserScan, ego_scan_topic, 10) - self.ego_odom_pub = self.create_publisher(Odometry, ego_odom_topic, 10) - self.ego_drive_published = False - if num_agents == 2: - self.opp_scan_pub = self.create_publisher(LaserScan, opp_scan_topic, 10) - self.ego_opp_odom_pub = self.create_publisher(Odometry, ego_opp_odom_topic, 10) - self.opp_odom_pub = self.create_publisher(Odometry, opp_odom_topic, 10) - self.opp_ego_odom_pub = self.create_publisher(Odometry, opp_ego_odom_topic, 10) - self.opp_drive_published = False - - # subscribers - self.ego_drive_sub = self.create_subscription( - AckermannDriveStamped, - ego_drive_topic, - self.drive_callback, - 10) - self.ego_reset_sub = self.create_subscription( - PoseWithCovarianceStamped, - '/initialpose', - self.ego_reset_callback, - 10) - if num_agents == 2: - self.opp_drive_sub = self.create_subscription( - AckermannDriveStamped, - opp_drive_topic, - self.opp_drive_callback, - 10) - self.opp_reset_sub = self.create_subscription( - PoseStamped, - '/goal_pose', - self.opp_reset_callback, - 10) - - if self.get_parameter('kb_teleop').value: - self.teleop_sub = self.create_subscription( - Twist, - '/cmd_vel', - self.teleop_callback, - 10) - - - def drive_callback(self, drive_msg): - self.ego_requested_speed = drive_msg.drive.speed - self.ego_steer = drive_msg.drive.steering_angle - self.ego_drive_published = True - - def opp_drive_callback(self, drive_msg): - self.opp_requested_speed = drive_msg.drive.speed - self.opp_steer = drive_msg.drive.steering_angle - self.opp_drive_published = True - - def ego_reset_callback(self, pose_msg): - rx = pose_msg.pose.pose.position.x - ry = pose_msg.pose.pose.position.y - rqx = pose_msg.pose.pose.orientation.x - rqy = pose_msg.pose.pose.orientation.y - rqz = pose_msg.pose.pose.orientation.z - rqw = pose_msg.pose.pose.orientation.w - _, _, rtheta = euler.quat2euler([rqw, rqx, rqy, rqz], axes='sxyz') - if self.has_opp: - opp_pose = [self.obs['poses_x'][1], self.obs['poses_y'][1], self.obs['poses_theta'][1]] - self.obs, _ , self.done, _ = self.env.reset(np.array([[rx, ry, rtheta], opp_pose])) - else: - self.obs, _ , self.done, _ = self.env.reset(np.array([[rx, ry, rtheta]])) - - def opp_reset_callback(self, pose_msg): - if self.has_opp: - rx = pose_msg.pose.position.x - ry = pose_msg.pose.position.y - rqx = pose_msg.pose.orientation.x - rqy = pose_msg.pose.orientation.y - rqz = pose_msg.pose.orientation.z - rqw = pose_msg.pose.orientation.w - _, _, rtheta = euler.quat2euler([rqw, rqx, rqy, rqz], axes='sxyz') - self.obs, _ , self.done, _ = self.env.reset(np.array([list(self.ego_pose), [rx, ry, rtheta]])) - def teleop_callback(self, twist_msg): - if not self.ego_drive_published: - self.ego_drive_published = True - - self.ego_requested_speed = twist_msg.linear.x - - if twist_msg.angular.z > 0.0: - self.ego_steer = 0.3 - elif twist_msg.angular.z < 0.0: - self.ego_steer = -0.3 - else: - self.ego_steer = 0.0 - - def drive_timer_callback(self): - if self.ego_drive_published and not self.has_opp: - self.obs, _, self.done, _ = self.env.step(np.array([[self.ego_steer, self.ego_requested_speed]])) - elif self.ego_drive_published and self.has_opp and self.opp_drive_published: - self.obs, _, self.done, _ = self.env.step(np.array([[self.ego_steer, self.ego_requested_speed], [self.opp_steer, self.opp_requested_speed]])) - self._update_sim_state() - - def timer_callback(self): - ts = self.get_clock().now().to_msg() - - # pub scans - scan = LaserScan() - scan.header.stamp = ts - scan.header.frame_id = self.ego_namespace + '/laser' - scan.angle_min = self.angle_min - scan.angle_max = self.angle_max - scan.angle_increment = self.angle_inc - scan.range_min = 0. - scan.range_max = 30. - scan.ranges = self.ego_scan - self.ego_scan_pub.publish(scan) - - if self.has_opp: - opp_scan = LaserScan() - opp_scan.header.stamp = ts - opp_scan.header.frame_id = self.opp_namespace + '/laser' - opp_scan.angle_min = self.angle_min - opp_scan.angle_max = self.angle_max - opp_scan.angle_increment = self.angle_inc - opp_scan.range_min = 0. - opp_scan.range_max = 30. - opp_scan.ranges = self.opp_scan - self.opp_scan_pub.publish(opp_scan) - - # pub tf - self._publish_odom(ts) - self._publish_transforms(ts) - self._publish_laser_transforms(ts) - self._publish_wheel_transforms(ts) - - def _update_sim_state(self): - self.ego_scan = list(self.obs['scans'][0]) - if self.has_opp: - self.opp_scan = list(self.obs['scans'][1]) - self.opp_pose[0] = self.obs['poses_x'][1] - self.opp_pose[1] = self.obs['poses_y'][1] - self.opp_pose[2] = self.obs['poses_theta'][1] - self.opp_speed[0] = self.obs['linear_vels_x'][1] - self.opp_speed[1] = self.obs['linear_vels_y'][1] - self.opp_speed[2] = self.obs['ang_vels_z'][1] - - self.ego_pose[0] = self.obs['poses_x'][0] - self.ego_pose[1] = self.obs['poses_y'][0] - self.ego_pose[2] = self.obs['poses_theta'][0] - self.ego_speed[0] = self.obs['linear_vels_x'][0] - self.ego_speed[1] = self.obs['linear_vels_y'][0] - self.ego_speed[2] = self.obs['ang_vels_z'][0] - - - - def _publish_odom(self, ts): - ego_odom = Odometry() - ego_odom.header.stamp = ts - ego_odom.header.frame_id = 'map' - ego_odom.child_frame_id = self.ego_namespace + '/base_link' - ego_odom.pose.pose.position.x = self.ego_pose[0] - ego_odom.pose.pose.position.y = self.ego_pose[1] - ego_quat = euler.euler2quat(0., 0., self.ego_pose[2], axes='sxyz') - ego_odom.pose.pose.orientation.x = ego_quat[1] - ego_odom.pose.pose.orientation.y = ego_quat[2] - ego_odom.pose.pose.orientation.z = ego_quat[3] - ego_odom.pose.pose.orientation.w = ego_quat[0] - ego_odom.twist.twist.linear.x = self.ego_speed[0] - ego_odom.twist.twist.linear.y = self.ego_speed[1] - ego_odom.twist.twist.angular.z = self.ego_speed[2] - self.ego_odom_pub.publish(ego_odom) - - if self.has_opp: - opp_odom = Odometry() - opp_odom.header.stamp = ts - opp_odom.header.frame_id = 'map' - opp_odom.child_frame_id = self.opp_namespace + '/base_link' - opp_odom.pose.pose.position.x = self.opp_pose[0] - opp_odom.pose.pose.position.y = self.opp_pose[1] - opp_quat = euler.euler2quat(0., 0., self.opp_pose[2], axes='sxyz') - opp_odom.pose.pose.orientation.x = opp_quat[1] - opp_odom.pose.pose.orientation.y = opp_quat[2] - opp_odom.pose.pose.orientation.z = opp_quat[3] - opp_odom.pose.pose.orientation.w = opp_quat[0] - opp_odom.twist.twist.linear.x = self.opp_speed[0] - opp_odom.twist.twist.linear.y = self.opp_speed[1] - opp_odom.twist.twist.angular.z = self.opp_speed[2] - self.opp_odom_pub.publish(opp_odom) - self.opp_ego_odom_pub.publish(ego_odom) - self.ego_opp_odom_pub.publish(opp_odom) - - def _publish_transforms(self, ts): - ego_t = Transform() - ego_t.translation.x = self.ego_pose[0] - ego_t.translation.y = self.ego_pose[1] - ego_t.translation.z = 0.0 - ego_quat = euler.euler2quat(0.0, 0.0, self.ego_pose[2], axes='sxyz') - ego_t.rotation.x = ego_quat[1] - ego_t.rotation.y = ego_quat[2] - ego_t.rotation.z = ego_quat[3] - ego_t.rotation.w = ego_quat[0] - - ego_ts = TransformStamped() - ego_ts.transform = ego_t - ego_ts.header.stamp = ts - ego_ts.header.frame_id = 'map' - ego_ts.child_frame_id = self.ego_namespace + '/base_link' - self.br.sendTransform(ego_ts) - - if self.has_opp: - opp_t = Transform() - opp_t.translation.x = self.opp_pose[0] - opp_t.translation.y = self.opp_pose[1] - opp_t.translation.z = 0.0 - opp_quat = euler.euler2quat(0.0, 0.0, self.opp_pose[2], axes='sxyz') - opp_t.rotation.x = opp_quat[1] - opp_t.rotation.y = opp_quat[2] - opp_t.rotation.z = opp_quat[3] - opp_t.rotation.w = opp_quat[0] - - opp_ts = TransformStamped() - opp_ts.transform = opp_t - opp_ts.header.stamp = ts - opp_ts.header.frame_id = 'map' - opp_ts.child_frame_id = self.opp_namespace + '/base_link' - self.br.sendTransform(opp_ts) - - def _publish_wheel_transforms(self, ts): - ego_wheel_ts = TransformStamped() - ego_wheel_quat = euler.euler2quat(0., 0., self.ego_steer, axes='sxyz') - ego_wheel_ts.transform.rotation.x = ego_wheel_quat[1] - ego_wheel_ts.transform.rotation.y = ego_wheel_quat[2] - ego_wheel_ts.transform.rotation.z = ego_wheel_quat[3] - ego_wheel_ts.transform.rotation.w = ego_wheel_quat[0] - ego_wheel_ts.header.stamp = ts - ego_wheel_ts.header.frame_id = self.ego_namespace + '/front_left_hinge' - ego_wheel_ts.child_frame_id = self.ego_namespace + '/front_left_wheel' - self.br.sendTransform(ego_wheel_ts) - ego_wheel_ts.header.frame_id = self.ego_namespace + '/front_right_hinge' - ego_wheel_ts.child_frame_id = self.ego_namespace + '/front_right_wheel' - self.br.sendTransform(ego_wheel_ts) - - if self.has_opp: - opp_wheel_ts = TransformStamped() - opp_wheel_quat = euler.euler2quat(0., 0., self.opp_steer, axes='sxyz') - opp_wheel_ts.transform.rotation.x = opp_wheel_quat[1] - opp_wheel_ts.transform.rotation.y = opp_wheel_quat[2] - opp_wheel_ts.transform.rotation.z = opp_wheel_quat[3] - opp_wheel_ts.transform.rotation.w = opp_wheel_quat[0] - opp_wheel_ts.header.stamp = ts - opp_wheel_ts.header.frame_id = self.opp_namespace + '/front_left_hinge' - opp_wheel_ts.child_frame_id = self.opp_namespace + '/front_left_wheel' - self.br.sendTransform(opp_wheel_ts) - opp_wheel_ts.header.frame_id = self.opp_namespace + '/front_right_hinge' - opp_wheel_ts.child_frame_id = self.opp_namespace + '/front_right_wheel' - self.br.sendTransform(opp_wheel_ts) - - def _publish_laser_transforms(self, ts): - ego_scan_ts = TransformStamped() - ego_scan_ts.transform.translation.x = self.scan_distance_to_base_link - # ego_scan_ts.transform.translation.z = 0.04+0.1+0.025 - ego_scan_ts.transform.rotation.w = 1. - ego_scan_ts.header.stamp = ts - ego_scan_ts.header.frame_id = self.ego_namespace + '/base_link' - ego_scan_ts.child_frame_id = self.ego_namespace + '/laser' - self.br.sendTransform(ego_scan_ts) - - if self.has_opp: - opp_scan_ts = TransformStamped() - opp_scan_ts.transform.translation.x = self.scan_distance_to_base_link - opp_scan_ts.transform.rotation.w = 1. - opp_scan_ts.header.stamp = ts - opp_scan_ts.header.frame_id = self.opp_namespace + '/base_link' - opp_scan_ts.child_frame_id = self.opp_namespace + '/laser' - self.br.sendTransform(opp_scan_ts) - -def main(args=None): - rclpy.init(args=args) - gym_bridge = GymBridge() - rclpy.spin(gym_bridge) - -if __name__ == '__main__': - main() diff --git a/f1tenth_gym_ros/launch/ego_racecar.xacro b/f1tenth_gym_ros/launch/ego_racecar.xacro deleted file mode 100644 index 0ca6741..0000000 --- a/f1tenth_gym_ros/launch/ego_racecar.xacro +++ /dev/null @@ -1,129 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/f1tenth_gym_ros/launch/gazebo_racecar.xacro b/f1tenth_gym_ros/launch/gazebo_racecar.xacro deleted file mode 100644 index 27a8438..0000000 --- a/f1tenth_gym_ros/launch/gazebo_racecar.xacro +++ /dev/null @@ -1,382 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - joint_states:=joint_states - - 50 - base_to_back_left_wheel - base_to_back_right_wheel - front_left_hinge_to_wheel - front_right_hinge_to_wheel - base_to_front_left_hinge - base_to_front_right_hinge - - - - - - - Gazebo/Blue - 0.2 - 0.2 - - - - Gazebo/White - ${wheel_mu1} - ${wheel_mu2} - - - - Gazebo/White - ${wheel_mu1} - ${wheel_mu2} - - - - Gazebo/White - ${wheel_mu1} - ${wheel_mu2} - - - - Gazebo/White - ${wheel_mu1} - ${wheel_mu2} - - - - Gazebo/Black - - - - - Gazebo/Red - - 0 0 0 0 0 0 - true - 10 - - 1.089 - - R8G8B8 - 640 - 480 - - - 0.05 - 8.0 - - - - - - image_raw:=image_raw - image_depth:=depth/image_raw - camera_info:=camera_info - camera_info_depth:=depth/camera_info - points:=depth/points - - camera - ego_racecar/camera_optical_frame - 0.07 - 0.1 - 100.0 - - - - - \ No newline at end of file diff --git a/f1tenth_gym_ros/launch/gazebo_test.launch.py b/f1tenth_gym_ros/launch/gazebo_test.launch.py deleted file mode 100644 index e450688..0000000 --- a/f1tenth_gym_ros/launch/gazebo_test.launch.py +++ /dev/null @@ -1,197 +0,0 @@ -#!/usr/bin/env python3 -# MIT License - F1Tenth Gym ROS with Gazebo support -# Based on original F1Tenth launch but adding Gazebo simulation - -from launch import LaunchDescription -from launch_ros.actions import Node -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command -from launch.actions import TimerAction -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from ament_index_python.packages import get_package_share_directory -import os -import yaml - -def generate_launch_description(): - ld = LaunchDescription() - - # Cargar configuración original de F1Tenth - config = os.path.join( - get_package_share_directory('f1tenth_gym_ros'), - 'config', - 'sim.yaml' - ) - config_dict = yaml.safe_load(open(config, 'r')) - has_opp = config_dict['bridge']['ros__parameters']['num_agent'] > 1 - - # === PARTE NUEVA: GAZEBO === - package_name = 'f1tenth_gym_ros' - - # Parámetros de Gazebo - gazebo_params_file = os.path.join(get_package_share_directory(package_name), 'config', 'gazebo_params.yaml') - - # Lanzar Gazebo Classic con mundo seleccionable - gazebo = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]), - launch_arguments={ - 'world': LaunchConfiguration('world'), # ESTA LÍNEA FALTA - 'extra_gazebo_args': '--ros-args --params-file ' + gazebo_params_file - }.items() - ) - - world_arg = DeclareLaunchArgument( - 'world', - default_value='', - description='Path to world file' - ) - - # === ROBOT STATE PUBLISHERS MODIFICADOS PARA GAZEBO === - - # Ego robot con soporte Gazebo (usando nuestro nuevo XACRO) - ego_robot_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='ego_robot_state_publisher', - parameters=[{ - 'robot_description': Command(['xacro ', os.path.join(get_package_share_directory('f1tenth_gym_ros'), 'launch', 'gazebo_racecar.xacro'), ' use_ros2_control:=false']), - 'use_sim_time': True # Importante para Gazebo - }], - remappings=[('/robot_description', 'ego_robot_description')] - ) - - # Spawner del ego robot en Gazebo - spawn_ego_robot = Node( - package='gazebo_ros', - executable='spawn_entity.py', - arguments=[ - '-topic', 'ego_robot_description', - '-entity', 'ego_racecar', - '-x', '0', '-y', '0', '-z', '0.1' - ], - output='screen' - ) - - # Robot oponente (si existe) - if has_opp: - opp_robot_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='opp_robot_state_publisher', - parameters=[{ - 'robot_description': Command(['xacro ', os.path.join(get_package_share_directory('f1tenth_gym_ros'), 'launch', 'opp_racecar.xacro')]), - 'use_sim_time': True - }], - remappings=[('/robot_description', 'opp_robot_description')] - ) - - # Spawner del robot oponente - spawn_opp_robot = Node( - package='gazebo_ros', - executable='spawn_entity.py', - arguments=[ - '-topic', 'opp_robot_description', - '-entity', 'opp_racecar', - '-x', '2', '-y', '0', '-z', '0.1' # Posición diferente - ], - output='screen' - ) - - # === PARTE ORIGINAL DE F1TENTH === - - # Bridge node (comunicación con el simulador F1Tenth) - bridge_node = Node( - package='f1tenth_gym_ros', - executable='gym_bridge', - name='bridge', - parameters=[config, {'use_sim_time': True}] # Añadido use_sim_time - ) - - # RViz - rviz_node = Node( - package='rviz2', - executable='rviz2', - name='rviz', - arguments=['-d', os.path.join(get_package_share_directory('f1tenth_gym_ros'), 'launch', 'gym_bridge.rviz')], - parameters=[{'use_sim_time': True}] - ) - - # Map server - map_server_node = Node( - package='nav2_map_server', - executable='map_server', - parameters=[ - {'yaml_filename': config_dict['bridge']['ros__parameters']['map_path'] + '.yaml'}, - {'topic': 'map'}, - {'frame_id': 'map'}, - {'output': 'screen'}, - {'use_sim_time': True} - ] - ) - - # Navigation lifecycle manager - nav_lifecycle_node = Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_localization', - output='screen', - parameters=[ - {'use_sim_time': True}, - {'autostart': True}, - {'node_names': ['map_server']} - ] - ) - - # Spawners de controladores ros2_control - ackermann_controller_spawner = TimerAction( - period=5.0, # Esperar 5 segundos - actions=[ - Node( - package="controller_manager", - executable="spawner", - arguments=["ackermann_steering_controller"], - parameters=[{'use_sim_time': True}], - output='screen' - ) - ] - ) - - joint_broadcaster_spawner = TimerAction( - period=3.0, # Esperar 3 segundos - actions=[ - Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - parameters=[{'use_sim_time': True}], - output='screen' - ) - ] - ) - - # === CONSTRUIR LAUNCH DESCRIPTION === - - # Gazebo primero - ld.add_action(gazebo) - - # Robots - ld.add_action(ego_robot_publisher) - ld.add_action(spawn_ego_robot) - - if has_opp: - ld.add_action(opp_robot_publisher) - ld.add_action(spawn_opp_robot) - - # F1Tenth original - ld.add_action(bridge_node) - ld.add_action(rviz_node) - ld.add_action(nav_lifecycle_node) - ld.add_action(map_server_node) - ld.add_action(world_arg) - # ros2_control spawners - ld.add_action(ackermann_controller_spawner) - ld.add_action(joint_broadcaster_spawner) - - return ld \ No newline at end of file diff --git a/f1tenth_gym_ros/launch/gym_bridge_launch.py b/f1tenth_gym_ros/launch/gym_bridge_launch.py deleted file mode 100644 index 7bc6acc..0000000 --- a/f1tenth_gym_ros/launch/gym_bridge_launch.py +++ /dev/null @@ -1,95 +0,0 @@ -# MIT License - -# Copyright (c) 2020 Hongrui Zheng - -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: - -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. - -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. - -from launch import LaunchDescription -from launch_ros.actions import Node -from launch.substitutions import Command -from ament_index_python.packages import get_package_share_directory -import os -import yaml - -def generate_launch_description(): - ld = LaunchDescription() - config = os.path.join( - get_package_share_directory('f1tenth_gym_ros'), - 'config', - 'sim.yaml' - ) - config_dict = yaml.safe_load(open(config, 'r')) - has_opp = config_dict['bridge']['ros__parameters']['num_agent'] > 1 - teleop = config_dict['bridge']['ros__parameters']['kb_teleop'] - - bridge_node = Node( - package='f1tenth_gym_ros', - executable='gym_bridge', - name='bridge', - parameters=[config] - ) - rviz_node = Node( - package='rviz2', - executable='rviz2', - name='rviz', - arguments=['-d', os.path.join(get_package_share_directory('f1tenth_gym_ros'), 'launch', 'gym_bridge.rviz')] - ) - map_server_node = Node( - package='nav2_map_server', - executable='map_server', - parameters=[{'yaml_filename': config_dict['bridge']['ros__parameters']['map_path'] + '.yaml'}, - {'topic': 'map'}, - {'frame_id': 'map'}, - {'output': 'screen'}, - {'use_sim_time': True}] - ) - nav_lifecycle_node = Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_localization', - output='screen', - parameters=[{'use_sim_time': True}, - {'autostart': True}, - {'node_names': ['map_server']}] - ) - ego_robot_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='ego_robot_state_publisher', - parameters=[{'robot_description': Command(['xacro ', os.path.join(get_package_share_directory('f1tenth_gym_ros'), 'launch', 'ego_racecar.xacro')])}], - remappings=[('/robot_description', 'ego_robot_description')] - ) - opp_robot_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='opp_robot_state_publisher', - parameters=[{'robot_description': Command(['xacro ', os.path.join(get_package_share_directory('f1tenth_gym_ros'), 'launch', 'opp_racecar.xacro')])}], - remappings=[('/robot_description', 'opp_robot_description')] - ) - - # finalize - ld.add_action(rviz_node) - ld.add_action(bridge_node) - ld.add_action(nav_lifecycle_node) - ld.add_action(map_server_node) - ld.add_action(ego_robot_publisher) - if has_opp: - ld.add_action(opp_robot_publisher) - - return ld \ No newline at end of file diff --git a/f1tenth_gym_ros/launch/opp_racecar.xacro b/f1tenth_gym_ros/launch/opp_racecar.xacro deleted file mode 100644 index bedec5a..0000000 --- a/f1tenth_gym_ros/launch/opp_racecar.xacro +++ /dev/null @@ -1,129 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/f1tenth_gym_ros/launch/ros2_control_ackermann.xacro b/f1tenth_gym_ros/launch/ros2_control_ackermann.xacro deleted file mode 100644 index 2d053f4..0000000 --- a/f1tenth_gym_ros/launch/ros2_control_ackermann.xacro +++ /dev/null @@ -1,68 +0,0 @@ - - - - - - gazebo_ros2_control/GazeboSystem - - - - - - - - - - - - - - - - - -10 - 10 - - - - - - - - -10 - 10 - - - - - - - - - -0.7854 - 0.7854 - - - - - - - - -0.7854 - 0.7854 - - - - - - - - - - $(find f1tenth_gym_ros)/config/ackermann_controllers.yaml - ego_robot_description - ego_robot_state_publisher - - - - \ No newline at end of file diff --git a/f1tenth_gym_ros/maps/SaoPaulo_map.png b/f1tenth_gym_ros/maps/SaoPaulo_map.png deleted file mode 100644 index 5b0876e..0000000 Binary files a/f1tenth_gym_ros/maps/SaoPaulo_map.png and /dev/null differ diff --git a/f1tenth_gym_ros/maps/SaoPaulo_map.yaml b/f1tenth_gym_ros/maps/SaoPaulo_map.yaml deleted file mode 100644 index eb0fb79..0000000 --- a/f1tenth_gym_ros/maps/SaoPaulo_map.yaml +++ /dev/null @@ -1,6 +0,0 @@ -image: SaoPaulo_map.png -resolution: 0.04877 -origin: [-27.988851027944648,-33.41588383183014, 0.000000] -negate: 0 -occupied_thresh: 0.45 -free_thresh: 0.196 diff --git a/f1tenth_gym_ros/maps/Spielberg_obs_map.png b/f1tenth_gym_ros/maps/Spielberg_obs_map.png deleted file mode 100644 index 56706fa..0000000 Binary files a/f1tenth_gym_ros/maps/Spielberg_obs_map.png and /dev/null differ diff --git a/f1tenth_gym_ros/maps/Spielberg_obs_map.yaml b/f1tenth_gym_ros/maps/Spielberg_obs_map.yaml deleted file mode 100644 index 68dcb93..0000000 --- a/f1tenth_gym_ros/maps/Spielberg_obs_map.yaml +++ /dev/null @@ -1,6 +0,0 @@ -image: Spielberg_obs_map.png -resolution: 0.05796 -origin: [-84.85359914210505,-36.30299725862132, 0.000000] -negate: 0 -occupied_thresh: 0.45 -free_thresh: 0.196 diff --git a/f1tenth_gym_ros/maps/levineB.png b/f1tenth_gym_ros/maps/levineB.png deleted file mode 100644 index 03930f3..0000000 Binary files a/f1tenth_gym_ros/maps/levineB.png and /dev/null differ diff --git a/f1tenth_gym_ros/maps/levineB.yaml b/f1tenth_gym_ros/maps/levineB.yaml deleted file mode 100644 index 1ec9b18..0000000 --- a/f1tenth_gym_ros/maps/levineB.yaml +++ /dev/null @@ -1,6 +0,0 @@ -image: levineB.png -resolution: 0.050000 -origin: [-25.0, -12.75, 0.000000] -negate: 0 -occupied_thresh: 0.65 -free_thresh: 0.196 diff --git a/f1tenth_gym_ros/package.xml b/f1tenth_gym_ros/package.xml deleted file mode 100644 index f65fff2..0000000 --- a/f1tenth_gym_ros/package.xml +++ /dev/null @@ -1,39 +0,0 @@ - - - - f1tenth_gym_ros - 0.0.0 - Bridge for using f1tenth_gym in ROS2 - Billy Zheng - MIT - - geometry_msgs - nav_msgs - sensor_msgs - ackermann_msgs - launch - launch_ros - tf2_ros - rclpy - joint_state_publisher - xacro - nav2_map_server - nav2_lifecycle_manager - teleop_twist_keyboard - - python-transforms3d-pip - gazebo_ros - ros2_control - ros2_controllers - gazebo_ros2_control - ackermann_steering_controller - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/f1tenth_gym_ros/resource/f1tenth_gym_ros b/f1tenth_gym_ros/resource/f1tenth_gym_ros deleted file mode 100644 index e69de29..0000000 diff --git a/f1tenth_gym_ros/setup.cfg b/f1tenth_gym_ros/setup.cfg deleted file mode 100644 index 80194fa..0000000 --- a/f1tenth_gym_ros/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/f1tenth_gym_ros -[install] -install_scripts=$base/lib/f1tenth_gym_ros diff --git a/f1tenth_gym_ros/setup.py b/f1tenth_gym_ros/setup.py deleted file mode 100644 index f6dab7e..0000000 --- a/f1tenth_gym_ros/setup.py +++ /dev/null @@ -1,32 +0,0 @@ -from setuptools import setup -import os -from glob import glob - -package_name = 'f1tenth_gym_ros' - -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']), - (os.path.join('share', package_name, 'launch'), glob('launch/*.py')), - (os.path.join('share', package_name, 'launch'), glob('launch/*.xacro')), - (os.path.join('share', package_name, 'launch'), glob('launch/*.rviz')), - (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='Billy Zheng', - maintainer_email='billyzheng.bz@gmail.com', - description='Bridge for using f1tenth_gym in ROS2', - license='MIT', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'gym_bridge = f1tenth_gym_ros.gym_bridge:main' - ], - }, -) diff --git a/f1tenth_gym_ros/test/test_copyright.py b/f1tenth_gym_ros/test/test_copyright.py deleted file mode 100644 index cc8ff03..0000000 --- a/f1tenth_gym_ros/test/test_copyright.py +++ /dev/null @@ -1,23 +0,0 @@ -# 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 - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/f1tenth_gym_ros/test/test_flake8.py b/f1tenth_gym_ros/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/f1tenth_gym_ros/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# 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) diff --git a/f1tenth_gym_ros/test/test_pep257.py b/f1tenth_gym_ros/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/f1tenth_gym_ros/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# 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_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/my_bot/CMakeLists.txt b/my_bot/CMakeLists.txt new file mode 100644 index 0000000..dfa34c8 --- /dev/null +++ b/my_bot/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.5) +project(my_bot) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install( + DIRECTORY config description launch worlds + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/my_bot/LICENSE.md b/my_bot/LICENSE.md new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/my_bot/LICENSE.md @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/my_bot/config/empty.yaml b/my_bot/config/empty.yaml new file mode 100644 index 0000000..04700ba --- /dev/null +++ b/my_bot/config/empty.yaml @@ -0,0 +1 @@ +# This is an empty file, so that git commits the folder correctly \ No newline at end of file diff --git a/my_bot/config/gazebo_params.yaml b/my_bot/config/gazebo_params.yaml new file mode 100644 index 0000000..d7caf6d --- /dev/null +++ b/my_bot/config/gazebo_params.yaml @@ -0,0 +1,3 @@ +gazebo: + ros__parameters: + publish_rate: 400.0 \ No newline at end of file diff --git a/my_bot/config/mapper_params_online_async.yaml b/my_bot/config/mapper_params_online_async.yaml new file mode 100644 index 0000000..2a0448f --- /dev/null +++ b/my_bot/config/mapper_params_online_async.yaml @@ -0,0 +1,77 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_footprint + scan_topic: /scan + use_map_saver: true + mode: localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + map_file_name: /home/ivan/dev_ws/map1_serial + # map_start_pose: [0.0, 0.0, 0.0] + map_start_at_dock: true + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + min_laser_range: 0.0 #for rastering images + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.1 + minimum_travel_heading: 0.1 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true + min_pass_through: 2 + occupancy_threshold: 0.1 diff --git a/f1tenth_gym_ros/launch/gym_bridge.rviz b/my_bot/config/mapping.rviz similarity index 59% rename from f1tenth_gym_ros/launch/gym_bridge.rviz rename to my_bot/config/mapping.rviz index bb54b01..443c684 100644 --- a/f1tenth_gym_ros/launch/gym_bridge.rviz +++ b/my_bot/config/mapping.rviz @@ -3,12 +3,9 @@ Panels: Help Height: 78 Name: Displays Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /Map1/Topic1 + Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 802 + Tree Height: 178 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -22,6 +19,13 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 + - Class: slam_toolbox::SlamToolboxPlugin + Name: SlamToolboxPlugin Visualization Manager: Class: "" Displays: @@ -29,7 +33,7 @@ Visualization Manager: Cell Size: 1 Class: rviz_default_plugins/Grid Color: 160; 160; 164 - Enabled: false + Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines @@ -43,120 +47,59 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false + - Class: rviz_default_plugins/TF Enabled: true - Name: Map - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map_updates - Use Timestamp: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 30.2736759185791 - Min Value: -0.7132953405380249 - Value: true - Axis: X - Channel Name: intensity - Class: rviz_default_plugins/LaserScan - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: LaserScan - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /scan - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /opp_robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - opp_racecar/back_left_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false + Frame Timeout: 15 + Frames: + All Enabled: true + base_footprint: Value: true - opp_racecar/back_right_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false + base_link: Value: true - opp_racecar/base_link: - Alpha: 1 - Show Axes: false - Show Trail: false + camera_link: Value: true - opp_racecar/front_left_hinge: - Alpha: 1 - Show Axes: false - Show Trail: false - opp_racecar/front_left_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false + camera_link_optical: Value: true - opp_racecar/front_right_hinge: - Alpha: 1 - Show Axes: false - Show Trail: false - opp_racecar/front_right_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false + caster_wheel: Value: true - opp_racecar/laser_model: - Alpha: 1 - Show Axes: false - Show Trail: false + chassis: Value: true - Name: OPP-RobotModel - TF Prefix: "" + laser_frame: + Value: true + left_wheel: + Value: true + map: + Value: true + odom: + Value: true + right_wheel: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: false + Show Names: false + Tree: + map: + {} + odom: + base_link: + base_footprint: + {} + chassis: + camera_link: + camera_link_optical: + {} + caster_wheel: + {} + laser_frame: + {} + left_wheel: + {} + right_wheel: + {} Update Interval: 0 Value: true - Visual Enabled: true - Alpha: 1 Class: rviz_default_plugins/RobotModel Collision Enabled: false @@ -167,7 +110,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /ego_robot_description + Value: /robot_description Enabled: true Links: All Links Enabled: true @@ -175,71 +118,158 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - ego_racecar/back_left_wheel: + base_footprint: Alpha: 1 Show Axes: false Show Trail: false - Value: true - ego_racecar/back_right_wheel: + base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - ego_racecar/base_link: + camera_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - ego_racecar/front_left_hinge: + camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + caster_wheel: Alpha: 1 Show Axes: false Show Trail: false - ego_racecar/front_left_wheel: + Value: true + chassis: Alpha: 1 Show Axes: false Show Trail: false Value: true - ego_racecar/front_right_hinge: + laser_frame: Alpha: 1 Show Axes: false Show Trail: false - ego_racecar/front_right_wheel: + Value: true + left_wheel: Alpha: 1 Show Axes: false Show Trail: false Value: true - ego_racecar/laser_model: + right_wheel: Alpha: 1 Show Axes: false Show Trail: false Value: true + Mass Properties: + Inertia: false + Mass: false Name: RobotModel TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/depth/image_raw + Value: true - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 25; 255; 0 - Enabled: false - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.1 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /planned_path - Unreliable: false + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/points + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false Value: true Enabled: true Global Options: @@ -256,6 +286,9 @@ Visualization Manager: - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile @@ -284,7 +317,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 10 + Distance: 5.952032089233398 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -299,24 +332,30 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5353981852531433 + Pitch: 1.1647965908050537 Target Frame: Value: Orbit (rviz) - Yaw: 3.1453983783721924 + Yaw: 3.5485846996307373 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 900 + Height: 1016 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000003abfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003ab000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003ab000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000075b000003ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000002170000035afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000013d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001800000008c0000002800fffffffb000000220053006c0061006d0054006f006f006c0062006f00780050006c007500670069006e0100000212000001850000018500ffffff000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004080000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false + SlamToolboxPlugin: + collapsed: false + Time: + collapsed: false Tool Properties: collapsed: false Views: collapsed: false - Width: 1600 - X: 52 - Y: 25 + Width: 1850 + X: 70 + Y: 27 diff --git a/my_bot/config/my_controllers.yaml b/my_bot/config/my_controllers.yaml new file mode 100644 index 0000000..9e59717 --- /dev/null +++ b/my_bot/config/my_controllers.yaml @@ -0,0 +1,68 @@ +controller_manager: + ros__parameters: + update_rate: 30 + use_sim_time: true + + diff_cont: + type: diff_drive_controller/DiffDriveController + + joint_broad: + type: joint_state_broadcaster/JointStateBroadcaster + +diff_cont: + ros__parameters: + + publish_rate: 50.0 + + base_frame_id: base_link + + left_wheel_names: ['left_wheel_joint'] + right_wheel_names: ['right_wheel_joint'] + wheel_separation: 0.20 + wheel_radius: 0.04 + + use_stamped_vel: false + + # open_loop: false + + # wheels_per_side: x + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + odom_frame_id: odom + # pose_covariance_diagonal: x + # twist_covariance_diagonal: x + # open_loop: x + enable_odom_tf: true + + # cmd_vel_timeout: x + # publish_limited_velocity: x + # velocity_rolling_window_size: x + + + # linear.x.has_velocity_limits: false + # linear.x.has_acceleration_limits: false + # linear.x.has_jerk_limits: false + # linear.x.max_velocity: NAN + # linear.x.min_velocity: NAN + # linear.x.max_acceleration: NAN + # linear.x.min_acceleration: NAN + # linear.x.max_jerk: NAN + # linear.x.min_jerk: NAN + + # angular.z.has_velocity_limits: false + # angular.z.has_acceleration_limits: false + # angular.z.has_jerk_limits: false + # angular.z.max_velocity: NAN + # angular.z.min_velocity: NAN + # angular.z.max_acceleration: NAN + # angular.z.min_acceleration: NAN + # angular.z.max_jerk: NAN + # angular.z.min_jerk: NAN + + + + +# joint_broad: +# ros__parameters: \ No newline at end of file diff --git a/my_bot/config/nav2_params.yaml b/my_bot/config/nav2_params.yaml new file mode 100644 index 0000000..954d1d8 --- /dev/null +++ b/my_bot/config/nav2_params.yaml @@ -0,0 +1,350 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 360 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.15 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 1.04 + max_vel_y: 0.0 + max_vel_theta: 4.0 + min_speed_xy: 0.0 + max_speed_xy: 1.04 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 10.0 + acc_lim_y: 0.0 + acc_lim_theta: 6.4 + decel_lim_x: -5.0 + decel_lim_y: 0.0 + decel_lim_theta: -6.4 + vx_samples: 20 + vy_samples: 0 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 1.0 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.15 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.15 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "/src/my_bot/maps/levine2.yaml" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 2.0 + min_rotational_vel: 1.0 + rotational_acc_lim: 8.4 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [1.04, 0.0, 4.0] + min_velocity: [-1.04, 0.0, -4.0] + max_accel: [5.0, 0.0, 6.4] + max_decel: [-5.0, 0.0, -6.4] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/my_bot/config/twist_mux.yaml b/my_bot/config/twist_mux.yaml new file mode 100644 index 0000000..7f10de0 --- /dev/null +++ b/my_bot/config/twist_mux.yaml @@ -0,0 +1,11 @@ +twist_mux: + ros__parameters: + topics: + navigation: + topic : cmd_vel + timeout : 0.5 + priority: 10 + teleop: + topic : cmd_vel_teleop + timeout : 0.5 + priority: 100 \ No newline at end of file diff --git a/my_bot/config/view_bot.rviz b/my_bot/config/view_bot.rviz new file mode 100644 index 0000000..7f6371d --- /dev/null +++ b/my_bot/config/view_bot.rviz @@ -0,0 +1,240 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 455 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + chassis: + Value: true + front_left_steer_link: + Value: true + front_left_wheel: + Value: true + front_right_steer_link: + Value: true + front_right_wheel: + Value: true + left_wheel: + Value: true + right_wheel: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + base_link: + chassis: + {} + front_left_steer_link: + front_left_wheel: + {} + front_right_steer_link: + front_right_wheel: + {} + left_wheel: + {} + right_wheel: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + chassis: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_steer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + front_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_steer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + front_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 0.7628582715988159 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6597976088523865 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.818588137626648 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 752 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000252fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000252000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000252000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000056b0000003efc0100000002fb0000000800540069006d006501000000000000056b000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002fa0000025200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1387 + X: 301 + Y: 114 diff --git a/my_bot/description/depth_camera.xacro b/my_bot/description/depth_camera.xacro new file mode 100644 index 0000000..1d4a57f --- /dev/null +++ b/my_bot/description/depth_camera.xacro @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Blue + + + 0 0 0 0 0 0 + true + 10 + + 1.089 + + B8G8R8 + 640 + 480 + + + 0.05 + 8.0 + + + + camera_link_optical + 0.1 + 100.0 + + + + + \ No newline at end of file diff --git a/my_bot/description/gazebo_control.xacro b/my_bot/description/gazebo_control.xacro new file mode 100644 index 0000000..256d9ad --- /dev/null +++ b/my_bot/description/gazebo_control.xacro @@ -0,0 +1,30 @@ + + + + + + + + left_wheel_joint + right_wheel_joint + + + 0.2 + 0.08 + + + 200 + 10.0 + + + odom + base_link + + true + true + true + + + + + diff --git a/my_bot/description/inertial_macros.xacro b/my_bot/description/inertial_macros.xacro new file mode 100644 index 0000000..5cf1cac --- /dev/null +++ b/my_bot/description/inertial_macros.xacro @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/my_bot/description/robot.urdf.xacro b/my_bot/description/robot.urdf.xacro new file mode 100644 index 0000000..6be109d --- /dev/null +++ b/my_bot/description/robot.urdf.xacro @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/my_bot/description/robot_core.xacro b/my_bot/description/robot_core.xacro new file mode 100644 index 0000000..0cee42f --- /dev/null +++ b/my_bot/description/robot_core.xacro @@ -0,0 +1,308 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/White + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Blue + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Blue + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Red + + + 0 0 0 0 0 0 + true + 10 + + + + 360 + -3.14 + 3.14 + + + + 0.3 + 12 + + + + + ~/out:=scan + + sensor_msgs/LaserScan + laser_frame + + + + + diff --git a/my_bot/description/ros2_control.xacro b/my_bot/description/ros2_control.xacro new file mode 100644 index 0000000..f0e97e6 --- /dev/null +++ b/my_bot/description/ros2_control.xacro @@ -0,0 +1,33 @@ + + + + + + + gazebo_ros2_control/GazeboSystem + + + + -10 + 10 + + + + + + + -10 + 10 + + + + + + + + + $(find my_bot)/config/my_controllers.yaml + + + + \ No newline at end of file diff --git a/f1tenth_gym_ros/launch/camera.launch.py b/my_bot/launch/camera.launch.py similarity index 100% rename from f1tenth_gym_ros/launch/camera.launch.py rename to my_bot/launch/camera.launch.py diff --git a/my_bot/launch/launch_sim.launch.py b/my_bot/launch/launch_sim.launch.py new file mode 100644 index 0000000..4a3e430 --- /dev/null +++ b/my_bot/launch/launch_sim.launch.py @@ -0,0 +1,82 @@ +import os + +from ament_index_python.packages import get_package_share_directory + + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch_ros.actions import Node + + + +def generate_launch_description(): + + + # Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled + # !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!! + + package_name='my_bot' #<--- CHANGE ME + + rsp = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory(package_name),'launch','rsp.launch.py' + )]), launch_arguments={'use_sim_time': 'true', 'use_ros2_control': 'true'}.items() + ) + + twist_mux_params = os.path.join(get_package_share_directory(package_name),'config','twist_mux.yaml') + twist_mux = Node( + package="twist_mux", + executable="twist_mux", + parameters=[twist_mux_params, {'use_sim_time': True}], + remappings=[('/cmd_vel_out','/diff_cont/cmd_vel_unstamped')] + ) + + gazebo_params_file = os.path.join(get_package_share_directory(package_name),'config','gazebo_params.yaml') + # Include the Gazebo launch file, provided by the gazebo_ros package + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]), + launch_arguments={'extra_gazebo_args': '--ros-args --params-file ' + gazebo_params_file}.items() + ) + + # Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot. + spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', + arguments=['-topic', 'robot_description', + '-entity', 'my_bot'], + output='screen') + + diff_drive_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["diff_cont"], + ) + + joint_broad_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_broad"], + ) + + teleop = Node( + package='teleop_twist_keyboard', + executable='teleop_twist_keyboard', + name='teleop_keyboard', + output='screen', + prefix='xterm -e', + remappings=[ + ('/cmd_vel', '/cmd_vel_teleop') + ] + ) + + # Launch them all! + return LaunchDescription([ + rsp, + twist_mux, + gazebo, + spawn_entity, + diff_drive_spawner, + joint_broad_spawner, + teleop + ]) diff --git a/my_bot/launch/launch_sim.py b/my_bot/launch/launch_sim.py new file mode 100644 index 0000000..5b1ed07 --- /dev/null +++ b/my_bot/launch/launch_sim.py @@ -0,0 +1,47 @@ +import os + +from ament_index_python.packages import get_package_share_directory + + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch_ros.actions import Node + + + +def generate_launch_description(): + + + # Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled + # !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!! + + package_name='my_bot' #<--- CHANGE ME + + rsp = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory(package_name),'launch','rsp.launch.py' + )]), launch_arguments={'use_sim_time': 'true'}.items() + ) + + # Include the Gazebo launch file, provided by the gazebo_ros package + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]), + ) + + # Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot. + spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', + arguments=['-topic', 'robot_description', + '-entity', 'my_bot'], + output='screen') + + + + # Launch them all! + return LaunchDescription([ + rsp, + gazebo, + spawn_entity, + ]) diff --git a/my_bot/launch/localization_launch.py b/my_bot/launch/localization_launch.py new file mode 100644 index 0000000..8a18781 --- /dev/null +++ b/my_bot/launch/localization_launch.py @@ -0,0 +1,192 @@ +# Copyright (c) 2018 Intel Corporation +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('my_bot') + + namespace = LaunchConfiguration('namespace') + map_yaml_file = LaunchConfiguration('map') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = ['map_server', 'amcl'] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'yaml_filename': map_yaml_file} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + description='Full path to map yaml file to load') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'config', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', + description='log level') + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_amcl', + executable='amcl', + name='amcl', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_localization', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + ] + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::MapServer', + name='map_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_amcl', + plugin='nav2_amcl::AmclNode', + name='amcl', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_localization', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + + # Add the actions to launch all of the localiztion nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/my_bot/launch/nav_launch.py b/my_bot/launch/nav_launch.py new file mode 100644 index 0000000..275b167 --- /dev/null +++ b/my_bot/launch/nav_launch.py @@ -0,0 +1,83 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + # Package name + package_name = 'my_bot' + + # Launch configuration variables + use_sim_time = LaunchConfiguration('use_sim_time') + map_file = LaunchConfiguration('map') + params_file = LaunchConfiguration('params_file') + + # Paths + nav2_launch_file_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch') + default_params_file = os.path.join( + get_package_share_directory(package_name), + 'config', + 'nav2_params.yaml' # Tu archivo corregido + ) + rviz_config_file = os.path.join( + get_package_share_directory(package_name), + 'config', + 'view_bot.rviz' + ) + + # Declare launch arguments + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', # True para simulación + description='Use simulation (Gazebo) clock if true' + ) + + declare_map_cmd = DeclareLaunchArgument( + 'map', + description='Full path to map file to load' + ) + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=default_params_file, + description='Full path to param file to load' + ) + + # Launch Nav2 + nav2_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_launch_file_dir, 'bringup_launch.py') + ), + launch_arguments={ + 'map': map_file, + 'use_sim_time': use_sim_time, + 'params_file': params_file + }.items(), + ) + + # Launch RViz (opcional) + rviz_cmd = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_file], + parameters=[{'use_sim_time': use_sim_time}], + output='screen' + ) + + # Create launch description + ld = LaunchDescription() + + # Add launch arguments + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_map_cmd) + ld.add_action(declare_params_file_cmd) + + # Add actions + ld.add_action(nav2_cmd) + ld.add_action(rviz_cmd) # Descomenta si quieres RViz automático + + return ld \ No newline at end of file diff --git a/my_bot/launch/navigation_launch.py b/my_bot/launch/navigation_launch.py new file mode 100644 index 0000000..26be797 --- /dev/null +++ b/my_bot/launch/navigation_launch.py @@ -0,0 +1,272 @@ +# Copyright (c) 2018 Intel Corporation +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('my_bot') + + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = ['controller_server', + 'smoother_server', + 'planner_server', + 'behavior_server', + 'bt_navigator', + 'waypoint_follower', + 'velocity_smoother'] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'autostart': autostart} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'config', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', + description='log level') + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + Node( + package='nav2_controller', + executable='controller_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_waypoint_follower', + executable='waypoint_follower', + name='waypoint_follower', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_velocity_smoother', + executable='velocity_smoother', + name='velocity_smoother', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]), + ] + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_controller', + plugin='nav2_controller::ControllerServer', + name='controller_server', + parameters=[configured_params], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + ComposableNode( + package='nav2_smoother', + plugin='nav2_smoother::SmootherServer', + name='smoother_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_planner', + plugin='nav2_planner::PlannerServer', + name='planner_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_behaviors', + plugin='behavior_server::BehaviorServer', + name='behavior_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_bt_navigator', + plugin='nav2_bt_navigator::BtNavigator', + name='bt_navigator', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_waypoint_follower', + plugin='nav2_waypoint_follower::WaypointFollower', + name='waypoint_follower', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_velocity_smoother', + plugin='nav2_velocity_smoother::VelocitySmoother', + name='velocity_smoother', + parameters=[configured_params], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_navigation', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + # Add the actions to launch all of the navigation nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/my_bot/launch/online_async_launch.py b/my_bot/launch/online_async_launch.py new file mode 100644 index 0000000..833d694 --- /dev/null +++ b/my_bot/launch/online_async_launch.py @@ -0,0 +1,59 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, LogInfo +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from nav2_common.launch import HasNodeParams + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time') + params_file = LaunchConfiguration('params_file') + default_params_file = os.path.join(get_package_share_directory("my_bot"), + 'config', 'mapper_params_online_async.yaml') + + declare_use_sim_time_argument = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation/Gazebo clock') + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=default_params_file, + description='Full path to the ROS2 parameters file to use for the slam_toolbox node') + + # If the provided param file doesn't have slam_toolbox params, we must pass the + # default_params_file instead. This could happen due to automatic propagation of + # LaunchArguments. See: + # https://github.com/ros-planning/navigation2/pull/2243#issuecomment-800479866 + has_node_params = HasNodeParams(source_file=params_file, + node_name='slam_toolbox') + + actual_params_file = PythonExpression(['"', params_file, '" if ', has_node_params, + ' else "', default_params_file, '"']) + + log_param_change = LogInfo(msg=['provided params_file ', params_file, + ' does not contain slam_toolbox parameters. Using default: ', + default_params_file], + condition=UnlessCondition(has_node_params)) + + start_async_slam_toolbox_node = Node( + parameters=[ + actual_params_file, + {'use_sim_time': use_sim_time} + ], + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + output='screen') + + ld = LaunchDescription() + + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_params_file_cmd) + ld.add_action(log_param_change) + ld.add_action(start_async_slam_toolbox_node) + + return ld \ No newline at end of file diff --git a/f1tenth_gym_ros/launch/rplidar.launch.py b/my_bot/launch/rplidar.launch.py similarity index 100% rename from f1tenth_gym_ros/launch/rplidar.launch.py rename to my_bot/launch/rplidar.launch.py diff --git a/my_bot/launch/rsp.launch.py b/my_bot/launch/rsp.launch.py new file mode 100644 index 0000000..b6713c6 --- /dev/null +++ b/my_bot/launch/rsp.launch.py @@ -0,0 +1,48 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration, Command +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + + # Check if we're told to use sim time + use_sim_time = LaunchConfiguration('use_sim_time') + use_ros2_control = LaunchConfiguration('use_ros2_control') + + # Process the URDF file + pkg_path = os.path.join(get_package_share_directory('my_bot')) + xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro') + #robot_description_config = xacro.process_file(xacro_file).toxml() + robot_description_config = Command(['xacro ', xacro_file, ' use_ros2_control:=', use_ros2_control]) + + # Create a robot_state_publisher node + params = {'robot_description': robot_description_config, 'use_sim_time': use_sim_time} + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[params] + ) + + + # Launch! + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use sim time if true'), + DeclareLaunchArgument( + 'use_ros2_control', + default_value='true', + description='Use ros2_control if true'), + + node_robot_state_publisher + ]) diff --git a/f1tenth_gym_ros/maps/Spielberg_map.png b/my_bot/maps/Spielberg_map.png similarity index 100% rename from f1tenth_gym_ros/maps/Spielberg_map.png rename to my_bot/maps/Spielberg_map.png diff --git a/f1tenth_gym_ros/maps/Spielberg_map.yaml b/my_bot/maps/Spielberg_map.yaml similarity index 100% rename from f1tenth_gym_ros/maps/Spielberg_map.yaml rename to my_bot/maps/Spielberg_map.yaml diff --git a/f1tenth_gym_ros/maps/levine.png b/my_bot/maps/levine.png similarity index 100% rename from f1tenth_gym_ros/maps/levine.png rename to my_bot/maps/levine.png diff --git a/f1tenth_gym_ros/maps/levine.yaml b/my_bot/maps/levine.yaml similarity index 85% rename from f1tenth_gym_ros/maps/levine.yaml rename to my_bot/maps/levine.yaml index 913ef94..e2e65fa 100644 --- a/f1tenth_gym_ros/maps/levine.yaml +++ b/my_bot/maps/levine.yaml @@ -3,4 +3,4 @@ resolution: 0.050000 origin: [-51.224998, -51.224998, 0.000000] negate: 0 occupied_thresh: 0.65 -free_thresh: 0.196 +free_thresh: 0.196 \ No newline at end of file diff --git a/my_bot/maps/levine2.pgm b/my_bot/maps/levine2.pgm new file mode 100644 index 0000000..478add7 Binary files /dev/null and b/my_bot/maps/levine2.pgm differ diff --git a/my_bot/maps/levine2.yaml b/my_bot/maps/levine2.yaml new file mode 100644 index 0000000..2c2c890 --- /dev/null +++ b/my_bot/maps/levine2.yaml @@ -0,0 +1,7 @@ +image: levine2.pgm +mode: trinary +resolution: 0.05 +origin: [-16.1, -3.5, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/my_bot/maps/map1_save.pgm b/my_bot/maps/map1_save.pgm new file mode 100644 index 0000000..6d29a68 Binary files /dev/null and b/my_bot/maps/map1_save.pgm differ diff --git a/my_bot/maps/map1_save.yaml b/my_bot/maps/map1_save.yaml new file mode 100644 index 0000000..f0c33e6 --- /dev/null +++ b/my_bot/maps/map1_save.yaml @@ -0,0 +1,7 @@ +image: map1_save.pgm +mode: trinary +resolution: 0.05 +origin: [-5.55, -5.02, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/my_bot/package.xml b/my_bot/package.xml new file mode 100644 index 0000000..d80d881 --- /dev/null +++ b/my_bot/package.xml @@ -0,0 +1,24 @@ + + + + my_bot + 0.0.0 + TODO: Package description + MY NAME + TODO: License declaration + + ament_cmake + + xacro + robot_state_publisher + gazebo_ros + rclpy + teleop_twist_keyboard + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/f1tenth_gym_ros/worlds/empty.world b/my_bot/worlds/empty.world similarity index 100% rename from f1tenth_gym_ros/worlds/empty.world rename to my_bot/worlds/empty.world diff --git a/f1tenth_gym_ros/worlds/levine.world b/my_bot/worlds/levine.world similarity index 96% rename from f1tenth_gym_ros/worlds/levine.world rename to my_bot/worlds/levine.world index 8317c0a..e3cfe8a 100644 --- a/f1tenth_gym_ros/worlds/levine.world +++ b/my_bot/worlds/levine.world @@ -1,4 +1,3 @@ - @@ -90,6 +89,347 @@ 0 0 + + + + 0.177215 0 0.060506 0 -0 0 + 0.79 + + 0.00271819 + 0 + 0.000910886 + 0.0116814 + 0 + 0.011264 + + + + 0.12 0 0.05 0 -0 0 + + + 0.24 0.16 0.1 + + + + + + + + + + + + + + + 10 + + + 0.24 0 0 0 -0 0 + + + 0.04 + + + + + + + + + 0.001 + 0.001 + + + + + + + + 10 + + + 0.2 0 0.12 0 -0 0 + + + 0.04 + 0.033 + + + + + + + + + + + + + + + 10 + + + 0.12 0 0.05 0 -0 0 + + + 0.24 0.2 0.1 + + + + + + + + 0.2 0 0.16 0 -0 0 + + + 0.025 0.09 0.02 + + + + + + + + 0.24 0 0 0 -0 0 + + + 0.04 + + + + + + + + 0.2 0 0.12 0 -0 0 + + + 0.04 + 0.033 + + + + + + + + 1 + 10 + + 1.089 + + B8G8R8 + 640 + 480 + + + 0.05 + 8 + + + + camera_link_optical + 0.1 + 100.0 + + 0.2 0 0.16 0 -0 0 + + + 1 + 10 + + + + 360 + -3.14 + 3.14 + 1 + + + 1 + 0 + 0 + + + + 0.3 + 12 + + + + + ~/out:=scan + + sensor_msgs/LaserScan + laser_frame + + 0.2 0 0.12 0 -0 0 + + 0 + 0 + 0 + + + 0 0.1 0 -1.5708 0 0 + base_link + left_wheel + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 0.1 + + 5.33333e-05 + 0 + 0 + 5.33333e-05 + 0 + 8e-05 + + + + 0 0 0 0 -0 0 + + + 0.04 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 + + + 0.04 + 0.04 + + + + + + + 0 + 0 + 0 + + + 0 -0.1 0 1.5708 -0 0 + base_link + right_wheel + + 0 0 -1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 0.1 + + 5.33333e-05 + 0 + 0 + 5.33333e-05 + 0 + 8e-05 + + + + 0 0 0 0 -0 0 + + + 0.04 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 + + + 0.04 + 0.04 + + + + + + + 0 + 0 + 0 + + 0 + + /home/ivan/dev_ws/install/my_bot/share/my_bot/config/my_controllers.yaml + + 0 0 0 0 -0 0 + 0.555504 1.12858 0.5 0 -0 0 diff --git a/f1tenth_gym_ros/worlds/obstacles.world b/my_bot/worlds/obstacles.world similarity index 100% rename from f1tenth_gym_ros/worlds/obstacles.world rename to my_bot/worlds/obstacles.world diff --git a/path_planning/package.xml b/path_planning/package.xml deleted file mode 100644 index 95c31e6..0000000 --- a/path_planning/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - path_planning - 0.0.0 - TODO: Package description - israel - TODO: License declaration - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - rclpy - tf2_ros - tf2_geometry_msgs - geometry_msgs - nav_msgs - - - ament_python - - diff --git a/path_planning/path_planning/__init__.py b/path_planning/path_planning/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/path_planning/path_planning/waypoint_recorder.py b/path_planning/path_planning/waypoint_recorder.py deleted file mode 100755 index 7f809db..0000000 --- a/path_planning/path_planning/waypoint_recorder.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - -import rclpy -from rclpy.node import Node -import os -import numpy as np -import pandas as pd -from nav_msgs.msg import Odometry -from tf_transformations import euler_from_quaternion - -class WaypointsLogger(Node): - def __init__(self): - # Initialize the node - super().__init__('waypoints_logger') - - # Configure the output file path - self.filename = "" # Define file path for saving waypoints - - # Initialize the DataFrame to store waypoints - self.waypoints_df = pd.DataFrame(columns=['x', 'y', 'w']) - - # Last recorded position (used to compare movements) - self.last_position = None - self.min_distance = 0.05 # Minimum distance to register a new waypoint (in meters) - - # Define the odometry topic to listen to - self.odom_topic = "" # Define the odometry topic here - self.create_subscription(Odometry, self.odom_topic, self.save_waypoint, 10) - - def save_waypoint(self, data): - """ - Callback function to save x, y, and w (orientation) from the robot. - You should implement logic to save the waypoint under certain conditions. - """ - pass # Implement saving the waypoint here - - def has_moved_significantly(self, x, y): - """ - Check if the robot has moved significantly (i.e., beyond the minimum distance). - Implement logic to calculate the distance between the current and last position. - """ - pass # Implement distance check here - - def save_to_file(self): - """ - Save the waypoints DataFrame to a CSV file. - Implement logic to save the waypoints to a file when the node shuts down. - """ - pass # Implement saving to CSV here - -def main(args=None): - rclpy.init(args=args) - node = WaypointsLogger() - - try: - # Run the main ROS loop to process callbacks - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - # Ensure to save the waypoints when shutting down - node.save_to_file() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/path_planning/resource/path_planning b/path_planning/resource/path_planning deleted file mode 100644 index e69de29..0000000 diff --git a/path_planning/setup.cfg b/path_planning/setup.cfg deleted file mode 100644 index 7411385..0000000 --- a/path_planning/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/path_planning -[install] -install_scripts=$base/lib/path_planning diff --git a/path_planning/setup.py b/path_planning/setup.py deleted file mode 100644 index 1e46e09..0000000 --- a/path_planning/setup.py +++ /dev/null @@ -1,26 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'path_planning' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='israel', - maintainer_email='israel@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'waypoint_recorder = path_planning.waypoint_recorder:main', - ], - }, -) diff --git a/path_planning/test/test_copyright.py b/path_planning/test/test_copyright.py deleted file mode 100644 index 97a3919..0000000 --- a/path_planning/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# 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' diff --git a/path_planning/test/test_flake8.py b/path_planning/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/path_planning/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# 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) diff --git a/path_planning/test/test_pep257.py b/path_planning/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/path_planning/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# 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_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/waypoints/levine_path.csv b/waypoints/levine_path.csv deleted file mode 100644 index 3f42452..0000000 --- a/waypoints/levine_path.csv +++ /dev/null @@ -1,1152 +0,0 @@ -x,y,w -0.0,0.0,0.0 -0.05,0.0,0.0 -0.1,0.0,0.0 -0.153,0.0,0.0 -0.204,0.0,0.0 -0.257,0.0,0.0 -0.31,0.0,0.0 -0.364,0.0,0.0 -0.419,0.0,0.0 -0.474,0.0,0.0 -0.524,0.0,0.0 -0.578,0.0,0.0 -0.628,0.0,0.0 -0.678,0.0,0.0 -0.733,0.0,0.0 -0.783,0.0,0.0 -0.838,0.0,0.0 -0.888,0.0,0.0 -0.943,0.0,0.0 -0.993,0.0,0.0 -1.048,0.0,0.0 -1.098,0.0,0.0 -1.153,0.0,0.0 -1.203,0.0,0.0 -1.258,0.0,0.0 -1.308,0.0,0.0 -1.358,0.0,0.0 -1.413,0.0,0.0 -1.463,0.0,0.0 -1.518,-0.0,-0.009 -1.568,-0.002,-0.053 -1.618,-0.005,-0.101 -1.668,-0.012,-0.148 -1.722,-0.021,-0.173 -1.776,-0.029,-0.139 -1.826,-0.035,-0.091 -1.876,-0.039,-0.044 -1.926,-0.04,-0.019 -1.976,-0.041,-0.044 -2.026,-0.045,-0.091 -2.08,-0.051,-0.143 -2.13,-0.059,-0.189 -2.179,-0.069,-0.199 -2.228,-0.079,-0.184 -2.282,-0.089,-0.184 -2.336,-0.099,-0.184 -2.39,-0.109,-0.184 -2.444,-0.119,-0.184 -2.498,-0.129,-0.184 -2.552,-0.139,-0.159 -2.602,-0.145,-0.126 -2.656,-0.152,-0.125 -2.706,-0.159,-0.125 -2.756,-0.165,-0.125 -2.81,-0.171,-0.109 -2.86,-0.176,-0.077 -2.91,-0.18,-0.076 -2.96,-0.183,-0.076 -3.01,-0.187,-0.076 -3.064,-0.191,-0.076 -3.114,-0.195,-0.076 -3.164,-0.199,-0.072 -3.214,-0.202,-0.033 -3.264,-0.203,-0.017 -3.314,-0.204,-0.017 -3.369,-0.204,-0.017 -3.419,-0.205,-0.017 -3.469,-0.206,-0.011 -3.519,-0.206,0.021 -3.569,-0.205,0.023 -3.619,-0.204,0.023 -3.669,-0.202,0.023 -3.719,-0.201,0.023 -3.769,-0.2,0.023 -3.819,-0.199,0.023 -3.869,-0.198,0.023 -3.919,-0.197,0.023 -3.969,-0.196,0.023 -4.019,-0.194,0.021 -4.074,-0.194,-0.02 -4.124,-0.196,-0.036 -4.174,-0.198,-0.036 -4.224,-0.199,-0.027 -4.274,-0.2,0.017 -4.324,-0.198,0.065 -4.378,-0.193,0.117 -4.428,-0.186,0.146 -4.482,-0.178,0.146 -4.532,-0.17,0.146 -4.586,-0.162,0.146 -4.636,-0.155,0.146 -4.69,-0.147,0.146 -4.74,-0.14,0.122 -4.794,-0.134,0.097 -4.844,-0.13,0.097 -4.894,-0.125,0.097 -4.944,-0.12,0.097 -4.998,-0.115,0.097 -5.048,-0.11,0.073 -5.098,-0.107,0.048 -5.148,-0.105,0.048 -5.198,-0.103,0.048 -5.248,-0.1,0.028 -5.303,-0.1,-0.001 -5.358,-0.1,-0.001 -5.408,-0.1,-0.001 -5.463,-0.1,-0.001 -5.518,-0.1,-0.001 -5.568,-0.101,-0.013 -5.618,-0.102,-0.049 -5.668,-0.105,-0.051 -5.718,-0.107,-0.051 -5.768,-0.11,-0.051 -5.818,-0.112,-0.051 -5.868,-0.115,-0.051 -5.918,-0.117,-0.051 -5.972,-0.12,-0.051 -6.022,-0.123,-0.051 -6.072,-0.125,-0.051 -6.122,-0.128,-0.051 -6.172,-0.13,-0.051 -6.222,-0.133,-0.051 -6.272,-0.135,-0.051 -6.322,-0.138,-0.051 -6.372,-0.141,-0.055 -6.422,-0.144,-0.091 -6.472,-0.149,-0.1 -6.526,-0.154,-0.1 -6.576,-0.159,-0.1 -6.626,-0.164,-0.1 -6.676,-0.169,-0.1 -6.73,-0.175,-0.1 -6.78,-0.18,-0.1 -6.83,-0.185,-0.1 -6.88,-0.19,-0.1 -6.934,-0.195,-0.1 -6.984,-0.2,-0.1 -7.034,-0.205,-0.1 -7.084,-0.21,-0.1 -7.138,-0.216,-0.1 -7.188,-0.221,-0.1 -7.238,-0.226,-0.1 -7.292,-0.231,-0.1 -7.342,-0.236,-0.1 -7.392,-0.241,-0.1 -7.442,-0.246,-0.1 -7.496,-0.252,-0.1 -7.546,-0.257,-0.1 -7.596,-0.262,-0.1 -7.646,-0.267,-0.1 -7.7,-0.272,-0.1 -7.75,-0.277,-0.076 -7.8,-0.28,-0.06 -7.85,-0.283,-0.06 -7.9,-0.286,-0.06 -7.95,-0.289,-0.059 -8.0,-0.292,-0.033 -8.05,-0.293,-0.029 -8.1,-0.294,-0.029 -8.15,-0.296,-0.028 -8.2,-0.297,0.005 -8.25,-0.295,0.052 -8.3,-0.292,0.068 -8.354,-0.288,0.068 -8.404,-0.285,0.068 -8.454,-0.282,0.069 -8.504,-0.277,0.102 -8.554,-0.271,0.146 -8.608,-0.263,0.155 -8.662,-0.254,0.155 -8.712,-0.246,0.164 -8.766,-0.236,0.213 -8.819,-0.223,0.265 -8.867,-0.209,0.313 -8.919,-0.191,0.365 -8.97,-0.17,0.417 -9.015,-0.148,0.465 -9.064,-0.122,0.517 -9.111,-0.094,0.569 -9.156,-0.063,0.622 -9.2,-0.03,0.671 -9.243,0.005,0.68 -9.286,0.039,0.68 -9.328,0.074,0.68 -9.371,0.109,0.7 -9.412,0.145,0.752 -9.448,0.18,0.799 -9.482,0.217,0.847 -9.517,0.259,0.899 -9.547,0.299,0.947 -9.576,0.34,0.994 -9.604,0.387,1.046 -9.628,0.431,1.094 -9.65,0.476,1.141 -9.67,0.522,1.189 -9.689,0.573,1.241 -9.704,0.621,1.289 -9.718,0.674,1.341 -9.728,0.723,1.388 -9.737,0.777,1.439 -9.743,0.827,1.451 -9.749,0.877,1.451 -9.756,0.931,1.451 -9.761,0.981,1.451 -9.768,1.035,1.451 -9.774,1.085,1.466 -9.779,1.135,1.475 -9.783,1.185,1.475 -9.789,1.239,1.475 -9.794,1.289,1.475 -9.798,1.339,1.475 -9.803,1.389,1.475 -9.808,1.443,1.475 -9.813,1.493,1.475 -9.818,1.543,1.491 -9.821,1.593,1.523 -9.823,1.643,1.524 -9.825,1.693,1.524 -9.828,1.743,1.524 -9.83,1.793,1.524 -9.832,1.843,1.524 -9.834,1.893,1.54 -9.835,1.943,1.579 -9.834,1.993,1.583 -9.834,2.043,1.583 -9.833,2.093,1.583 -9.832,2.147,1.583 -9.832,2.197,1.583 -9.831,2.247,1.583 -9.831,2.297,1.583 -9.83,2.347,1.583 -9.829,2.397,1.583 -9.829,2.447,1.583 -9.828,2.497,1.583 -9.827,2.547,1.583 -9.827,2.602,1.583 -9.826,2.652,1.583 -9.825,2.707,1.583 -9.825,2.757,1.583 -9.824,2.807,1.583 -9.824,2.857,1.583 -9.823,2.907,1.583 -9.822,2.957,1.583 -9.822,3.007,1.583 -9.821,3.057,1.583 -9.821,3.107,1.583 -9.82,3.157,1.583 -9.819,3.207,1.583 -9.819,3.257,1.583 -9.818,3.307,1.583 -9.817,3.357,1.583 -9.817,3.412,1.583 -9.816,3.462,1.583 -9.815,3.517,1.583 -9.815,3.567,1.583 -9.814,3.617,1.583 -9.814,3.672,1.583 -9.813,3.722,1.583 -9.812,3.772,1.583 -9.812,3.822,1.583 -9.811,3.872,1.583 -9.81,3.922,1.583 -9.81,3.977,1.583 -9.809,4.027,1.583 -9.809,4.082,1.583 -9.808,4.132,1.583 -9.807,4.182,1.583 -9.807,4.237,1.583 -9.806,4.287,1.583 -9.805,4.337,1.583 -9.805,4.392,1.583 -9.804,4.442,1.583 -9.803,4.492,1.583 -9.803,4.547,1.583 -9.802,4.597,1.583 -9.801,4.652,1.583 -9.801,4.707,1.583 -9.8,4.757,1.583 -9.8,4.807,1.583 -9.799,4.857,1.583 -9.798,4.907,1.583 -9.798,4.962,1.583 -9.797,5.012,1.583 -9.796,5.062,1.583 -9.796,5.117,1.583 -9.795,5.167,1.583 -9.794,5.217,1.583 -9.794,5.267,1.583 -9.793,5.317,1.583 -9.793,5.372,1.583 -9.792,5.422,1.583 -9.791,5.472,1.583 -9.791,5.527,1.583 -9.79,5.577,1.583 -9.789,5.627,1.583 -9.789,5.682,1.583 -9.788,5.732,1.583 -9.787,5.787,1.583 -9.787,5.842,1.583 -9.786,5.892,1.583 -9.785,5.947,1.583 -9.785,6.002,1.583 -9.784,6.052,1.583 -9.784,6.102,1.583 -9.783,6.152,1.583 -9.782,6.202,1.583 -9.782,6.257,1.583 -9.781,6.307,1.583 -9.781,6.362,1.55 -9.783,6.412,1.534 -9.785,6.462,1.534 -9.786,6.512,1.534 -9.788,6.562,1.534 -9.79,6.612,1.534 -9.792,6.662,1.534 -9.794,6.712,1.534 -9.796,6.762,1.534 -9.797,6.812,1.534 -9.799,6.862,1.538 -9.8,6.912,1.577 -9.799,6.962,1.625 -9.795,7.012,1.673 -9.788,7.066,1.714 -9.781,7.116,1.716 -9.773,7.17,1.716 -9.765,7.22,1.716 -9.757,7.274,1.716 -9.749,7.328,1.716 -9.742,7.378,1.716 -9.734,7.432,1.716 -9.727,7.482,1.716 -9.719,7.536,1.716 -9.712,7.586,1.716 -9.704,7.64,1.716 -9.696,7.69,1.728 -9.686,7.744,1.779 -9.674,7.797,1.831 -9.658,7.85,1.883 -9.64,7.902,1.936 -9.619,7.953,1.988 -9.595,8.002,2.04 -9.569,8.051,2.092 -9.541,8.098,2.145 -9.512,8.139,2.192 -9.482,8.179,2.24 -9.447,8.221,2.292 -9.413,8.258,2.339 -9.374,8.296,2.392 -9.337,8.33,2.421 -9.299,8.363,2.421 -9.258,8.399,2.421 -9.22,8.432,2.446 -9.177,8.466,2.496 -9.137,8.496,2.508 -9.096,8.525,2.508 -9.056,8.555,2.517 -9.011,8.586,2.566 -8.968,8.612,2.614 -8.924,8.636,2.661 -8.879,8.658,2.709 -8.829,8.68,2.761 -8.777,8.699,2.813 -8.725,8.715,2.866 -8.676,8.728,2.913 -8.627,8.738,2.961 -8.573,8.746,3.013 -8.523,8.752,3.05 -8.469,8.757,3.052 -8.419,8.761,3.052 -8.369,8.766,3.052 -8.319,8.77,3.068 -8.269,8.773,3.107 -8.219,8.774,3.111 -8.169,8.776,3.111 -8.119,8.777,3.111 -8.069,8.779,3.111 -8.019,8.78,3.111 -7.969,8.782,3.119 -7.919,8.782,-3.119 -7.865,8.78,-3.095 -7.815,8.777,-3.095 -7.765,8.775,-3.095 -7.715,8.773,-3.095 -7.665,8.77,-3.095 -7.615,8.768,-3.095 -7.565,8.766,-3.095 -7.515,8.763,-3.095 -7.465,8.761,-3.095 -7.415,8.759,-3.097 -7.365,8.757,-3.132 -7.31,8.757,3.139 -7.255,8.757,3.139 -7.2,8.757,3.139 -7.15,8.758,3.139 -7.1,8.758,3.139 -7.045,8.758,3.139 -6.99,8.758,3.139 -6.935,8.758,3.139 -6.88,8.758,3.139 -6.825,8.758,3.139 -6.77,8.759,3.139 -6.715,8.759,3.139 -6.66,8.759,3.139 -6.605,8.759,3.139 -6.555,8.759,3.139 -6.5,8.759,3.139 -6.445,8.759,3.139 -6.395,8.759,3.139 -6.345,8.76,3.139 -6.29,8.76,3.139 -6.235,8.76,3.139 -6.185,8.76,3.139 -6.13,8.76,3.139 -6.075,8.76,3.139 -6.02,8.76,3.139 -5.965,8.76,3.139 -5.915,8.761,3.139 -5.86,8.761,3.139 -5.81,8.761,3.139 -5.755,8.761,3.139 -5.7,8.761,3.139 -5.645,8.761,3.139 -5.59,8.761,3.139 -5.54,8.762,3.139 -5.485,8.762,3.139 -5.435,8.762,3.139 -5.38,8.762,3.139 -5.325,8.762,3.139 -5.27,8.762,3.139 -5.215,8.762,3.139 -5.16,8.762,3.139 -5.11,8.763,3.139 -5.06,8.763,3.139 -5.005,8.763,3.139 -4.95,8.763,3.139 -4.895,8.763,3.139 -4.84,8.763,3.139 -4.785,8.763,3.139 -4.735,8.764,3.139 -4.685,8.764,3.139 -4.63,8.764,3.139 -4.575,8.764,3.139 -4.52,8.764,3.139 -4.465,8.764,3.139 -4.41,8.764,3.139 -4.355,8.764,3.139 -4.305,8.765,3.139 -4.25,8.765,3.139 -4.195,8.765,3.139 -4.145,8.765,3.139 -4.09,8.765,3.139 -4.035,8.765,3.139 -3.985,8.765,3.139 -3.93,8.765,3.139 -3.88,8.766,3.139 -3.825,8.766,3.139 -3.775,8.766,3.139 -3.72,8.766,3.139 -3.67,8.766,3.139 -3.615,8.766,3.139 -3.565,8.766,3.139 -3.51,8.767,3.139 -3.455,8.767,3.139 -3.405,8.767,3.139 -3.35,8.767,3.139 -3.3,8.767,3.139 -3.245,8.767,3.139 -3.195,8.767,3.139 -3.14,8.767,3.139 -3.09,8.768,3.139 -3.035,8.768,3.139 -2.985,8.768,3.139 -2.93,8.768,3.139 -2.88,8.768,3.139 -2.825,8.768,3.139 -2.775,8.768,3.139 -2.72,8.768,3.139 -2.67,8.769,3.139 -2.615,8.769,3.139 -2.565,8.769,3.139 -2.51,8.769,3.139 -2.455,8.769,3.139 -2.405,8.769,3.139 -2.35,8.769,3.139 -2.3,8.769,3.139 -2.25,8.77,3.139 -2.195,8.77,3.139 -2.14,8.77,3.139 -2.09,8.77,3.139 -2.035,8.77,3.139 -1.985,8.77,3.139 -1.935,8.77,3.139 -1.885,8.77,3.139 -1.835,8.771,3.139 -1.785,8.771,3.139 -1.73,8.771,3.139 -1.68,8.771,3.139 -1.63,8.771,3.139 -1.578,8.771,3.139 -1.524,8.771,3.139 -1.471,8.772,3.139 -1.416,8.772,3.139 -1.362,8.772,3.139 -1.312,8.772,3.139 -1.258,8.772,3.139 -1.208,8.772,3.139 -1.153,8.772,3.139 -1.098,8.772,3.139 -1.043,8.773,3.139 -0.988,8.773,3.139 -0.933,8.773,3.139 -0.878,8.773,3.139 -0.827,8.773,3.139 -0.775,8.773,3.139 -0.722,8.773,3.139 -0.668,8.773,3.139 -0.615,8.774,3.139 -0.561,8.774,3.139 -0.506,8.774,3.139 -0.452,8.774,3.139 -0.398,8.774,3.139 -0.343,8.774,3.139 -0.289,8.774,3.139 -0.235,8.775,3.139 -0.18,8.775,3.139 -0.125,8.775,3.139 -0.075,8.775,3.139 -0.023,8.775,3.139 --0.031,8.775,3.139 --0.087,8.775,3.139 --0.138,8.775,3.139 --0.191,8.776,3.139 --0.245,8.776,3.139 --0.299,8.776,3.139 --0.355,8.776,3.139 --0.41,8.776,3.139 --0.466,8.776,3.139 --0.522,8.776,3.139 --0.578,8.777,3.139 --0.634,8.777,3.139 --0.69,8.777,3.139 --0.746,8.777,3.139 --0.803,8.777,3.139 --0.859,8.777,3.139 --0.915,8.777,3.139 --0.972,8.778,3.139 --1.028,8.778,3.139 --1.084,8.778,3.139 --1.141,8.778,3.139 --1.197,8.778,3.139 --1.253,8.778,3.139 --1.31,8.778,3.139 --1.366,8.778,3.139 --1.422,8.779,3.139 --1.479,8.779,3.139 --1.535,8.779,3.139 --1.592,8.779,3.139 --1.648,8.779,3.139 --1.704,8.779,3.139 --1.761,8.779,3.139 --1.817,8.78,3.139 --1.873,8.78,3.139 --1.93,8.78,3.139 --1.986,8.78,3.139 --2.042,8.78,3.139 --2.099,8.78,3.139 --2.155,8.78,3.139 --2.212,8.781,3.139 --2.268,8.781,3.139 --2.324,8.781,3.139 --2.381,8.781,3.139 --2.437,8.781,3.139 --2.493,8.781,3.139 --2.55,8.781,3.139 --2.606,8.782,3.139 --2.663,8.782,3.139 --2.719,8.782,3.139 --2.775,8.782,3.139 --2.832,8.782,3.139 --2.888,8.782,3.139 --2.944,8.782,3.139 --3.001,8.782,3.139 --3.057,8.783,3.139 --3.113,8.783,3.139 --3.17,8.783,3.139 --3.226,8.783,3.139 --3.283,8.783,3.139 --3.339,8.783,3.139 --3.395,8.783,3.139 --3.452,8.784,3.139 --3.508,8.784,3.139 --3.564,8.784,3.139 --3.621,8.784,3.139 --3.677,8.784,3.139 --3.733,8.784,3.139 --3.79,8.784,3.139 --3.846,8.785,3.139 --3.903,8.785,3.139 --3.959,8.785,3.139 --4.015,8.785,3.139 --4.072,8.785,3.139 --4.128,8.785,3.139 --4.184,8.785,3.139 --4.241,8.786,3.139 --4.297,8.786,3.139 --4.354,8.786,3.139 --4.41,8.786,3.139 --4.466,8.786,3.139 --4.523,8.786,3.139 --4.579,8.786,3.139 --4.635,8.786,3.139 --4.692,8.787,3.139 --4.748,8.787,3.139 --4.804,8.787,3.139 --4.861,8.787,3.139 --4.917,8.787,3.139 --4.974,8.787,3.139 --5.03,8.787,3.139 --5.086,8.788,3.139 --5.143,8.788,3.139 --5.199,8.788,3.139 --5.255,8.788,3.139 --5.312,8.788,3.139 --5.368,8.788,3.139 --5.425,8.788,3.139 --5.481,8.789,3.139 --5.537,8.789,3.139 --5.594,8.789,3.139 --5.65,8.789,3.139 --5.706,8.789,3.139 --5.763,8.789,3.139 --5.819,8.789,3.139 --5.875,8.79,3.139 --5.932,8.79,3.139 --5.988,8.79,3.139 --6.045,8.79,3.139 --6.101,8.79,3.139 --6.157,8.79,3.139 --6.214,8.79,3.139 --6.27,8.791,3.139 --6.326,8.791,3.139 --6.383,8.791,3.139 --6.439,8.791,3.139 --6.496,8.791,3.139 --6.552,8.791,3.139 --6.608,8.791,3.139 --6.665,8.791,3.139 --6.721,8.792,3.139 --6.777,8.792,3.139 --6.834,8.792,3.139 --6.89,8.792,3.139 --6.946,8.792,3.139 --7.003,8.792,3.139 --7.059,8.792,3.139 --7.116,8.793,3.139 --7.172,8.793,3.139 --7.228,8.793,3.139 --7.285,8.793,3.139 --7.341,8.793,3.139 --7.397,8.793,3.139 --7.454,8.793,3.139 --7.51,8.794,3.139 --7.566,8.794,3.139 --7.623,8.794,3.139 --7.679,8.794,3.139 --7.736,8.794,3.139 --7.792,8.794,3.139 --7.848,8.794,3.139 --7.905,8.795,3.139 --7.961,8.795,3.139 --8.017,8.795,3.139 --8.074,8.795,3.139 --8.13,8.795,3.139 --8.187,8.795,3.139 --8.243,8.795,3.139 --8.299,8.795,3.139 --8.356,8.796,3.139 --8.412,8.796,3.139 --8.468,8.796,3.139 --8.525,8.796,3.139 --8.581,8.796,3.139 --8.637,8.796,3.139 --8.694,8.796,3.139 --8.75,8.797,3.139 --8.807,8.797,3.139 --8.863,8.797,3.139 --8.919,8.797,3.139 --8.976,8.797,3.139 --9.032,8.797,3.139 --9.088,8.797,3.139 --9.145,8.798,3.139 --9.201,8.798,3.139 --9.258,8.798,3.139 --9.314,8.798,3.139 --9.37,8.798,3.139 --9.427,8.798,3.139 --9.483,8.798,3.139 --9.539,8.799,3.139 --9.596,8.799,3.139 --9.652,8.799,3.139 --9.708,8.799,3.139 --9.765,8.799,3.139 --9.821,8.799,3.139 --9.878,8.799,3.139 --9.934,8.799,3.139 --9.99,8.8,3.139 --10.047,8.8,3.139 --10.103,8.8,3.139 --10.159,8.8,3.139 --10.216,8.8,3.139 --10.272,8.797,-3.125 --10.328,8.794,-3.116 --10.385,8.793,-3.116 --10.441,8.791,-3.116 --10.497,8.79,-3.116 --10.554,8.788,-3.116 --10.61,8.787,-3.116 --10.666,8.786,-3.116 --10.723,8.784,-3.116 --10.779,8.782,-3.114 --10.835,8.776,-3.083 --10.89,8.763,-3.032 --10.945,8.75,-2.998 --11.0,8.741,-2.994 --11.056,8.733,-2.994 --11.112,8.724,-2.994 --11.168,8.716,-2.994 --11.223,8.708,-2.994 --11.279,8.7,-2.994 --11.335,8.694,-3.015 --11.392,8.695,-3.058 --11.448,8.694,-3.072 --11.504,8.69,-3.072 --11.56,8.686,-3.072 --11.617,8.682,-3.072 --11.673,8.679,-3.076 --11.729,8.681,-3.112 --11.785,8.688,3.128 --11.841,8.691,3.119 --11.898,8.692,3.119 --11.954,8.694,3.119 --12.01,8.695,3.119 --12.067,8.696,3.119 --12.123,8.697,3.119 --12.18,8.699,3.119 --12.236,8.7,3.119 --12.292,8.701,3.119 --12.349,8.703,3.119 --12.405,8.704,3.119 --12.461,8.704,3.13 --12.517,8.697,-3.109 --12.572,8.685,-3.058 --12.627,8.671,-3.006 --12.681,8.654,-2.955 --12.734,8.636,-2.916 --12.789,8.621,-2.91 --12.843,8.608,-2.91 --12.898,8.596,-2.91 --12.953,8.58,-2.895 --13.004,8.559,-2.847 --13.055,8.533,-2.796 --13.104,8.505,-2.744 --13.151,8.475,-2.692 --13.197,8.442,-2.641 --13.241,8.407,-2.589 --13.283,8.37,-2.538 --13.324,8.33,-2.486 --13.362,8.289,-2.435 --13.398,8.245,-2.383 --13.433,8.202,-2.349 --13.472,8.161,-2.345 --13.512,8.121,-2.345 --13.551,8.08,-2.341 --13.585,8.036,-2.305 --13.615,7.988,-2.254 --13.643,7.939,-2.202 --13.668,7.888,-2.15 --13.69,7.836,-2.099 --13.71,7.784,-2.047 --13.727,7.73,-1.996 --13.741,7.675,-1.944 --13.752,7.62,-1.892 --13.761,7.564,-1.841 --13.766,7.508,-1.789 --13.771,7.452,-1.755 --13.781,7.397,-1.752 --13.791,7.341,-1.752 --13.801,7.286,-1.752 --13.811,7.23,-1.752 --13.821,7.175,-1.752 --13.831,7.119,-1.752 --13.841,7.064,-1.752 --13.852,7.008,-1.752 --13.862,6.953,-1.752 --13.872,6.898,-1.752 --13.882,6.842,-1.752 --13.892,6.787,-1.752 --13.901,6.731,-1.745 --13.904,6.675,-1.704 --13.904,6.618,-1.665 --13.908,6.562,-1.659 --13.913,6.506,-1.659 --13.917,6.45,-1.659 --13.922,6.394,-1.659 --13.926,6.337,-1.652 --13.924,6.281,-1.612 --13.917,6.225,-1.561 --13.911,6.169,-1.538 --13.909,6.113,-1.537 --13.907,6.057,-1.537 --13.905,6.0,-1.537 --13.903,5.944,-1.537 --13.902,5.888,-1.537 --13.9,5.831,-1.538 --13.901,5.775,-1.558 --13.908,5.719,-1.602 --13.913,5.663,-1.615 --13.916,5.607,-1.615 --13.918,5.55,-1.615 --13.921,5.494,-1.615 --13.922,5.438,-1.604 --13.916,5.382,-1.56 --13.906,5.326,-1.509 --13.896,5.271,-1.48 --13.891,5.215,-1.478 --13.885,5.158,-1.478 --13.88,5.102,-1.478 --13.875,5.046,-1.478 --13.87,4.99,-1.478 --13.865,4.934,-1.478 --13.859,4.878,-1.478 --13.854,4.822,-1.478 --13.849,4.766,-1.478 --13.844,4.709,-1.478 --13.839,4.653,-1.478 --13.833,4.597,-1.478 --13.828,4.541,-1.478 --13.823,4.485,-1.478 --13.818,4.429,-1.478 --13.813,4.373,-1.478 --13.807,4.317,-1.478 --13.802,4.26,-1.478 --13.797,4.204,-1.478 --13.792,4.148,-1.478 --13.787,4.092,-1.478 --13.781,4.036,-1.478 --13.776,3.98,-1.479 --13.775,3.923,-1.505 --13.78,3.867,-1.552 --13.784,3.811,-1.571 --13.784,3.755,-1.571 --13.784,3.698,-1.571 --13.784,3.642,-1.571 --13.784,3.586,-1.571 --13.784,3.529,-1.571 --13.784,3.473,-1.571 --13.784,3.416,-1.571 --13.784,3.36,-1.571 --13.784,3.304,-1.571 --13.784,3.247,-1.571 --13.784,3.191,-1.571 --13.784,3.135,-1.571 --13.784,3.078,-1.571 --13.784,3.022,-1.571 --13.784,2.966,-1.571 --13.784,2.909,-1.571 --13.784,2.853,-1.571 --13.784,2.796,-1.571 --13.784,2.74,-1.571 --13.784,2.684,-1.571 --13.784,2.627,-1.571 --13.784,2.571,-1.571 --13.784,2.515,-1.571 --13.784,2.458,-1.571 --13.784,2.402,-1.571 --13.784,2.345,-1.571 --13.784,2.289,-1.571 --13.784,2.233,-1.571 --13.784,2.176,-1.571 --13.784,2.12,-1.571 --13.784,2.064,-1.571 --13.784,2.007,-1.571 --13.784,1.951,-1.571 --13.784,1.895,-1.571 --13.784,1.838,-1.571 --13.784,1.782,-1.571 --13.784,1.725,-1.571 --13.784,1.669,-1.571 --13.784,1.613,-1.571 --13.784,1.556,-1.571 --13.784,1.5,-1.571 --13.784,1.444,-1.571 --13.784,1.387,-1.571 --13.784,1.331,-1.571 --13.784,1.275,-1.571 --13.784,1.218,-1.571 --13.784,1.162,-1.571 --13.784,1.105,-1.569 --13.778,1.049,-1.538 --13.767,0.994,-1.486 --13.753,0.94,-1.435 --13.736,0.886,-1.383 --13.716,0.833,-1.332 --13.693,0.781,-1.28 --13.669,0.731,-1.237 --13.649,0.678,-1.228 --13.63,0.625,-1.228 --13.611,0.572,-1.228 --13.592,0.519,-1.228 --13.573,0.466,-1.228 --13.554,0.413,-1.228 --13.535,0.36,-1.228 --13.513,0.308,-1.207 --13.485,0.259,-1.158 --13.454,0.212,-1.107 --13.42,0.167,-1.055 --13.384,0.123,-1.003 --13.346,0.081,-0.952 --13.306,0.042,-0.9 --13.264,0.004,-0.848 --13.22,-0.031,-0.797 --13.175,-0.064,-0.745 --13.127,-0.095,-0.694 --13.078,-0.123,-0.642 --13.028,-0.148,-0.59 --12.977,-0.171,-0.539 --12.924,-0.191,-0.487 --12.87,-0.209,-0.436 --12.816,-0.223,-0.384 --12.761,-0.235,-0.332 --12.705,-0.244,-0.281 --12.649,-0.25,-0.229 --12.593,-0.254,-0.178 --12.537,-0.254,-0.126 --12.48,-0.251,-0.074 --12.424,-0.246,-0.023 --12.368,-0.237,0.029 --12.313,-0.23,0.057 --12.256,-0.226,0.059 --12.2,-0.222,0.059 --12.144,-0.219,0.059 --12.088,-0.216,0.059 --12.031,-0.212,0.059 --11.975,-0.209,0.059 --11.919,-0.206,0.059 --11.863,-0.202,0.059 --11.806,-0.199,0.059 --11.75,-0.196,0.059 --11.694,-0.192,0.059 --11.637,-0.189,0.059 --11.581,-0.186,0.059 --11.525,-0.182,0.059 --11.469,-0.179,0.059 --11.412,-0.176,0.055 --11.356,-0.179,0.019 --11.3,-0.185,-0.015 --11.244,-0.187,-0.019 --11.187,-0.188,-0.019 --11.131,-0.189,-0.014 --11.075,-0.183,0.022 --11.02,-0.173,0.073 --10.964,-0.161,0.112 --10.909,-0.153,0.118 --10.853,-0.146,0.118 --10.797,-0.14,0.118 --10.741,-0.134,0.107 --10.684,-0.136,0.063 --10.628,-0.141,0.02 --10.572,-0.142,0.011 --10.515,-0.142,0.011 --10.459,-0.141,0.011 --10.403,-0.14,0.011 --10.346,-0.14,0.011 --10.29,-0.139,0.011 --10.234,-0.139,0.011 --10.177,-0.138,0.011 --10.121,-0.137,0.011 --10.065,-0.137,0.011 --10.008,-0.136,0.011 --9.952,-0.136,0.011 --9.895,-0.135,0.011 --9.839,-0.134,0.015 --9.783,-0.127,0.051 --9.728,-0.115,0.1 --9.673,-0.105,0.118 --9.617,-0.098,0.118 --9.561,-0.092,0.118 --9.505,-0.085,0.118 --9.449,-0.078,0.118 --9.393,-0.072,0.116 --9.336,-0.071,0.085 --9.28,-0.075,0.035 --9.224,-0.078,0.012 --9.168,-0.078,0.011 --9.111,-0.077,0.011 --9.055,-0.077,0.011 --8.999,-0.076,0.011 --8.942,-0.076,0.011 --8.886,-0.073,0.022 --8.83,-0.069,0.039 --8.773,-0.066,0.039 --8.717,-0.064,0.039 --8.661,-0.062,0.039 --8.604,-0.06,0.039 --8.548,-0.057,0.039 --8.492,-0.055,0.039 --8.435,-0.053,0.039 --8.379,-0.051,0.039 --8.323,-0.049,0.039 --8.266,-0.047,0.039 --8.21,-0.044,0.039 --8.154,-0.042,0.039 --8.097,-0.04,0.039 --8.041,-0.038,0.039 --7.985,-0.036,0.039 --7.929,-0.033,0.039 --7.872,-0.031,0.039 --7.816,-0.029,0.039 --7.76,-0.027,0.039 --7.71,-0.025,0.039 --7.657,-0.023,0.039 --7.606,-0.022,0.031 --7.555,-0.028,-0.011 --7.5,-0.034,-0.036 --7.449,-0.036,-0.036 --7.397,-0.038,-0.036 --7.343,-0.04,-0.036 --7.288,-0.042,-0.036 --7.233,-0.044,-0.036 --7.178,-0.046,-0.036 --7.122,-0.048,-0.036 --7.067,-0.05,-0.036 --7.011,-0.052,-0.036 --6.955,-0.054,-0.036 --6.898,-0.056,-0.036 --6.842,-0.058,-0.036 --6.786,-0.06,-0.036 --6.73,-0.062,-0.036 --6.673,-0.064,-0.036 --6.617,-0.066,-0.036 --6.561,-0.068,-0.036 --6.505,-0.07,-0.036 --6.448,-0.072,-0.036 --6.392,-0.074,-0.036 --6.336,-0.076,-0.036 --6.279,-0.078,-0.036 --6.223,-0.08,-0.036 --6.167,-0.082,-0.036 --6.11,-0.084,-0.036 --6.054,-0.086,-0.036 --5.998,-0.088,-0.036 --5.941,-0.09,-0.036 --5.885,-0.092,-0.036 --5.829,-0.094,-0.036 --5.772,-0.094,-0.017 --5.716,-0.092,-0.008 --5.66,-0.093,-0.008 --5.603,-0.093,-0.008 --5.547,-0.094,-0.008 --5.49,-0.094,-0.008 --5.434,-0.095,-0.008 --5.378,-0.094,-0.004 --5.322,-0.089,0.031 --5.266,-0.082,0.054 --5.209,-0.079,0.055 --5.153,-0.076,0.055 --5.097,-0.072,0.055 --5.041,-0.069,0.055 --4.984,-0.066,0.055 --4.928,-0.063,0.055 --4.872,-0.06,0.055 --4.815,-0.057,0.055 --4.759,-0.054,0.055 --4.703,-0.051,0.055 --4.647,-0.048,0.055 --4.59,-0.045,0.055 --4.534,-0.042,0.055 --4.478,-0.038,0.055 --4.421,-0.035,0.055 --4.365,-0.032,0.055 --4.309,-0.029,0.055 --4.253,-0.026,0.055 --4.196,-0.023,0.055 --4.14,-0.02,0.055 --4.084,-0.017,0.055 --4.027,-0.014,0.055 --3.971,-0.011,0.055 --3.915,-0.008,0.055 --3.859,-0.005,0.055 --3.802,-0.001,0.055 --3.746,0.001,0.048 --3.69,-0.003,0.008 --3.634,-0.009,-0.021 --3.577,-0.011,-0.023 --3.521,-0.012,-0.023 --3.465,-0.014,-0.023 --3.408,-0.015,-0.023 --3.352,-0.016,-0.023 --3.296,-0.017,-0.023 --3.239,-0.019,-0.023 --3.183,-0.02,-0.023 --3.127,-0.021,-0.023 --3.07,-0.023,-0.023 --3.014,-0.024,-0.023 --2.958,-0.024,-0.016 --2.902,-0.018,0.025 --2.846,-0.011,0.053 --2.789,-0.007,0.055 --2.733,-0.004,0.055 --2.677,-0.001,0.055 --2.621,0.002,0.055 --2.564,0.005,0.055 --2.508,0.008,0.055 --2.452,0.011,0.055 --2.395,0.015,0.055 --2.339,0.014,0.034 --2.283,0.008,-0.009 --2.227,0.005,-0.023 --2.171,0.003,-0.023 --2.114,0.002,-0.023 --2.058,0.001,-0.023 --2.001,-0.001,-0.023 --1.945,-0.002,-0.023 --1.889,-0.003,-0.023 --1.832,-0.005,-0.023 --1.776,-0.006,-0.023 --1.72,-0.007,-0.023 --1.663,-0.008,-0.023 --1.607,-0.01,-0.023 --1.551,-0.011,-0.023 --1.494,-0.012,-0.023 --1.438,-0.014,-0.023 --1.382,-0.015,-0.023 --1.325,-0.016,-0.023 --1.269,-0.017,-0.023 --1.213,-0.019,-0.023 --1.156,-0.02,-0.023 --1.1,-0.021,-0.023 --1.043,-0.023,-0.023 --0.993,-0.024,-0.023 --0.943,-0.025,-0.023 --0.892,-0.026,-0.023 --0.84,-0.027,-0.023 --0.788,-0.028,-0.023 --0.736,-0.03,-0.023 --0.685,-0.031,-0.023 --0.635,-0.032,-0.023 --0.585,-0.033,-0.023 --0.534,-0.034,-0.023 --0.481,-0.035,-0.023 -0.0,0.0,0.0 \ No newline at end of file