diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 58496c3f..fe4603a4 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,11 +1,11 @@ { - "name": "Autonomous Devcontainer", + "name": "Autonomous Dev", "dockerComposeFile": "docker-compose.yml", "service": "autonomous_ros2", "shutdownAction": "none", - "postCreateCommand": "rosdep install --from-paths src -y --ignore-src && colcon build --symlink-install", + "postCreateCommand": "rosdep install --from-paths src -y --ignore-src && colcon build", "workspaceFolder": "/ws", "customizations": { @@ -28,7 +28,8 @@ "redhat.vscode-xml", "twxs.cmake", "VisualStudioExptTeam.intellicode-api-usage-examples", - "VisualStudioExptTeam.vscodeintellicode" + "VisualStudioExptTeam.vscodeintellicode", + "redhat.vscode-yaml" ] } } diff --git a/.devcontainer/docker-compose.yml b/.devcontainer/docker-compose.yml index 6fc5e4f8..3cae9e4b 100644 --- a/.devcontainer/docker-compose.yml +++ b/.devcontainer/docker-compose.yml @@ -7,8 +7,10 @@ services: context: .. dockerfile: .devcontainer/ros2_ws.Dockerfile environment: - - DISPLAY + - DISPLAY=host.docker.internal - NVIDIA_VISIBLE_DEVICES=all + - NVIDIA_DRIVER_CAPABILITIES=all + - ROS_DOMAIN_ID=47 volumes: - ..:/ws network_mode: host @@ -20,7 +22,12 @@ services: resources: reservations: devices: - - capabilities: [gpu] + - driver: nvidia + device_ids: [] + capabilities: [gpu] + privileged: true + devices: + - /dev:/dev autonomous_zed: image: zed_sdk @@ -28,27 +35,33 @@ services: context: .. dockerfile: .devcontainer/zed.Dockerfile environment: - - DISPLAY + - DISPLAY=host.docker.internal - NVIDIA_VISIBLE_DEVICES=all + - NVIDIA_DRIVER_CAPABILITIES=all + - ROS_DOMAIN_ID=47 volumes: + - ../bringup:/ws/bringup - ../src/perception/:/ws/src/perception/ + - ../src/visualization/:/ws/src/visualization/ - ../src/moa/moa_description:/ws/src/moa/moa_description - ../src/moa/moa_msgs:/ws/src/moa/moa_msgs - ../ros_entrypoint.sh:/ws/ros_entrypoint.sh - /tmp/.X11-unix:/tmp/.X11-unix - /dev:/dev - - # command: bash -c ". /ws/ros_entrypoint.sh && ros2 launch zed_wrapper zed_camera.launch.py camera_model:='zed2i'" + + command: bash -c ". /ws/ros_entrypoint.sh && ros2 launch zed_wrapper zed_camera.launch.py camera_model:='zed2i'" network_mode: host ipc: host stdin_open: true tty: true - runtime: nvidia + # runtime: nvidia deploy: resources: reservations: devices: - - capabilities: [gpu] + - driver: nvidia + device_ids: [] + capabilities: [gpu] privileged: true devices: - - /dev:/dev \ No newline at end of file + - /dev:/dev diff --git a/.devcontainer/ros2_perception.Dockerfile b/.devcontainer/ros2_perception.Dockerfile index 19e645bc..2ad89336 100644 --- a/.devcontainer/ros2_perception.Dockerfile +++ b/.devcontainer/ros2_perception.Dockerfile @@ -1,12 +1,26 @@ -FROM stereolabs/zed:4.0-tools-devel-l4t-r35.4 -LABEL Name=zed_sdk Version=0.0.1 +FROM nvcr.io/nvidia/l4t-base:35.4.1 +LABEL Name=autonomous Version=0.0.1 SHELL [ "/bin/bash", "-c" ] WORKDIR /ws -ENV LANG C.UTF-8 -ENV LC_ALL C.UTF-8 +RUN apt-get update && \ + apt-get install -y --no-install-recommends \ + libopenblas-dev \ + libopenmpi-dev \ + openmpi-bin \ + openmpi-common \ + gfortran \ + libomp-dev \ + nvidia-cuda-dev \ + nvidia-cudnn8-dev && \ + rm -rf /var/lib/apt/lists/* && \ + apt-get clean + +RUN apt update && apt install locales && \ + locale-gen en_US en_US.UTF-8 && \ + update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 RUN apt update && apt install -y gnupg wget software-properties-common && \ add-apt-repository universe @@ -21,61 +35,39 @@ RUN apt update && apt install curl -y && \ echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu focal main" | \ tee /etc/apt/sources.list.d/ros2.list > /dev/null -# setup timezone & install packages -RUN apt-get update && apt-get install -q -y --no-install-recommends \ - tzdata \ - dirmngr \ - gnupg2 \ - git \ +ENV ROS_DISTRO humble + +RUN apt update && apt install --no-install-recommends -y \ ros-humble-ros-base \ + ros-dev-tools \ build-essential \ python3-colcon-common-extensions \ python3-colcon-mixin \ python3-rosdep \ - python3-vcstool && \ + python3-vcstool \ + python3-pip \ + ros-humble-foxglove-bridge && \ rm -rf /var/lib/apt/lists/* && \ - apt-get clean + apt-get clean && \ + echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc + +COPY . . -ENV ROS_DISTRO humble - -# setup colcon mixin and metadata -RUN rosdep init && \ - rosdep update --rosdistro $ROS_DISTRO && \ - colcon mixin add default \ - https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml && \ - colcon mixin update && \ - colcon metadata add default \ - https://raw.githubusercontent.com/colcon/colcon-metadata-repository/master/index.yaml && \ - colcon metadata update +RUN rosdep init && rosdep update --rosdistro $ROS_DISTRO && apt-get update && \ + cd /ws && \ + rosdep install --from-paths src -y -r --ignore-src --rosdistro=$ROS_DISTRO --os=ubuntu:jammy && \ + rm -rf /var/lib/apt/lists/* -COPY ./src/perception/ /ws/src/perception/ -COPY ./src/moa/moa_description /ws/src/moa/moa_description -COPY ./src/moa/moa_msgs /ws/src/moa/moa_msgs +ENV PYTORCH_URL=https://developer.download.nvidia.com/compute/redist/jp/v512/pytorch/torch-2.1.0a0+41361538.nv23.06-cp38-cp38-linux_aarch64.whl PYTORCH_WHL=torch-2.1.0a0+41361538.nv23.06-cp38-cp38-linux_aarch64.whl -# install ros2 packages -RUN cd /ws/src/ && \ - git clone --recursive https://github.com/stereolabs/zed-ros2-wrapper.git && \ - cd .. && \ - source /opt/ros/humble/setup.bash && \ - rosdep update && apt-get update && \ - rosdep install --from-paths src -y -r --ignore-src --rosdistro=$ROS_DISTRO --os=ubuntu:jammy && \ - rm -rf /var/lib/apt/lists/* && \ - apt-get clean +RUN cd /opt && \ + wget --quiet --show-progress --progress=bar:force:noscroll --no-check-certificate ${PYTORCH_URL} -O ${PYTORCH_WHL} && \ + pip3 install --verbose ${PYTORCH_WHL} -RUN cd /usr/local/zed && \ - pip install requests && \ - python3 get_python_api.py +RUN python3 -c 'import torch; print(f"PyTorch version: {torch.__version__}"); print(f"CUDA available: {torch.cuda.is_available()}"); print(f"cuDNN version: {torch.backends.cudnn.version()}"); print(torch.__config__.show());' RUN source /opt/ros/humble/setup.bash && \ colcon build --parallel-workers $(nproc) --symlink-install \ - --event-handlers console_direct+ --base-paths src \ - --cmake-args ' -DCMAKE_BUILD_TYPE=Release' \ - ' -DCMAKE_LIBRARY_PATH=/usr/local/cuda/lib64/stubs' \ - ' -DCMAKE_CXX_FLAGS="-Wl,--allow-shlib-undefined"' - -RUN echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc && \ - echo "source /ws/install/setup.bash" >> ~/.bashrc - -RUN . ~/.bashrc + --event-handlers console_direct+ --base-paths src -CMD ["bash"] \ No newline at end of file +CMD [ "bash" ] \ No newline at end of file diff --git a/.devcontainer/zed.Dockerfile b/.devcontainer/zed.Dockerfile index af911f5f..8f810f49 100644 --- a/.devcontainer/zed.Dockerfile +++ b/.devcontainer/zed.Dockerfile @@ -1,7 +1,10 @@ -FROM stereolabs/zed:4.0-tools-devel-l4t-r35.4 +FROM stereolabs/zed:4.1-tools-devel-l4t-r35.4 LABEL Name=zed_sdk Version=0.0.1 -SHELL [ "/bin/bash", "-c" ] +SHELL [ "/bin/bash", "-c"] + +SHELL [ "/bin/bash", "-c"] + WORKDIR /ws @@ -22,7 +25,22 @@ RUN apt update && apt install curl -y && \ tee /etc/apt/sources.list.d/ros2.list > /dev/null # setup timezone & install packages -RUN apt-get update && apt-get install -q -y --no-install-recommends \ +RUN apt-get update +RUN sudo apt-get upgrade -y +RUN apt-get update + + +RUN sudo apt-get install ros-humble-desktop -y + +RUN apt-get install -q -y --no-install-recommends \ +RUN apt-get update +RUN sudo apt-get upgrade -y +RUN apt-get update + + +RUN sudo apt-get install ros-humble-desktop -y + +RUN apt-get install -q -y --no-install-recommends \ tzdata \ dirmngr \ gnupg2 \ @@ -32,46 +50,52 @@ RUN apt-get update && apt-get install -q -y --no-install-recommends \ python3-colcon-common-extensions \ python3-colcon-mixin \ python3-rosdep \ - python3-vcstool && \ - rm -rf /var/lib/apt/lists/* && \ - apt-get clean + python3-vcstool +RUN rm -rf /var/lib/apt/lists/* +RUN apt-get clean + +ENV ROS_DISTRO humble -ENV ROS_DISTRO humble +# install ros2 packages +RUN cd /ws/src/ +RUN git clone --recursive https://github.com/stereolabs/zed-ros2-wrapper.git # setup colcon mixin and metadata -RUN rosdep init && \ - rosdep update --rosdistro $ROS_DISTRO && \ - colcon mixin add default \ - https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml && \ - colcon mixin update && \ - colcon metadata add default \ - https://raw.githubusercontent.com/colcon/colcon-metadata-repository/master/index.yaml && \ - colcon metadata update - -COPY ./src/perception/ /ws/src/perception/ -COPY ./src/moa/moa_description /ws/src/moa/moa_description -COPY ./src/moa/moa_msgs /ws/src/moa/moa_msgs +RUN rosdep init +RUN rosdep update --rosdistro $ROS_DISTRO +RUN colcon mixin add default \ + https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml +RUN colcon mixin update +RUN colcon metadata add default \ + https://raw.githubusercontent.com/colcon/colcon-metadata-repository/master/index.yaml +RUN colcon metadata update # install ros2 packages -RUN cd /ws/src/ && \ +RUN mkdir src && \ + cd /ws/src/ && \ git clone --recursive https://github.com/stereolabs/zed-ros2-wrapper.git && \ cd .. && \ source /opt/ros/humble/setup.bash && \ - rosdep update && apt-get update && \ - rosdep install --from-paths src -y -r --ignore-src --rosdistro=$ROS_DISTRO --os=ubuntu:jammy && \ + rosdep update && \ + apt-get update && \ + rosdep install --from-paths src -y -r --ignore-src --rosdistro=$ROS_DISTRO --os=ubuntu:jammy --skip-keys="point_cloud_transport_plugins draco_point_cloud_transport" || true && \ rm -rf /var/lib/apt/lists/* && \ apt-get clean RUN cd /usr/local/zed && \ - pip install requests && \ - python3 get_python_api.py + pip install requests + # python3 get_python_api.py \ + # pip3 install ultralytics \ + # pip3 install torch \ + # pip3 install pyopengl \ + # pip3 install pyzed RUN source /opt/ros/humble/setup.bash && \ colcon build --parallel-workers $(nproc) --symlink-install \ --event-handlers console_direct+ --base-paths src \ --cmake-args ' -DCMAKE_BUILD_TYPE=Release' \ ' -DCMAKE_LIBRARY_PATH=/usr/local/cuda/lib64/stubs' \ - ' -DCMAKE_CXX_FLAGS="-Wl,--allow-shlib-undefined"' + ' -DCMAKE_CXX_FLAGS="-Wl,--allow-shlib-undefined"' || true RUN echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc && \ echo "source /ws/install/setup.bash" >> ~/.bashrc diff --git a/.docker_templates/docker-compose.CPU.yml b/.docker_templates/docker-compose.CPU.yml index 3bb0d1d5..98ce104c 100644 --- a/.docker_templates/docker-compose.CPU.yml +++ b/.docker_templates/docker-compose.CPU.yml @@ -26,12 +26,10 @@ services: autonomous_zed: image: zed_sdk - privileged: true - build: - context: .. - dockerfile: .devcontainer/zed.Dockerfile + context: . + dockerfile: ./zed.Dockerfile environment: - DISPLAY diff --git a/.docker_templates/zed.Dev.Dockerfile b/.docker_templates/zed.Dev.Dockerfile index be85dd35..ea073700 100644 --- a/.docker_templates/zed.Dev.Dockerfile +++ b/.docker_templates/zed.Dev.Dockerfile @@ -1,4 +1,4 @@ -FROM stereolabs/zed:4.0-devel-cuda12.1-ubuntu22.04 +FROM stereolabs/zed:4.1-devel-cuda12.1-ubuntu22.04 LABEL Name=zed_sdk Version=0.0.1 SHELL [ "/bin/bash", "-c" ] @@ -68,4 +68,8 @@ RUN source /opt/ros/humble/setup.bash && \ RUN echo "source /ws/install/setup.bash" >> ~/.bashrc +RUN . ~/.bashrc + +COPY ./.devcontainer/SN31421864.conf /usr/local/zed/settings/SN31421864.conf + CMD ["bash"] \ No newline at end of file diff --git a/.docker_templates/zed.Jetson.Dockerfile b/.docker_templates/zed.Jetson.Dockerfile index 92aab028..edf724ee 100644 --- a/.docker_templates/zed.Jetson.Dockerfile +++ b/.docker_templates/zed.Jetson.Dockerfile @@ -1,4 +1,4 @@ -FROM stereolabs/zed:4.0-tools-devel-l4t-r35.4 +FROM stereolabs/zed:4.1-tools-devel-l4t-r35.4 LABEL Name=zed_sdk Version=0.0.1 SHELL [ "/bin/bash", "-c" ] @@ -48,32 +48,38 @@ RUN rosdep init && \ https://raw.githubusercontent.com/colcon/colcon-metadata-repository/master/index.yaml && \ colcon metadata update -COPY ./src/perception/ /ws/src/perception/ -COPY ./src/moa/moa_description /ws/src/moa/moa_description -COPY ./src/moa/moa_msgs /ws/src/moa/moa_msgs - # install ros2 packages -RUN cd /ws/src/ && \ +RUN mkdir src && \ + cd /ws/src/ && \ git clone --recursive https://github.com/stereolabs/zed-ros2-wrapper.git && \ cd .. && \ source /opt/ros/humble/setup.bash && \ - rosdep update && apt-get update && \ - rosdep install --from-paths src -y -r --ignore-src --rosdistro=$ROS_DISTRO --os=ubuntu:jammy && \ + rosdep update && \ + apt-get update && \ + rosdep install --from-paths src -y -r --ignore-src --rosdistro=$ROS_DISTRO --os=ubuntu:jammy --skip-keys="point_cloud_transport_plugins draco_point_cloud_transport" || true && \ rm -rf /var/lib/apt/lists/* && \ apt-get clean RUN cd /usr/local/zed && \ - pip install requests && \ - python3 get_python_api.py + pip install requests + # python3 get_python_api.py \ + # pip3 install ultralytics \ + # pip3 install torch \ + # pip3 install pyopengl \ + # pip3 install pyzed RUN source /opt/ros/humble/setup.bash && \ colcon build --parallel-workers $(nproc) --symlink-install \ --event-handlers console_direct+ --base-paths src \ --cmake-args ' -DCMAKE_BUILD_TYPE=Release' \ ' -DCMAKE_LIBRARY_PATH=/usr/local/cuda/lib64/stubs' \ - ' -DCMAKE_CXX_FLAGS="-Wl,--allow-shlib-undefined"' + ' -DCMAKE_CXX_FLAGS="-Wl,--allow-shlib-undefined"' || true RUN echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc && \ echo "source /ws/install/setup.bash" >> ~/.bashrc +RUN . ~/.bashrc + +COPY ./.devcontainer/SN31421864.conf /usr/local/zed/settings/SN31421864.conf + CMD ["bash"] \ No newline at end of file diff --git a/.gitignore b/.gitignore index 020a6218..7659e5f3 100644 --- a/.gitignore +++ b/.gitignore @@ -7,6 +7,19 @@ kaggle.json .devcontainer/zed.Dockerfile .devcontainer/ros2_ws.Dockerfile .vscode/settings.json -.gitignore .vscode/c_cpp_properties.json -**/__pycache__/ \ No newline at end of file +**/__pycache__/ +*.msg +*.srv +.venv/* +wandb/ +*.onnx +*.svo2 +*.pt +./scrum/planningDesign/shortHorzion/build +./scrum/planningDesign/shortHorzion/install +./scrum/planningDesign/shortHorzion/log +**/CMakeFiles* +**/cmake_build* +logging/ +bags/ \ No newline at end of file diff --git a/.gitmodules b/.gitmodules index b0d45b0c..3890a0d8 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,10 +1,12 @@ -[submodule "src/hardware_drivers/foxglove-bridge"] - path = src/hardware_drivers/foxglove-bridge - url = https://github.com/foxglove/ros-foxglove-bridge.git -[submodule "src/moa/cone-detection/cone_detection/yolov7"] - path = src/perception/cone-detection/cone_detection/yolov7 - url = https://github.com/WongKinYiu/yolov7.git [submodule "src/hardware_drivers/CanTalk"] path = src/hardware_drivers/CanTalk url = https://github.com/UOA-FSAE/CanTalk.git - branch = main + branch = nightly +[submodule "ROS2-protobuff-compiler"] + path = ROS2-protobuff-compiler + url = https://github.com/UOA-FSAE/ROS2-protobuff-compiler.git + branch = trunk +[submodule "Formula-Student-Driverless-Simulator"] + path = Formula-Student-Driverless-Simulator + url = https://github.com/FS-Driverless/Formula-Student-Driverless-Simulator.git + branch = master diff --git a/Formula-Student-Driverless-Simulator b/Formula-Student-Driverless-Simulator new file mode 160000 index 00000000..8915e051 --- /dev/null +++ b/Formula-Student-Driverless-Simulator @@ -0,0 +1 @@ +Subproject commit 8915e05183c8e8f3bf2ecaed38f24508b71504c8 diff --git a/README.md b/README.md index 53b0cbda..3c96e808 100644 --- a/README.md +++ b/README.md @@ -53,3 +53,24 @@ For detailed documentation about this project, please visit the **Wiki** tab on ### Ros2 Running commands: `ros2 ` + +# autonomous + +1. build a service (eg. jetson, autonomous_ros2) +``` +make build target=jetson +``` + +2. run a service +``` +make up target=jetson +``` + + + + + + + +ros2 bag +ros2 bag record -o bags/loop1 /moa/image_throttled /moa/cone_detection /moa/car_position /moa/car_velocity /moa/cmd_vel /moa/pub_raw_can \ No newline at end of file diff --git a/ROS2-protobuff-compiler b/ROS2-protobuff-compiler new file mode 160000 index 00000000..17eba53c --- /dev/null +++ b/ROS2-protobuff-compiler @@ -0,0 +1 @@ +Subproject commit 17eba53c9d405dca5c040659fa6c23cbc6c586a4 diff --git a/bag_data/rosbag2_2025_03_06-20_57_16/metadata.yaml b/bag_data/rosbag2_2025_03_06-20_57_16/metadata.yaml new file mode 100644 index 00000000..b13c0d04 --- /dev/null +++ b/bag_data/rosbag2_2025_03_06-20_57_16/metadata.yaml @@ -0,0 +1,21 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: sqlite3 + duration: + nanoseconds: 0 + starting_time: + nanoseconds_since_epoch: 9223372036854775807 + message_count: 0 + topics_with_message_count: + [] + compression_format: "" + compression_mode: "" + relative_file_paths: + - rosbag2_2025_03_06-20_57_16_0.db3 + files: + - path: rosbag2_2025_03_06-20_57_16_0.db3 + starting_time: + nanoseconds_since_epoch: 9223372036854775807 + duration: + nanoseconds: 0 + message_count: 0 \ No newline at end of file diff --git a/bag_data/rosbag2_2025_03_06-20_57_16/rosbag2_2025_03_06-20_57_16_0.db3 b/bag_data/rosbag2_2025_03_06-20_57_16/rosbag2_2025_03_06-20_57_16_0.db3 new file mode 100644 index 00000000..8d37ede6 Binary files /dev/null and b/bag_data/rosbag2_2025_03_06-20_57_16/rosbag2_2025_03_06-20_57_16_0.db3 differ diff --git a/bag_data/rosbag2_2025_03_06-21_00_01/metadata.yaml b/bag_data/rosbag2_2025_03_06-21_00_01/metadata.yaml new file mode 100644 index 00000000..eae7d682 --- /dev/null +++ b/bag_data/rosbag2_2025_03_06-21_00_01/metadata.yaml @@ -0,0 +1,21 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: sqlite3 + duration: + nanoseconds: 0 + starting_time: + nanoseconds_since_epoch: 9223372036854775807 + message_count: 0 + topics_with_message_count: + [] + compression_format: "" + compression_mode: "" + relative_file_paths: + - rosbag2_2025_03_06-21_00_01_0.db3 + files: + - path: rosbag2_2025_03_06-21_00_01_0.db3 + starting_time: + nanoseconds_since_epoch: 9223372036854775807 + duration: + nanoseconds: 0 + message_count: 0 \ No newline at end of file diff --git a/bag_data/rosbag2_2025_03_06-21_00_01/rosbag2_2025_03_06-21_00_01_0.db3 b/bag_data/rosbag2_2025_03_06-21_00_01/rosbag2_2025_03_06-21_00_01_0.db3 new file mode 100644 index 00000000..8d37ede6 Binary files /dev/null and b/bag_data/rosbag2_2025_03_06-21_00_01/rosbag2_2025_03_06-21_00_01_0.db3 differ diff --git a/src/moa/moa_bringup/launch/base.py b/bringup/base.py similarity index 96% rename from src/moa/moa_bringup/launch/base.py rename to bringup/base.py index 4311a7e7..9e637c9d 100644 --- a/src/moa/moa_bringup/launch/base.py +++ b/bringup/base.py @@ -1,5 +1,7 @@ import launch +from launch_ros.actions import Node import launch_ros.actions +from launch import LaunchDescription from launch.actions.declare_launch_argument import DeclareLaunchArgument from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource diff --git a/bringup/main_system_launch.py b/bringup/main_system_launch.py new file mode 100644 index 00000000..416353eb --- /dev/null +++ b/bringup/main_system_launch.py @@ -0,0 +1,85 @@ +import launch +import launch_ros.actions +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python import get_package_share_directory +import os + +def generate_launch_description(): + return launch.LaunchDescription([ + # Base launch files + DeclareLaunchArgument( + 'can_id', + default_value='0x300', + description='The frame ID for the CAN messages containing Ackermann commands that are sent to the car' + ), + + DeclareLaunchArgument( + 'candapter_topic', + default_value='pub_raw_can', + description='The subscriber and publisher topic for the Can Adapter node' + ), + + launch_ros.actions.Node( + package='moa_controllers', + executable='ack_to_can_node', + name='ack_to_can_node', + parameters=[{'can_id': launch.substitutions.LaunchConfiguration('can_id')}], + ), + + # # uncomment when CAN interface is completed + # launch_ros.actions.Node( + # package='moa_driver', + # executable='can_interface_jnano', + # name='can_interface_jnano'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('moa_description'), 'launch'), + '/urdf_model.py'])), + + launch_ros.actions.Node( + package='moa_controllers', + executable='as_status_node', + name='as_status_node', + ), + + + launch_ros.actions.Node( + package='CanTalk', + executable='candapter_node', + name='candapter_node', + remappings=[('can',launch.substitutions.LaunchConfiguration('candapter_topic'))], + ), + + # Major systems + # TODO: Tuning parameter should be tuned in launch file + + launch_ros.actions.Node( + package='aruco_detection', + executable='aruco_detection', + name='cone_detection_aruco', + ), + + launch_ros.actions.Node( + package='cone_mapping', + executable='dbscan', + name='cone_mapping', + ), + + launch_ros.actions.Node( + package='path_planning', + executable='center_line', + name='path_planning', + ), + + launch_ros.actions.Node( + package='head_to_goal_control', + executable='controller', + name='controller', + ), + + ]) + +# TO DO: add correct nodes to the launch file \ No newline at end of file diff --git a/bringup/motec_mock_controller_test_launch.py b/bringup/motec_mock_controller_test_launch.py new file mode 100644 index 00000000..55631ab3 --- /dev/null +++ b/bringup/motec_mock_controller_test_launch.py @@ -0,0 +1,49 @@ +from launch_ros.actions import Node +from launch import LaunchDescription +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +from ament_index_python import get_package_share_directory +import os + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'can_id', + default_value='0x300', + description='The frame ID for the CAN messages containing Ackermann commands that are sent to the car' + ), + + DeclareLaunchArgument( + 'candapter_topic', + default_value='pub_raw_can', + description='The subscriber and publisher topic for the Can Adapter node' + ), + + Node( + package='moa_controllers', + executable='ack_to_can_node', + name='ack_to_can_node', + parameters=[{'can_id': LaunchConfiguration('can_id')}], + ), + + # # uncomment when CAN interface is completed + # Node( + # package='moa_driver', + # executable='can_interface_jnano', + # name='can_interface_jnano'), + + Node( + package='CanTalk', + executable='candapter_node', + name='candapter_node', + remappings=[('can', LaunchConfiguration('candapter_topic'))], + ), + Node( + package='moa_controllers', + executable='mock_stimulus', + name='mock_stimulus', + ), + ]) diff --git a/bringup/visualization_launch.py b/bringup/visualization_launch.py new file mode 100644 index 00000000..b8c2a043 --- /dev/null +++ b/bringup/visualization_launch.py @@ -0,0 +1,38 @@ +import launch +import launch_ros.actions +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python import get_package_share_directory +import os + +def generate_launch_description(): + return launch.LaunchDescription([ + launch_ros.actions.Node( + package='foxglove_bridge', + executable='foxglove_bridge', + name='foxglove_bridge', + parameters=[{'port':8765}] + ), + + launch_ros.actions.Node( + package='cone_map_foxglove_visualizer', + executable='visualizer', + name='cone_map_visualizer', + ), + + launch_ros.actions.Node( + package='path_planning_visualization', + executable='visualize', + name='path_planning_visualizer', + ), + + launch_ros.actions.Node( + package='pure_pursuit_visualizer', + executable='visualizer', + name='controller_visualizer', + ), + + ]) + +# TO DO: Add the visualization nodes to the launch file \ No newline at end of file diff --git a/cone_detection.ipynb b/cone_detection.ipynb deleted file mode 100644 index 240023ea..00000000 --- a/cone_detection.ipynb +++ /dev/null @@ -1,157 +0,0 @@ -{ - "cells": [ - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - }, - { - "attachments": {}, - "cell_type": "markdown", - "metadata": { - "id": "AESQQWiNDCBq" - }, - "source": [ - "### Initialise\n", - "Import YOLOv7 and install requirements" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "s3zPjQTpBMeX" - }, - "outputs": [], - "source": [ - "! git clone https://github.com/WongKinYiu/yolov7.git # clone\n", - "% cd yolov7\n", - "! pip install --upgrade pip\n", - "! pip install -r requirements.txt # install modules" - ] - }, - { - "attachments": {}, - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Install Kaggle and move Kaggle json file to appropriate folder" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "! pip install -q kaggle" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "! mkdir ~/.kaggle\n", - "! cp kaggle.json ~/.kaggle/\n", - "! chmod 600 ~/.kaggle/kaggle.json" - ] - }, - { - "attachments": {}, - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Import and unzip cones dataset" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "! kaggle datasets download -d wsin536/fsae-cones\n", - "! mkdir cone-dataset\n", - "! unzip fsae-cones.zip -d cone-dataset" - ] - }, - { - "attachments": {}, - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## IMPORTANT STEP:\n", - "- Open 'data.yaml' file in 'cone-dataset'\n", - "- Update path of 'train', 'test' and 'valid' folders" - ] - }, - { - "attachments": {}, - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Download COCO starting checkpoint" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "% cd /content/yolov7\n", - "! wget https://github.com/WongKinYiu/yolov7/releases/download/v0.1/yolov7_training.pt" - ] - }, - { - "attachments": {}, - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Train data\n", - "\n", - "#### Double check data.yaml file path" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "%cd /content/yolov7\n", - "!python train.py --batch 16 --epochs 55 --data /cone-dataset/data.yaml --weights 'yolov7_training.pt' --device 0" - ] - } - ], - "metadata": { - "accelerator": "GPU", - "colab": { - "gpuType": "T4", - "provenance": [] - }, - "gpuClass": "standard", - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.10.6" - } - }, - "nbformat": 4, - "nbformat_minor": 0 -} diff --git a/src/perception/cone-detection/cone_detection/yolov7m.pt b/cone_detection_model.engine similarity index 67% rename from src/perception/cone-detection/cone_detection/yolov7m.pt rename to cone_detection_model.engine index f8263624..1fbe9c5d 100644 Binary files a/src/perception/cone-detection/cone_detection/yolov7m.pt and b/cone_detection_model.engine differ diff --git a/docker-compose.CPU.yml b/docker-compose.CPU.yml deleted file mode 100644 index 59108bf0..00000000 --- a/docker-compose.CPU.yml +++ /dev/null @@ -1,49 +0,0 @@ -version: '3.4' - -services: - autonomous_ros2: - image: autonomous - - build: - context: . - dockerfile: ./ros2_ws.Dockerfile - - environment: - - DISPLAY - - NVIDIA_VISIBLE_DEVICES=0 - - NVIDIA_DRIVER_CAPABILITIES=all - - - ROS_DOMAIN_ID=47 # Its 47 for obvious reasons - - volumes: - - ..:/ws - - network_mode: host - ipc: host - stdin_open: true - tty: true - - - autonomous_zed: - image: zed_sdk - - build: - context: . - dockerfile: ./zed.Dockerfile - - environment: - - DISPLAY - - NVIDIA_VISIBLE_DEVICES=0 - - NVIDIA_DRIVER_CAPABILITIES=all - - ROS_DOMAIN_ID=47 - - command: "ros2 launch zed_wrapper zed_camera.launch.py camera_model:='zed2i'" - - volumes: - - ../ros_entrypoint.sh:/ws/ros_entrypoint.sh - - - network_mode: host - ipc: host - stdin_open: true - tty: true diff --git a/docker-compose.GPU.yml b/docker-compose.GPU.yml deleted file mode 100644 index 9fa33b02..00000000 --- a/docker-compose.GPU.yml +++ /dev/null @@ -1,65 +0,0 @@ -version: '3.4' - -services: - autonomous_ros2: - image: autonomous - - build: - context: . - dockerfile: ./ros2_ws.Dockerfile - - environment: - - DISPLAY - - NVIDIA_VISIBLE_DEVICES=0 - - NVIDIA_DRIVER_CAPABILITIES=all - - - ROS_DOMAIN_ID=47 # Its 47 for obvious reasons - - volumes: - - ..:/ws - - network_mode: host - ipc: host - stdin_open: true - tty: true - - deploy: - resources: - reservations: - devices: - - driver: nvidia - device_ids: [] - capabilities: [gpu] - - - autonomous_zed: - image: zed_sdk - - build: - context: . - dockerfile: ./zed.Dockerfile - - environment: - - DISPLAY - - NVIDIA_VISIBLE_DEVICES=0 - - NVIDIA_DRIVER_CAPABILITIES=all - - ROS_DOMAIN_ID=47 - - command: "ros2 launch zed_wrapper zed_camera.launch.py camera_model:='zed2i'" - - volumes: - - ../ros_entrypoint.sh:/ws/ros_entrypoint.sh - - - network_mode: host - ipc: host - stdin_open: true - tty: true - - deploy: - resources: - reservations: - devices: - - driver: nvidia - device_ids: [] - capabilities: [gpu] diff --git a/mock_params.yaml b/mock_params.yaml new file mode 100644 index 00000000..86b5aaf7 --- /dev/null +++ b/mock_params.yaml @@ -0,0 +1,9 @@ +/tutorial/our_stimulus: + ros__parameters: + accel: 0.0 + angle: 10.0 + angular_vel: 0.0 + use_sim_time: false + vel: 11.0 + verbose: false + diff --git a/notes.txt b/notes.txt deleted file mode 100644 index 7f3852b6..00000000 --- a/notes.txt +++ /dev/null @@ -1,22 +0,0 @@ - -1. Follow tutorial: https://medium.com/augmented-startups/yolov7-training-on-custom-data-b86d23e6623\ -2. To transfer FSOCO JSON files to txt files: - - Create workspace on Roboflow - - Upload images and bounding boxes - - Export txt files for YOLOv7 -4. create and run venv -3. run python3 train.py --weights yolov7.pt --data "/data/cones_custom.yaml" --workers 4 --batch-size 4 --img 416 --cfg cfg/training/yolov7.yaml --name yolov7 --hyp data/hyp.scratch.p5.yaml --epochs 50 - - -Resources: -https://github.com/MarkDana/RealtimeConeDetection -This repo has Trained single-class YoloV3 model and a small dataset - -https://github.com/jhan15/traffic_cones_detection -This repo has trained multi-class YoloV5 model. - -https://www.kaggle.com/c/cone-detection-challenge-ECEN489 -The cone detection challenge dataset - -https://universe.roboflow.com/robotica-xftin/traffic-cones-4laxg/dataset/1/images -The cone dataset diff --git a/pyzed-4.0-cp310-cp310-linux_x86_64.whl b/pyzed-4.0-cp310-cp310-linux_x86_64.whl deleted file mode 100644 index 37ee4785..00000000 Binary files a/pyzed-4.0-cp310-cp310-linux_x86_64.whl and /dev/null differ diff --git a/requirements/install_dependencies.py b/requirements/install_dependencies.py new file mode 100755 index 00000000..0ad44820 --- /dev/null +++ b/requirements/install_dependencies.py @@ -0,0 +1,38 @@ +#!/home/tanish/venvs/fix_requirements_file/bin/python3 +from progress.bar import IncrementalBar +import sys +import subprocess + +def install(package): + result = subprocess.run([sys.executable, "-m", "pip", "install", "--progress-bar","on", package], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) + return result.returncode + +def main(): + # read in all packages + with open("requirements.txt", 'r') as file: + packages = [line.strip() for line in file] + + # progress bar + error_free_packages = [] + errored_packages = [] + pack_len = len(packages) + with IncrementalBar('Processing', max=pack_len) as bar: + for i,package in enumerate(packages): + exit_code = install(package) # install package + if exit_code != 0: + print(f"\npackage {package} failed at line {i+1}") + errored_packages.append(package) + else: + error_free_packages.append(package) + bar.next() + + # write new file + with open("requirements_error_free.txt", "w") as file: + for package in error_free_packages: + file.write(package+"\n") + with open("requirements_errored.txt", "w") as file: + for package in errored_packages: + file.write(package+"\n") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/requirements/requirements_error_free.txt b/requirements/requirements_error_free.txt new file mode 100644 index 00000000..69187585 --- /dev/null +++ b/requirements/requirements_error_free.txt @@ -0,0 +1,140 @@ +action-msgs==1.2.1 +actionlib-msgs==4.2.3 +ament-index-python==1.4.0 +ament-package==0.14.0 +angles==1.15.0 +appdirs==1.4.4 +argcomplete==1.8.1 +attrs==21.2.0 +bcrypt==3.2.0 +beautifulsoup4==4.10.0 +beniget==0.4.1 +blinker==1.4 +Brotli==1.0.9 +builtin-interfaces==1.2.1 +certifi==2020.6.20 +chardet==4.0.0 +click==8.0.3 +colorama==0.4.4 +composition-interfaces==1.2.1 +cryptography==3.4.8 +cv-bridge==3.2.1 +cycler==0.11.0 +decorator==4.4.2 +diagnostic-msgs==4.2.3 +distro==1.7.0 +docopt==0.6.2 +docutils==0.17.1 +domain-coordinator==0.10.0 +empy==3.3.4 +example-interfaces==0.9.3 +fasteners==0.14.1 +flake8==4.0.1 +fonttools==4.29.1 +fs==2.4.12 +future==0.18.2 +gast==0.5.2 +geometry-msgs==4.2.3 +html5lib==1.1 +httplib2==0.20.2 +idna==3.3 +importlib-metadata==4.6.4 +iniconfig==1.1.1 +interactive-markers==2.3.2 +jeepney==0.7.1 +keyring==23.5.0 +kiwisolver==1.3.2 +lark==1.1.1 +laser-geometry==2.4.0 +launchpadlib==1.10.16 +lazr.restfulclient==0.14.4 +lazr.uri==1.0.6 +lifecycle-msgs==1.2.1 +lockfile==0.12.2 +lxml==4.8.0 +macaroonbakery==1.3.1 +Mako==1.1.3 +map-msgs==2.1.0 +MarkupSafe==2.0.1 +matplotlib==3.5.1 +mccabe==0.6.1 +monotonic==1.6 +more-itertools==8.10.0 +mpi4py==3.1.3 +nav-msgs==4.2.3 +netifaces==0.11.0 +numpy==1.24.2 +oauthlib==3.2.0 +olefile==0.46 +osrf-pycommon==2.0.2 +packaging==21.3 +paramiko==2.9.3 +pexpect==4.8.0 +Pillow==9.0.1 +pipreqs==0.4.11 +pluggy==0.13.0 +ply==3.11 +protobuf==3.12.4 +psutil==5.9.0 +ptyprocess==0.7.0 +py==1.10.0 +pycodestyle==2.8.0 +pydocstyle==6.1.1 +pydot==1.4.2 +pyflakes==2.4.0 +Pygments==2.11.2 +PyJWT==2.3.0 +pymacaroons==0.13.0 +PyNaCl==1.5.0 +pyparsing==2.4.7 +PyQt5==5.15.6 +PyQt5-sip==12.9.1 +pyRFC3339==1.1 +pyserial==3.5 +pytest==6.2.5 +python-dateutil==2.8.1 +pythran==0.10.0 +pytz==2022.1 +pyxdg==0.27 +rcl-interfaces==1.2.1 +reportlab==3.6.8 +requests==2.25.1 +resource-retriever==3.1.1 +rmw-dds-common==1.6.0 +roman==3.3 +rosgraph-msgs==1.2.1 +rosidl-generator-py==0.14.4 +rosidl-runtime-py==0.9.3 +rpyutils==0.2.1 +scipy==1.8.0 +SecretStorage==3.3.1 +sensor-msgs==4.2.3 +sensor-msgs-py==4.2.3 +shape-msgs==4.2.3 +six==1.16.0 +snowballstemmer==2.2.0 +soupsieve==2.3.1 +spidev==3.1 +sros2==0.10.4 +statistics-msgs==1.2.1 +std-msgs==4.2.3 +std-srvs==4.2.3 +stereo-msgs==4.2.3 +sympy==1.9 +toml==0.10.2 +trajectory-msgs==4.2.3 +turtlesim==1.4.2 +typing_extensions==4.5.0 +ufoLib2==0.13.1 +unicodedata2==14.0.0 +unique-identifier-msgs==2.2.1 +urllib3==1.26.5 +visualization-msgs==4.2.3 +wadllib==1.3.6 +webencodings==0.5.1 +xdg==5 +yarg==0.1.9 +zipp==1.0.0 +numpy +opencv-python +opencv-contrib-python diff --git a/requirements.txt b/requirements/requirements_errored.txt similarity index 53% rename from requirements.txt rename to requirements/requirements_errored.txt index 1e17198a..bf7f174d 100644 --- a/requirements.txt +++ b/requirements/requirements_errored.txt @@ -1,55 +1,25 @@ -action-msgs==1.2.1 action-tutorials-interfaces==0.20.3 action-tutorials-py==0.20.3 -actionlib-msgs==4.2.3 ament-cmake-test==1.3.3 ament-copyright==0.12.5 ament-cppcheck==0.12.5 ament-cpplint==0.12.5 ament-flake8==0.12.5 -ament-index-python==1.4.0 ament-lint==0.12.5 ament-lint-cmake==0.12.5 -ament-package==0.14.0 ament-pep257==0.12.5 ament-uncrustify==0.12.5 ament-xmllint==0.12.5 -angles==1.15.0 -appdirs==1.4.4 apturl==0.5.2 -argcomplete==1.8.1 -attrs==21.2.0 -bcrypt==3.2.0 -beautifulsoup4==4.10.0 -beniget==0.4.1 -blinker==1.4 Brlapi==0.8.3 -Brotli==1.0.9 -builtin-interfaces==1.2.1 catkin-pkg-modules==0.5.2 -certifi==2020.6.20 -chardet==4.0.0 -click==8.0.3 -colorama==0.4.4 command-not-found==0.3 -composition-interfaces==1.2.1 -cryptography==3.4.8 cupshelpers==1.0 -cv-bridge==3.2.1 -cycler==0.11.0 dbus-python==1.2.18 -decorator==4.4.2 defer==1.0.6 demo-nodes-py==0.20.3 -diagnostic-msgs==4.2.3 -distro==1.7.0 distro-info===1.1build1 -docopt==0.6.2 -docutils==0.17.1 -domain-coordinator==0.10.0 duplicity==0.8.21 -empy==3.3.4 -example-interfaces==0.9.3 examples-rclpy-executors==0.15.1 examples-rclpy-minimal-action-client==0.15.1 examples-rclpy-minimal-action-server==0.15.1 @@ -57,109 +27,35 @@ examples-rclpy-minimal-client==0.15.1 examples-rclpy-minimal-publisher==0.15.1 examples-rclpy-minimal-service==0.15.1 examples-rclpy-minimal-subscriber==0.15.1 -fasteners==0.14.1 -flake8==4.0.1 -fonttools==4.29.1 -fs==2.4.12 -future==0.18.2 -gast==0.5.2 -geometry-msgs==4.2.3 -html5lib==1.1 -httplib2==0.20.2 -idna==3.3 image-geometry==3.2.1 -importlib-metadata==4.6.4 -iniconfig==1.1.1 -interactive-markers==2.3.2 -jeepney==0.7.1 -keyring==23.5.0 -kiwisolver==1.3.2 language-selector==0.1 -lark==1.1.1 -laser-geometry==2.4.0 launch==1.0.4 launch-ros==0.19.4 launch-testing==1.0.4 launch-testing-ros==0.19.4 launch-xml==1.0.4 launch-yaml==1.0.4 -launchpadlib==1.10.16 -lazr.restfulclient==0.14.4 -lazr.uri==1.0.6 -lifecycle-msgs==1.2.1 -lockfile==0.12.2 logging-demo==0.20.3 louis==3.20.0 -lxml==4.8.0 lz4==3.1.3+dfsg -macaroonbakery==1.3.1 -Mako==1.1.3 -map-msgs==2.1.0 -MarkupSafe==2.0.1 -matplotlib==3.5.1 -mccabe==0.6.1 message-filters==4.3.2 -monotonic==1.6 -more-itertools==8.10.0 -mpi4py==3.1.3 mpmath==0.0.0 -nav-msgs==4.2.3 -netifaces==0.11.0 -numpy==1.24.2 -oauthlib==3.2.0 -olefile==0.46 -osrf-pycommon==2.0.2 -packaging==21.3 -paramiko==2.9.3 pcl-msgs==1.0.0 pendulum-msgs==0.20.3 -pexpect==4.8.0 -Pillow==9.0.1 -pipreqs==0.4.11 -pluggy==0.13.0 -ply==3.11 -protobuf==3.12.4 -psutil==5.9.0 -ptyprocess==0.7.0 -py==1.10.0 pycairo==1.20.1 -pycodestyle==2.8.0 pycups==2.0.1 -pydocstyle==6.1.1 -pydot==1.4.2 -pyflakes==2.4.0 -Pygments==2.11.2 PyGObject==3.42.1 -PyJWT==2.3.0 -pymacaroons==0.13.0 -PyNaCl==1.5.0 -pyparsing==2.4.7 -PyQt5==5.15.6 -PyQt5-sip==12.9.1 -pyRFC3339==1.1 -pyserial==3.5 -pytest==6.2.5 python-apt==2.4.0+ubuntu1 -python-dateutil==2.8.1 python-debian===0.1.43ubuntu1 python-qt-binding==1.1.1 -pythran==0.10.0 -pytz==2022.1 -pyxdg==0.27 PyYAML==5.4.1 qt-dotgraph==2.2.2 qt-gui==2.2.2 qt-gui-cpp==2.2.2 qt-gui-py-common==2.2.2 quality-of-service-demo-py==0.20.3 -rcl-interfaces==1.2.1 rclpy==3.3.7 rcutils==5.1.2 -reportlab==3.6.8 -requests==2.25.1 -resource-retriever==3.1.1 -rmw-dds-common==1.6.0 -roman==3.3 ros2action==0.18.5 ros2bag==0.15.4 ros2cli==0.18.5 @@ -178,15 +74,12 @@ ros2topic==0.18.5 rosbag2-interfaces==0.15.4 rosbag2-py==0.15.4 rosdistro-modules==0.9.0 -rosgraph-msgs==1.2.1 rosidl-adapter==3.1.4 rosidl-cli==3.1.4 rosidl-cmake==3.1.4 rosidl-generator-c==3.1.4 rosidl-generator-cpp==3.1.4 -rosidl-generator-py==0.14.4 rosidl-parser==3.1.4 -rosidl-runtime-py==0.9.3 rosidl-typesupport-c==2.0.0 rosidl-typesupport-cpp==2.0.0 rosidl-typesupport-fastrtps-c==2.2.0 @@ -194,7 +87,6 @@ rosidl-typesupport-fastrtps-cpp==2.2.0 rosidl-typesupport-introspection-c==3.1.4 rosidl-typesupport-introspection-cpp==3.1.4 rospkg-modules==1.4.0 -rpyutils==0.2.1 rqt-action==2.0.1 rqt-bag==1.1.4 rqt-bag-plugins==1.1.4 @@ -212,22 +104,7 @@ rqt-service-caller==1.0.5 rqt-shell==1.0.2 rqt-srv==1.0.3 rqt-topic==1.5.0 -scipy==1.8.0 screen-resolution-extra==0.0.0 -SecretStorage==3.3.1 -sensor-msgs==4.2.3 -sensor-msgs-py==4.2.3 -shape-msgs==4.2.3 -six==1.16.0 -snowballstemmer==2.2.0 -soupsieve==2.3.1 -spidev==3.1 -sros2==0.10.4 -statistics-msgs==1.2.1 -std-msgs==4.2.3 -std-srvs==4.2.3 -stereo-msgs==4.2.3 -sympy==1.9 systemd-python==234 teleop-twist-keyboard==2.3.2 tf2-geometry-msgs==0.25.2 @@ -236,30 +113,13 @@ tf2-msgs==0.25.2 tf2-py==0.25.2 tf2-ros-py==0.25.2 tf2-tools==0.25.2 -toml==0.10.2 topic-monitor==0.20.3 torch==1.13.1+cu116 torchaudio==0.13.1+cu116 torchvision==0.14.1+cu116 -trajectory-msgs==4.2.3 -turtlesim==1.4.2 -typing_extensions==4.5.0 ubuntu-advantage-tools==8001 ubuntu-drivers-common==0.0.0 -ufoLib2==0.13.1 ufw==0.36.1 unattended-upgrades==0.1 -unicodedata2==14.0.0 -unique-identifier-msgs==2.2.1 -urllib3==1.26.5 usb-creator==0.3.7 -visualization-msgs==4.2.3 -wadllib==1.3.6 -webencodings==0.5.1 -xdg==5 xkit==0.0.0 -yarg==0.1.9 -zipp==1.0.0 -numpy -opencv-python -opencv-contrib-python diff --git a/rosbags/rosbag2_2024_04_03-22_35_15/metadata.yaml b/rosbags/rosbag2_2024_04_03-22_35_15/metadata.yaml new file mode 100644 index 00000000..e28936c5 --- /dev/null +++ b/rosbags/rosbag2_2024_04_03-22_35_15/metadata.yaml @@ -0,0 +1,32 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: sqlite3 + duration: + nanoseconds: 197049731678 + starting_time: + nanoseconds_since_epoch: 1712136916278537686 + message_count: 5786 + topics_with_message_count: + - topic_metadata: + name: /car_position + type: geometry_msgs/msg/Pose + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + message_count: 2938 + - topic_metadata: + name: /cone_detection + type: moa_msgs/msg/ConeMap + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + message_count: 2848 + compression_format: "" + compression_mode: "" + relative_file_paths: + - rosbag2_2024_04_03-22_35_15_0.db3 + files: + - path: rosbag2_2024_04_03-22_35_15_0.db3 + starting_time: + nanoseconds_since_epoch: 1712136916278537686 + duration: + nanoseconds: 197049731678 + message_count: 5786 \ No newline at end of file diff --git a/rosbags/rosbag2_2024_04_03-22_35_15/rosbag2_2024_04_03-22_35_15_0.db3 b/rosbags/rosbag2_2024_04_03-22_35_15/rosbag2_2024_04_03-22_35_15_0.db3 new file mode 100644 index 00000000..a3a045ec Binary files /dev/null and b/rosbags/rosbag2_2024_04_03-22_35_15/rosbag2_2024_04_03-22_35_15_0.db3 differ diff --git a/roszed_entrypoint.sh b/roszed_entrypoint.sh deleted file mode 100755 index 937df79f..00000000 --- a/roszed_entrypoint.sh +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/bash -set -e - -source /opt/ros/humble/setup.bash -source /ws/install/setup.bash - -exec "$@" diff --git a/run_CAN_talker.sh b/run_CAN_talker.sh deleted file mode 100755 index 94d5f9b7..00000000 --- a/run_CAN_talker.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash -source install/setup.bash -colcon build --packages-select CanTalk && ros2 run CanTalk candapter_node diff --git a/run_cone_map_visualizer.sh b/run_cone_map_visualizer.sh deleted file mode 100755 index 773cbef9..00000000 --- a/run_cone_map_visualizer.sh +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/bash -source install/setup.bash -ros2 run cone_map_foxglove_visualizer visualizer - diff --git a/run_controller.sh b/run_controller.sh deleted file mode 100755 index 044b3082..00000000 --- a/run_controller.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/bash -source install/setup.bash -colcon build --packages-select head_to_goal_control && ros2 run head_to_goal_control controller -#colcon build --packages-select stanley_controller && ros2 run stanley_controller controller - diff --git a/run_controller_visualizer.sh b/run_controller_visualizer.sh deleted file mode 100755 index c19eb1bb..00000000 --- a/run_controller_visualizer.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/bash -source install/setup.bash -ros2 run pure_pursuit_visualizer visualizer - - diff --git a/run_path_planning.sh b/run_path_planning.sh deleted file mode 100755 index cd1ff896..00000000 --- a/run_path_planning.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash -source install/setup.bash -colcon build --packages-select path_planning && ros2 run path_planning center_line diff --git a/run_path_planning_visualizer.sh b/run_path_planning_visualizer.sh deleted file mode 100755 index 0ea55c03..00000000 --- a/run_path_planning_visualizer.sh +++ /dev/null @@ -1,6 +0,0 @@ -#!/bin/bash -source install/setup.bash -ros2 run path_planning_visualization visualize - - - diff --git a/perception_init.sh b/scripts/perception_init.sh similarity index 100% rename from perception_init.sh rename to scripts/perception_init.sh diff --git a/scripts/run_CAN_talker.sh b/scripts/run_CAN_talker.sh new file mode 100755 index 00000000..4b1425b2 --- /dev/null +++ b/scripts/run_CAN_talker.sh @@ -0,0 +1,3 @@ +#!/bin/bash +source install/setup.bash +colcon build --packages-select CanTalk && ros2 run cantalk candapter_node diff --git a/run_ack_to_can.sh b/scripts/run_ack_to_can.sh similarity index 100% rename from run_ack_to_can.sh rename to scripts/run_ack_to_can.sh diff --git a/run_aruco_detect.sh b/scripts/run_aruco_detect.sh similarity index 100% rename from run_aruco_detect.sh rename to scripts/run_aruco_detect.sh diff --git a/run_base.sh b/scripts/run_base.sh similarity index 100% rename from run_base.sh rename to scripts/run_base.sh diff --git a/run_cone_detect.sh b/scripts/run_cone_detect.sh similarity index 100% rename from run_cone_detect.sh rename to scripts/run_cone_detect.sh diff --git a/run_cone_map.sh b/scripts/run_cone_map.sh similarity index 52% rename from run_cone_map.sh rename to scripts/run_cone_map.sh index c8d4acab..b8845d4a 100755 --- a/run_cone_map.sh +++ b/scripts/run_cone_map.sh @@ -1,3 +1,4 @@ #!/bin/bash source install/setup.bash -colcon build --packages-select cone_mapping && ros2 run cone_mapping listener +#colcon build --packages-select cone_mapping && ros2 run cone_mapping kf +colcon build --packages-select cone_mapping && ros2 run cone_mapping kalman_filter diff --git a/scripts/run_cone_map_visualiser.sh b/scripts/run_cone_map_visualiser.sh new file mode 100755 index 00000000..a4f72402 --- /dev/null +++ b/scripts/run_cone_map_visualiser.sh @@ -0,0 +1,4 @@ +#!/bin/bash +source install/setup.bash +colcon build --packages-select cone_map_foxglove_visualiser && ros2 run cone_map_foxglove_visualiser visualiser + diff --git a/scripts/run_controller.sh b/scripts/run_controller.sh new file mode 100755 index 00000000..b2ded2b4 --- /dev/null +++ b/scripts/run_controller.sh @@ -0,0 +1,5 @@ +#!/bin/bash +source install/setup.bash +#colcon build --packages-select head_to_goal_control && ros2 run head_to_goal_control controller +colcon build --packages-select stanley_controller && ros2 run stanley_controller controller + diff --git a/scripts/run_controller_visualiser.sh b/scripts/run_controller_visualiser.sh new file mode 100755 index 00000000..3273bf8c --- /dev/null +++ b/scripts/run_controller_visualiser.sh @@ -0,0 +1,5 @@ +#!/bin/bash +source install/setup.bash +colcon build --packages-select pure_pursuit_visualiser && ros2 run pure_pursuit_visualiser visualiser + + diff --git a/run_foxglove_ros.sh b/scripts/run_foxglove_ros.sh similarity index 100% rename from run_foxglove_ros.sh rename to scripts/run_foxglove_ros.sh diff --git a/scripts/run_kart_all.sh b/scripts/run_kart_all.sh new file mode 100644 index 00000000..4344a937 --- /dev/null +++ b/scripts/run_kart_all.sh @@ -0,0 +1,9 @@ +#!/bin/bash +./run_zed_launch.sh +./run_path_planning.sh +./run_controller.sh + +ros2 run CANTalk candapter_node + + + diff --git a/run_localization.sh b/scripts/run_localization.sh similarity index 100% rename from run_localization.sh rename to scripts/run_localization.sh diff --git a/scripts/run_path_planning.sh b/scripts/run_path_planning.sh new file mode 100755 index 00000000..27b16204 --- /dev/null +++ b/scripts/run_path_planning.sh @@ -0,0 +1,6 @@ +#!/bin/bash +source install/setup.bash +colcon build --packages-select path_planning +ros2 run path_planning fasttube +#ros2 run path_planning trajectory_generation + diff --git a/scripts/run_path_planning_visualiser.sh b/scripts/run_path_planning_visualiser.sh new file mode 100755 index 00000000..6c81c416 --- /dev/null +++ b/scripts/run_path_planning_visualiser.sh @@ -0,0 +1,6 @@ +#!/bin/bash +source install/setup.bash +colcon build --packages-select path_planning_visualiser && ros2 run path_planning_visualiser visualiser + + + diff --git a/run_simulation_car_control.sh b/scripts/run_simulation_car_control.sh similarity index 100% rename from run_simulation_car_control.sh rename to scripts/run_simulation_car_control.sh diff --git a/scripts/run_zed_launch.sh b/scripts/run_zed_launch.sh new file mode 100755 index 00000000..d928915c --- /dev/null +++ b/scripts/run_zed_launch.sh @@ -0,0 +1,9 @@ +#!/bin/bash +source install/setup.bash +colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node +## with sim +# colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node recording1.svo2 +# colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node recording2.svo2 + +ros2 run zed_launch zed_launch_node +ros2 launch foxglove_bridge foxglove_bridge_launch.xml \ No newline at end of file diff --git a/run_zed_wrapper.sh b/scripts/run_zed_wrapper.sh similarity index 100% rename from run_zed_wrapper.sh rename to scripts/run_zed_wrapper.sh diff --git a/scripts/sim_car.sh b/scripts/sim_car.sh new file mode 100755 index 00000000..bcf43dd6 --- /dev/null +++ b/scripts/sim_car.sh @@ -0,0 +1,3 @@ +#!/bin/bash +ros2 topic pub --once /race_controller/create std_msgs/String "data: test" +ros2 topic pub --once /test/cmd_throttle std_msgs/Float32 "data: 3.0" \ No newline at end of file diff --git a/scripts/z_recording_simulation.sh b/scripts/z_recording_simulation.sh new file mode 100755 index 00000000..c42923e8 --- /dev/null +++ b/scripts/z_recording_simulation.sh @@ -0,0 +1,12 @@ +#!/bin/bash + +gnome-terminal -- bash -c "source scripts/run_foxglove_ros.sh; exec bash" + +gnome-terminal -- bash -c "source scripts/run_cone_map.sh; exec bash" +gnome-terminal -- bash -c "source scripts/run_cone_map_visualiser.sh; exec bash" + +gnome-terminal -- bash -c "source scripts/run_path_planning_visualiser.sh; exec bash" +gnome-terminal -- bash -c "source scripts/run_path_planning.sh; exec bash" + +gnome-terminal -- bash -c "source scripts/run_zed_launch.sh; exec bash" + diff --git a/scripts/z_unity_simulation.sh b/scripts/z_unity_simulation.sh new file mode 100644 index 00000000..6c276e57 --- /dev/null +++ b/scripts/z_unity_simulation.sh @@ -0,0 +1,13 @@ +#!/bin/bash +ros2 topic pub --once /race_controller/create std_msgs/msg/String "data: 'test'" + +gnome-terminal -- bash -c "source scripts/run_cone_map.sh; exec bash" +gnome-terminal -- bash -c "source scripts/run_cone_map_visualizer.sh; exec bash" +gnome-terminal -- bash -c "source scripts/run_controller_visualizer.sh; exec bash" +gnome-terminal -- bash -c "source scripts/run_controller.sh; exec bash" +gnome-terminal -- bash -c "source scripts/run_foxglove_ros.sh; exec bash" +gnome-terminal -- bash -c "source scripts/run_localization.sh; exec bash" +gnome-terminal -- bash -c "source scripts/run_path_planning_visualizer.sh; exec bash" +gnome-terminal -- bash -c "source scripts/run_path_planning.sh; exec bash" +gnome-terminal -- bash -c "source scripts/run_simulation_car_control.sh; exec bash" + diff --git a/ros_entrypoint.sh b/scripts_entrypoint/ros_entrypoint.sh old mode 100755 new mode 100644 similarity index 100% rename from ros_entrypoint.sh rename to scripts_entrypoint/ros_entrypoint.sh diff --git a/scripts_interfaces/interfaces.proto b/scripts_interfaces/interfaces.proto new file mode 100644 index 00000000..34e1bf49 --- /dev/null +++ b/scripts_interfaces/interfaces.proto @@ -0,0 +1,89 @@ +syntax = "proto3"; + +message AllStates { + uint16 id = 1; + repeated ackermann_msgs.AckermannDrive states = 2; +} + +message AllTrajectories { + uint16 id = 1; + repeated geometry_msgs.PoseArray trajectories = 2; +} +message BoundaryStamped { + std_msgs.Header header = 1; + repeated float32 coords = 2; +} + +message CAN { + uint16 id = 1; + bool is_rtr = 2; + repeated uint8 data = 3; +} + +message CandidateState { + Ackermann ackermann = 1; + float32 lateralVel = 2; +} + +message CandidateStates { + repeated CandidateState candidateState = 1; +} + +message CANStamped { + std_msgs.Header header = 1; + CAN can = 2; +} + +message Detections { + geometry_msgs.Pose car_pose = 1; + repeated geometry_msgs.Point yellow = 2; + repeated geometry_msgs.Point blue = 3; + repeated geometry_msgs.Point small_orange = 4; + repeated geometry_msgs.Point big_orange = 5; +} + +message Track { + repeated geometry_msgs.Point cones = 1; +} + +message HardwareStates { + uint8 ebs_active = 1; + uint8 ts_active = 2; + uint8 in_gear = 3; + uint8 master_switch_on = 4; + uint8 asb_ready = 5; + uint8 brakes_engaged = 6; +} +message HardwareStatesStamped { + std_msgs.Header header = 1; + HardwareStates hardware_states = 2; +} +message MissionStates { + uint8 mission_selected = 1; + uint8 mission_finished = 2; +} +message MissionStatesStamped { + std_msgs.Header header = 1; + MissionStates mission_states = 2; +} + +message OccupancyGrid{ + std_msgs.Header header = 1; + repeated Rptuint8 occupancyGrid = 2; + float32 resolution = 3; +} + +message Pulse{ + bool value = 0; + string event_type = 1; +} + +service Services { + rpc CANSendReq (CANSendReqRequest) returns (CANSendReqResponce); +} +message CANSendReqRequest { + CAN can = 1; +} +message CANSendReqResponce { + bool sent =1; +} diff --git a/scripts_interfaces/interfaces_clean.bash b/scripts_interfaces/interfaces_clean.bash new file mode 100755 index 00000000..4b16a9ff --- /dev/null +++ b/scripts_interfaces/interfaces_clean.bash @@ -0,0 +1 @@ +python3 ROS2-protobuff-compiler/protobuf2rosmsg.py -m src/moa/moa_msgs/msg -s src/moa/moa_msgs/srv -c \ No newline at end of file diff --git a/scripts_interfaces/interfaces_clean.bat b/scripts_interfaces/interfaces_clean.bat new file mode 100644 index 00000000..53be5451 --- /dev/null +++ b/scripts_interfaces/interfaces_clean.bat @@ -0,0 +1 @@ +python %~dp0ROS2-protobuff-compiler\protobuf2rosmsg.py -m %~dp0src\moa\moa_msgs\msg -s %~dp0src\moa\moa_msgs\srv -c \ No newline at end of file diff --git a/scripts_interfaces/interfaces_run.bash b/scripts_interfaces/interfaces_run.bash new file mode 100755 index 00000000..bd0bf134 --- /dev/null +++ b/scripts_interfaces/interfaces_run.bash @@ -0,0 +1 @@ +python3 ROS2-protobuff-compiler/protobuf2rosmsg.py -f scripts_interfaces/interfaces.proto -m src/moa/moa_msgs/msg -s src/moa/moa_msgs/srv -c \ No newline at end of file diff --git a/scripts_interfaces/interfaces_run.bat b/scripts_interfaces/interfaces_run.bat new file mode 100644 index 00000000..960e0334 --- /dev/null +++ b/scripts_interfaces/interfaces_run.bat @@ -0,0 +1 @@ +python %~dp0ROS2-protobuff-compiler\protobuf2rosmsg.py -f %~dp0interfaces.proto -m %~dp0src\moa\moa_msgs\msg -s %~dp0src\moa\moa_msgs\srv -c \ No newline at end of file diff --git a/src/perception/aruco_detection/aruco_detection/__init__.py b/scrum/planningDesign/path_planning.py similarity index 100% rename from src/perception/aruco_detection/aruco_detection/__init__.py rename to scrum/planningDesign/path_planning.py diff --git a/scrum/planningDesign/shortHorzion/track_utils/CMakeLists.txt b/scrum/planningDesign/shortHorzion/track_utils/CMakeLists.txt new file mode 100644 index 00000000..8c60ef44 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.8) +project(track_utils) + +# find_package(ament_cmake REQUIRED) +# find_package(geometry_msgs REQUIRED) +find_package(CGAL REQUIRED) + +add_subdirectory(test) + +# Path where FindCorrade.cmake & FindMagnum.cmake can be found, adapt as needed +set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/modules/" ${CMAKE_MODULE_PATH}) +set(CMAKE_PREFIX_PATH "" ${CMAKE_PREFIX_PATH}) +set(CMAKE_BUILD_RPATH "${PROJECT_SOURCE_DIR}/cmake_build_scrum") + +file(GLOB SRC_FILES + src/*.cpp + EXCLUDE src/TriangulateCenterPoints.cpp +) + +file(GLOB INCLUDE_FILES + src/*.hpp +) + +# Add the library +add_library(track_utils_lib + ${SRC_FILES} +) +target_link_libraries(track_utils_lib + CGAL::CGAL + mpfr + gmp +) +target_include_directories(track_utils_lib PUBLIC INCLUDE_FILES) +# ament_target_dependencies(track_utils_lib geometry_msgs) + + + +# Add the test executable +add_executable(test_curvature_calculator + test/test_curvature_calculator.cpp +) +target_link_libraries(test_curvature_calculator track_utils_lib) +# ament_target_dependencies(test_curvature_calculator geometry_msgs) + +# Install library and executables +install(TARGETS + track_utils_lib + test_curvature_calculator + DESTINATION lib/${PROJECT_NAME} +) + +# ament_package() diff --git a/scrum/planningDesign/shortHorzion/track_utils/Makefile b/scrum/planningDesign/shortHorzion/track_utils/Makefile new file mode 100644 index 00000000..f4d3cfec --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/Makefile @@ -0,0 +1,447 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 3.22 + +# Default target executed when no arguments are given to make. +default_target: all +.PHONY : default_target + +# Allow only one "make -f Makefile2" at a time, but pass parallelism. +.NOTPARALLEL: + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Disable VCS-based implicit rules. +% : %,v + +# Disable VCS-based implicit rules. +% : RCS/% + +# Disable VCS-based implicit rules. +% : RCS/%,v + +# Disable VCS-based implicit rules. +% : SCCS/s.% + +# Disable VCS-based implicit rules. +% : s.% + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Command-line flag to silence nested $(MAKE). +$(VERBOSE)MAKESILENT = -s + +#Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E rm -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils + +#============================================================================= +# Targets provided globally by CMake. + +# Special rule for the target edit_cache +edit_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "No interactive CMake dialog available..." + /usr/bin/cmake -E echo No\ interactive\ CMake\ dialog\ available. +.PHONY : edit_cache + +# Special rule for the target edit_cache +edit_cache/fast: edit_cache +.PHONY : edit_cache/fast + +# Special rule for the target rebuild_cache +rebuild_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." + /usr/bin/cmake --regenerate-during-build -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) +.PHONY : rebuild_cache + +# Special rule for the target rebuild_cache +rebuild_cache/fast: rebuild_cache +.PHONY : rebuild_cache/fast + +# Special rule for the target list_install_components +list_install_components: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\"" +.PHONY : list_install_components + +# Special rule for the target list_install_components +list_install_components/fast: list_install_components +.PHONY : list_install_components/fast + +# Special rule for the target install +install: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." + /usr/bin/cmake -P cmake_install.cmake +.PHONY : install + +# Special rule for the target install +install/fast: preinstall/fast + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." + /usr/bin/cmake -P cmake_install.cmake +.PHONY : install/fast + +# Special rule for the target install/local +install/local: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." + /usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake +.PHONY : install/local + +# Special rule for the target install/local +install/local/fast: preinstall/fast + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." + /usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake +.PHONY : install/local/fast + +# Special rule for the target install/strip +install/strip: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." + /usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake +.PHONY : install/strip + +# Special rule for the target install/strip +install/strip/fast: preinstall/fast + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." + /usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake +.PHONY : install/strip/fast + +# The main all target +all: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/CMakeFiles /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils//CMakeFiles/progress.marks + $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 all + $(CMAKE_COMMAND) -E cmake_progress_start /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/CMakeFiles 0 +.PHONY : all + +# The main clean target +clean: + $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 clean +.PHONY : clean + +# The main clean target +clean/fast: clean +.PHONY : clean/fast + +# Prepare targets for installation. +preinstall: all + $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 preinstall +.PHONY : preinstall + +# Prepare targets for installation. +preinstall/fast: + $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 preinstall +.PHONY : preinstall/fast + +# clear depends +depend: + $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 +.PHONY : depend + +#============================================================================= +# Target rules for targets named track_utils_lib + +# Build rule for target. +track_utils_lib: cmake_check_build_system + $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 track_utils_lib +.PHONY : track_utils_lib + +# fast build rule for target. +track_utils_lib/fast: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/build +.PHONY : track_utils_lib/fast + +#============================================================================= +# Target rules for targets named test_curvature_calculator + +# Build rule for target. +test_curvature_calculator: cmake_check_build_system + $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 test_curvature_calculator +.PHONY : test_curvature_calculator + +# fast build rule for target. +test_curvature_calculator/fast: + $(MAKE) $(MAKESILENT) -f CMakeFiles/test_curvature_calculator.dir/build.make CMakeFiles/test_curvature_calculator.dir/build +.PHONY : test_curvature_calculator/fast + +#============================================================================= +# Target rules for targets named planning_unit_tests + +# Build rule for target. +planning_unit_tests: cmake_check_build_system + $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 planning_unit_tests +.PHONY : planning_unit_tests + +# fast build rule for target. +planning_unit_tests/fast: + $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/build +.PHONY : planning_unit_tests/fast + +src/TriangulateCenterPoints.o: src/TriangulateCenterPoints.cpp.o +.PHONY : src/TriangulateCenterPoints.o + +# target to build an object file +src/TriangulateCenterPoints.cpp.o: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/TriangulateCenterPoints.cpp.o +.PHONY : src/TriangulateCenterPoints.cpp.o + +src/TriangulateCenterPoints.i: src/TriangulateCenterPoints.cpp.i +.PHONY : src/TriangulateCenterPoints.i + +# target to preprocess a source file +src/TriangulateCenterPoints.cpp.i: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/TriangulateCenterPoints.cpp.i +.PHONY : src/TriangulateCenterPoints.cpp.i + +src/TriangulateCenterPoints.s: src/TriangulateCenterPoints.cpp.s +.PHONY : src/TriangulateCenterPoints.s + +# target to generate assembly for a file +src/TriangulateCenterPoints.cpp.s: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/TriangulateCenterPoints.cpp.s +.PHONY : src/TriangulateCenterPoints.cpp.s + +src/cone.o: src/cone.cpp.o +.PHONY : src/cone.o + +# target to build an object file +src/cone.cpp.o: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/cone.cpp.o +.PHONY : src/cone.cpp.o + +src/cone.i: src/cone.cpp.i +.PHONY : src/cone.i + +# target to preprocess a source file +src/cone.cpp.i: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/cone.cpp.i +.PHONY : src/cone.cpp.i + +src/cone.s: src/cone.cpp.s +.PHONY : src/cone.s + +# target to generate assembly for a file +src/cone.cpp.s: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/cone.cpp.s +.PHONY : src/cone.cpp.s + +src/corner.o: src/corner.cpp.o +.PHONY : src/corner.o + +# target to build an object file +src/corner.cpp.o: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/corner.cpp.o +.PHONY : src/corner.cpp.o + +src/corner.i: src/corner.cpp.i +.PHONY : src/corner.i + +# target to preprocess a source file +src/corner.cpp.i: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/corner.cpp.i +.PHONY : src/corner.cpp.i + +src/corner.s: src/corner.cpp.s +.PHONY : src/corner.s + +# target to generate assembly for a file +src/corner.cpp.s: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/corner.cpp.s +.PHONY : src/corner.cpp.s + +src/curvature_calculator.o: src/curvature_calculator.cpp.o +.PHONY : src/curvature_calculator.o + +# target to build an object file +src/curvature_calculator.cpp.o: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/curvature_calculator.cpp.o +.PHONY : src/curvature_calculator.cpp.o + +src/curvature_calculator.i: src/curvature_calculator.cpp.i +.PHONY : src/curvature_calculator.i + +# target to preprocess a source file +src/curvature_calculator.cpp.i: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/curvature_calculator.cpp.i +.PHONY : src/curvature_calculator.cpp.i + +src/curvature_calculator.s: src/curvature_calculator.cpp.s +.PHONY : src/curvature_calculator.s + +# target to generate assembly for a file +src/curvature_calculator.cpp.s: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/curvature_calculator.cpp.s +.PHONY : src/curvature_calculator.cpp.s + +src/track.o: src/track.cpp.o +.PHONY : src/track.o + +# target to build an object file +src/track.cpp.o: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/track.cpp.o +.PHONY : src/track.cpp.o + +src/track.i: src/track.cpp.i +.PHONY : src/track.i + +# target to preprocess a source file +src/track.cpp.i: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/track.cpp.i +.PHONY : src/track.cpp.i + +src/track.s: src/track.cpp.s +.PHONY : src/track.s + +# target to generate assembly for a file +src/track.cpp.s: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/track.cpp.s +.PHONY : src/track.cpp.s + +src/tutorial.o: src/tutorial.cpp.o +.PHONY : src/tutorial.o + +# target to build an object file +src/tutorial.cpp.o: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/tutorial.cpp.o +.PHONY : src/tutorial.cpp.o + +src/tutorial.i: src/tutorial.cpp.i +.PHONY : src/tutorial.i + +# target to preprocess a source file +src/tutorial.cpp.i: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/tutorial.cpp.i +.PHONY : src/tutorial.cpp.i + +src/tutorial.s: src/tutorial.cpp.s +.PHONY : src/tutorial.s + +# target to generate assembly for a file +src/tutorial.cpp.s: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/tutorial.cpp.s +.PHONY : src/tutorial.cpp.s + +src/util.o: src/util.cpp.o +.PHONY : src/util.o + +# target to build an object file +src/util.cpp.o: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/util.cpp.o +.PHONY : src/util.cpp.o + +src/util.i: src/util.cpp.i +.PHONY : src/util.i + +# target to preprocess a source file +src/util.cpp.i: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/util.cpp.i +.PHONY : src/util.cpp.i + +src/util.s: src/util.cpp.s +.PHONY : src/util.s + +# target to generate assembly for a file +src/util.cpp.s: + $(MAKE) $(MAKESILENT) -f CMakeFiles/track_utils_lib.dir/build.make CMakeFiles/track_utils_lib.dir/src/util.cpp.s +.PHONY : src/util.cpp.s + +test/test_curvature_calculator.o: test/test_curvature_calculator.cpp.o +.PHONY : test/test_curvature_calculator.o + +# target to build an object file +test/test_curvature_calculator.cpp.o: + $(MAKE) $(MAKESILENT) -f CMakeFiles/test_curvature_calculator.dir/build.make CMakeFiles/test_curvature_calculator.dir/test/test_curvature_calculator.cpp.o +.PHONY : test/test_curvature_calculator.cpp.o + +test/test_curvature_calculator.i: test/test_curvature_calculator.cpp.i +.PHONY : test/test_curvature_calculator.i + +# target to preprocess a source file +test/test_curvature_calculator.cpp.i: + $(MAKE) $(MAKESILENT) -f CMakeFiles/test_curvature_calculator.dir/build.make CMakeFiles/test_curvature_calculator.dir/test/test_curvature_calculator.cpp.i +.PHONY : test/test_curvature_calculator.cpp.i + +test/test_curvature_calculator.s: test/test_curvature_calculator.cpp.s +.PHONY : test/test_curvature_calculator.s + +# target to generate assembly for a file +test/test_curvature_calculator.cpp.s: + $(MAKE) $(MAKESILENT) -f CMakeFiles/test_curvature_calculator.dir/build.make CMakeFiles/test_curvature_calculator.dir/test/test_curvature_calculator.cpp.s +.PHONY : test/test_curvature_calculator.cpp.s + +# Help Target +help: + @echo "The following are some of the valid targets for this Makefile:" + @echo "... all (the default if no target is provided)" + @echo "... clean" + @echo "... depend" + @echo "... edit_cache" + @echo "... install" + @echo "... install/local" + @echo "... install/strip" + @echo "... list_install_components" + @echo "... rebuild_cache" + @echo "... planning_unit_tests" + @echo "... test_curvature_calculator" + @echo "... track_utils_lib" + @echo "... src/TriangulateCenterPoints.o" + @echo "... src/TriangulateCenterPoints.i" + @echo "... src/TriangulateCenterPoints.s" + @echo "... src/cone.o" + @echo "... src/cone.i" + @echo "... src/cone.s" + @echo "... src/corner.o" + @echo "... src/corner.i" + @echo "... src/corner.s" + @echo "... src/curvature_calculator.o" + @echo "... src/curvature_calculator.i" + @echo "... src/curvature_calculator.s" + @echo "... src/track.o" + @echo "... src/track.i" + @echo "... src/track.s" + @echo "... src/tutorial.o" + @echo "... src/tutorial.i" + @echo "... src/tutorial.s" + @echo "... src/util.o" + @echo "... src/util.i" + @echo "... src/util.s" + @echo "... test/test_curvature_calculator.o" + @echo "... test/test_curvature_calculator.i" + @echo "... test/test_curvature_calculator.s" +.PHONY : help + + + +#============================================================================= +# Special targets to cleanup operation of make. + +# Special rule to run CMake to check the build system integrity. +# No rule that depends on this can have commands that come from listfiles +# because they might be regenerated. +cmake_check_build_system: + $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 +.PHONY : cmake_check_build_system + diff --git a/scrum/planningDesign/shortHorzion/track_utils/cmake_install.cmake b/scrum/planningDesign/shortHorzion/track_utils/cmake_install.cmake new file mode 100644 index 00000000..8e8ee0b3 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/cmake_install.cmake @@ -0,0 +1,84 @@ +# Install script for directory: /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils + +# Set the install prefix +if(NOT DEFINED CMAKE_INSTALL_PREFIX) + set(CMAKE_INSTALL_PREFIX "/usr/local") +endif() +string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") + +# Set the install configuration name. +if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) + if(BUILD_TYPE) + string(REGEX REPLACE "^[^A-Za-z0-9_]+" "" + CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") + else() + set(CMAKE_INSTALL_CONFIG_NAME "") + endif() + message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") +endif() + +# Set the component getting installed. +if(NOT CMAKE_INSTALL_COMPONENT) + if(COMPONENT) + message(STATUS "Install component: \"${COMPONENT}\"") + set(CMAKE_INSTALL_COMPONENT "${COMPONENT}") + else() + set(CMAKE_INSTALL_COMPONENT) + endif() +endif() + +# Install shared libraries without execute permission? +if(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) + set(CMAKE_INSTALL_SO_NO_EXE "1") +endif() + +# Is this installation the result of a crosscompile? +if(NOT DEFINED CMAKE_CROSSCOMPILING) + set(CMAKE_CROSSCOMPILING "FALSE") +endif() + +# Set default install directory permissions. +if(NOT DEFINED CMAKE_OBJDUMP) + set(CMAKE_OBJDUMP "/usr/bin/objdump") +endif() + +if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT) + file(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/lib/track_utils" TYPE STATIC_LIBRARY FILES "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/libtrack_utils_lib.a") +endif() + +if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT) + if(EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/track_utils/test_curvature_calculator" AND + NOT IS_SYMLINK "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/track_utils/test_curvature_calculator") + file(RPATH_CHECK + FILE "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/track_utils/test_curvature_calculator" + RPATH "") + endif() + file(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/lib/track_utils" TYPE EXECUTABLE FILES "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test_curvature_calculator") + if(EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/track_utils/test_curvature_calculator" AND + NOT IS_SYMLINK "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/track_utils/test_curvature_calculator") + file(RPATH_CHANGE + FILE "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/track_utils/test_curvature_calculator" + OLD_RPATH "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/cmake_build_scrum:" + NEW_RPATH "") + if(CMAKE_INSTALL_DO_STRIP) + execute_process(COMMAND "/usr/bin/strip" "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/lib/track_utils/test_curvature_calculator") + endif() + endif() +endif() + +if(NOT CMAKE_INSTALL_LOCAL_ONLY) + # Include the install script for each subdirectory. + include("/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test/cmake_install.cmake") + +endif() + +if(CMAKE_INSTALL_COMPONENT) + set(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt") +else() + set(CMAKE_INSTALL_MANIFEST "install_manifest.txt") +endif() + +string(REPLACE ";" "\n" CMAKE_INSTALL_MANIFEST_CONTENT + "${CMAKE_INSTALL_MANIFEST_FILES}") +file(WRITE "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/${CMAKE_INSTALL_MANIFEST}" + "${CMAKE_INSTALL_MANIFEST_CONTENT}") diff --git a/scrum/planningDesign/shortHorzion/track_utils/include/track_utils/curvature_calculator.hpp b/scrum/planningDesign/shortHorzion/track_utils/include/track_utils/curvature_calculator.hpp new file mode 100644 index 00000000..e1240a4b --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/include/track_utils/curvature_calculator.hpp @@ -0,0 +1,29 @@ +#ifndef TRACK_UTILS_CURVATURE_CALCULATOR_HPP_ +#define TRACK_UTILS_CURVATURE_CALCULATOR_HPP_ + +#include "geometry_msgs/msg/point.hpp" +#include +#include +#include + +namespace track_utils { + +constexpr double CURVATURE_THRESHOLD = 0.4; + +std::optional calculateCurvature(const geometry_msgs::msg::Point& p1, + const geometry_msgs::msg::Point& p2, + const geometry_msgs::msg::Point& p3); + + +std::optional calculateBearingDifference(const geometry_msgs::msg::Point& left_cone, + const geometry_msgs::msg::Point& right_cone, + const geometry_msgs::msg::Point& current_position, + const geometry_msgs::msg::Point& previous_position); + +std::optional detectCornerStart( + const std::vector& upcoming_points); + +double calculateArcLength(double time, double speed); + +}// namespace track_utils +#endif // TRACK_UTILS_CURVATURE_CALCULATOR_HPP_ diff --git a/scrum/planningDesign/shortHorzion/track_utils/modules/FindMagnum.cmake b/scrum/planningDesign/shortHorzion/track_utils/modules/FindMagnum.cmake new file mode 100644 index 00000000..4160b72f --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/modules/FindMagnum.cmake @@ -0,0 +1,1417 @@ +#.rst: +# Find Magnum +# ----------- +# +# Finds the Magnum library. Basic usage:: +# +# find_package(Magnum REQUIRED) +# +# This module tries to find the base Magnum library and then defines the +# following: +# +# Magnum_FOUND - Whether the base library was found +# MAGNUM_DEPLOY_PREFIX - Prefix where to put final application +# executables, defaults to ``.``. If a relative path is used, it's relative +# to :variable:`CMAKE_INSTALL_PREFIX`. +# MAGNUM_PLUGINS_DEBUG_DIR - Base directory with dynamic plugins for +# debug builds, defaults to magnum-d/ subdirectory of dir where Magnum +# library was found +# MAGNUM_PLUGINS_RELEASE_DIR - Base directory with dynamic plugins for +# release builds, defaults to magnum/ subdirectory of dir where Magnum +# library was found +# MAGNUM_PLUGINS_DIR - Base directory with dynamic plugins, defaults +# to :variable:`MAGNUM_PLUGINS_RELEASE_DIR` in release builds and +# multi-configuration builds or to :variable:`MAGNUM_PLUGINS_DEBUG_DIR` in +# debug builds +# MAGNUM_PLUGINS_FONT[|_DEBUG|_RELEASE]_DIR - Directory with dynamic font +# plugins +# MAGNUM_PLUGINS_FONTCONVERTER[|_DEBUG|_RELEASE]_DIR - Directory with dynamic +# font converter plugins +# MAGNUM_PLUGINS_IMAGECONVERTER[|_DEBUG|_RELEASE]_DIR - Directory with dynamic +# image converter plugins +# MAGNUM_PLUGINS_SCENECONVERTER[|_DEBUG|_RELEASE]_DIR - Directory with dynamic +# scene converter plugins +# MAGNUM_PLUGINS_IMPORTER[|_DEBUG|_RELEASE]_DIR - Directory with dynamic +# importer plugins +# MAGNUM_PLUGINS_AUDIOIMPORTER[|_DEBUG|_RELEASE]_DIR - Directory with dynamic +# audio importer plugins +# +# If Magnum is built for Emscripten, the following variables contain paths to +# various support files: +# +# MAGNUM_EMSCRIPTENAPPLICATION_JS - Path to the EmscriptenApplication.js file +# MAGNUM_WINDOWLESSEMSCRIPTENAPPLICATION_JS - Path to the +# WindowlessEmscriptenApplication.js file +# MAGNUM_WEBAPPLICATION_CSS - Path to the WebApplication.css file +# +# This command will try to find only the base library, not the optional +# components. The base library depends on Corrade and OpenGL libraries (or +# OpenGL ES libraries). Additional dependencies are specified by the +# components. The optional components are: +# +# AnyAudioImporter - Any audio importer +# AnyImageConverter - Any image converter +# AnyImageImporter - Any image importer +# AnySceneConverter - Any scene converter +# AnySceneImporter - Any scene importer +# Audio - Audio library +# DebugTools - DebugTools library +# GL - GL library +# MaterialTools - MaterialTools library +# MeshTools - MeshTools library +# Primitives - Primitives library +# SceneGraph - SceneGraph library +# SceneTools - SceneTools library +# Shaders - Shaders library +# ShaderTools - ShaderTools library +# Text - Text library +# TextureTools - TextureTools library +# Trade - Trade library +# Vk - Vk library +# AndroidApplication - Android application +# EmscriptenApplication - Emscripten application +# GlfwApplication - GLFW application +# GlxApplication - GLX application +# Sdl2Application - SDL2 application +# XEglApplication - X/EGL application +# WindowlessCglApplication - Windowless CGL application +# WindowlessEglApplication - Windowless EGL application +# WindowlessGlxApplication - Windowless GLX application +# WindowlessIosApplication - Windowless iOS application +# WindowlessWglApplication - Windowless WGL application +# CglContext - CGL context +# EglContext - EGL context +# GlxContext - GLX context +# WglContext - WGL context +# OpenGLTester - OpenGLTester class +# VulkanTester - VulkanTester class +# MagnumFont - Magnum bitmap font plugin +# MagnumFontConverter - Magnum bitmap font converter plugin +# ObjImporter - OBJ importer plugin +# TgaImageConverter - TGA image converter plugin +# TgaImporter - TGA importer plugin +# WavAudioImporter - WAV audio importer plugin +# distancefieldconverter - magnum-distancefieldconverter executable +# fontconverter - magnum-fontconverter executable +# imageconverter - magnum-imageconverter executable +# sceneconverterter - magnum-sceneconverter executable +# shaderconverterter - magnum-shaderconverter executable +# gl-info - magnum-gl-info executable +# vk-info - magnum-vk-info executable +# al-info - magnum-al-info executable +# +# Example usage with specifying additional components is:: +# +# find_package(Magnum REQUIRED Trade MeshTools Primitives GlfwApplication) +# +# For each component is then defined: +# +# Magnum_*_FOUND - Whether the component was found +# Magnum::* - Component imported target +# +# If exactly one ``*Application`` or exactly one ``Windowless*Application`` +# component is requested and found, its target is available in convenience +# alias ``Magnum::Application`` / ``Magnum::WindowlessApplication`` to simplify +# porting. Similarly, if exactly one ``*Context`` component is requested and +# found, its target is available in convenience alias ``Magnum::GLContext``. +# +# The package is found if either debug or release version of each requested +# library (or plugin) is found. If both debug and release libraries (or +# plugins) are found, proper version is chosen based on actual build +# configuration of the project (i.e. Debug build is linked to debug libraries, +# Release build to release libraries). Note that this autodetection might fail +# for the :variable:`MAGNUM_PLUGINS_DIR` variable, especially on +# multi-configuration build systems. You can make use of +# ``CORRADE_IS_DEBUG_BUILD`` preprocessor variable along with +# ``MAGNUM_PLUGINS_*_DEBUG_DIR`` / ``MAGNUM_PLUGINS_*_RELEASE_DIR`` variables +# to decide in preprocessing step. +# +# Features of found Magnum library are exposed in these variables: +# +# MAGNUM_BUILD_DEPRECATED - Defined if compiled with deprecated features +# included +# MAGNUM_BUILD_STATIC - Defined if compiled as static libraries +# MAGNUM_BUILD_STATIC_UNIQUE_GLOBALS - Defined if static libraries keep the +# globals unique even across different shared libraries +# MAGNUM_TARGET_GL - Defined if compiled with OpenGL interop +# MAGNUM_TARGET_GLES - Defined if compiled for OpenGL ES +# MAGNUM_TARGET_WEBGL - Defined if compiled for WebGL +# MAGNUM_TARGET_GLES2 - Defined if compiled for OpenGL ES 2.0 / WebGL +# 1 instead of OpenGL ES 3.0+ / WebGL 2 +# MAGNUM_TARGET_EGL - Defined if compiled for EGL instead of a +# platform-specific OpenGL support library like CGL, EAGL, GLX or WGL +# MAGNUM_TARGET_VK - Defined if compiled with Vulkan interop +# +# The following variables are provided for backwards compatibility purposes +# only when MAGNUM_BUILD_DEPRECATED is enabled and will be removed in a future +# release: +# +# MAGNUM_BUILD_MULTITHREADED - Alias to CORRADE_BUILD_MULTITHREADED. Use +# CORRADE_BUILD_MULTITHREADED instead. +# MAGNUM_TARGET_HEADLESS - Alias to MAGNUM_TARGET_EGL, unless on iOS, +# Android, Emscripten or Windows RT. Use MAGNUM_TARGET_EGL instead. +# MAGNUM_TARGET_DESKTOP_GLES` - Defined if compiled for OpenGL ES but +# GLX / WGL is used instead of EGL. Use MAGNUM_TARGET_EGL instead. +# MAGNUM_TARGET_GLES3 - Defined if compiled for OpenGL ES 3.0+ / +# WebGL 2. Use an inverse of the MAGNUM_TARGET_GLES2 variable instead. +# +# Additionally these variables are defined for internal usage: +# +# MAGNUM_INCLUDE_DIR - Root include dir (w/o dependencies) +# MAGNUM_LIBRARY - Magnum library (w/o dependencies) +# MAGNUM_LIBRARY_DEBUG - Debug version of Magnum library, if found +# MAGNUM_LIBRARY_RELEASE - Release version of Magnum library, if found +# MAGNUM_*_LIBRARY - Component libraries (w/o dependencies) +# MAGNUM_*_LIBRARY_DEBUG - Debug version of given library, if found +# MAGNUM_*_LIBRARY_RELEASE - Release version of given library, if found +# MAGNUM_PLATFORM_JS - Path to MagnumPlatform.js file +# MAGNUM_BINARY_INSTALL_DIR - Binary installation directory +# MAGNUM_LIBRARY_INSTALL_DIR - Library installation directory +# MAGNUM_DATA_INSTALL_DIR - Data installation directory +# MAGNUM_PLUGINS_[DEBUG|RELEASE]_BINARY_INSTALL_DIR - Plugin binary +# installation directory +# MAGNUM_PLUGINS_[DEBUG|RELEASE]_LIBRARY_INSTALL_DIR - Plugin library +# installation directory +# MAGNUM_PLUGINS_SHADERCONVERTER_[DEBUG|RELEASE]_BINARY_INSTALL_DIR - Shader +# converter plugin binary installation directory +# MAGNUM_PLUGINS_SHADERCONVERTER_[DEBUG|RELEASE]_LIBRARY_INSTALL_DIR - Shader +# converter plugin library installation directory +# MAGNUM_PLUGINS_FONT_[DEBUG|RELEASE]_BINARY_INSTALL_DIR - Font plugin binary +# installation directory +# MAGNUM_PLUGINS_FONT_[DEBUG|RELEASE]_LIBRARY_INSTALL_DIR - Font plugin +# library installation directory +# MAGNUM_PLUGINS_FONTCONVERTER_[DEBUG|RELEASE]_BINARY_INSTALL_DIR - Font +# converter plugin binary installation directory +# MAGNUM_PLUGINS_FONTCONVERTER_[DEBUG|RELEASE]_LIBRARY_INSTALL_DIR - Font +# converter plugin library installation directory +# MAGNUM_PLUGINS_IMAGECONVERTER_[DEBUG|RELEASE]_BINARY_INSTALL_DIR - Image +# converter plugin binary installation directory +# MAGNUM_PLUGINS_IMAGECONVERTER_[DEBUG|RELEASE]_LIBRARY_INSTALL_DIR - Image +# converter plugin library installation directory +# MAGNUM_PLUGINS_IMPORTER_[DEBUG|RELEASE]_BINARY_INSTALL_DIR - Importer +# plugin binary installation directory +# MAGNUM_PLUGINS_IMPORTER_[DEBUG|RELEASE]_LIBRARY_INSTALL_DIR - Importer +# plugin library installation directory +# MAGNUM_PLUGINS_SCENECONVERTER_[DEBUG|RELEASE]_BINARY_INSTALL_DIR - Scene +# converter plugin binary installation directory +# MAGNUM_PLUGINS_SCENECONVERTER_[DEBUG|RELEASE]_LIBRARY_INSTALL_DIR - Scene +# converter plugin library installation directory +# MAGNUM_PLUGINS_AUDIOIMPORTER_[DEBUG|RELEASE]_BINARY_INSTALL_DIR - Audio +# importer plugin binary installation directory +# MAGNUM_PLUGINS_AUDIOIMPORTER_[DEBUG|RELEASE]_LIBRARY_INSTALL_DIR - Audio +# importer plugin library installation directory +# MAGNUM_INCLUDE_INSTALL_DIR - Header installation directory +# MAGNUM_PLUGINS_INCLUDE_INSTALL_DIR - Plugin header installation directory +# + +# +# This file is part of Magnum. +# +# Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, +# 2020, 2021, 2022, 2023, 2024, 2025 +# Vladimír VondruÅ¡ +# +# 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. +# + +# CMake policies used by FindMagnum are popped again at the end. +cmake_policy(PUSH) +# Prefer GLVND when finding OpenGL. If this causes problems (known to fail with +# NVidia drivers in Debian Buster, reported on 2019-04-09), users can override +# this by setting OpenGL_GL_PREFERENCE to LEGACY. +if(POLICY CMP0072) + cmake_policy(SET CMP0072 NEW) +endif() + +# Corrade library dependencies +set(_MAGNUM_CORRADE_DEPENDENCIES ) +foreach(_magnum_component ${Magnum_FIND_COMPONENTS}) + set(_MAGNUM_${_magnum_component}_CORRADE_DEPENDENCIES ) + + # Unrolling the transitive dependencies here so this doesn't need to be + # after resolving inter-component dependencies. Listing also all plugins. + if(_magnum_component MATCHES "^(Audio|DebugTools|MeshTools|Primitives|SceneTools|ShaderTools|Text|TextureTools|Trade|.+Importer|.+ImageConverter|.+Font|.+ShaderConverter)$") + list(APPEND _MAGNUM_${_magnum_component}_CORRADE_DEPENDENCIES PluginManager) + endif() + if(_magnum_component STREQUAL DebugTools) + # DebugTools depends on TestSuite optionally, so if it's not there + # assume it wasn't compiled against it. Also, all variables from the + # FindCorrade module overwrite the local variables here (in particular + # _component, _COMPONENT and such), so we need to prefix extensively. + find_package(Corrade QUIET COMPONENTS TestSuite) + if(Corrade_TestSuite_FOUND) + list(APPEND _MAGNUM_${_magnum_component}_CORRADE_DEPENDENCIES TestSuite) + endif() + endif() + + list(APPEND _MAGNUM_CORRADE_DEPENDENCIES ${_MAGNUM_${_magnum_component}_CORRADE_DEPENDENCIES}) +endforeach() +find_package(Corrade REQUIRED Utility ${_MAGNUM_CORRADE_DEPENDENCIES}) + +# Root include dir +find_path(MAGNUM_INCLUDE_DIR + NAMES Magnum/Magnum.h) +mark_as_advanced(MAGNUM_INCLUDE_DIR) + +# Configuration file +find_file(_MAGNUM_CONFIGURE_FILE configure.h + HINTS ${MAGNUM_INCLUDE_DIR}/Magnum/) +mark_as_advanced(_MAGNUM_CONFIGURE_FILE) + +# We need to open configure.h file from MAGNUM_INCLUDE_DIR before we check for +# the components. Bail out with proper error message if it wasn't found. The +# complete check with all components is further below. +if(NOT MAGNUM_INCLUDE_DIR) + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Magnum + REQUIRED_VARS MAGNUM_INCLUDE_DIR _MAGNUM_CONFIGURE_FILE) +endif() + +# Read flags from configuration +file(READ ${_MAGNUM_CONFIGURE_FILE} _magnumConfigure) +string(REGEX REPLACE ";" "\\\\;" _magnumConfigure "${_magnumConfigure}") +string(REGEX REPLACE "\n" ";" _magnumConfigure "${_magnumConfigure}") +set(_magnumFlags + BUILD_DEPRECATED + BUILD_STATIC + BUILD_STATIC_UNIQUE_GLOBALS + TARGET_GL + TARGET_GLES + TARGET_GLES2 + TARGET_WEBGL + TARGET_EGL + TARGET_VK) +foreach(_magnumFlag ${_magnumFlags}) + list(FIND _magnumConfigure "#define MAGNUM_${_magnumFlag}" _magnum_${_magnumFlag}) + if(NOT _magnum_${_magnumFlag} EQUAL -1) + set(MAGNUM_${_magnumFlag} 1) + endif() +endforeach() + +# For compatibility only, to be removed at some point. Refer to +# src/Magnum/configure.h.cmake for the decision logic here. +if(MAGNUM_BUILD_DEPRECATED) + if(CORRADE_BUILD_MULTITHREADED) + set(MAGNUM_BUILD_MULTITHREADED 1) + endif() + if(NOT CORRADE_TARGET_IOS AND NOT CORRADE_TARGET_ANDROID AND NOT CORRADE_TARGET_EMSCRIPTEN AND NOT CORRADE_TARGET_WINDOWS_RT) + if(NOT MAGNUM_TARGET_GLES AND MAGNUM_TARGET_EGL) + set(MAGNUM_TARGET_HEADLESS 1) + endif() + if(MAGNUM_TARGET_GLES AND NOT MAGNUM_TARGET_EGL) + set(MAGNUM_TARGET_DESKTOP_GLES 1) + endif() + endif() + if(MAGNUM_TARGET_GLES AND NOT MAGNUM_TARGET_GLES2) + set(MAGNUM_TARGET_GLES3 1) + endif() +endif() + +# CMake module dir for dependencies. It might not be present at all if no +# feature that needs them is enabled, in which case it'll be left at NOTFOUND. +# But in that case it should also not be subsequently needed for any +# find_package(). If this is called from a superproject, the +# _MAGNUM_DEPENDENCY_MODULE_DIR is already set by modules/CMakeLists.txt. +find_path(_MAGNUM_DEPENDENCY_MODULE_DIR + NAMES + FindEGL.cmake FindGLFW.cmake FindOpenAL.cmake FindOpenGLES2.cmake + FindOpenGLES3.cmake FindSDL2.cmake FindVulkan.cmake + PATH_SUFFIXES share/cmake/Magnum/dependencies) +mark_as_advanced(_MAGNUM_DEPENDENCY_MODULE_DIR) + +# If the module dir is found and is not present in CMAKE_MODULE_PATH already +# (such as when someone explicitly added it, or if it's the Magnum's modules/ +# dir in case of a superproject), add it as the first before all other. Set a +# flag to remove it again at the end, so the modules don't clash with Find +# modules of the same name from other projects. +if(_MAGNUM_DEPENDENCY_MODULE_DIR AND NOT _MAGNUM_DEPENDENCY_MODULE_DIR IN_LIST CMAKE_MODULE_PATH) + set(CMAKE_MODULE_PATH ${_MAGNUM_DEPENDENCY_MODULE_DIR} ${CMAKE_MODULE_PATH}) + set(_MAGNUM_REMOVE_DEPENDENCY_MODULE_DIR_FROM_CMAKE_PATH ON) +else() + unset(_MAGNUM_REMOVE_DEPENDENCY_MODULE_DIR_FROM_CMAKE_PATH) +endif() + +# Base Magnum library +if(NOT TARGET Magnum::Magnum) + add_library(Magnum::Magnum UNKNOWN IMPORTED) + + # Try to find both debug and release version + find_library(MAGNUM_LIBRARY_DEBUG Magnum-d) + find_library(MAGNUM_LIBRARY_RELEASE Magnum) + mark_as_advanced(MAGNUM_LIBRARY_DEBUG + MAGNUM_LIBRARY_RELEASE) + + # Set the MAGNUM_LIBRARY variable based on what was found, use that + # information to guess also build type of dynamic plugins + if(MAGNUM_LIBRARY_DEBUG AND MAGNUM_LIBRARY_RELEASE) + set(MAGNUM_LIBRARY ${MAGNUM_LIBRARY_RELEASE}) + get_filename_component(_MAGNUM_PLUGINS_DIR_PREFIX ${MAGNUM_LIBRARY_DEBUG} PATH) + if(CMAKE_BUILD_TYPE STREQUAL "Debug") + set(_MAGNUM_PLUGINS_DIR_SUFFIX "-d") + endif() + elseif(MAGNUM_LIBRARY_DEBUG) + set(MAGNUM_LIBRARY ${MAGNUM_LIBRARY_DEBUG}) + get_filename_component(_MAGNUM_PLUGINS_DIR_PREFIX ${MAGNUM_LIBRARY_DEBUG} PATH) + set(_MAGNUM_PLUGINS_DIR_SUFFIX "-d") + elseif(MAGNUM_LIBRARY_RELEASE) + set(MAGNUM_LIBRARY ${MAGNUM_LIBRARY_RELEASE}) + get_filename_component(_MAGNUM_PLUGINS_DIR_PREFIX ${MAGNUM_LIBRARY_RELEASE} PATH) + endif() + + # On DLL platforms the plugins are stored in bin/ instead of lib/, modify + # _MAGNUM_PLUGINS_DIR_PREFIX accordingly + if(CORRADE_TARGET_WINDOWS) + get_filename_component(_MAGNUM_PLUGINS_DIR_PREFIX ${_MAGNUM_PLUGINS_DIR_PREFIX} PATH) + set(_MAGNUM_PLUGINS_DIR_PREFIX ${_MAGNUM_PLUGINS_DIR_PREFIX}/bin) + endif() + + if(MAGNUM_LIBRARY_RELEASE) + set_property(TARGET Magnum::Magnum APPEND PROPERTY + IMPORTED_CONFIGURATIONS RELEASE) + set_property(TARGET Magnum::Magnum PROPERTY + IMPORTED_LOCATION_RELEASE ${MAGNUM_LIBRARY_RELEASE}) + endif() + + if(MAGNUM_LIBRARY_DEBUG) + set_property(TARGET Magnum::Magnum APPEND PROPERTY + IMPORTED_CONFIGURATIONS DEBUG) + set_property(TARGET Magnum::Magnum PROPERTY + IMPORTED_LOCATION_DEBUG ${MAGNUM_LIBRARY_DEBUG}) + endif() + + # Include directories + set_property(TARGET Magnum::Magnum APPEND PROPERTY INTERFACE_INCLUDE_DIRECTORIES + ${MAGNUM_INCLUDE_DIR}) + + # Dependent libraries + set_property(TARGET Magnum::Magnum APPEND PROPERTY INTERFACE_LINK_LIBRARIES + Corrade::Utility) +else() + set(MAGNUM_LIBRARY Magnum::Magnum) +endif() + +# Component distinction (listing them explicitly to avoid mistakes with finding +# components from other repositories) +set(_MAGNUM_LIBRARY_COMPONENTS + Audio DebugTools GL MaterialTools MeshTools Primitives SceneGraph + SceneTools Shaders ShaderTools Text TextureTools Trade + WindowlessEglApplication EglContext OpenGLTester) +# These libraries are excluded from DLL detection if Magnum is built as shared. +# Additionally, all *Application and *Context libraries are excluded as well. +set(_MAGNUM_LIBRARY_COMPONENTS_ALWAYS_STATIC + OpenGLTester) +set(_MAGNUM_PLUGIN_COMPONENTS + AnyAudioImporter AnyImageConverter AnyImageImporter AnySceneConverter + AnySceneImporter MagnumFont MagnumFontConverter ObjImporter + TgaImageConverter TgaImporter WavAudioImporter) +set(_MAGNUM_EXECUTABLE_COMPONENTS + imageconverter sceneconverter shaderconverter gl-info al-info) +# Audio and Vk libs aren't enabled by default, and none of the Context, +# Application, Tester libs nor plugins are. Keep in sync with Magnum's root +# CMakeLists.txt. +set(_MAGNUM_IMPLICITLY_ENABLED_COMPONENTS + DebugTools MeshTools SceneGraph Shaders ShaderTools Text TextureTools Trade + GL Primitives) +if(NOT CORRADE_TARGET_EMSCRIPTEN) + list(APPEND _MAGNUM_LIBRARY_COMPONENTS Vk VulkanTester) + list(APPEND _MAGNUM_LIBRARY_COMPONENTS_ALWAYS_STATIC VulkanTester) + list(APPEND _MAGNUM_EXECUTABLE_COMPONENTS vk-info) +endif() +if(NOT CORRADE_TARGET_ANDROID) + list(APPEND _MAGNUM_LIBRARY_COMPONENTS Sdl2Application) +endif() +if(NOT CORRADE_TARGET_ANDROID AND NOT CORRADE_TARGET_IOS AND NOT CORRADE_TARGET_EMSCRIPTEN) + list(APPEND _MAGNUM_LIBRARY_COMPONENTS GlfwApplication) +endif() +if(CORRADE_TARGET_ANDROID) + list(APPEND _MAGNUM_LIBRARY_COMPONENTS AndroidApplication) +endif() +if(CORRADE_TARGET_EMSCRIPTEN) + list(APPEND _MAGNUM_LIBRARY_COMPONENTS EmscriptenApplication) +endif() +if(CORRADE_TARGET_IOS) + list(APPEND _MAGNUM_LIBRARY_COMPONENTS WindowlessIosApplication) +elseif(CORRADE_TARGET_APPLE AND NOT MAGNUM_TARGET_GLES) + list(APPEND _MAGNUM_LIBRARY_COMPONENTS WindowlessCglApplication CglContext) +endif() +if(CORRADE_TARGET_UNIX AND NOT CORRADE_TARGET_APPLE) + list(APPEND _MAGNUM_LIBRARY_COMPONENTS GlxApplication XEglApplication WindowlessGlxApplication GlxContext) +endif() +if(CORRADE_TARGET_WINDOWS) + list(APPEND _MAGNUM_LIBRARY_COMPONENTS WindowlessWglApplication WglContext) +endif() +if(CORRADE_TARGET_UNIX OR CORRADE_TARGET_WINDOWS) + list(APPEND _MAGNUM_EXECUTABLE_COMPONENTS fontconverter distancefieldconverter) +endif() + +# Inter-component dependencies +set(_MAGNUM_Audio_DEPENDENCIES ) + +# Trade is used by CompareImage. If Trade is not enabled, CompareImage is not +# compiled at all. +set(_MAGNUM_DebugTools_DEPENDENCIES Trade) +set(_MAGNUM_DebugTools_Trade_DEPENDENCY_IS_OPTIONAL ON) +# MeshTools, Primitives, SceneGraph and Shaders are used only for GL renderers +# in DebugTools. All of this is optional, compiled in only if the base library +# was selected. +if(MAGNUM_TARGET_GL) + list(APPEND _MAGNUM_DebugTools_DEPENDENCIES MeshTools Primitives SceneGraph Shaders GL) + set(_MAGNUM_DebugTools_MeshTools_DEPENDENCY_IS_OPTIONAL ON) + set(_MAGNUM_DebugTools_Primitives_DEPENDENCY_IS_OPTIONAL ON) + set(_MAGNUM_DebugTools_SceneGraph_DEPENDENCY_IS_OPTIONAL ON) + set(_MAGNUM_DebugTools_Shaders_DEPENDENCY_IS_OPTIONAL ON) + set(_MAGNUM_DebugTools_GL_DEPENDENCY_IS_OPTIONAL ON) +endif() + +set(_MAGNUM_MaterialTools_DEPENDENCIES Trade) + +set(_MAGNUM_MeshTools_DEPENDENCIES Trade) +if(MAGNUM_TARGET_GL) + list(APPEND _MAGNUM_MeshTools_DEPENDENCIES GL) +endif() + +set(_MAGNUM_OpenGLTester_DEPENDENCIES GL) +if(MAGNUM_TARGET_EGL) + list(APPEND _MAGNUM_OpenGLTester_DEPENDENCIES WindowlessEglApplication) +elseif(CORRADE_TARGET_IOS) + list(APPEND _MAGNUM_OpenGLTester_DEPENDENCIES WindowlessIosApplication) +elseif(CORRADE_TARGET_APPLE) + list(APPEND _MAGNUM_OpenGLTester_DEPENDENCIES WindowlessCglApplication) +elseif(CORRADE_TARGET_UNIX) + list(APPEND _MAGNUM_OpenGLTester_DEPENDENCIES WindowlessGlxApplication) +elseif(CORRADE_TARGET_WINDOWS) + list(APPEND _MAGNUM_OpenGLTester_DEPENDENCIES WindowlessWglApplication) +endif() + +set(_MAGNUM_Primitives_DEPENDENCIES MeshTools Trade) +if(MAGNUM_TARGET_GL) + # GL not required by Primitives themselves, but transitively by MeshTools + list(APPEND _MAGNUM_Primitives_DEPENDENCIES GL) +endif() + +set(_MAGNUM_SceneGraph_DEPENDENCIES ) +set(_MAGNUM_SceneTools_DEPENDENCIES Trade) +set(_MAGNUM_Shaders_DEPENDENCIES GL) + +set(_MAGNUM_Text_DEPENDENCIES TextureTools) +if(MAGNUM_TARGET_GL) + list(APPEND _MAGNUM_Text_DEPENDENCIES GL) +endif() + +set(_MAGNUM_TextureTools_DEPENDENCIES ) +if(MAGNUM_TARGET_GL) + list(APPEND _MAGNUM_TextureTools_DEPENDENCIES GL) +endif() + +set(_MAGNUM_Trade_DEPENDENCIES ) +set(_MAGNUM_VulkanTester_DEPENDENCIES Vk) +set(_MAGNUM_AndroidApplication_DEPENDENCIES GL) + +set(_MAGNUM_EmscriptenApplication_DEPENDENCIES) +if(MAGNUM_TARGET_GL) + list(APPEND _MAGNUM_EmscriptenApplication_DEPENDENCIES GL) +endif() + +set(_MAGNUM_GlfwApplication_DEPENDENCIES ) +if(MAGNUM_TARGET_GL) + list(APPEND _MAGNUM_GlfwApplication_DEPENDENCIES GL) +endif() + +set(_MAGNUM_GlxApplication_DEPENDENCIES GL) + +set(_MAGNUM_Sdl2Application_DEPENDENCIES ) +if(MAGNUM_TARGET_GL) + list(APPEND _MAGNUM_Sdl2Application_DEPENDENCIES GL) +endif() + +set(_MAGNUM_WindowlessCglApplication_DEPENDENCIES GL) +set(_MAGNUM_WindowlessEglApplication_DEPENDENCIES GL) +set(_MAGNUM_WindowlessGlxApplication_DEPENDENCIES GL) +set(_MAGNUM_WindowlessIosApplication_DEPENDENCIES GL) +set(_MAGNUM_WindowlessWglApplication_DEPENDENCIES GL) +set(_MAGNUM_XEglApplication_DEPENDENCIES GL) +set(_MAGNUM_CglContext_DEPENDENCIES GL) +set(_MAGNUM_EglContext_DEPENDENCIES GL) +set(_MAGNUM_GlxContext_DEPENDENCIES GL) +set(_MAGNUM_WglContext_DEPENDENCIES GL) + +set(_MAGNUM_MagnumFont_DEPENDENCIES Trade TgaImporter GL) # and below +set(_MAGNUM_MagnumFontConverter_DEPENDENCIES Trade TgaImageConverter) # and below +set(_MAGNUM_ObjImporter_DEPENDENCIES MeshTools) # and below +foreach(_component ${_MAGNUM_PLUGIN_COMPONENTS}) + if(_component MATCHES ".+AudioImporter") + list(APPEND _MAGNUM_${_component}_DEPENDENCIES Audio) + elseif(_component MATCHES ".+ShaderConverter") + list(APPEND _MAGNUM_${_component}_DEPENDENCIES ShaderTools) + elseif(_component MATCHES ".+(Importer|ImageConverter|SceneConverter)") + list(APPEND _MAGNUM_${_component}_DEPENDENCIES Trade) + elseif(_component MATCHES ".+(Font|FontConverter)") + list(APPEND _MAGNUM_${_component}_DEPENDENCIES Text TextureTools) + endif() +endforeach() + +# Ensure that all inter-component dependencies are specified as well +set(_MAGNUM_ADDITIONAL_COMPONENTS ) +foreach(_component ${Magnum_FIND_COMPONENTS}) + # Mark the dependencies as required if the component is also required, but + # only if they themselves are not optional (for example parts of DebugTools + # are present only if their respective base library is compiled) + if(Magnum_FIND_REQUIRED_${_component}) + foreach(_dependency ${_MAGNUM_${_component}_DEPENDENCIES}) + if(NOT _MAGNUM_${_component}_${_dependency}_DEPENDENCY_IS_OPTIONAL) + set(Magnum_FIND_REQUIRED_${_dependency} TRUE) + endif() + endforeach() + endif() + + list(APPEND _MAGNUM_ADDITIONAL_COMPONENTS ${_MAGNUM_${_component}_DEPENDENCIES}) +endforeach() + +# Join the lists, remove duplicate components +set(_MAGNUM_ORIGINAL_FIND_COMPONENTS ${Magnum_FIND_COMPONENTS}) +if(_MAGNUM_ADDITIONAL_COMPONENTS) + list(INSERT Magnum_FIND_COMPONENTS 0 ${_MAGNUM_ADDITIONAL_COMPONENTS}) +endif() +if(Magnum_FIND_COMPONENTS) + list(REMOVE_DUPLICATES Magnum_FIND_COMPONENTS) +endif() + +# Special cases of include paths. Libraries not listed here have a path suffix +# and include name derived from the library name in the loop below. +set(_MAGNUM_MATERIALTOOLS_INCLUDE_PATH_NAMES PhongToPbrMetallicRoughness.h) +set(_MAGNUM_MESHTOOLS_INCLUDE_PATH_NAMES CompressIndices.h) +set(_MAGNUM_OPENGLTESTER_INCLUDE_PATH_SUFFIX Magnum/GL) +set(_MAGNUM_VULKANTESTER_INCLUDE_PATH_SUFFIX Magnum/Vk) +set(_MAGNUM_PRIMITIVES_INCLUDE_PATH_NAMES Cube.h) +set(_MAGNUM_SCENETOOLS_INCLUDE_PATH_NAMES Hierarchy.h) + +# Find all components. Maintain a list of components that'll need to have +# their optional dependencies checked. +set(_MAGNUM_OPTIONAL_DEPENDENCIES_TO_ADD ) +foreach(_component ${Magnum_FIND_COMPONENTS}) + string(TOUPPER ${_component} _COMPONENT) + + # Create imported target in case the library is found. If the project is + # added as subproject to CMake, the target already exists and all the + # required setup is already done from the build tree. + if(TARGET "Magnum::${_component}") # Quotes to "fix" KDE's higlighter + set(Magnum_${_component}_FOUND TRUE) + else() + # Library components + if(_component IN_LIST _MAGNUM_LIBRARY_COMPONENTS) + # Include path names to find, unless specified above already. + # Application and context libraries are a special case as well. + if(_component MATCHES ".+Application") + set(_MAGNUM_${_COMPONENT}_INCLUDE_PATH_SUFFIX Magnum/Platform) + elseif(_component MATCHES ".+Context") + set(_MAGNUM_${_COMPONENT}_INCLUDE_PATH_SUFFIX Magnum/Platform) + set(_MAGNUM_${_COMPONENT}_INCLUDE_PATH_NAMES GLContext.h) + endif() + if(NOT _MAGNUM_${_COMPONENT}_INCLUDE_PATH_SUFFIX) + set(_MAGNUM_${_COMPONENT}_INCLUDE_PATH_SUFFIX Magnum/${_component}) + endif() + if(NOT _MAGNUM_${_COMPONENT}_INCLUDE_PATH_NAMES) + set(_MAGNUM_${_COMPONENT}_INCLUDE_PATH_NAMES ${_component}.h) + endif() + + # Try to find both debug and release version + find_library(MAGNUM_${_COMPONENT}_LIBRARY_DEBUG Magnum${_component}-d) + find_library(MAGNUM_${_COMPONENT}_LIBRARY_RELEASE Magnum${_component}) + mark_as_advanced(MAGNUM_${_COMPONENT}_LIBRARY_DEBUG + MAGNUM_${_COMPONENT}_LIBRARY_RELEASE) + + # On Windows, if we have a dynamic build of given library, find the + # DLLs as well. Abuse find_program() since the DLLs should be + # alongside usual executables. On MinGW they however have a lib + # prefix. + if(CORRADE_TARGET_WINDOWS AND NOT MAGNUM_BUILD_STATIC AND NOT _component IN_LIST _MAGNUM_LIBRARY_COMPONENTS_ALWAYS_STATIC AND NOT _component MATCHES ".+Application" AND NOT _component MATCHES ".+Context") + find_program(MAGNUM_${_COMPONENT}_DLL_DEBUG ${CMAKE_SHARED_LIBRARY_PREFIX}Magnum${_component}-d.dll) + find_program(MAGNUM_${_COMPONENT}_DLL_RELEASE ${CMAKE_SHARED_LIBRARY_PREFIX}Magnum${_component}.dll) + mark_as_advanced(MAGNUM_${_COMPONENT}_DLL_DEBUG + MAGNUM_${_COMPONENT}_DLL_RELEASE) + # If not on Windows or on a static build, unset the DLL variables + # to avoid leaks when switching shared and static builds + else() + unset(MAGNUM_${_COMPONENT}_DLL_DEBUG CACHE) + unset(MAGNUM_${_COMPONENT}_DLL_RELEASE CACHE) + endif() + + # Plugin components + elseif(_component IN_LIST _MAGNUM_PLUGIN_COMPONENTS) + # AudioImporter plugin specific name suffixes + if(_component MATCHES ".+AudioImporter$") + set(_MAGNUM_${_COMPONENT}_PATH_SUFFIX audioimporters) + + # Audio importer class is Audio::*Importer, thus we need to + # convert *AudioImporter.h to *Importer.h + string(REPLACE "AudioImporter" "Importer" _MAGNUM_${_COMPONENT}_HEADER_NAME "${_component}") + if(NOT _MAGNUM_${_COMPONENT}_INCLUDE_PATH_NAMES) + set(_MAGNUM_${_COMPONENT}_INCLUDE_PATH_NAMES ${_MAGNUM_${_COMPONENT}_HEADER_NAME}.h) + endif() + + # ShaderConverter plugin specific name suffixes + elseif(_component MATCHES ".+ShaderConverter$") + set(_MAGNUM_${_COMPONENT}_PATH_SUFFIX shaderconverters) + + # Importer plugin specific name suffixes + elseif(_component MATCHES ".+Importer$") + set(_MAGNUM_${_COMPONENT}_PATH_SUFFIX importers) + + # Font plugin specific name suffixes + elseif(_component MATCHES ".+Font$") + set(_MAGNUM_${_COMPONENT}_PATH_SUFFIX fonts) + + # ImageConverter plugin specific name suffixes + elseif(_component MATCHES ".+ImageConverter$") + set(_MAGNUM_${_COMPONENT}_PATH_SUFFIX imageconverters) + + # SceneConverter plugin specific name suffixes + elseif(_component MATCHES ".+SceneConverter$") + set(_MAGNUM_${_COMPONENT}_PATH_SUFFIX sceneconverters) + + # FontConverter plugin specific name suffixes + elseif(_component MATCHES ".+FontConverter$") + set(_MAGNUM_${_COMPONENT}_PATH_SUFFIX fontconverters) + endif() + + # Include path names to find, unless specified above + if(NOT _MAGNUM_${_COMPONENT}_INCLUDE_PATH_SUFFIX) + set(_MAGNUM_${_COMPONENT}_INCLUDE_PATH_SUFFIX MagnumPlugins/${_component}) + endif() + if(NOT _MAGNUM_${_COMPONENT}_INCLUDE_PATH_NAMES) + set(_MAGNUM_${_COMPONENT}_INCLUDE_PATH_NAMES ${_component}.h) + endif() + + # Dynamic plugins don't have any prefix (e.g. `lib` on Linux), + # search with empty prefix and then reset that back so we don't + # accidentally break something else + set(_tmp_prefixes "${CMAKE_FIND_LIBRARY_PREFIXES}") + set(CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES};") + + # Try to find both debug and release version. Dynamic and static + # debug libraries are in different places. Static debug plugins are + # in magnum/ with a -d suffix while dynamic debug plugins are in + # magnum-d/ with no suffix. Problem is that Vcpkg's library linking + # automagic needs the static libs to be in the root library + # directory along with everything else and so we need to search for + # the -d suffixed version *before* the unsuffixed so it doesn't + # pick the release library for both debug and release. + find_library(MAGNUM_${_COMPONENT}_LIBRARY_DEBUG ${_component}-d + PATH_SUFFIXES magnum/${_MAGNUM_${_COMPONENT}_PATH_SUFFIX}) + find_library(MAGNUM_${_COMPONENT}_LIBRARY_DEBUG ${_component} + PATH_SUFFIXES magnum-d/${_MAGNUM_${_COMPONENT}_PATH_SUFFIX}) + find_library(MAGNUM_${_COMPONENT}_LIBRARY_RELEASE ${_component} + PATH_SUFFIXES magnum/${_MAGNUM_${_COMPONENT}_PATH_SUFFIX}) + mark_as_advanced(MAGNUM_${_COMPONENT}_LIBRARY_DEBUG + MAGNUM_${_COMPONENT}_LIBRARY_RELEASE) + + # Reset back + set(CMAKE_FIND_LIBRARY_PREFIXES "${_tmp_prefixes}") + + # Executables + elseif(_component IN_LIST _MAGNUM_EXECUTABLE_COMPONENTS) + find_program(MAGNUM_${_COMPONENT}_EXECUTABLE magnum-${_component}) + mark_as_advanced(MAGNUM_${_COMPONENT}_EXECUTABLE) + + # Something unknown, skip. FPHSA will take care of handling this below. + else() + continue() + endif() + + # Find library/plugin includes + if(_component IN_LIST _MAGNUM_LIBRARY_COMPONENTS OR _component IN_LIST _MAGNUM_PLUGIN_COMPONENTS) + find_path(_MAGNUM_${_COMPONENT}_INCLUDE_DIR + NAMES ${_MAGNUM_${_COMPONENT}_INCLUDE_PATH_NAMES} + HINTS ${MAGNUM_INCLUDE_DIR}/${_MAGNUM_${_COMPONENT}_INCLUDE_PATH_SUFFIX}) + mark_as_advanced(_MAGNUM_${_COMPONENT}_INCLUDE_DIR) + endif() + + # Determine if the plugin is static or dynamic by reading the + # per-plugin config file. Plugins use this for automatic import if + # static. + # TODO: add per-library configure.h as well, for consistency with + # extras, plugins and integration + if(_component IN_LIST _MAGNUM_PLUGIN_COMPONENTS) + find_file(_MAGNUM_${_COMPONENT}_CONFIGURE_FILE configure.h + HINTS ${_MAGNUM_${_COMPONENT}_INCLUDE_DIR}) + mark_as_advanced(_MAGNUM_${_COMPONENT}_CONFIGURE_FILE) + + # If the file wasn't found, skip this so it fails on the FPHSA + # below and not right here. + if(_MAGNUM_${_COMPONENT}_CONFIGURE_FILE) + file(READ ${_MAGNUM_${_COMPONENT}_CONFIGURE_FILE} _magnumPluginConfigure) + string(REGEX REPLACE ";" "\\\\;" _magnumPluginConfigure "${_magnumPluginConfigure}") + string(REGEX REPLACE "\n" ";" _magnumPluginConfigure "${_magnumPluginConfigure}") + list(FIND _magnumPluginConfigure "#define MAGNUM_${_COMPONENT}_BUILD_STATIC" _magnumPluginBuildStatic) + if(NOT _magnumPluginBuildStatic EQUAL -1) + # The variable is inconsistently named between C++ and + # CMake in extras, plugins and integration, keep it + # underscored / private until that's resolved + set(_MAGNUM_${_COMPONENT}_BUILD_STATIC ON) + endif() + endif() + endif() + + # Decide if the library was found. If not, skip the rest, which + # populates the target properties and finds additional dependencies. + # This means that the rest can also rely on that e.g. FindEGL.cmake is + # present in _MAGNUM_DEPENDENCY_MODULE_DIR -- given that the library + # needing EGL was found, it likely also installed FindEGL for itself. + if( + # If the component is a library, it should have the include dir + ((_component IN_LIST _MAGNUM_LIBRARY_COMPONENTS OR _component IN_LIST _MAGNUM_PLUGIN_COMPONENTS) AND _MAGNUM_${_COMPONENT}_INCLUDE_DIR AND ( + # And it should have a debug library, and a DLL found if + # expected + (MAGNUM_${_COMPONENT}_LIBRARY_DEBUG AND ( + NOT DEFINED MAGNUM_${_COMPONENT}_DLL_DEBUG OR + MAGNUM_${_COMPONENT}_DLL_DEBUG)) OR + # Or have a release library, and a DLL found if expected + (MAGNUM_${_COMPONENT}_LIBRARY_RELEASE AND ( + NOT DEFINED MAGNUM_${_COMPONENT}_DLL_RELEASE OR + MAGNUM_${_COMPONENT}_DLL_RELEASE)))) OR + # If the component is an executable, it should have just the + # location + (_component IN_LIST _MAGNUM_EXECUTABLE_COMPONENTS AND MAGNUM_${_COMPONENT}_EXECUTABLE) + ) + set(Magnum_${_component}_FOUND TRUE) + else() + set(Magnum_${_component}_FOUND FALSE) + continue() + endif() + + # Target and location for libraries + if(_component IN_LIST _MAGNUM_LIBRARY_COMPONENTS) + if(MAGNUM_BUILD_STATIC OR _component IN_LIST _MAGNUM_LIBRARY_COMPONENTS_ALWAYS_STATIC OR _component MATCHES ".+Application" OR _component MATCHES ".+Context") + add_library(Magnum::${_component} STATIC IMPORTED) + else() + add_library(Magnum::${_component} SHARED IMPORTED) + endif() + + foreach(_CONFIG DEBUG RELEASE) + if(NOT MAGNUM_${_COMPONENT}_LIBRARY_${_CONFIG}) + continue() + endif() + + set_property(TARGET Magnum::${_component} APPEND PROPERTY + IMPORTED_CONFIGURATIONS ${_CONFIG}) + # Unfortunately for a DLL the two properties are swapped out, + # *.lib goes to IMPLIB, so it's duplicated like this + if(DEFINED MAGNUM_${_COMPONENT}_DLL_${_CONFIG}) + # Quotes to "fix" KDE's higlighter + set_target_properties("Magnum::${_component}" PROPERTIES + IMPORTED_LOCATION_${_CONFIG} ${MAGNUM_${_COMPONENT}_DLL_${_CONFIG}} + IMPORTED_IMPLIB_${_CONFIG} ${MAGNUM_${_COMPONENT}_LIBRARY_${_CONFIG}}) + else() + set_property(TARGET Magnum::${_component} PROPERTY + IMPORTED_LOCATION_${_CONFIG} ${MAGNUM_${_COMPONENT}_LIBRARY_${_CONFIG}}) + endif() + endforeach() + + # Target and location for plugins. Not dealing with DLL locations for + # those. + elseif(_component IN_LIST _MAGNUM_PLUGIN_COMPONENTS) + add_library(Magnum::${_component} UNKNOWN IMPORTED) + + foreach(_CONFIG DEBUG RELEASE) + if(NOT MAGNUM_${_COMPONENT}_LIBRARY_${_CONFIG}) + continue() + endif() + + set_property(TARGET Magnum::${_component} APPEND PROPERTY + IMPORTED_CONFIGURATIONS ${_CONFIG}) + set_property(TARGET Magnum::${_component} PROPERTY + IMPORTED_LOCATION_${_CONFIG} ${MAGNUM_${_COMPONENT}_LIBRARY_${_CONFIG}}) + endforeach() + + # Target and location for executable components + elseif(_component IN_LIST _MAGNUM_EXECUTABLE_COMPONENTS) + add_executable(Magnum::${_component} IMPORTED) + + set_property(TARGET Magnum::${_component} PROPERTY + IMPORTED_LOCATION ${MAGNUM_${_COMPONENT}_EXECUTABLE}) + endif() + + # Applications + if(_component MATCHES ".+Application") + # Android application dependencies + if(_component STREQUAL AndroidApplication) + find_package(EGL) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES android EGL::EGL) + + # Emscripten application dependencies + elseif(_component STREQUAL EmscriptenApplication) + # Emscripten has various stuff implemented in JS + if(CORRADE_TARGET_EMSCRIPTEN) + find_file(MAGNUM_PLATFORM_JS MagnumPlatform.js + PATH_SUFFIXES lib) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + # TODO switch to INTERFACE_LINK_OPTIONS and SHELL: once + # we require CMake 3.13 unconditionally + INTERFACE_LINK_LIBRARIES "--js-library ${MAGNUM_PLATFORM_JS}") + endif() + + # GLFW application dependencies + elseif(_component STREQUAL GlfwApplication) + find_package(GLFW) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES GLFW::GLFW) + # Use the Foundation framework on Apple to query the DPI awareness + if(CORRADE_TARGET_APPLE) + find_library(_MAGNUM_APPLE_FOUNDATION_FRAMEWORK_LIBRARY Foundation) + mark_as_advanced(_MAGNUM_APPLE_FOUNDATION_FRAMEWORK_LIBRARY) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES ${_MAGNUM_APPLE_FOUNDATION_FRAMEWORK_LIBRARY}) + # Needed for opt-in DPI queries + elseif(CORRADE_TARGET_UNIX) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES ${CMAKE_DL_LIBS}) + endif() + + # With GLVND (since CMake 3.10) we need to explicitly link to + # GLX/EGL because libOpenGL doesn't provide it. For EGL we have + # our own EGL find module, which makes things simpler. The + # upstream FindOpenGL is anything but simple. Also can't use + # OpenGL_OpenGL_FOUND, because that one is set also if GLVND is + # *not* found. WTF. Also can't just check for + # OPENGL_opengl_LIBRARY because that's set even if + # OpenGL_GL_PREFERENCE is explicitly set to LEGACY. + if(MAGNUM_TARGET_GL) + if(MAGNUM_TARGET_EGL) + find_package(EGL) + set_property(TARGET Magnum::${_component} APPEND + PROPERTY INTERFACE_LINK_LIBRARIES EGL::EGL) + elseif(CORRADE_TARGET_UNIX AND NOT CORRADE_TARGET_APPLE) + find_package(OpenGL) + if(OPENGL_opengl_LIBRARY AND OpenGL_GL_PREFERENCE STREQUAL GLVND) + set_property(TARGET Magnum::${_component} APPEND + PROPERTY INTERFACE_LINK_LIBRARIES OpenGL::GLX) + endif() + endif() + endif() + + # SDL2 application dependencies + elseif(_component STREQUAL Sdl2Application) + find_package(SDL2) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES SDL2::SDL2) + # Use the Foundation framework on Apple to query the DPI awareness + if(CORRADE_TARGET_APPLE) + find_library(_MAGNUM_APPLE_FOUNDATION_FRAMEWORK_LIBRARY Foundation) + mark_as_advanced(_MAGNUM_APPLE_FOUNDATION_FRAMEWORK_LIBRARY) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES ${_MAGNUM_APPLE_FOUNDATION_FRAMEWORK_LIBRARY}) + # Needed for opt-in DPI queries + elseif(CORRADE_TARGET_UNIX) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES ${CMAKE_DL_LIBS}) + # Emscripten has various stuff implemented in JS + elseif(CORRADE_TARGET_EMSCRIPTEN) + find_file(MAGNUM_PLATFORM_JS MagnumPlatform.js + PATH_SUFFIXES lib) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + # TODO switch to INTERFACE_LINK_OPTIONS and SHELL: once + # we require CMake 3.13 unconditionally + INTERFACE_LINK_LIBRARIES "--js-library ${MAGNUM_PLATFORM_JS}") + endif() + + # With GLVND (since CMake 3.10) we need to explicitly link to + # GLX/EGL because libOpenGL doesn't provide it. For EGL we have + # our own EGL find module, which makes things simpler. The + # upstream FindOpenGL is anything but simple. Also can't use + # OpenGL_OpenGL_FOUND, because that one is set also if GLVND is + # *not* found. WTF. Also can't just check for + # OPENGL_opengl_LIBRARY because that's set even if + # OpenGL_GL_PREFERENCE is explicitly set to LEGACY. + if(MAGNUM_TARGET_GL) + if(MAGNUM_TARGET_EGL) + find_package(EGL) + set_property(TARGET Magnum::${_component} APPEND + PROPERTY INTERFACE_LINK_LIBRARIES EGL::EGL) + elseif(CORRADE_TARGET_UNIX AND NOT CORRADE_TARGET_APPLE) + find_package(OpenGL) + if(OPENGL_opengl_LIBRARY AND OpenGL_GL_PREFERENCE STREQUAL GLVND) + set_property(TARGET Magnum::${_component} APPEND + PROPERTY INTERFACE_LINK_LIBRARIES OpenGL::GLX) + endif() + endif() + endif() + + # (Windowless) GLX application dependencies + elseif(_component STREQUAL GlxApplication OR _component STREQUAL WindowlessGlxApplication) + find_package(X11) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_INCLUDE_DIRECTORIES ${X11_INCLUDE_DIR}) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES ${X11_LIBRARIES}) + + # With GLVND (since CMake 3.10) we need to explicitly link to + # GLX because libOpenGL doesn't provide it. Also can't use + # OpenGL_OpenGL_FOUND, because that one is set also if GLVND is + # *not* found. WTF. Also can't just check for + # OPENGL_opengl_LIBRARY because that's set even if + # OpenGL_GL_PREFERENCE is explicitly set to LEGACY. + # + # If MAGNUM_TARGET_GLES and MAGNUM_TARGET_EGL is set, these + # applications can be built only if GLVND is available as + # otherwise there would be a conflict between libGL and + # libGLES. Thus, if GLVND is not available, it won't link + # libGLX here, but that shouldn't be a problem since the + # application library won't exist either. + find_package(OpenGL) + if(OPENGL_opengl_LIBRARY AND OpenGL_GL_PREFERENCE STREQUAL GLVND) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES OpenGL::GLX) + endif() + + # Windowless CGL application has no additional dependencies + + # Windowless EGL application dependencies + elseif(_component STREQUAL WindowlessEglApplication) + find_package(EGL) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES EGL::EGL) + + # Windowless iOS application dependencies + elseif(_component STREQUAL WindowlessIosApplication) + # We need to link to Foundation framework to use ObjC + find_library(_MAGNUM_IOS_FOUNDATION_FRAMEWORK_LIBRARY Foundation) + mark_as_advanced(_MAGNUM_IOS_FOUNDATION_FRAMEWORK_LIBRARY) + find_package(EGL) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES EGL::EGL ${_MAGNUM_IOS_FOUNDATION_FRAMEWORK_LIBRARY}) + + # Windowless WGL application has no additional dependencies + + # X/EGL application dependencies + elseif(_component STREQUAL XEglApplication) + find_package(EGL) + find_package(X11) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_INCLUDE_DIRECTORIES ${X11_INCLUDE_DIR}) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES EGL::EGL ${X11_LIBRARIES}) + endif() + + # Context libraries + elseif(_component MATCHES ".+Context") + # GLX context dependencies + if(_component STREQUAL GlxContext) + # With GLVND (since CMake 3.10) we need to explicitly link to + # GLX because libOpenGL doesn't provide it. Also can't use + # OpenGL_OpenGL_FOUND, because that one is set also if GLVND is + # *not* found. If GLVND is not used, link to X11 instead. Also + # can't just check for OPENGL_opengl_LIBRARY because that's set + # even if OpenGL_GL_PREFERENCE is explicitly set to LEGACY. + find_package(OpenGL) + if(OPENGL_opengl_LIBRARY AND OpenGL_GL_PREFERENCE STREQUAL GLVND) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES OpenGL::GLX) + else() + find_package(X11) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_INCLUDE_DIRECTORIES ${X11_INCLUDE_DIR}) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES ${X11_LIBRARIES}) + endif() + + # EGL context dependencies + elseif(_component STREQUAL EglContext) + find_package(EGL) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES EGL::EGL) + endif() + + # No additional dependencies for CGL context + # No additional dependencies for WGL context + + # Audio library + elseif(_component STREQUAL Audio) + find_package(OpenAL) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES OpenAL::OpenAL) + + # No special setup for DebugTools library + + # GL library + elseif(_component STREQUAL GL) + if(NOT MAGNUM_TARGET_GLES OR (MAGNUM_TARGET_GLES AND NOT MAGNUM_TARGET_EGL AND NOT CORRADE_TARGET_IOS)) + # If the GLVND library (CMake 3.10+) was found, link to the + # imported target. Otherwise (and also on all systems except + # Linux) link to the classic libGL. Can't use + # OpenGL_OpenGL_FOUND, because that one is set also if GLVND is + # *not* found. WTF. Also can't just check for + # OPENGL_opengl_LIBRARY because that's set even if + # OpenGL_GL_PREFERENCE is explicitly set to LEGACY. + find_package(OpenGL REQUIRED) + if(OPENGL_opengl_LIBRARY AND OpenGL_GL_PREFERENCE STREQUAL GLVND) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES OpenGL::OpenGL) + else() + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES ${OPENGL_gl_LIBRARY}) + endif() + elseif(MAGNUM_TARGET_GLES2) + find_package(OpenGLES2 REQUIRED) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES OpenGLES2::OpenGLES2) + else() + find_package(OpenGLES3 REQUIRED) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES OpenGLES3::OpenGLES3) + endif() + + # No special setup for MaterialTools library + # No special setup for MeshTools library + # No special setup for OpenGLTester library + # No special setup for VulkanTester library + # No special setup for Primitives library + # No special setup for SceneGraph library + # No special setup for SceneTools library + # No special setup for ShaderTools library + # No special setup for Shaders library + # No special setup for Text library + # No special setup for TextureTools library + # No special setup for Trade library + + # Vk library + elseif(_component STREQUAL Vk) + find_package(Vulkan REQUIRED) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES Vulkan::Vulkan) + endif() + + # No special setup for AnyAudioImporter plugin + # No special setup for AnyImageConverter plugin + # No special setup for AnyImageImporter plugin + # No special setup for AnySceneImporter plugin + # No special setup for MagnumFont plugin + # No special setup for MagnumFontConverter plugin + # No special setup for ObjImporter plugin + # No special setup for TgaImageConverter plugin + # No special setup for TgaImporter plugin + # No special setup for WavAudioImporter plugin + + # Automatic import of static plugins + if(_component IN_LIST _MAGNUM_PLUGIN_COMPONENTS AND _MAGNUM_${_COMPONENT}_BUILD_STATIC) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_SOURCES ${_MAGNUM_${_COMPONENT}_INCLUDE_DIR}/importStaticPlugin.cpp) + endif() + + # Link to core Magnum library, add inter-library dependencies. If there + # are optional dependencies, defer adding them to later once we know if + # they were found or not. + if(_component IN_LIST _MAGNUM_LIBRARY_COMPONENTS OR _component IN_LIST _MAGNUM_PLUGIN_COMPONENTS) + foreach(_dependency ${_MAGNUM_${_component}_CORRADE_DEPENDENCIES}) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES Corrade::${_dependency}) + endforeach() + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES Magnum::Magnum) + set(_MAGNUM_${_component}_OPTIONAL_DEPENDENCIES_TO_ADD ) + foreach(_dependency ${_MAGNUM_${_component}_DEPENDENCIES}) + if(NOT _MAGNUM_${_component}_${_dependency}_DEPENDENCY_IS_OPTIONAL) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES Magnum::${_dependency}) + else() + list(APPEND _MAGNUM_${_component}_OPTIONAL_DEPENDENCIES_TO_ADD + ${_dependency}) + endif() + endforeach() + if(_MAGNUM_${_component}_OPTIONAL_DEPENDENCIES_TO_ADD) + list(APPEND _MAGNUM_OPTIONAL_DEPENDENCIES_TO_ADD ${_component}) + endif() + endif() + endif() + + # Global aliases for Windowless*Application, *Application and *Context + # components. If already set, unset them to avoid ambiguity. + if(_component MATCHES "Windowless.+Application") + if(NOT DEFINED _MAGNUM_WINDOWLESSAPPLICATION_ALIAS) + set(_MAGNUM_WINDOWLESSAPPLICATION_ALIAS Magnum::${_component}) + else() + unset(_MAGNUM_WINDOWLESSAPPLICATION_ALIAS) + endif() + elseif(_component MATCHES ".+Application") + if(NOT DEFINED _MAGNUM_APPLICATION_ALIAS) + set(_MAGNUM_APPLICATION_ALIAS Magnum::${_component}) + else() + unset(_MAGNUM_APPLICATION_ALIAS) + endif() + elseif(_component MATCHES ".+Context") + if(NOT DEFINED _MAGNUM_GLCONTEXT_ALIAS) + set(_MAGNUM_GLCONTEXT_ALIAS Magnum::${_component}) + else() + unset(_MAGNUM_GLCONTEXT_ALIAS) + endif() + endif() +endforeach() + +# Emscripten-specific files and flags +if(CORRADE_TARGET_EMSCRIPTEN) + find_file(MAGNUM_EMSCRIPTENAPPLICATION_JS EmscriptenApplication.js + PATH_SUFFIXES share/magnum) + find_file(MAGNUM_WINDOWLESSEMSCRIPTENAPPLICATION_JS WindowlessEmscriptenApplication.js + PATH_SUFFIXES share/magnum) + find_file(MAGNUM_WEBAPPLICATION_CSS WebApplication.css + PATH_SUFFIXES share/magnum) + mark_as_advanced( + MAGNUM_EMSCRIPTENAPPLICATION_JS + MAGNUM_WINDOWLESSEMSCRIPTENAPPLICATION_JS + MAGNUM_WEBAPPLICATION_CSS) + set(MAGNUM_EXTRAS_NEEDED + MAGNUM_EMSCRIPTENAPPLICATION_JS + MAGNUM_WINDOWLESSEMSCRIPTENAPPLICATION_JS + MAGNUM_WEBAPPLICATION_CSS) +endif() + +# For CMake 3.16+ with REASON_FAILURE_MESSAGE, provide additional potentially +# useful info about the failed components. +if(NOT CMAKE_VERSION VERSION_LESS 3.16) + set(_MAGNUM_REASON_FAILURE_MESSAGE ) + # Go only through the originally specified find_package() components, not + # the dependencies added by us afterwards + foreach(_component ${_MAGNUM_ORIGINAL_FIND_COMPONENTS}) + if(Magnum_${_component}_FOUND) + continue() + endif() + + # If it's not known at all, tell the user -- it might be a new library + # and an old Find module, or something platform-specific. + if(NOT _component IN_LIST _MAGNUM_LIBRARY_COMPONENTS AND NOT _component IN_LIST _MAGNUM_PLUGIN_COMPONENTS AND NOT _component IN_LIST _MAGNUM_EXECUTABLE_COMPONENTS) + list(APPEND _MAGNUM_REASON_FAILURE_MESSAGE "${_component} is not a known component on this platform.") + # Otherwise, if it's not among implicitly built components, hint that + # the user may need to enable it + # TODO: currently, the _FOUND variable doesn't reflect if dependencies + # were found. When it will, this needs to be updated to avoid + # misleading messages. + elseif(NOT _component IN_LIST _MAGNUM_IMPLICITLY_ENABLED_COMPONENTS) + string(TOUPPER ${_component} _COMPONENT) + list(APPEND _MAGNUM_REASON_FAILURE_MESSAGE "${_component} is not built by default. Make sure you enabled MAGNUM_WITH_${_COMPONENT} when building Magnum.") + # Otherwise we have no idea. Better be silent than to print something + # misleading. + else() + endif() + endforeach() + + string(REPLACE ";" " " _MAGNUM_REASON_FAILURE_MESSAGE "${_MAGNUM_REASON_FAILURE_MESSAGE}") + set(_MAGNUM_REASON_FAILURE_MESSAGE REASON_FAILURE_MESSAGE "${_MAGNUM_REASON_FAILURE_MESSAGE}") +endif() + +# Remove Magnum's dependency module dir from CMAKE_MODULE_PATH again. Do it +# before the FPHSA call which may exit early in case of a failure. +if(_MAGNUM_REMOVE_DEPENDENCY_MODULE_DIR_FROM_CMAKE_PATH) + list(REMOVE_ITEM CMAKE_MODULE_PATH ${_MAGNUM_DEPENDENCY_MODULE_DIR}) + unset(_MAGNUM_REMOVE_DEPENDENCY_MODULE_DIR_FROM_CMAKE_PATH) +endif() + +# Complete the check with also all components +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Magnum + REQUIRED_VARS MAGNUM_INCLUDE_DIR MAGNUM_LIBRARY ${MAGNUM_EXTRAS_NEEDED} + HANDLE_COMPONENTS + ${_MAGNUM_REASON_FAILURE_MESSAGE}) + +# Components with optional dependencies -- add them once we know if they were +# found or not. +foreach(_component ${_MAGNUM_OPTIONAL_DEPENDENCIES_TO_ADD}) + foreach(_dependency ${_MAGNUM_${_component}_OPTIONAL_DEPENDENCIES_TO_ADD}) + if(Magnum_${_dependency}_FOUND) + set_property(TARGET Magnum::${_component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES Magnum::${_dependency}) + endif() + endforeach() +endforeach() + +# Create Windowless*Application, *Application and *Context aliases +# TODO: ugh why can't I make an alias of IMPORTED target? +if(_MAGNUM_WINDOWLESSAPPLICATION_ALIAS AND NOT TARGET Magnum::WindowlessApplication) + get_target_property(_MAGNUM_WINDOWLESSAPPLICATION_ALIASED_TARGET ${_MAGNUM_WINDOWLESSAPPLICATION_ALIAS} ALIASED_TARGET) + if(_MAGNUM_WINDOWLESSAPPLICATION_ALIASED_TARGET) + add_library(Magnum::WindowlessApplication ALIAS ${_MAGNUM_WINDOWLESSAPPLICATION_ALIASED_TARGET}) + else() + add_library(Magnum::WindowlessApplication UNKNOWN IMPORTED) + foreach(property IMPORTED_CONFIGURATIONS INTERFACE_INCLUDE_DIRECTORIES INTERFACE_COMPILE_DEFINITIONS INTERFACE_COMPILE_OPTIONS INTERFACE_LINK_LIBRARIES) + get_target_property(_MAGNUM_WINDOWLESSAPPLICATION_${property} ${_MAGNUM_WINDOWLESSAPPLICATION_ALIAS} ${property}) + if(_MAGNUM_WINDOWLESSAPPLICATION_${property}) + set_target_properties(Magnum::WindowlessApplication PROPERTIES + ${property} "${_MAGNUM_WINDOWLESSAPPLICATION_${property}}") + endif() + endforeach() + get_target_property(_MAGNUM_WINDOWLESSAPPLICATION_IMPORTED_LOCATION_RELEASE ${_MAGNUM_WINDOWLESSAPPLICATION_ALIAS} IMPORTED_LOCATION_RELEASE) + get_target_property(_MAGNUM_WINDOWLESSAPPLICATION_IMPORTED_LOCATION_DEBUG ${_MAGNUM_WINDOWLESSAPPLICATION_ALIAS} IMPORTED_LOCATION_DEBUG) + if(_MAGNUM_WINDOWLESSAPPLICATION_IMPORTED_LOCATION_RELEASE) + set_target_properties(Magnum::WindowlessApplication PROPERTIES + IMPORTED_LOCATION_RELEASE ${_MAGNUM_WINDOWLESSAPPLICATION_IMPORTED_LOCATION_RELEASE}) + endif() + if(_MAGNUM_WINDOWLESSAPPLICATION_IMPORTED_LOCATION_DEBUG) + set_target_properties(Magnum::WindowlessApplication PROPERTIES + IMPORTED_LOCATION_DEBUG ${_MAGNUM_WINDOWLESSAPPLICATION_IMPORTED_LOCATION_DEBUG}) + endif() + endif() + # Prevent creating the alias again + unset(_MAGNUM_WINDOWLESSAPPLICATION_ALIAS) +endif() +if(_MAGNUM_APPLICATION_ALIAS AND NOT TARGET Magnum::Application) + get_target_property(_MAGNUM_APPLICATION_ALIASED_TARGET ${_MAGNUM_APPLICATION_ALIAS} ALIASED_TARGET) + if(_MAGNUM_APPLICATION_ALIASED_TARGET) + add_library(Magnum::Application ALIAS ${_MAGNUM_APPLICATION_ALIASED_TARGET}) + else() + add_library(Magnum::Application UNKNOWN IMPORTED) + foreach(property IMPORTED_CONFIGURATIONS INTERFACE_INCLUDE_DIRECTORIES INTERFACE_COMPILE_DEFINITIONS INTERFACE_COMPILE_OPTIONS INTERFACE_LINK_LIBRARIES) + get_target_property(_MAGNUM_APPLICATION_${property} + ${_MAGNUM_APPLICATION_ALIAS} ${property}) + if(_MAGNUM_APPLICATION_${property}) + set_target_properties(Magnum::Application PROPERTIES ${property} + "${_MAGNUM_APPLICATION_${property}}") + endif() + endforeach() + get_target_property(_MAGNUM_APPLICATION_IMPORTED_LOCATION_RELEASE ${_MAGNUM_APPLICATION_ALIAS} IMPORTED_LOCATION_RELEASE) + get_target_property(_MAGNUM_APPLICATION_IMPORTED_LOCATION_DEBUG ${_MAGNUM_APPLICATION_ALIAS} IMPORTED_LOCATION_DEBUG) + if(_MAGNUM_APPLICATION_IMPORTED_LOCATION_RELEASE) + set_target_properties(Magnum::Application PROPERTIES + IMPORTED_LOCATION_RELEASE ${_MAGNUM_APPLICATION_IMPORTED_LOCATION_RELEASE}) + endif() + if(_MAGNUM_APPLICATION_IMPORTED_LOCATION_DEBUG) + set_target_properties(Magnum::Application PROPERTIES + IMPORTED_LOCATION_DEBUG ${_MAGNUM_APPLICATION_IMPORTED_LOCATION_DEBUG}) + endif() + endif() + # Prevent creating the alias again + unset(_MAGNUM_APPLICATION_ALIAS) +endif() +if(_MAGNUM_GLCONTEXT_ALIAS AND NOT TARGET Magnum::GLContext) + get_target_property(_MAGNUM_GLCONTEXT_ALIASED_TARGET ${_MAGNUM_GLCONTEXT_ALIAS} ALIASED_TARGET) + if(_MAGNUM_GLCONTEXT_ALIASED_TARGET) + add_library(Magnum::GLContext ALIAS ${_MAGNUM_GLCONTEXT_ALIASED_TARGET}) + else() + add_library(Magnum::GLContext UNKNOWN IMPORTED) + foreach(property IMPORTED_CONFIGURATIONS INTERFACE_INCLUDE_DIRECTORIES INTERFACE_COMPILE_DEFINITIONS INTERFACE_COMPILE_OPTIONS INTERFACE_LINK_LIBRARIES) + get_target_property(_MAGNUM_GLCONTEXT_${property} ${_MAGNUM_GLCONTEXT_ALIAS} ${property}) + if(_MAGNUM_GLCONTEXT_${property}) + set_target_properties(Magnum::GLContext PROPERTIES ${property} + "${_MAGNUM_GLCONTEXT_${property}}") + endif() + endforeach() + get_target_property(_MAGNUM_GLCONTEXT_IMPORTED_LOCATION_RELEASE ${_MAGNUM_GLCONTEXT_ALIAS} IMPORTED_LOCATION_RELEASE) + get_target_property(_MAGNUM_GLCONTEXT_IMPORTED_LOCATION_DEBUG ${_MAGNUM_GLCONTEXT_ALIAS} IMPORTED_LOCATION_DEBUG) + if(_MAGNUM_GLCONTEXT_IMPORTED_LOCATION_RELEASE) + set_target_properties(Magnum::GLContext PROPERTIES + IMPORTED_LOCATION_RELEASE ${_MAGNUM_GLCONTEXT_IMPORTED_LOCATION_RELEASE}) + endif() + if(_MAGNUM_GLCONTEXT_IMPORTED_LOCATION_DEBUG) + set_target_properties(Magnum::GLContext PROPERTIES + IMPORTED_LOCATION_DEBUG ${_MAGNUM_GLCONTEXT_IMPORTED_LOCATION_DEBUG}) + endif() + endif() + # Prevent creating the alias again + unset(_MAGNUM_GLCONTEXT_ALIAS) +endif() + +# Installation and deploy dirs +set(MAGNUM_DEPLOY_PREFIX "." + CACHE STRING "Prefix where to put final application executables") + +include(${CORRADE_LIB_SUFFIX_MODULE}) +set(MAGNUM_BINARY_INSTALL_DIR bin) +set(MAGNUM_LIBRARY_INSTALL_DIR lib${LIB_SUFFIX}) +set(MAGNUM_DATA_INSTALL_DIR share/magnum) +set(MAGNUM_INCLUDE_INSTALL_DIR include/Magnum) +set(MAGNUM_EXTERNAL_INCLUDE_INSTALL_DIR include/MagnumExternal) +set(MAGNUM_PLUGINS_INCLUDE_INSTALL_DIR include/MagnumPlugins) +if(MAGNUM_BUILD_DEPRECATED AND MAGNUM_INCLUDE_INSTALL_PREFIX AND NOT MAGNUM_INCLUDE_INSTALL_PREFIX STREQUAL ".") + message(DEPRECATION "MAGNUM_INCLUDE_INSTALL_PREFIX is obsolete as its primary use was for old Android NDK versions. Please switch to the NDK r19+ layout instead of using this variable and recreate your build directory to get rid of this warning.") + set(MAGNUM_DATA_INSTALL_DIR ${MAGNUM_INCLUDE_INSTALL_PREFIX}/${MAGNUM_DATA_INSTALL_DIR}) + set(MAGNUM_INCLUDE_INSTALL_DIR ${MAGNUM_INCLUDE_INSTALL_PREFIX}/${MAGNUM_INCLUDE_INSTALL_DIR}) + set(MAGNUM_EXTERNAL_INCLUDE_INSTALL_DIR ${MAGNUM_INCLUDE_INSTALL_PREFIX}/${MAGNUM_EXTERNAL_INCLUDE_INSTALL_DIR}) + set(MAGNUM_PLUGINS_INCLUDE_INSTALL_DIR ${MAGNUM_INCLUDE_INSTALL_PREFIX}/${MAGNUM_PLUGINS_INCLUDE_INSTALL_DIR}) +endif() + +set(MAGNUM_PLUGINS_DEBUG_BINARY_INSTALL_DIR ${MAGNUM_BINARY_INSTALL_DIR}/magnum-d) +set(MAGNUM_PLUGINS_DEBUG_LIBRARY_INSTALL_DIR ${MAGNUM_LIBRARY_INSTALL_DIR}/magnum-d) +set(MAGNUM_PLUGINS_RELEASE_BINARY_INSTALL_DIR ${MAGNUM_BINARY_INSTALL_DIR}/magnum) +set(MAGNUM_PLUGINS_RELEASE_LIBRARY_INSTALL_DIR ${MAGNUM_LIBRARY_INSTALL_DIR}/magnum) + +set(MAGNUM_PLUGINS_SHADERCONVERTER_DEBUG_BINARY_INSTALL_DIR ${MAGNUM_PLUGINS_DEBUG_BINARY_INSTALL_DIR}/shaderconverters) +set(MAGNUM_PLUGINS_SHADERCONVERTER_DEBUG_LIBRARY_INSTALL_DIR ${MAGNUM_PLUGINS_DEBUG_LIBRARY_INSTALL_DIR}/shaderconverters) +set(MAGNUM_PLUGINS_SHADERCONVERTER_RELEASE_LIBRARY_INSTALL_DIR ${MAGNUM_PLUGINS_RELEASE_LIBRARY_INSTALL_DIR}/shaderconverters) +set(MAGNUM_PLUGINS_SHADERCONVERTER_RELEASE_BINARY_INSTALL_DIR ${MAGNUM_PLUGINS_RELEASE_BINARY_INSTALL_DIR}/shaderconverters) +set(MAGNUM_PLUGINS_FONT_DEBUG_BINARY_INSTALL_DIR ${MAGNUM_PLUGINS_DEBUG_BINARY_INSTALL_DIR}/fonts) +set(MAGNUM_PLUGINS_FONT_DEBUG_LIBRARY_INSTALL_DIR ${MAGNUM_PLUGINS_DEBUG_LIBRARY_INSTALL_DIR}/fonts) +set(MAGNUM_PLUGINS_FONT_RELEASE_BINARY_INSTALL_DIR ${MAGNUM_PLUGINS_RELEASE_BINARY_INSTALL_DIR}/fonts) +set(MAGNUM_PLUGINS_FONT_RELEASE_LIBRARY_INSTALL_DIR ${MAGNUM_PLUGINS_RELEASE_LIBRARY_INSTALL_DIR}/fonts) +set(MAGNUM_PLUGINS_FONTCONVERTER_DEBUG_BINARY_INSTALL_DIR ${MAGNUM_PLUGINS_DEBUG_BINARY_INSTALL_DIR}/fontconverters) +set(MAGNUM_PLUGINS_FONTCONVERTER_RELEASE_LIBRARY_INSTALL_DIR ${MAGNUM_PLUGINS_RELEASE_LIBRARY_INSTALL_DIR}/fontconverters) +set(MAGNUM_PLUGINS_IMAGECONVERTER_DEBUG_BINARY_INSTALL_DIR ${MAGNUM_PLUGINS_DEBUG_BINARY_INSTALL_DIR}/imageconverters) +set(MAGNUM_PLUGINS_IMAGECONVERTER_DEBUG_LIBRARY_INSTALL_DIR ${MAGNUM_PLUGINS_DEBUG_LIBRARY_INSTALL_DIR}/imageconverters) +set(MAGNUM_PLUGINS_IMAGECONVERTER_RELEASE_LIBRARY_INSTALL_DIR ${MAGNUM_PLUGINS_RELEASE_LIBRARY_INSTALL_DIR}/imageconverters) +set(MAGNUM_PLUGINS_IMAGECONVERTER_RELEASE_BINARY_INSTALL_DIR ${MAGNUM_PLUGINS_RELEASE_BINARY_INSTALL_DIR}/imageconverters) +set(MAGNUM_PLUGINS_IMPORTER_DEBUG_BINARY_INSTALL_DIR ${MAGNUM_PLUGINS_DEBUG_BINARY_INSTALL_DIR}/importers) +set(MAGNUM_PLUGINS_IMPORTER_DEBUG_LIBRARY_INSTALL_DIR ${MAGNUM_PLUGINS_DEBUG_LIBRARY_INSTALL_DIR}/importers) +set(MAGNUM_PLUGINS_IMPORTER_RELEASE_BINARY_INSTALL_DIR ${MAGNUM_PLUGINS_RELEASE_BINARY_INSTALL_DIR}/importers) +set(MAGNUM_PLUGINS_IMPORTER_RELEASE_LIBRARY_INSTALL_DIR ${MAGNUM_PLUGINS_RELEASE_LIBRARY_INSTALL_DIR}/importers) +set(MAGNUM_PLUGINS_SCENECONVERTER_DEBUG_BINARY_INSTALL_DIR ${MAGNUM_PLUGINS_DEBUG_BINARY_INSTALL_DIR}/sceneconverters) +set(MAGNUM_PLUGINS_SCENECONVERTER_DEBUG_LIBRARY_INSTALL_DIR ${MAGNUM_PLUGINS_DEBUG_LIBRARY_INSTALL_DIR}/sceneconverters) +set(MAGNUM_PLUGINS_SCENECONVERTER_RELEASE_LIBRARY_INSTALL_DIR ${MAGNUM_PLUGINS_RELEASE_LIBRARY_INSTALL_DIR}/sceneconverters) +set(MAGNUM_PLUGINS_SCENECONVERTER_RELEASE_BINARY_INSTALL_DIR ${MAGNUM_PLUGINS_RELEASE_BINARY_INSTALL_DIR}/sceneconverters) +set(MAGNUM_PLUGINS_AUDIOIMPORTER_DEBUG_BINARY_INSTALL_DIR ${MAGNUM_PLUGINS_DEBUG_BINARY_INSTALL_DIR}/audioimporters) +set(MAGNUM_PLUGINS_AUDIOIMPORTER_DEBUG_LIBRARY_INSTALL_DIR ${MAGNUM_PLUGINS_DEBUG_LIBRARY_INSTALL_DIR}/audioimporters) +set(MAGNUM_PLUGINS_AUDIOIMPORTER_RELEASE_BINARY_INSTALL_DIR ${MAGNUM_PLUGINS_RELEASE_BINARY_INSTALL_DIR}/audioimporters) +set(MAGNUM_PLUGINS_AUDIOIMPORTER_RELEASE_LIBRARY_INSTALL_DIR ${MAGNUM_PLUGINS_RELEASE_LIBRARY_INSTALL_DIR}/audioimporters) + +# Get base plugin directory from main library location. This is *not* PATH, +# because CMake always converts the path to an absolute location internally, +# making it impossible to specify relative paths there. Sorry in advance for +# not having the dir selection button in CMake GUI. +set(MAGNUM_PLUGINS_DEBUG_DIR "" + CACHE STRING "Base directory where to look for Magnum plugins for debug builds") +set(MAGNUM_PLUGINS_RELEASE_DIR "" + CACHE STRING "Base directory where to look for Magnum plugins for release builds") +set(MAGNUM_PLUGINS_DIR "" + CACHE STRING "Base directory where to look for Magnum plugins") + +# Plugin directories. Set only if the above are non-empty. otherwise empty as +# well. +if(MAGNUM_PLUGINS_DIR) + set(MAGNUM_PLUGINS_FONT_DIR ${MAGNUM_PLUGINS_DIR}/fonts) + set(MAGNUM_PLUGINS_FONTCONVERTER_DIR ${MAGNUM_PLUGINS_DIR}/fontconverters) + set(MAGNUM_PLUGINS_IMAGECONVERTER_DIR ${MAGNUM_PLUGINS_DIR}/imageconverters) + set(MAGNUM_PLUGINS_IMPORTER_DIR ${MAGNUM_PLUGINS_DIR}/importers) + set(MAGNUM_PLUGINS_SCENECONVERTER_DIR ${MAGNUM_PLUGINS_DIR}/sceneconverters) + set(MAGNUM_PLUGINS_AUDIOIMPORTER_DIR ${MAGNUM_PLUGINS_DIR}/audioimporters) +endif() +if(MAGNUM_PLUGINS_DEBUG_DIR) + set(MAGNUM_PLUGINS_FONT_DEBUG_DIR ${MAGNUM_PLUGINS_DEBUG_DIR}/fonts) + set(MAGNUM_PLUGINS_FONTCONVERTER_DEBUG_DIR ${MAGNUM_PLUGINS_DEBUG_DIR}/fontconverters) + set(MAGNUM_PLUGINS_IMAGECONVERTER_DEBUG_DIR ${MAGNUM_PLUGINS_DEBUG_DIR}/imageconverters) + set(MAGNUM_PLUGINS_IMPORTER_DEBUG_DIR ${MAGNUM_PLUGINS_DEBUG_DIR}/importers) + set(MAGNUM_PLUGINS_FONT_RELEASE_DIR ${MAGNUM_PLUGINS_RELEASE_DIR}/fonts) + set(MAGNUM_PLUGINS_SCENECONVERTER_DEBUG_DIR ${MAGNUM_PLUGINS_DEBUG_DIR}/sceneconverters) + set(MAGNUM_PLUGINS_AUDIOIMPORTER_DEBUG_DIR ${MAGNUM_PLUGINS_DEBUG_DIR}/audioimporters) +endif() +if(MAGNUM_PLUGINS_RELEASE_DIR) + set(MAGNUM_PLUGINS_FONTCONVERTER_RELEASE_DIR ${MAGNUM_PLUGINS_RELEASE_DIR}/fontconverters) + set(MAGNUM_PLUGINS_IMAGECONVERTER_RELEASE_DIR ${MAGNUM_PLUGINS_RELEASE_DIR}/imageconverters) + set(MAGNUM_PLUGINS_IMPORTER_RELEASE_DIR ${MAGNUM_PLUGINS_RELEASE_DIR}/importers) + set(MAGNUM_PLUGINS_SCENECONVERTER_RELEASE_DIR ${MAGNUM_PLUGINS_RELEASE_DIR}/sceneconverters) + set(MAGNUM_PLUGINS_AUDIOIMPORTER_RELEASE_DIR ${MAGNUM_PLUGINS_RELEASE_DIR}/audioimporters) +endif() + +# Resets CMake policies set at the top of the file to not affect other code. +cmake_policy(POP) \ No newline at end of file diff --git a/scrum/planningDesign/shortHorzion/track_utils/package.xml b/scrum/planningDesign/shortHorzion/track_utils/package.xml new file mode 100644 index 00000000..514be388 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/package.xml @@ -0,0 +1,18 @@ + + + + track_utils + 0.0.0 + TODO: Package description + sivasriram + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/DataTypes.hpp b/scrum/planningDesign/shortHorzion/track_utils/src/DataTypes.hpp new file mode 100644 index 00000000..95273db3 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/DataTypes.hpp @@ -0,0 +1,139 @@ +#pragma once + +#ifndef DATA_TYPES_H +#define DATA_TYPES_H + +//includes +#include +#include +#include +#include + +// Done +namespace planning { +/** + * @brief Class representing a point + * + */ + + +class Angle { +private: + double degrees; + +public: + // Constructor + Angle(double deg = 0.0) : degrees(deg) {} + + // Set angle in degrees + void setDegrees(double deg) { + degrees = deg; + } + + // Get angle in degrees + double getDegrees() const { + return degrees; + } + + // Set angle in radians + void setRadians(double rad) { + degrees = rad * (180.0 / M_PI); + } + + // Get angle in radians + double getRadians() const { + return degrees * (M_PI / 180.0); + } + + // Normalize the angle to be within [0, 360) degrees + void normalize() { + degrees = fmod(degrees, 360.0); + if (degrees < 0) { + degrees += 360.0; + } + } + + Angle difference(Angle other) { + double angle = getDegrees(); + angle -= other.getDegrees(); + return Angle(angle); + } + + void scale(double scalar) { + degrees = degrees*scalar; + } + + // Print the angle + void print() const { + std::cout << "Angle: " << degrees << " degrees (" << getRadians() << " radians)" << std::endl; + } + + friend std::ostream& operator<<(std::ostream& os, const Angle& obj) { + os << "Angle: {degrees: " << obj.getDegrees() << "°, radians: " << obj.getRadians() << "}"; + return os; + } +}; + +struct Point { + double x; + double y; + + template + Point(std::complex z) : x(z.r), y(z.i) {}; + Point(double x = 0, double y = 0) : x(x), y(y) {}; + + double distanceTo(const Point& other) const { + return std::sqrt(std::pow(other.x - x, 2) + std::pow(other.y - y, 2)); + } + + Point operator-(const Point& other) const { + return {x - other.x, y - other.y}; + } + + Point operator+(const Point& other) const { + return {x + other.x, y + other.y}; + } + + Point operator/(int divisor) { + return {x/divisor, y/divisor}; + } + + Point center(const Point& other) const { + return (*this + other)/2; + } + + double dot(const Point& other) const { + return x * other.x + y * other.y; + } + + double mag() const { + return std::sqrt(x * x + y * y); + } + + friend bool operator<(const Point& l, const Point& r) + { + return std::tie(l.x, l.y) < std::tie(r.x, r.y); // keep the same order + } + +}; + + +struct InertialPose { + Point pos; + double curvature = 0; + Angle bearing = 0; +public: + InertialPose(const Point& point) : pos(point) {}; + InertialPose(const Point& point, const double curvature, const Angle bearing) : pos(point), curvature(curvature), bearing(bearing) {}; + + friend bool operator<(const InertialPose& l, const InertialPose& r) + { + return std::tie(l.pos, l.pos) + < std::tie(r.pos, r.pos); // keep the same order + } +}; + + +} + +#endif // DATA_TYPES_H diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/TriangulateCenterPoints.cpp b/scrum/planningDesign/shortHorzion/track_utils/src/TriangulateCenterPoints.cpp new file mode 100644 index 00000000..02eba576 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/TriangulateCenterPoints.cpp @@ -0,0 +1,106 @@ +// #include +// #include +// #include +// #include +// #include +// #include // for std::sqrt, std::hypot +// #include + +// #include "cone.hpp" +// #include "track.hpp" +// #include "util.hpp" +// #include "DataTypes.hpp" + +// // ------------ Begin CGAL Includes ------------ +// #include +// #include +// // --------------------------------------------- + +// // Define a CGAL Kernel +// typedef CGAL::Exact_predicates_inexact_constructions_kernel K; +// typedef CGAL::Delaunay_triangulation_2 Delaunay; +// typedef K::Point_2 Point; + + + +// std::vector triangulateCenterPoints(std::vector &cones) { +// // 1) Build the Delaunay triangulation +// Delaunay dt; +// // Map each CGAL Vertex_handle to the cone's colour +// std::map color_map; + +// for (auto &cone : cones) { +// Delaunay::Vertex_handle vh = dt.insert(Point(cone.getPos().x, cone.getPos().y)); +// color_map[vh] = cone.getConeType(); +// } + +// // 2) Identify valid faces (triangles) that have more than one color among their vertices +// std::vector valid_faces; +// for (auto f = dt.finite_faces_begin(); f != dt.finite_faces_end(); ++f) { +// // Collect the colours of the triangle's vertices +// std::set face_colors; +// for (int i = 0; i < 3; ++i) { +// face_colors.insert(color_map[f->vertex(i)]); +// } +// // If more than one colour is present, keep it +// if (face_colors.size() > 1) { +// valid_faces.push_back(f); +// } +// } + +// // 3) Collect unique midpoints of edges that connect differently-colored cones +// // duplicates are due to same edges being shared between two triangles +// std::set midpoint_set; + +// for (auto &face : valid_faces) { +// // Each face has 3 edges: (v0,v1), (v1,v2), (v2,v0) +// for (int i = 0; i < 3; ++i) { +// Delaunay::Vertex_handle vh1 = face->vertex(i); +// Delaunay::Vertex_handle vh2 = face->vertex((i+1) % 3); + +// if (color_map[vh1] != color_map[vh2]) { +// Point p1 = vh1->point(); +// Point p2 = vh2->point(); +// double mx = (p1.x() + p2.x()) / 2.0; +// double my = (p1.y() + p2.y()) / 2.0; +// midpoint_set.insert(planning::InertialPose(planning::Point(mx, my))); +// } +// } +// } +// auto center_points_vector = planning::set_to_vector(midpoint_set); + +// return center_points_vector; +// } + + +// // int main() { +// // // 1) Define your cone data (just as in Python) +// // std::vector cones = { +// // { 2.0, 0.0, "b" }, +// // { -1.0, 2.0, "y" }, +// // { 3.0, 2.0, "b" }, +// // { 4.5, 3.5, "b" }, +// // { 0.5, 3.5, "y" }, +// // { -2.0, -2.0, "y" }, +// // { 2.0, -2.0, "b" }, +// // { -2.0, -4.0, "y" }, +// // { 2.0, -4.0, "b" }, +// // { -2.0, 0.0, "y" } +// // }; + +// // // 2) Call the function that does all the triangulation & midpoint logic +// // std::set> center_points = triangulateCenterPoints(cones); + +// // // 3) Print the results +// // std::cout << "Unique center points (midpoints of differently-colored edges):\n"; +// // for (auto &pt : center_points) { +// // std::cout << "(" << pt.first << ", " << pt.second << ")\n"; +// // } + +// // //example using matchCentrePoints +// // std::pair candidate = {1.0, 1.0}; +// // auto match = matchCenterPoints(center_points, candidate, 1.0); +// // std::cout << "Candidate (1.0, 1.0) matched or inserted => (" +// // << match.first << ", " << match.second << ")\n"; +// // return 0; +// // } diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/accel_track.hpp b/scrum/planningDesign/shortHorzion/track_utils/src/accel_track.hpp new file mode 100644 index 00000000..f49c940f --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/accel_track.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include "track.hpp" + +using namespace planning; + +class AccelTrack : public Track { +public: + AccelTrack() {}; + + std::pair getEnd() const override { + + return std::pair{Cone{{-30, 0}, BIG_ORANGE}, Cone{{30, 0}, BIG_ORANGE}}; + } + std::pair getStart() const override { + return getEnd(); + } +private: + IntrinsicConeProp main_cone_prop; +}; \ No newline at end of file diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/autocross_track.hpp b/scrum/planningDesign/shortHorzion/track_utils/src/autocross_track.hpp new file mode 100644 index 00000000..c0dc7ce1 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/autocross_track.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include "track.hpp" + +using namespace planning; + +class AutoCrossTrack : public Track { +public: + AutoCrossTrack() {}; + ~AutoCrossTrack() = default; + + std::pair getStart() override; + std::pair getEnd() override { + return {}; + } +}; \ No newline at end of file diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/cone.cpp b/scrum/planningDesign/shortHorzion/track_utils/src/cone.cpp new file mode 100644 index 00000000..10909349 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/cone.cpp @@ -0,0 +1,4 @@ +#include "cone.hpp" + +// int Cone::nextId_ = 0; + diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/cone.hpp b/scrum/planningDesign/shortHorzion/track_utils/src/cone.hpp new file mode 100644 index 00000000..d017d411 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/cone.hpp @@ -0,0 +1,93 @@ +#pragma once + +#ifndef CONE_HPP +#define CONE_HPP + +#include +#include +#include + +#include +#include "DataTypes.hpp" + +// ****************** IMPORTANT ****************** +// Length units are in metres + +namespace planning { + +/** + * @brief Enum for cone types + * + */ +enum ConeType { + BIG_ORANGE, + SMALL_ORANGE, + BLUE, + YELLOW +}; + + +/** + * @brief A fly-weight for storing common properties shared amongst a category + * of instances + * + */ +class IntrinsicConeProp { + +private: + double width; + double height; + +public: + + IntrinsicConeProp(double width, double height) : width(width), height(height) {} + + virtual ~IntrinsicConeProp() {} + + inline double getWidth() { + return width; + } + + inline double getHeight() { + return height; + } + +}; + +class Cone { +public: + static inline int nextId = 0; +private: + + int id; + Point pos; // Position of the cone + ConeType coneType; // Type of the cone + +public: + Cone(const Point pos, const ConeType coneType): id(nextId++), pos(pos), coneType(coneType) {}; + + Cone(const Cone cone) : id(nextId++), pos(cone.pos), coneType(cone.conetype) {}; + + Point getPos() const { + return pos; + } + + int getId() const { + return id; + } + + static int getNextId() { + return nextId; + } + + int getConeType() const { + return coneType; + } + + void setPos(Point newPos) { + pos = newpos; + } +}; +} + +#endif // CONE_HPP diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/constants.hpp b/scrum/planningDesign/shortHorzion/track_utils/src/constants.hpp new file mode 100644 index 00000000..a1fe19fd --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/constants.hpp @@ -0,0 +1,11 @@ +#pragma once + +namespace track_utils { + + const double MATCHING_THRESHOLD = 5.0; + const double PERCEPTION_DELAY = 0.8; // cone detection and classification pipeline latency. + const double RESPONSE_DELAY = 0.2; // planning pipeline latency microseconds + + const double CONE_SPACING = 5; + +} \ No newline at end of file diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/corner.cpp b/scrum/planningDesign/shortHorzion/track_utils/src/corner.cpp new file mode 100644 index 00000000..6a98b0fe --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/corner.cpp @@ -0,0 +1,18 @@ +#include "corner.hpp" +#include + +Point Corner::getStartLeft() const{ + if(!leftBoundary.empty()){ + return leftBoundary.front(); + } else{ + throw std::runtime_error("Left Boundary is empty"); + } +} + +Point Corner::getStartRight() const{ + if(!rightBoundary.empty()){ + return rightBoundary.front(); + } else{ + throw std::runtime_error("Right Boundary is empty"); + } +} diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/corner.hpp b/scrum/planningDesign/shortHorzion/track_utils/src/corner.hpp new file mode 100644 index 00000000..e9d7248f --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/corner.hpp @@ -0,0 +1,26 @@ +#pragma once + +#ifndef CORNER_HPP +#define CORNER_HPP + +#include +#include "DataTypes.hpp" + +using namespace planning; +class Corner{ + +public: + + //attributes + std::vector leftBoundary; + std::vector rightBoundary; + + //constructor + Corner(); + + //methods + Point getStartLeft() const; + Point getStartRight() const; +}; + +#endif // CORNER_HPP \ No newline at end of file diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/curvature_calculator.cpp b/scrum/planningDesign/shortHorzion/track_utils/src/curvature_calculator.cpp new file mode 100644 index 00000000..cf74267c --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/curvature_calculator.cpp @@ -0,0 +1,120 @@ +// #include "track_utils/include/track_utils/curvature_calculator.hpp" + +// namespace track_utils { + +// // Function to calculate curvature given three points +// std::optional calculateCurvature( +// const geometry_msgs::msg::Point& p1, +// const geometry_msgs::msg::Point& p2, +// const geometry_msgs::msg::Point& p3) { + +// // Length of sides of the traingle formed by the three points +// double a = std::hypot(p2.x - p1.x, p2.y - p1.y); +// double b = std::hypot(p3.x - p2.x, p3.y - p2.y); +// double c = std::hypot(p3.x - p1.x, p3.y - p1.y); + +// //semi perimeter and area of triangle +// double s = (a + b + c) / 2.0; +// double area = std::sqrt(s * (s - a) * (s - b) * (s - c)); + +// if (area == 0.0) { +// return std::nullopt; +// } + +// //Heron's formula to calcuate radius +// double radius = (a * b * c) / (4.0 * area); +// double curvature = 1.0 / radius; + +// return curvature; +// } + +// // Function to calculate bearing difference between cones and positions in degrees +// std::optional calculateBearingDifference( +// const geometry_msgs::msg::Point& left_cone, +// const geometry_msgs::msg::Point& right_cone, +// const geometry_msgs::msg::Point& current_position, +// const geometry_msgs::msg::Point& previous_position) { + +// //vector from left cone to right cone +// double x_cone_diff = right_cone.x - left_cone.x; +// double y_cone_diff = right_cone.y - left_cone.y; + +// //finding cone vector's normal +// double normal_x = y_cone_diff; +// double normal_y = -(x_cone_diff); +// double normal_magnitude = std::hypot(normal_x, normal_y); + +// if(normal_magnitude == 0.0){ +// return std::nullopt; +// } + +// //convert cone's normal vector to unit vector +// normal_x /= normal_magnitude; +// normal_y /= normal_magnitude; + +// //calculate car's heading +// double heading_x = current_position.x - previous_position.x; +// double heading_y = current_position.y - previous_position.y; +// double heading_magnitude = std::hypot(heading_x, heading_y); + +// if (heading_magnitude == 0.0){ +// return std::nullopt; +// } + +// //convert heading vector to unit vector +// heading_x /= heading_magnitude; +// heading_y /= heading_magnitude; + +// //calculate dot product and limit range +// double dot_product = normal_x * heading_x + normal_y * heading_y; +// dot_product = std::clamp(dot_product, -1.0, 1.0); + +// //angle between two unit vectors +// double angle_radians = std::acos(dot_product); + +// //return angle in degrees +// return angle_radians * (180/M_PI); + +// } + +// std::optional detectCornerStart( +// const std::vector& upcoming_points) { + +// if (upcoming_points.size() < 3){ +// return std::nullopt; +// } + +// for (size_t i = 0; i < upcoming_points.size() -2; i++){ + +// auto curvature = calculateCurvature( +// upcoming_points[i], +// upcoming_points[i+1], +// upcoming_points[i+2] +// ); + +// if (curvature.has_value() && std::abs(curvature.value()) > CURVATURE_THRESHOLD){ +// return upcoming_points[i]; +// } + +// } + +// return std::nullopt; + +// } + +// //time from how fast we can decelerate +// //different time parameters e.g. delay for info from camera, delay for computing, delay for taking action +// double calculateArcLength(double time, double speed){ +// return time * speed; +// } + +// double determineDistanceTraveled() { +// actionDelay = 1; // time to carry out corrective actions some function of speed. + +// constexpr double const_time_delay = PERCEPTION_DELAY + RESPONSE_DELAY; +// double variable_time_delay = +// } + +// //Function toi calculate corner end point and final bearinng + +// } // namespace track_utils diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/skidpad_track.hpp b/scrum/planningDesign/shortHorzion/track_utils/src/skidpad_track.hpp new file mode 100644 index 00000000..c05f9388 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/skidpad_track.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include "track.hpp" + +using namespace planning; + +class SkidpadTrack : public Track { +public: + SkidpadTrack() {}; + ~SkidpadTrack() = default; + + std::pair getEnd() override; + std::pair getStart() override; +}; diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/track.cpp b/scrum/planningDesign/shortHorzion/track_utils/src/track.cpp new file mode 100644 index 00000000..b2038fa5 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/track.cpp @@ -0,0 +1,942 @@ +#include "track.hpp" +#include "DataTypes.hpp" +#include "constants.hpp" +#include "util.hpp" +#include +#include +#include +#include + + + + +// ------------ Begin CGAL Includes ------------ +#include +#include +// --------------------------------------------- + +// Define a CGAL Kernel +typedef CGAL::Exact_predicates_inexact_constructions_kernel K; +typedef CGAL::Delaunay_triangulation_2 Delaunay; + + +namespace planning { + +class InvalidVectorLengthException : public std::exception { +public: + InvalidVectorLengthException(const char* message) : msg_(message) {} + virtual const char* what() const noexcept override { + return msg_; + } +private: + const char* msg_; +}; + + +Track::~Track() { + // No special cleanup required +}; + + +std::vector Track::getConeVector() const{ + return map_to_vector(coneMap); +} + +/** + * @brief Sets the track as a closed loop (i.e., track forms a complete circuit) + * + * This affects how track progression and nearest neighbor calculations are performed, + * as points at the end of the track connect back to the beginning. + * + */ +void Track::setClosedLoop() { + closedLoop = true; +} + +//DONE +/** + * @brief returns the cones that are within a radius 'range' from a point. + * + * WARNING computationally intensive + * + * @param position + * @param range + * @return std::vector + */ +[[nodiscard]] +std::vector Track::getLocalCones(const Point& position, const uint8_t range) const +{ + std::vector localCones; + for (auto& [id, cone] : coneMap) { + if (position.distanceTo(cone.getPos()) <= range) { + localCones.push_back(cone); + } + } + return localCones; +} + + +/** + * @brief returns a vector of center points that are within a set range + * + * @param point the queried location + * @param range distance within which a center point should be within from a point + * @return std::pair + */ +[[nodiscard]] +std::vector Track::getLocalCenterPoints(const Point& point, const double range) const +{ + std::vector localCenterPoints; + + //Iterate through all the centrepoints in the track + for (const auto& centerPoint : centerPoints){ + + //calculating distance between the queried point and the centrepoint + double distance = point.distanceTo(centerPoint.pos); + + //checking if the distance is within the range + if (distance <= static_cast(range)){ + localCenterPoints.push_back(centerPoint); + } + } + + return localCenterPoints; + +} + + +// /** +// * @brief returns a pair of center points that are the closest to a given point +// * and on either side of the point. +// * +// * @param point the queried location +// * @param range distance within which a center point should be within from a point +// * @return std::pair +// */ +[[nodiscard]] +std::pair, std::optional> Track::getNearestCenterPoints(const Point& point) const { + // Edge case: Empty track + if (centerPoints.empty()) { + return {std::nullopt, std::nullopt}; + } + + // Edge case: Single point track + if (centerPoints.size() == 1) { + return {centerPoints[0], std::nullopt}; + } + + // Find the closest center point + size_t closestIdx = 0; + double minDist = std::numeric_limits::max(); + + for (size_t i = 0; i < centerPoints.size(); ++i) { + double dist = point.distanceTo(centerPoints[i].pos); + if (dist < minDist) { + minDist = dist; + closestIdx = i; + } + } + + // Handle open loop cases + if (!closedLoop) { + // If closest point is first point, check if point is before track + if (closestIdx == 0){ + //if queried point lies between frist two centerpoints, return both centerpoints + if(in_opposing_or_normal_direction(point, centerPoints[0].pos, centerPoints[1].pos)) { + return {centerPoints[0], centerPoints[1]}; + } + //else if both centrepoints lie on the same side, means queried point is before first centerpoint, so return just the first centre point + return {std::nullopt, centerPoints[0]}; + } + + // If closest point is last point, check if point is after track + if (closestIdx == centerPoints.size() - 1){ + //if the last centrepoint and second to last centrepoing lie in either side of the queried point, bot should be returned + if(in_opposing_or_normal_direction(point, centerPoints.back().pos, centerPoints[centerPoints.size() - 2].pos)) { + return {centerPoints[centerPoints.size() - 2], centerPoints.back()}; + } + //else if the lie in the same direction relative wo queried point, queried point must be positioned after the last cenrepoint. so only return the last cp + return {centerPoints.back(), std::nullopt}; + } + } + // These are all closed loop scenarios + + if (closedLoop){ + + if(closestIdx == 0){ + //if queried point lies between frist two centerpoints, return both centerpoints + if(in_opposing_or_normal_direction(point, centerPoints[0].pos, centerPoints[1].pos)) { + return {centerPoints[0], centerPoints[1]}; + } + // if not, then point must lie between the last centrepoint in the vector and the first + return {centerPoints.back(), centerPoints[0] }; + } + + if (closestIdx == centerPoints.size() - 1){ + //if the last centrepoint and second to last centrepoint lie in either side of the queried point, both should be returned + if(in_opposing_or_normal_direction(point, centerPoints.back().pos, centerPoints[centerPoints.size() - 2].pos)) { + return {centerPoints[centerPoints.size() - 2], centerPoints.back()}; + } + //else if the lie in the same direction relative wo queried point, queried point must be positioned after the last cenrepoint, and befoire the first cp, so return the latter two + return {centerPoints.back(), centerPoints[0]}; + } + } + + // Get indices for adjacent points + size_t prevIdx = (closestIdx == 0) ? (closedLoop ? centerPoints.size() - 1 : 0) : closestIdx - 1; + size_t nextIdx = (closestIdx == centerPoints.size() - 1) ? (closedLoop ? 0 : centerPoints.size() - 1) : closestIdx + 1; + + // Check previous point + if (in_opposing_or_normal_direction(point, centerPoints[closestIdx].pos, centerPoints[prevIdx].pos)) { + return {centerPoints[prevIdx], centerPoints[closestIdx]}; + } + + // Check next point + if (in_opposing_or_normal_direction(point, centerPoints[closestIdx].pos, centerPoints[nextIdx].pos)) { + return {centerPoints[closestIdx], centerPoints[nextIdx]}; + } + + // If no opposing points found, return closest point and nearest neighbor + double distToPrev = point.distanceTo(centerPoints[prevIdx].pos); + double distToNext = point.distanceTo(centerPoints[nextIdx].pos); + + if (distToPrev < distToNext) { + return {centerPoints[prevIdx], centerPoints[closestIdx]}; + } else { + return {centerPoints[closestIdx], centerPoints[nextIdx]}; + } +} + +/** + * @brief gets the curvature of the middle (2nd) of 3 consecutive points + * + * @param InertialPoses + * @return std::optional + */ +[[nodiscard]] +double Track::getCurvature(const std::tuple& centerPoints) const +{ + Point p1 = std::get<0>(centerPoints); + Point p2 = std::get<1>(centerPoints); + Point p3 = std::get<2>(centerPoints); + + // Length of sides of the traingle formed by the three points + double a = std::hypot(p2.x - p1.x, p2.y - p1.y); + double b = std::hypot(p3.x - p2.x, p3.y - p2.y); + double c = std::hypot(p3.x - p1.x, p3.y - p1.y); + + //semi perimeter and area of triangle + double s = (a + b + c) / 2.0; + double area = std::sqrt(s * (s - a) * (s - b) * (s - c)); + + if (area == 0.0) { + return 0; + } + + //Heron's formula to calcuate radius + double radius = (a * b * c) / (4.0 * area); + double curvature = 1.0 / radius; + + return curvature; +} + + +/** + * @brief returns the weighted average of the curvature of the nearest center points + * @param point + * @return double + */ +[[nodiscard]] +std::optional Track::getCurvature(const Point& position) const +{ + std::pair, std::optional> nearestCenterPoints = getNearestCenterPoints(position); + + if (nearestCenterPoints.first && nearestCenterPoints.second) { + const auto& firstPoint = nearestCenterPoints.first.value(); + const auto& secondPoint = nearestCenterPoints.second.value(); + + // Use interpolation when both points are available + return interpolate_curvature( + firstPoint.pos, firstPoint.curvature, + secondPoint.pos, secondPoint.curvature, + position + ); + } else if (nearestCenterPoints.first) { + // Use the curvature of the first point when only it is available + return nearestCenterPoints.first.value().curvature; + } else if (nearestCenterPoints.second) { + // Use the curvature of the second point when only it is available + return nearestCenterPoints.second.value().curvature; + } else { + // Return nullopt when no nearby points are found + return std::nullopt; + } +} + +/** + * @brief + * + * @param point F + * @return double + */ +[[nodiscard]] +double Track::getCurvature(const Point& position, const std::pair& nearestCenterPoints) const +{ + return interpolate_curvature(nearestCenterPoints.first.pos, nearestCenterPoints.first.curvature, nearestCenterPoints.second.pos, nearestCenterPoints.second.curvature, position); +} + +/** + * @brief returns the weighted + * + * @param point + * @throws InvalidVectorLengthException + * @return double + */ +[[nodiscard]] +double Track::getCurvature(const Point& position, const std::vector& localCenterPoints) const { + if (localCenterPoints.size() < 2) { + // char size[5] = {((char) localCenterPoints.size())}; + // char* errorMsg = strcat("localCenterPoints.size(): ", size); + // throw InvalidVectorLengthException(errorMsg); + std::cerr << "invalid vector length" << std::endl; + } + return interpolate_curvature(localCenterPoints[0].pos, localCenterPoints[0].curvature, localCenterPoints[1].pos, localCenterPoints[1].curvature, position); +} + + +// WIP +/** + * @brief gets the curvature + * + * @param position + * @param range (metres) + * @return std::optional + */ + [[nodiscard]] +std::optional Track::getLocalCurvature(const Point& position, const uint8_t range) const +{ + // center points should already be sorted. + auto localCenterPoints = getLocalCenterPoints(position, range); + + if (localCenterPoints.size() < 2) { + return std::nullopt; // Need at least 3 points to calculate curvature + } + + // calculate curvature + return interpolate_curvature(localCenterPoints[0].pos, localCenterPoints[0].curvature, localCenterPoints[1].pos, localCenterPoints[1].curvature, position); +} + + +//DONE +/** + * @brief Inserts a new cone into the track's cone map + * + * @param cone Inserts a new cone into the track's cone map + * @throws Prints error message if cone is null or if cone ID already exists + */ +void Track::insertCone(const Cone cone) +{ + int coneID = cone.getId(); + auto result = coneMap.emplace(coneID, cone); + if (!result.second) { + std::cerr << "Error: Cone with ID " << coneID << " already exists." << std::endl; + } +} + + +/** + * @brief Inserts a new center point into the track's centerline, determine + * where is placed (ordered in the vectore), calcualte the curvature of it, and + * the bearning so that the point becomes an inertial pose (having info about the curvature, bearning etc) + * c + * + * + * @param point : coordinates to be added as a center point + */ +void Track::initialiseCenterPoint(std::vector&& points, bool is_closed) { + // Validate input + if (points.size() < 3) { + throw std::invalid_argument("At least 3 points are required to initialize a track."); + } + + // Sort points using nearest neighbor algorithm + nearestNeighbourSort(points); + + // Clear existing center points + centerPoints.clear(); + + // Process points + for (size_t i = 0; i < points.size(); ++i) { + Point a = points[(i == 0 && is_closed) ? points.size() - 1 : i - 1]; + Point b = points[i]; + Point c = points[(i == points.size() - 1 && is_closed) ? 0 : i + 1]; + + std::tuple point_tuple = {a, b, c}; + Angle bearing = getBearing(point_tuple); + double curvature = Track::getCurvature(point_tuple); + + centerPoints.emplace_back(b, curvature, bearing); + } +} + +// /** +// * @brief +// * +// * Uses triangulation algorithm to generate optimal center points based on track boundaries +// * +// * @return std::vector vector of triangulated center points +// */ + +std::vector triangulateCenterPoints(std::vector &cones) { + // 1) Build the Delaunay triangulation + Delaunay dt; + // Map each CGAL Vertex_handle to the cone's colour + std::map color_map; + + for (auto &cone : cones) { + Delaunay::Vertex_handle vh = dt.insert(K::Point_2(cone.getPos().x, cone.getPos().y)); + color_map[vh] = cone.getConeType(); + } + + // 2) Identify valid faces (triangles) that have more than one color among their vertices + std::vector valid_faces; + for (auto f = dt.finite_faces_begin(); f != dt.finite_faces_end(); ++f) { + // Collect the colours of the triangle's vertices + std::set face_colors; + for (int i = 0; i < 3; ++i) { + face_colors.insert(color_map[f->vertex(i)]); + } + // If more than one colour is present, keep it + if (face_colors.size() > 1) { + valid_faces.push_back(f); + } + } + + // 3) Collect unique midpoints of edges that connect differently-colored cones + // duplicates are due to same edges being shared between two triangles + std::set midpoint_set; + + for (auto &face : valid_faces) { + // Each face has 3 edges: (v0,v1), (v1,v2), (v2,v0) + for (int i = 0; i < 3; ++i) { + Delaunay::Vertex_handle vh1 = face->vertex(i); + Delaunay::Vertex_handle vh2 = face->vertex((i+1) % 3); + + if (color_map[vh1] != color_map[vh2]) { + K::Point_2 p1 = vh1->point(); + K::Point_2 p2 = vh2->point(); + double mx = (p1.x() + p2.x()) / 2.0; + double my = (p1.y() + p2.y()) / 2.0; + midpoint_set.insert(planning::InertialPose(planning::Point(mx, my))); + } + } + } + auto center_points_vector = planning::set_to_vector(midpoint_set); + + return center_points_vector; +} + + + +/** + * @brief returns a vector points that didn't match any pre-existing points + * with any currently stored center points. Ie the difference of the two vector of points. + * + * @param points Vector of points to be matched with the track + * @return std::vector : Vector of matched and aligned center points + */ +std::vector Track::matchCenterPoints(const std::vector& points, double threshold) const +{ + // If there are no existing center points, return input points + if (centerPoints.empty()){ + return points; + } + + //if input poins empty, reutrn empty vector + if(points.empty()){ + return std::vector(); + } + + std::vector unmatchedPoints; + + //for each input point check if it matches with existing centerpoint + for (const auto& point : points) { + bool matched = false; + + for (const auto& centerPoint : centerPoints){ + if (point.distanceTo(centerPoint.pos) <= threshold){ + matched = true; + break; + } + } + + if (!matched){ + unmatchedPoints.push_back(point); + } + } + + return unmatchedPoints; + +} + + +// planning::Point matchCenterPoints( +// std::set& center_points, +// const planning::Point& candidate, +// double threshold) +// { +// for (auto &pt : center_points) { +// // Euclidean distance +// double dx = pt.x - candidate.x; +// double dy = pt.y - candidate.y; +// double dist = std::sqrt(dx * dx + dy * dy); + +// if (dist < threshold) { +// // Found a point close enough => return it immediately +// return pt; +// } +// } +// // If we get here, no point was within threshold => insert candidate +// center_points.insert(candidate); +// return candidate; +// } + + +//DONE +/** + * @brief Sorts points in a vector using nearest neighbor algorithm + * + * Reorganizes points so that each point is followed by its nearest unvisited neighbor, + * creating a continuous path through all points. + * + * @param points : Vector of points to be sorted (modified in place) + */ +void Track::nearestNeighbourSort(std::vector& points) +{ + if (points.empty()){ + return; + } + + std::vector sorted; + + + sorted.push_back(points.front()); + points.erase(points.begin()); + + while (!points.empty()) { + auto nearestIt = std::min_element(points.begin(), points.end(), [&](const Point& a, const Point& b) { + return sorted.back().distanceTo(a) < sorted.back().distanceTo(b); + }); + sorted.push_back(*nearestIt); + points.erase(nearestIt); + } + // points = sorted; + + //I made this modification to update original points instead of the local copy + std::swap(points, sorted); +} + + +/** + * @brief Inserts a new point into the center line using nearest neighbor algorithm + * + * Finds the optimal insertion point between existing center points and calculates + * appropriate curvature and bearing values for the new point. + * + * @param point : Point to be inserted into the center line + */ +void Track::nearestNeighbourInsert(const Point& point) +{ + // get insertion location + size_t index = getNearestNeighbourInsertionPoint(point); + + // if (index == 0 || index >= centerPoints.size()){ + // double curvature = 0; + // double bearing = 0; + // InertialPose newCenterPoint{point, curvature, bearing}; + // centerPoints.insert(centerPoints.begin() + index, newCenterPoint); + // return; + // } + + // get curvature + InertialPose cp1 = centerPoints[index-1]; + InertialPose cp2 = centerPoints[index]; + std::pair cps = {cp1, cp2}; + + std::optional curvature_o = getCurvature(point, cps); + + double curvature; + + if (curvature_o) { + curvature = curvature_o.value(); + } + + // get bearing + Angle bearing = getBearing(point, cps); + + InertialPose newCenterPoint{point, curvature, bearing}; + // instantiate a InertialPos + centerPoints.insert(centerPoints.begin() + index, newCenterPoint); +} + +/** + * @brief Given a Point, find the best insertion index in centerPoints so that the order remains as continuous as possible. + * + * @param point + * @return size_t + */ +size_t Track::getNearestNeighbourInsertionPoint(const Point& point) const{ + + //insert at the beginning of the list if center points list is empty + if (centerPoints.empty()){ + return 0; + } + + //if point is closest to the inital center point, isert at the start + if(point.distanceTo(initialCenterPoint) < point.distanceTo(centerPoints.front().pos)){ + return 0; + } + //if point is closest to the new center point, isert at the end + if (point.distanceTo(newCenterPoint) < point.distanceTo(centerPoints.back().pos)){ + return centerPoints.size(); + } + + // make use of allPoints and offset insertion index accordingly + + size_t closestIndex = 0; + double minDistance = std::numeric_limits::max(); + + // Find closest center point that exists + for (size_t i = 0; i < centerPoints.size(); i++){ + double distance = point.distanceTo(centerPoints[i].pos); + if (distance < minDistance){ + minDistance = distance; + closestIndex = i; + } + } + + double prevDistance = centerPoints[closestIndex -1].pos.distanceTo(point); + double nextDistance = (closestIndex + 1 < centerPoints.size()) + ? centerPoints[closestIndex + 1].pos.distanceTo(point) + : std::numeric_limits::max(); + + return (prevDistance < nextDistance) ? closestIndex : closestIndex + 1; +} + + +/** + * @brief retrive local points and return the weighted average of their bearings + * + * @return Angle + */ +[[nodiscard]] +std::optional Track::getLocalBearing(const Point& position, uint8_t radius) const { + + // Retrieve center points within the given radius + auto centerPoints = getLocalCenterPoints(position, radius); + + if (centerPoints.size() < 2) { + return std::nullopt; // Not enough points to compute a meaningful bearing + } + + // Find the two nearest center points + std::pair nearestCenterPoints{centerPoints[0], centerPoints[0]}; + double minDist1 = std::numeric_limits::max(); + double minDist2 = std::numeric_limits::max(); + + bool firstAssigned = false; + bool secondAssigned = false; + + for (const auto& cp : centerPoints){ + double dist = position.distanceTo(cp.pos); + if (dist < minDist1) { + minDist2 = minDist1; + if (firstAssigned) { + nearestCenterPoints.second = nearestCenterPoints.first; + secondAssigned = true; + } + minDist1 = dist; + nearestCenterPoints.first = cp; + firstAssigned = true; + } else if (dist < minDist2) { + minDist2 = dist; + nearestCenterPoints.second = cp; + secondAssigned = true; + } + } + + // Ensure both points are assigned before computing the bearing + if (!firstAssigned || !secondAssigned) { + return std::nullopt; + } + + // Compute the weighted average of the bearings + return std::optional(getBearing(position, nearestCenterPoints)); + +} + +/** + * @brief get the differnece between the heading of the vehichle and the bearing of its current position on the track + * + * @param vehicle + * @return Angle + */ +[[nodiscard]] +std::optional Track::getDifferenceInBearing(const Vehicle& vehicle) const +{ + //find nearest points from vehicle's position + //we should now have vehicles poijnts and two nearest center points + std::optional trackBearingOpt = getBearing(vehicle.pos); + + if (!trackBearingOpt) { + return std::nullopt; + } + + //can probably use getbearing to find the bearing at the vehicle's position + Angle vehicleHeading = vehicle.bearing; + //find the difference between the bearing of the vehcile and it's position. + return trackBearingOpt.value().difference(vehicleHeading); + +} + +/** + * @brief Calculates rate of change of curvature between two center points + * + * @param centerPoints Pair of InertialPose objects representing consecutive track points + * @return double : Rate of change of curvature between the points + */ +[[nodiscard]] +double Track::getROCofCurvature(const std::pair& centerPoints) const +{ + //extract curvatures of the points + double kappa1 = centerPoints.first.curvature; + double kappa2 = centerPoints.second.curvature; + + // calculate arc length between points + double ds = centerPoints.first.pos.distanceTo(centerPoints.second.pos); + + //prevention of div by 0 + if (ds < 1e-6){ + return 0.0; + } + + //calculare ROC (dk/ds): spatial derivative of curvature wrt arc length + return (kappa2 - kappa1) / ds; + +} + +template +struct Circle { + Point center; + double radius; + Circle(std::pair, double>& pair) { + center = pair.first; + radius = pair.second; + } +}; +Angle Track::getBearing(const std::tuple& points) const { + + // find center and radius of a circle that intersects the 3 points + + std::complex z1 = {std::get<0>(points).x, std::get<0>(points).y}; + std::complex z2 = {std::get<1>(points).x, std::get<1>(points).y}; + std::complex z3 = {std::get<2>(points).x, std::get<2>(points).y}; + std::pair, double> circle = circle_from_3_points(z1, z2, z3); + + std::complex center = circle.first; + double radius = circle.second; + + std::complex z = (z2-center); + Angle bearing = std::atan2(-z.imag(),z.real()); //(x,y) ---> (-y,x) + return bearing; +} + +/** + * @brief Calculate the bearing at a given position by finding nearest center points + * @param position Point to calculate bearing at + * @return Angle + */ +[[nodiscard]] +std::optional Track::getBearing(const Point& position) const +{ + // obtaining nearest center points to position + auto nearestPoints = getNearestCenterPoints(position); + //if we don't have valid center pioints then return 0 angle + + if(!nearestPoints.first || ! nearestPoints.second){ + if (nearestPoints.first) { + return nearestPoints.first.value().bearing; + } else if (nearestPoints.second) { + return nearestPoints.second.value().bearing; + } else { + return std::nullopt; + } + } + + std::pair centerPointPair = { + nearestPoints.first.value(), + nearestPoints.second.value() + }; + + return getBearing(position, centerPointPair); +} + +/** + * @brief Calculate the bearing at a position given two nearest center points + * + * @param position Point to calculate bearing at + * @param nearestCenterPoints Pair of nearest center points with their poses + * @return Angle Weighted average bearing based on distances to center points + */ +[[nodiscard]] +Angle Track::getBearing(const Point& position, const std::pair& nearestCenterPoints) const { + + //excracting points from nearestCenterPoints + const auto& point1 = nearestCenterPoints.first.pos; + const auto& point2 = nearestCenterPoints.second.pos; + + //Calcualting the distances from position to each centerpoint + double distanceToPoint1 = position.distanceTo(point1); + double distanceToPoint2 = position.distanceTo(point2); + + const double epsilon = 1e-6; + + //weights based on inverse distnace + double weight1 = 1.0/ (distanceToPoint1 + epsilon); + double weight2 = 1.0 / (distanceToPoint2 + epsilon); + + //normalise wieghts to sum to 1 + double totalWeight = weight1 + weight2; + weight1 /= totalWeight; + weight2 /= totalWeight; + + //Extracting the bearings at each point + Angle bearing1 = nearestCenterPoints.first.bearing; + Angle bearing2 = nearestCenterPoints.second.bearing; + + //Calculate weighted average bearing + double averageDegrees = bearing1.getDegrees() * weight1 + bearing2.getDegrees() * weight2; + + //create and normalise result + Angle result; + result.setDegrees(averageDegrees); + result.normalize(); + + return result; + +} + +/** + * @brief Calculates rate of change of curvature at a specific position + * + * Finds nearest center points and interpolates the rate of change of curvature + * at the given position. + * + * @param position Point at which to calculate rate of change of curvature + * @return double Interpolated rate of change of curvature at the position + */ +[[nodiscard]] +double Track::getROCofCurvature(const Point& position) const +{ + //get nearest center points + auto nearestPoints = getNearestCenterPoints(position); + + //check if we have valid centerpoints: + if (!nearestPoints.first || !nearestPoints.second) { + return 0.0; + } + + // Create pair of InertialPoses + std::pair centerPointPair = { + nearestPoints.first.value(), + nearestPoints.second.value() + }; + + //basic ROC between the two nearest centerpoints + double baseROC = getROCofCurvature(centerPointPair); + + //caluclate weight for interpolation + double dist1 = position.distanceTo(centerPointPair.first.pos); + double dist2 = position.distanceTo(centerPointPair.second.pos); + double totalDist = dist1 + dist2; + + if (totalDist < 1e-6) { + return baseROC; + } + + // Weight the ROC based on relative distances + // When closer to first point, ROC is more influenced by the approaching change + // When closer to second point, ROC is more influenced by the current change + double weight2 = dist1 / totalDist; + double weight1 = 1.0 - weight2; + + //return ROC of curvature at the point influenced by weight + return baseROC * (weight1 + weight2); +} + +/** + * @brief Helper function to calcualte total progression for a given point + * + * @param position + * @return double + */ +[[nodiscard]] +double Track::calculateProgression(const Point& position) const { + + // variable to hold the total distance, determine intial point (i.e + // initalCenterPoint e first element in the centerPoints vector) + double totalDistance = 0.0; + + Point startPoint = (initialCenterPoint.x != 0 && initialCenterPoint.y != 0) ? initialCenterPoint : centerPoints[0].pos; + + //iterate over the centerpoints and sum the distances along the way + for (size_t i = 1; i < centerPoints.size(); i++){ + const Point& prevPoint = centerPoints[i - 1].pos; + const Point& currentPoint = centerPoints[i].pos; + + double segmentDistance = prevPoint.distanceTo(currentPoint); + + //check if pos is between prevPoint and currentPoint + double prevDist = prevPoint.distanceTo(position); + double currentDist = currentPoint.distanceTo(position); + + // If the position is between prevPoint and currentPoint, calculate the partial distance + if (prevDist <= currentDist) { + totalDistance += prevDist; + totalDistance += segmentDistance; + break; + } + + } + return totalDistance; +} + + +/** + * @brief Use helper function to calculate the distance travelled by the vehicle + * from the intial center point + * + * @param vehicle + * @return double + */ +double Track::getProgression(const Vehicle vehicle) { + return calculateProgression(vehicle.pos); +} + +/** + * @brief Use helper function to calculate the distance along the trakc a + * speicifc point is from the intial center point + * + * @param position + * @return double + */ +double Track::getProgression(const Point position) { + return calculateProgression(position); +} + +std::vector Track::getCenterPoints(){ + return this->centerPoints; +} + + +} //end of planning namespace \ No newline at end of file diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/track.hpp b/scrum/planningDesign/shortHorzion/track_utils/src/track.hpp new file mode 100644 index 00000000..dd76c338 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/track.hpp @@ -0,0 +1,97 @@ +#pragma once +#ifndef TRACK_HPP +#define TRACK_HPP + +#include +#include +#include +// #include "planning.hpp" +#include "cone.hpp" +#include "DataTypes.hpp" +#include "constants.hpp" +#include "vehicle.hpp" +#include + +#include + +// tested +// implemented +// wip +// declared +// todo + +// ****************** IMPORTANT ****************** +// Length units are in metres, angles in degrees + +namespace planning { + + constexpr double PI = M_PI; + constexpr double TWO_PI = 2.0 * M_PI; + +class Track{ + + protected: + std::map coneMap; + std::vector centerPoints; + Point newCenterPoint; + Point initialCenterPoint; + bool closedLoop = false; + + public: + Track() = default; + virtual ~Track() = 0; + + virtual std::pair getEnd() const = 0; + virtual std::pair getStart() const = 0; + + void setClosedLoop(); + + std::vector getConeVector() const; + //std::vector getConeVector() const; + std::vector getLocalCones(const Point& position, const uint8_t range) const; // implemented and tested + std::vector getLocalCenterPoints(const Point& point, const double range) const; // implemented and tested + std::pair, std::optional> getNearestCenterPoints(const Point& point) const; //implemented and tested + + double getCurvature(const std::tuple& centerPoints) const; // implemented and tested + std::optional getCurvature(const Point& position) const ; // implemented + double getCurvature(const Point& position, const std::pair& nearestCenterPoints) const; // implemented + double getCurvature(const Point& position, const std::vector& localCenterPoints) const; // implemented + std::optional getLocalCurvature(const Point& position, const uint8_t range) const; //implemented + + Angle getBearing(const std::tuple& points) const; + std::optional getBearing(const Point& position) const; // implemented + Angle getBearing(const Point& position, const std::pair& nearestCenterPoints) const; // implemented + std::optional getLocalBearing(const Point& position, const uint8_t radius) const; // implemented + + std::optional getDifferenceInBearing(const Vehicle& vehicle) const; //implemented + + double getROCofCurvature(const std::pair&) const; //implemented + double getROCofCurvature(const Point&) const; //implemented + + void insertCone(const Cone cone); //implemented + void initialiseCenterPoint(std::vector&& points, bool is_closed); //implemented + std::vector triangulateCenterPoints(std::vector &cones) const; // TODO: Done - Winola to transfer + std::vector matchCenterPoints(const std::vector& points, double threshold) const; //implemented + void nearestNeighbourSort(std::vector& points); //implemented + void nearestNeighbourInsert(const Point& point); //implemented + size_t getNearestNeighbourInsertionPoint(const Point& point) const; //implemented + + double calculateProgression(const Point& position) const; //implemented + double getProgression(const Vehicle vehicle); //implemented + double getProgression(const Point position); + + std::vector getCenterPoints(); //implemented + + /* + + + Track::getCurvatureROCAt(Point): double + + Track::extrapolateTrack(distance: double): std::vector + */ + }; + +} + + + + +#endif // TRACK_HPP \ No newline at end of file diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/trackdrive_track.hpp b/scrum/planningDesign/shortHorzion/track_utils/src/trackdrive_track.hpp new file mode 100644 index 00000000..c52a466f --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/trackdrive_track.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include "track.hpp" + +using namespace planning; + +class TrackdriveTrack : public Track { +public: + TrackdriveTrack() {}; + ~TrackdriveTrack() = default; + + std::pair getEnd() override; + std::pair getStart() override; +} \ No newline at end of file diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/tutorial.cpp b/scrum/planningDesign/shortHorzion/track_utils/src/tutorial.cpp new file mode 100644 index 00000000..7d7bfb8e --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/tutorial.cpp @@ -0,0 +1,121 @@ +// #include + +// #include + +// // lvalue = rvalue +// // at addr 10 +// int a = 1; + +// // ptr to lvalue +// // at addr 5 +// int *b = &a; + + +// // ptr to ptr to lvalue +// // at addr 2 +// int **bp = &b; + +// // reference to an lvalue +// // at addr 10 +// int &c = a; + +// // at addr 55 +// int d = a; + +// // const lvalue +// int const e = 1; + +// // const reference to a const lvalue +// int const &f = e; + +// // const refernce to a lvalue +// int const &g = a; + +// // reference to and rvalue +// int &&h = 1; + + + +// void assign(int &&other) { +// // addr 10 +// a = 5; +// } + + +// class Thing { +// public: +// int a; +// int b = 10; +// bool c = false; + +// Thing() : a(1) {} +// }; + + +// Thing&& getThing() { +// return Thing(); +// } + +// // lvalue = lvalue +// Thing b = getThing(); + + + + + + +// main() { +// // addr 10 +// a = 1; + +// // addr 777 +// int the_other = 1000; + +// assign(std::move(the_other)); + + + +// } + + + + +// int counter() { +// static int count = 0; +// return count++; +// } + + + + +// /* +// >> main.exe +// 0 +// 1 +// 2 +// 3 +// 4 +// 5 +// 6 +// 7 +// 8 +// 9 +// */ + + + +// class Animal { +// static bool fur; +// Animal() { + +// } + +// void disableFur() { +// fur = false; +// } + +// void setFur(bool other_fur) { +// Animal::fur = other_fur; +// } +// }; + diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/util.cpp b/scrum/planningDesign/shortHorzion/track_utils/src/util.cpp new file mode 100644 index 00000000..11eaba79 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/util.cpp @@ -0,0 +1,113 @@ +#include "util.hpp" +namespace planning { + +/** + * @brief determine whether common_vertex point queried_head is behind or on the line segment defined by points common_vertex and projection_head + * + * @param common_vertex :one point in segemnt + * @param projection_head :other point in question + * @param queried_head :point queried + * @return true + * @return false + */ +bool in_opposing_or_normal_direction(const Point& common_vertex, const Point& projection_head, const Point& queried_head) { + Point line_of_projection = projection_head - common_vertex; + Point projecting_line = queried_head - common_vertex; + + double dot = projecting_line.dot(line_of_projection); + + return dot <= 0; // Only check if it's behind or perpendicular +} + +/** + * Determines if point `c` is behind the line segment `line_of_projection`. + * + * A point is considered "behind" the line segment if it lies on the opposite side + * of the line segment as the direction from `a` to `b`. In aprticular Used to make sure that getnearestCenterpoints + * will always returntwo cp's on either side of queried point. If the points form + * an accute angle the points are considered lying in the same direction. If the + * points form a obtuse angle they are considerd lying in the oposite directions. + * + * @param common_vertex: The starting point of the line segment. + * @param fixed_head The head of the line segment beginning from common_vertex. + * @param queried_head The head of the line segment in which to determine + * the relative location. + * @return True if `queried_vertex` is behind the line segment `line_of_projection`, false otherwise. + */ +bool in_opposing_direction(const Point& common_vertex, const Point& fixed_head, const Point& queried_head) { + //TODO: Siva has changed this too: same reasoning as above + + Point line_of_projection = fixed_head - common_vertex; // Direction vector ab + Point projecting_line = queried_head - common_vertex; // Vector ac + + // return v.dot(d) < 0; // Check projection + + double dot = line_of_projection.dot(projecting_line); // cp to determine rel position + return dot < 0; +} + +// Function to get the magnitude of AC's component along AB +double projection_magnitude(const Point& a, const Point& b, const Point& c) { + + + Point ab = b - a; // Vector AB + Point ac = c - a; // Vector AC + + double ab_magnitude = ab.mag(); // Magnitude of AB + if (ab_magnitude == 0) return 0; // Prevent division by zero + + // Calculate the projection magnitude: (ac . ab) / |ab| + return ac.dot(ab) / ab_magnitude; +} + +// Compute Euclidean distance between two points +double distance(const Point& p1, const Point& p2) { + return std::sqrt((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y)); +} + +// Lagrange interpolation of curvature using arc length +// double interpolate_curvature(const Point& A, double kappa_A, +// const Point& B, double kappa_B, +// const Point& C) { +// // Compute arc-length distances +// double s_A = 0; // Reference point +// double s_B = A.distanceTo(B); +// double s_C = A.distanceTo(C); + +// // Lagrange interpolation formula using arc-length +// return kappa_A * (s_C - s_B) / (s_A - s_B) + +// kappa_B * (s_C - s_A) / (s_B - s_A); +// } +// Lagrange interpolation of curvature using arc length and handling edge cases properly +double interpolate_curvature(const Point& A, double kappa_A, + const Point& B, double kappa_B, + const Point& C) { + // Handle the special case: A and B are the same point + if (A.distanceTo(B) < 1e-10) { + return kappa_A; + } + + // Compute arc-length distances correctly handling points before A + double s_A = 0; // Reference point + double s_B = A.distanceTo(B); + + // Instead of just distance, use projection to handle points before A correctly + double proj_magnitude = projection_magnitude(A, B, C); + double s_C = proj_magnitude * s_B / A.distanceTo(B); + + // Check if C is behind A (in the opposite direction of B) + if (in_opposing_direction(A, B, C)) { + // If C is behind A, we need to make s_C negative + s_C = -A.distanceTo(C); + } else { + // If C is in the same direction as B, use the projection + s_C = proj_magnitude; + } + + // Lagrange interpolation formula using arc-length + return kappa_A * (s_C - s_B) / (s_A - s_B) + + kappa_B * (s_C - s_A) / (s_B - s_A); +} + + +} \ No newline at end of file diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/util.hpp b/scrum/planningDesign/shortHorzion/track_utils/src/util.hpp new file mode 100644 index 00000000..8636b894 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/util.hpp @@ -0,0 +1,60 @@ +#ifndef UTIL_HPP +#define UTIL_HPP + +#include +#include +#include +#include +#include "DataTypes.hpp" + + +namespace planning { + bool in_opposing_or_normal_direction(const Point& p1, const Point& p2, const Point& p3); //implemented and tested + bool in_opposing_direction(const Point& p1, const Point& p2, const Point& p3); //implemented and tested + double projection_magnitude(const Point& p1, const Point& p2, const Point& p3); //implemented and tested + double distance(const Point& p1, const Point& p2); //implemented and tested + double interpolate_curvature(const Point& p1, double curvature1, const Point& p2, double curvature2, const Point& p3); //implemented and tested + + template + std::vector set_to_vector(const std::set& set) { + std::vector vector; + vector.reserve(set.size()); + + std::copy(set.begin(), set.end(), std::back_inserter(vector)); + return vector; + } + + template + std::vector map_to_vector(const std::map& map) { + std::vector vector; + vector.reserve(map.size()); + + for (const auto& pair : map) { + vector.push_back(pair.second); + } + return vector; + } + + template + std::pair, T> circle_from_3_points(std::complex z1, std::complex z2, std::complex z3) { + + if ((z1 == z2) || (z2 == z3) || (z3 == z1)) { + // error + } + + std::complex w = (z3 - z1)/(z2 - z1); + + // small tolerance for floating point comparisons + if (abs(w.imag()) <= 0.000001) { + // error + } + std::complex j2{0,2}; + std::complex c = (z2 - z1)*(w - pow(abs(w), 2))/(j2*w.imag()) + z1; + T r = abs(z1 - c); + + return {c, r}; //centre of circle c ( as a complex), radius of circle r (as a real number (T)) + } + +} + +#endif //UTIL_HPP \ No newline at end of file diff --git a/scrum/planningDesign/shortHorzion/track_utils/src/vehicle.hpp b/scrum/planningDesign/shortHorzion/track_utils/src/vehicle.hpp new file mode 100644 index 00000000..eef4889e --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/src/vehicle.hpp @@ -0,0 +1,111 @@ +#pragma once + +#include "DataTypes.hpp" +#include +#include +#include +#include +#include + +namespace planning { + +/** + * @brief A data class for storing details about a vehicles position and + * + */ +struct Vehicle { + Point pos; + Angle bearing; + double velocity; + Angle angular_velocity; + + + //timestamp of the last update + rclcpp::Time lastUpdateTime; + + //Default constructor + Vehicle(): pos(0.0, 0.0), bearing(0.0), velocity(0.0), angular_velocity(0.0) {} + + /* + * @brief update the vehicle's state from posestamped message + * + * @param poseMsg The pose message containing position and orientation data + */ + void updateFromPose(const geometry_msgs::msg::PoseStamped& poseMsg){ + //update position + pos.x = poseMsg.pose.position.x; + pos.y = poseMsg.pose.position.y; + + //extract bearing from quaternion + tf2::Quaternion q( + poseMsg.pose.orientation.x, + poseMsg.pose.orientation.y, + poseMsg.pose.orientation.z, + poseMsg.pose.orientation.w); + + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + //to degrees + bearing.setRadians(yaw); + lastUpdateTime = rclcpp::Time(poseMsg.header.stamp); + + } + + /** + * @brief + * + * @return ** void + */ + void updateFromVelocity(){ + double vx = twistMsg.twist.linear.x; + double vy = twistMsg.twist.linear.y; + velocity = std::sqrt(vx* vx + vy * vy); + angular_velocity = twistMsg.twist.angular.z; + + lastUpdateTime = rclcpp::Time(twistMsg.header.stamp); + + } + + const Point& getPostion const{ + return pos; + } + + /** + * @brief Get the current bearing/heading of the vehicle + * + * @return const Angle& Reference to the bearing + */ + const Angle& getHeading() const { + return bearing; + } + + /** + * @brief Get the current velocity magnitude + * + * @return double The velocity in m/s + */ + double getVelocity() const { + return velocity; + } + + /** + * @brief Get the current angular velocity + * + * @return double The angular velocity in rad/s + */ + double getAngularVelocity() const { + return angular_velocity; + } + + /** + * @brief Get the timestamp of the last update + * + * @return const rclcpp::Time& Reference to the timestamp + */ + const rclcpp::Time& getLastUpdateTime() const { + return lastUpdateTime; + } +}; + + +} \ No newline at end of file diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/CMakeDirectoryInformation.cmake b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/CMakeDirectoryInformation.cmake new file mode 100644 index 00000000..b7f4f8d6 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/CMakeDirectoryInformation.cmake @@ -0,0 +1,16 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 3.22 + +# Relative path conversion top directories. +set(CMAKE_RELATIVE_PATH_TOP_SOURCE "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils") +set(CMAKE_RELATIVE_PATH_TOP_BINARY "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils") + +# Force unix paths in dependencies. +set(CMAKE_FORCE_UNIX_PATHS 1) + + +# The C and CXX include file regular expressions for this directory. +set(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$") +set(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$") +set(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN}) +set(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN}) diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/DependInfo.cmake b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/DependInfo.cmake new file mode 100644 index 00000000..44c670f7 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/DependInfo.cmake @@ -0,0 +1,27 @@ + +# Consider dependencies only in project. +set(CMAKE_DEPENDS_IN_PROJECT_ONLY OFF) + +# The set of languages for which implicit dependencies are needed: +set(CMAKE_DEPENDS_LANGUAGES + ) + +# The set of dependency files which are needed: +set(CMAKE_DEPENDS_DEPENDENCY_FILES + "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/TriangulateCenterPoints.cpp" "test/CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.o" "gcc" "test/CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.o.d" + "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/cone.cpp" "test/CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.o" "gcc" "test/CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.o.d" + "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/corner.cpp" "test/CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.o" "gcc" "test/CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.o.d" + "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/curvature_calculator.cpp" "test/CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.o" "gcc" "test/CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.o.d" + "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/track.cpp" "test/CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.o" "gcc" "test/CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.o.d" + "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/tutorial.cpp" "test/CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.o" "gcc" "test/CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.o.d" + "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/util.cpp" "test/CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.o" "gcc" "test/CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.o.d" + "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test/track_test.cpp" "test/CMakeFiles/planning_unit_tests.dir/track_test.cpp.o" "gcc" "test/CMakeFiles/planning_unit_tests.dir/track_test.cpp.o.d" + ) + +# Targets to which this target links. +set(CMAKE_TARGET_LINKED_INFO_FILES + "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/CMakeFiles/track_utils_lib.dir/DependInfo.cmake" + ) + +# Fortran module output directory. +set(CMAKE_Fortran_TARGET_MODULE_DIR "") diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/build.make b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/build.make new file mode 100644 index 00000000..34048c04 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/build.make @@ -0,0 +1,227 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 3.22 + +# Delete rule output on recipe failure. +.DELETE_ON_ERROR: + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Disable VCS-based implicit rules. +% : %,v + +# Disable VCS-based implicit rules. +% : RCS/% + +# Disable VCS-based implicit rules. +% : RCS/%,v + +# Disable VCS-based implicit rules. +% : SCCS/s.% + +# Disable VCS-based implicit rules. +% : s.% + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Command-line flag to silence nested $(MAKE). +$(VERBOSE)MAKESILENT = -s + +#Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E rm -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils + +# Include any dependencies generated for this target. +include test/CMakeFiles/planning_unit_tests.dir/depend.make +# Include any dependencies generated by the compiler for this target. +include test/CMakeFiles/planning_unit_tests.dir/compiler_depend.make + +# Include the progress variables for this target. +include test/CMakeFiles/planning_unit_tests.dir/progress.make + +# Include the compile flags for this target's objects. +include test/CMakeFiles/planning_unit_tests.dir/flags.make + +test/CMakeFiles/planning_unit_tests.dir/track_test.cpp.o: test/CMakeFiles/planning_unit_tests.dir/flags.make +test/CMakeFiles/planning_unit_tests.dir/track_test.cpp.o: test/track_test.cpp +test/CMakeFiles/planning_unit_tests.dir/track_test.cpp.o: test/CMakeFiles/planning_unit_tests.dir/compiler_depend.ts + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/CMakeFiles --progress-num=$(CMAKE_PROGRESS_1) "Building CXX object test/CMakeFiles/planning_unit_tests.dir/track_test.cpp.o" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -MD -MT test/CMakeFiles/planning_unit_tests.dir/track_test.cpp.o -MF CMakeFiles/planning_unit_tests.dir/track_test.cpp.o.d -o CMakeFiles/planning_unit_tests.dir/track_test.cpp.o -c /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test/track_test.cpp + +test/CMakeFiles/planning_unit_tests.dir/track_test.cpp.i: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/planning_unit_tests.dir/track_test.cpp.i" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test/track_test.cpp > CMakeFiles/planning_unit_tests.dir/track_test.cpp.i + +test/CMakeFiles/planning_unit_tests.dir/track_test.cpp.s: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/planning_unit_tests.dir/track_test.cpp.s" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test/track_test.cpp -o CMakeFiles/planning_unit_tests.dir/track_test.cpp.s + +test/CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.o: test/CMakeFiles/planning_unit_tests.dir/flags.make +test/CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.o: src/TriangulateCenterPoints.cpp +test/CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.o: test/CMakeFiles/planning_unit_tests.dir/compiler_depend.ts + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/CMakeFiles --progress-num=$(CMAKE_PROGRESS_2) "Building CXX object test/CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.o" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -MD -MT test/CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.o -MF CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.o.d -o CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.o -c /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/TriangulateCenterPoints.cpp + +test/CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.i: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.i" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/TriangulateCenterPoints.cpp > CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.i + +test/CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.s: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.s" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/TriangulateCenterPoints.cpp -o CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.s + +test/CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.o: test/CMakeFiles/planning_unit_tests.dir/flags.make +test/CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.o: src/cone.cpp +test/CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.o: test/CMakeFiles/planning_unit_tests.dir/compiler_depend.ts + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/CMakeFiles --progress-num=$(CMAKE_PROGRESS_3) "Building CXX object test/CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.o" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -MD -MT test/CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.o -MF CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.o.d -o CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.o -c /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/cone.cpp + +test/CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.i: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.i" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/cone.cpp > CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.i + +test/CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.s: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.s" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/cone.cpp -o CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.s + +test/CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.o: test/CMakeFiles/planning_unit_tests.dir/flags.make +test/CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.o: src/corner.cpp +test/CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.o: test/CMakeFiles/planning_unit_tests.dir/compiler_depend.ts + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/CMakeFiles --progress-num=$(CMAKE_PROGRESS_4) "Building CXX object test/CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.o" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -MD -MT test/CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.o -MF CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.o.d -o CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.o -c /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/corner.cpp + +test/CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.i: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.i" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/corner.cpp > CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.i + +test/CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.s: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.s" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/corner.cpp -o CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.s + +test/CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.o: test/CMakeFiles/planning_unit_tests.dir/flags.make +test/CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.o: src/curvature_calculator.cpp +test/CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.o: test/CMakeFiles/planning_unit_tests.dir/compiler_depend.ts + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/CMakeFiles --progress-num=$(CMAKE_PROGRESS_5) "Building CXX object test/CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.o" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -MD -MT test/CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.o -MF CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.o.d -o CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.o -c /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/curvature_calculator.cpp + +test/CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.i: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.i" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/curvature_calculator.cpp > CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.i + +test/CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.s: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.s" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/curvature_calculator.cpp -o CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.s + +test/CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.o: test/CMakeFiles/planning_unit_tests.dir/flags.make +test/CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.o: src/track.cpp +test/CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.o: test/CMakeFiles/planning_unit_tests.dir/compiler_depend.ts + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/CMakeFiles --progress-num=$(CMAKE_PROGRESS_6) "Building CXX object test/CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.o" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -MD -MT test/CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.o -MF CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.o.d -o CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.o -c /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/track.cpp + +test/CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.i: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.i" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/track.cpp > CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.i + +test/CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.s: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.s" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/track.cpp -o CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.s + +test/CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.o: test/CMakeFiles/planning_unit_tests.dir/flags.make +test/CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.o: src/tutorial.cpp +test/CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.o: test/CMakeFiles/planning_unit_tests.dir/compiler_depend.ts + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/CMakeFiles --progress-num=$(CMAKE_PROGRESS_7) "Building CXX object test/CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.o" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -MD -MT test/CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.o -MF CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.o.d -o CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.o -c /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/tutorial.cpp + +test/CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.i: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.i" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/tutorial.cpp > CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.i + +test/CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.s: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.s" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/tutorial.cpp -o CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.s + +test/CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.o: test/CMakeFiles/planning_unit_tests.dir/flags.make +test/CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.o: src/util.cpp +test/CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.o: test/CMakeFiles/planning_unit_tests.dir/compiler_depend.ts + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/CMakeFiles --progress-num=$(CMAKE_PROGRESS_8) "Building CXX object test/CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.o" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -MD -MT test/CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.o -MF CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.o.d -o CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.o -c /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/util.cpp + +test/CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.i: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.i" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/util.cpp > CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.i + +test/CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.s: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.s" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/src/util.cpp -o CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.s + +# Object files for target planning_unit_tests +planning_unit_tests_OBJECTS = \ +"CMakeFiles/planning_unit_tests.dir/track_test.cpp.o" \ +"CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.o" \ +"CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.o" \ +"CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.o" \ +"CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.o" \ +"CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.o" \ +"CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.o" \ +"CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.o" + +# External object files for target planning_unit_tests +planning_unit_tests_EXTERNAL_OBJECTS = + +test/planning_unit_tests: test/CMakeFiles/planning_unit_tests.dir/track_test.cpp.o +test/planning_unit_tests: test/CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.o +test/planning_unit_tests: test/CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.o +test/planning_unit_tests: test/CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.o +test/planning_unit_tests: test/CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.o +test/planning_unit_tests: test/CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.o +test/planning_unit_tests: test/CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.o +test/planning_unit_tests: test/CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.o +test/planning_unit_tests: test/CMakeFiles/planning_unit_tests.dir/build.make +test/planning_unit_tests: /usr/lib/x86_64-linux-gnu/libboost_unit_test_framework.so.1.74.0 +test/planning_unit_tests: libtrack_utils_lib.a +test/planning_unit_tests: /usr/lib/x86_64-linux-gnu/libgmpxx.so +test/planning_unit_tests: /usr/lib/x86_64-linux-gnu/libmpfr.so +test/planning_unit_tests: /usr/lib/x86_64-linux-gnu/libgmp.so +test/planning_unit_tests: test/CMakeFiles/planning_unit_tests.dir/link.txt + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --bold --progress-dir=/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/CMakeFiles --progress-num=$(CMAKE_PROGRESS_9) "Linking CXX executable planning_unit_tests" + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/planning_unit_tests.dir/link.txt --verbose=$(VERBOSE) + +# Rule to build all files generated by this target. +test/CMakeFiles/planning_unit_tests.dir/build: test/planning_unit_tests +.PHONY : test/CMakeFiles/planning_unit_tests.dir/build + +test/CMakeFiles/planning_unit_tests.dir/clean: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test && $(CMAKE_COMMAND) -P CMakeFiles/planning_unit_tests.dir/cmake_clean.cmake +.PHONY : test/CMakeFiles/planning_unit_tests.dir/clean + +test/CMakeFiles/planning_unit_tests.dir/depend: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/DependInfo.cmake --color=$(COLOR) +.PHONY : test/CMakeFiles/planning_unit_tests.dir/depend + diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/cmake_clean.cmake b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/cmake_clean.cmake new file mode 100644 index 00000000..8108d1ac --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/cmake_clean.cmake @@ -0,0 +1,25 @@ +file(REMOVE_RECURSE + "CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.o" + "CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.o.d" + "CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.o" + "CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.o.d" + "CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.o" + "CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.o.d" + "CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.o" + "CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.o.d" + "CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.o" + "CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.o.d" + "CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.o" + "CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.o.d" + "CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.o" + "CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.o.d" + "CMakeFiles/planning_unit_tests.dir/track_test.cpp.o" + "CMakeFiles/planning_unit_tests.dir/track_test.cpp.o.d" + "planning_unit_tests" + "planning_unit_tests.pdb" +) + +# Per-language clean rules from dependency scanning. +foreach(lang CXX) + include(CMakeFiles/planning_unit_tests.dir/cmake_clean_${lang}.cmake OPTIONAL) +endforeach() diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/compiler_depend.make b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/compiler_depend.make new file mode 100644 index 00000000..6357a72f --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/compiler_depend.make @@ -0,0 +1,2 @@ +# Empty compiler generated dependencies file for planning_unit_tests. +# This may be replaced when dependencies are built. diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/compiler_depend.ts b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/compiler_depend.ts new file mode 100644 index 00000000..96b9336c --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/compiler_depend.ts @@ -0,0 +1,2 @@ +# CMAKE generated file: DO NOT EDIT! +# Timestamp file for compiler generated dependencies management for planning_unit_tests. diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/depend.make b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/depend.make new file mode 100644 index 00000000..90a4fdd9 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/depend.make @@ -0,0 +1,2 @@ +# Empty dependencies file for planning_unit_tests. +# This may be replaced when dependencies are built. diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/flags.make b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/flags.make new file mode 100644 index 00000000..d907c1dc --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/flags.make @@ -0,0 +1,10 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 3.22 + +# compile CXX with /usr/bin/c++ +CXX_DEFINES = -DBOOST_ALL_NO_LIB -DBOOST_UNIT_TEST_FRAMEWORK_DYN_LINK -DCGAL_USE_GMPXX=1 + +CXX_INCLUDES = -I/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test/../src -I"/home/sivasriram/boost-ver/boost_1_87_0/boost/**" -I/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/INCLUDE_FILES + +CXX_FLAGS = -frounding-math + diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/link.txt b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/link.txt new file mode 100644 index 00000000..274fd02b --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/link.txt @@ -0,0 +1 @@ +/usr/bin/c++ CMakeFiles/planning_unit_tests.dir/track_test.cpp.o CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.o CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.o CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.o CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.o CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.o CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.o CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.o -o planning_unit_tests /usr/lib/x86_64-linux-gnu/libboost_unit_test_framework.so.1.74.0 ../libtrack_utils_lib.a /usr/lib/x86_64-linux-gnu/libgmpxx.so /usr/lib/x86_64-linux-gnu/libmpfr.so /usr/lib/x86_64-linux-gnu/libgmp.so -lmpfr -lgmp diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/progress.make b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/progress.make new file mode 100644 index 00000000..d4f6ce35 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/planning_unit_tests.dir/progress.make @@ -0,0 +1,10 @@ +CMAKE_PROGRESS_1 = 1 +CMAKE_PROGRESS_2 = 2 +CMAKE_PROGRESS_3 = 3 +CMAKE_PROGRESS_4 = 4 +CMAKE_PROGRESS_5 = 5 +CMAKE_PROGRESS_6 = 6 +CMAKE_PROGRESS_7 = 7 +CMAKE_PROGRESS_8 = 8 +CMAKE_PROGRESS_9 = 9 + diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/progress.marks b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/progress.marks new file mode 100644 index 00000000..98d9bcb7 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeFiles/progress.marks @@ -0,0 +1 @@ +17 diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/CMakeLists.txt b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeLists.txt new file mode 100644 index 00000000..d89136d1 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/CMakeLists.txt @@ -0,0 +1,52 @@ +project(test) +cmake_minimum_required(VERSION 3.22) + + +set(Boost_USE_STATIC_LIBS OFF) + +# Specify the Boost version required (you can adjust this depending on your Boost version) +find_package(Boost 1.74 REQUIRED COMPONENTS unit_test_framework) + +# Enable testing +enable_testing() + +file(GLOB SRC_FILES + ${PROJECT_SOURCE_DIR}/../src/*.cpp +) +file(GLOB INCLUDE_FILES + ${PROJECT_SOURCE_DIR}/../src/*.hpp +) + +if(Boost_FOUND) + message(STATUS "BOOST PACKAGE FOUND") + message(STATUS "Boost_INCLUDE_DIRS " ${Boost_INCLUDE_DIRS}) + message(STATUS "Boost_UNIT_TEST_FRAMEWORK_LIBRARY " ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY}) +endif() + +foreach(ITEM IN LISTS SRC_FILES) + message(STATUS "source file: ${ITEM}") +endforeach() +foreach(ITEM IN LISTS INCLUDE_FILES) + message(STATUS "include file: ${ITEM}") +endforeach() + +add_executable(planning_unit_tests + track_test.cpp + ${SRC_FILES} + +) + +target_include_directories(planning_unit_tests PUBLIC + ${PROJECT_SOURCE_DIR}/../src/ + ~/boost-ver/boost_1_87_0/boost/** +) + +target_link_libraries(planning_unit_tests + ${Boost_INCLUDE_DIRS} + # Boost::UnitTestFramework + ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY} + track_utils_lib +) + +# Add a CTest command +add_test(NAME MyClassTest COMMAND planning_unit_tests) \ No newline at end of file diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/CTestTestfile.cmake b/scrum/planningDesign/shortHorzion/track_utils/test/CTestTestfile.cmake new file mode 100644 index 00000000..3cf2dc7d --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/CTestTestfile.cmake @@ -0,0 +1,8 @@ +# CMake generated Testfile for +# Source directory: /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test +# Build directory: /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test +# +# This file includes the relevant testing commands required for +# testing this directory and lists subdirectories to be tested as well. +add_test([=[MyClassTest]=] "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test/planning_unit_tests") +set_tests_properties([=[MyClassTest]=] PROPERTIES _BACKTRACE_TRIPLES "/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test/CMakeLists.txt;52;add_test;/mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test/CMakeLists.txt;0;") diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/Makefile b/scrum/planningDesign/shortHorzion/track_utils/test/Makefile new file mode 100644 index 00000000..31d1fed5 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/Makefile @@ -0,0 +1,420 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 3.22 + +# Default target executed when no arguments are given to make. +default_target: all +.PHONY : default_target + +# Allow only one "make -f Makefile2" at a time, but pass parallelism. +.NOTPARALLEL: + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Disable VCS-based implicit rules. +% : %,v + +# Disable VCS-based implicit rules. +% : RCS/% + +# Disable VCS-based implicit rules. +% : RCS/%,v + +# Disable VCS-based implicit rules. +% : SCCS/s.% + +# Disable VCS-based implicit rules. +% : s.% + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Command-line flag to silence nested $(MAKE). +$(VERBOSE)MAKESILENT = -s + +#Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E rm -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils + +#============================================================================= +# Targets provided globally by CMake. + +# Special rule for the target edit_cache +edit_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "No interactive CMake dialog available..." + /usr/bin/cmake -E echo No\ interactive\ CMake\ dialog\ available. +.PHONY : edit_cache + +# Special rule for the target edit_cache +edit_cache/fast: edit_cache +.PHONY : edit_cache/fast + +# Special rule for the target rebuild_cache +rebuild_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." + /usr/bin/cmake --regenerate-during-build -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) +.PHONY : rebuild_cache + +# Special rule for the target rebuild_cache +rebuild_cache/fast: rebuild_cache +.PHONY : rebuild_cache/fast + +# Special rule for the target list_install_components +list_install_components: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\"" +.PHONY : list_install_components + +# Special rule for the target list_install_components +list_install_components/fast: list_install_components +.PHONY : list_install_components/fast + +# Special rule for the target install +install: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." + /usr/bin/cmake -P cmake_install.cmake +.PHONY : install + +# Special rule for the target install +install/fast: preinstall/fast + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." + /usr/bin/cmake -P cmake_install.cmake +.PHONY : install/fast + +# Special rule for the target install/local +install/local: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." + /usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake +.PHONY : install/local + +# Special rule for the target install/local +install/local/fast: preinstall/fast + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." + /usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake +.PHONY : install/local/fast + +# Special rule for the target install/strip +install/strip: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." + /usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake +.PHONY : install/strip + +# Special rule for the target install/strip +install/strip/fast: preinstall/fast + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." + /usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake +.PHONY : install/strip/fast + +# The main all target +all: cmake_check_build_system + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(CMAKE_COMMAND) -E cmake_progress_start /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/CMakeFiles /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test//CMakeFiles/progress.marks + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 test/all + $(CMAKE_COMMAND) -E cmake_progress_start /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/CMakeFiles 0 +.PHONY : all + +# The main clean target +clean: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 test/clean +.PHONY : clean + +# The main clean target +clean/fast: clean +.PHONY : clean/fast + +# Prepare targets for installation. +preinstall: all + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 test/preinstall +.PHONY : preinstall + +# Prepare targets for installation. +preinstall/fast: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 test/preinstall +.PHONY : preinstall/fast + +# clear depends +depend: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 +.PHONY : depend + +# Convenience name for target. +test/CMakeFiles/planning_unit_tests.dir/rule: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 test/CMakeFiles/planning_unit_tests.dir/rule +.PHONY : test/CMakeFiles/planning_unit_tests.dir/rule + +# Convenience name for target. +planning_unit_tests: test/CMakeFiles/planning_unit_tests.dir/rule +.PHONY : planning_unit_tests + +# fast build rule for target. +planning_unit_tests/fast: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/build +.PHONY : planning_unit_tests/fast + +__/src/TriangulateCenterPoints.o: __/src/TriangulateCenterPoints.cpp.o +.PHONY : __/src/TriangulateCenterPoints.o + +# target to build an object file +__/src/TriangulateCenterPoints.cpp.o: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.o +.PHONY : __/src/TriangulateCenterPoints.cpp.o + +__/src/TriangulateCenterPoints.i: __/src/TriangulateCenterPoints.cpp.i +.PHONY : __/src/TriangulateCenterPoints.i + +# target to preprocess a source file +__/src/TriangulateCenterPoints.cpp.i: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.i +.PHONY : __/src/TriangulateCenterPoints.cpp.i + +__/src/TriangulateCenterPoints.s: __/src/TriangulateCenterPoints.cpp.s +.PHONY : __/src/TriangulateCenterPoints.s + +# target to generate assembly for a file +__/src/TriangulateCenterPoints.cpp.s: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/TriangulateCenterPoints.cpp.s +.PHONY : __/src/TriangulateCenterPoints.cpp.s + +__/src/cone.o: __/src/cone.cpp.o +.PHONY : __/src/cone.o + +# target to build an object file +__/src/cone.cpp.o: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.o +.PHONY : __/src/cone.cpp.o + +__/src/cone.i: __/src/cone.cpp.i +.PHONY : __/src/cone.i + +# target to preprocess a source file +__/src/cone.cpp.i: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.i +.PHONY : __/src/cone.cpp.i + +__/src/cone.s: __/src/cone.cpp.s +.PHONY : __/src/cone.s + +# target to generate assembly for a file +__/src/cone.cpp.s: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/cone.cpp.s +.PHONY : __/src/cone.cpp.s + +__/src/corner.o: __/src/corner.cpp.o +.PHONY : __/src/corner.o + +# target to build an object file +__/src/corner.cpp.o: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.o +.PHONY : __/src/corner.cpp.o + +__/src/corner.i: __/src/corner.cpp.i +.PHONY : __/src/corner.i + +# target to preprocess a source file +__/src/corner.cpp.i: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.i +.PHONY : __/src/corner.cpp.i + +__/src/corner.s: __/src/corner.cpp.s +.PHONY : __/src/corner.s + +# target to generate assembly for a file +__/src/corner.cpp.s: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/corner.cpp.s +.PHONY : __/src/corner.cpp.s + +__/src/curvature_calculator.o: __/src/curvature_calculator.cpp.o +.PHONY : __/src/curvature_calculator.o + +# target to build an object file +__/src/curvature_calculator.cpp.o: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.o +.PHONY : __/src/curvature_calculator.cpp.o + +__/src/curvature_calculator.i: __/src/curvature_calculator.cpp.i +.PHONY : __/src/curvature_calculator.i + +# target to preprocess a source file +__/src/curvature_calculator.cpp.i: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.i +.PHONY : __/src/curvature_calculator.cpp.i + +__/src/curvature_calculator.s: __/src/curvature_calculator.cpp.s +.PHONY : __/src/curvature_calculator.s + +# target to generate assembly for a file +__/src/curvature_calculator.cpp.s: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/curvature_calculator.cpp.s +.PHONY : __/src/curvature_calculator.cpp.s + +__/src/track.o: __/src/track.cpp.o +.PHONY : __/src/track.o + +# target to build an object file +__/src/track.cpp.o: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.o +.PHONY : __/src/track.cpp.o + +__/src/track.i: __/src/track.cpp.i +.PHONY : __/src/track.i + +# target to preprocess a source file +__/src/track.cpp.i: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.i +.PHONY : __/src/track.cpp.i + +__/src/track.s: __/src/track.cpp.s +.PHONY : __/src/track.s + +# target to generate assembly for a file +__/src/track.cpp.s: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/track.cpp.s +.PHONY : __/src/track.cpp.s + +__/src/tutorial.o: __/src/tutorial.cpp.o +.PHONY : __/src/tutorial.o + +# target to build an object file +__/src/tutorial.cpp.o: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.o +.PHONY : __/src/tutorial.cpp.o + +__/src/tutorial.i: __/src/tutorial.cpp.i +.PHONY : __/src/tutorial.i + +# target to preprocess a source file +__/src/tutorial.cpp.i: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.i +.PHONY : __/src/tutorial.cpp.i + +__/src/tutorial.s: __/src/tutorial.cpp.s +.PHONY : __/src/tutorial.s + +# target to generate assembly for a file +__/src/tutorial.cpp.s: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/tutorial.cpp.s +.PHONY : __/src/tutorial.cpp.s + +__/src/util.o: __/src/util.cpp.o +.PHONY : __/src/util.o + +# target to build an object file +__/src/util.cpp.o: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.o +.PHONY : __/src/util.cpp.o + +__/src/util.i: __/src/util.cpp.i +.PHONY : __/src/util.i + +# target to preprocess a source file +__/src/util.cpp.i: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.i +.PHONY : __/src/util.cpp.i + +__/src/util.s: __/src/util.cpp.s +.PHONY : __/src/util.s + +# target to generate assembly for a file +__/src/util.cpp.s: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/__/src/util.cpp.s +.PHONY : __/src/util.cpp.s + +track_test.o: track_test.cpp.o +.PHONY : track_test.o + +# target to build an object file +track_test.cpp.o: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/track_test.cpp.o +.PHONY : track_test.cpp.o + +track_test.i: track_test.cpp.i +.PHONY : track_test.i + +# target to preprocess a source file +track_test.cpp.i: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/track_test.cpp.i +.PHONY : track_test.cpp.i + +track_test.s: track_test.cpp.s +.PHONY : track_test.s + +# target to generate assembly for a file +track_test.cpp.s: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(MAKE) $(MAKESILENT) -f test/CMakeFiles/planning_unit_tests.dir/build.make test/CMakeFiles/planning_unit_tests.dir/track_test.cpp.s +.PHONY : track_test.cpp.s + +# Help Target +help: + @echo "The following are some of the valid targets for this Makefile:" + @echo "... all (the default if no target is provided)" + @echo "... clean" + @echo "... depend" + @echo "... edit_cache" + @echo "... install" + @echo "... install/local" + @echo "... install/strip" + @echo "... list_install_components" + @echo "... rebuild_cache" + @echo "... planning_unit_tests" + @echo "... __/src/TriangulateCenterPoints.o" + @echo "... __/src/TriangulateCenterPoints.i" + @echo "... __/src/TriangulateCenterPoints.s" + @echo "... __/src/cone.o" + @echo "... __/src/cone.i" + @echo "... __/src/cone.s" + @echo "... __/src/corner.o" + @echo "... __/src/corner.i" + @echo "... __/src/corner.s" + @echo "... __/src/curvature_calculator.o" + @echo "... __/src/curvature_calculator.i" + @echo "... __/src/curvature_calculator.s" + @echo "... __/src/track.o" + @echo "... __/src/track.i" + @echo "... __/src/track.s" + @echo "... __/src/tutorial.o" + @echo "... __/src/tutorial.i" + @echo "... __/src/tutorial.s" + @echo "... __/src/util.o" + @echo "... __/src/util.i" + @echo "... __/src/util.s" + @echo "... track_test.o" + @echo "... track_test.i" + @echo "... track_test.s" +.PHONY : help + + + +#============================================================================= +# Special targets to cleanup operation of make. + +# Special rule to run CMake to check the build system integrity. +# No rule that depends on this can have commands that come from listfiles +# because they might be regenerated. +cmake_check_build_system: + cd /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils && $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 +.PHONY : cmake_check_build_system + diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/cmake_install.cmake b/scrum/planningDesign/shortHorzion/track_utils/test/cmake_install.cmake new file mode 100644 index 00000000..7e517c97 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/cmake_install.cmake @@ -0,0 +1,44 @@ +# Install script for directory: /mnt/c/Users/sivas/Desktop/FSAE/autonomous_nighly_clone/autonomous/scrum/planningDesign/shortHorzion/track_utils/test + +# Set the install prefix +if(NOT DEFINED CMAKE_INSTALL_PREFIX) + set(CMAKE_INSTALL_PREFIX "/usr/local") +endif() +string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") + +# Set the install configuration name. +if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) + if(BUILD_TYPE) + string(REGEX REPLACE "^[^A-Za-z0-9_]+" "" + CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") + else() + set(CMAKE_INSTALL_CONFIG_NAME "") + endif() + message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") +endif() + +# Set the component getting installed. +if(NOT CMAKE_INSTALL_COMPONENT) + if(COMPONENT) + message(STATUS "Install component: \"${COMPONENT}\"") + set(CMAKE_INSTALL_COMPONENT "${COMPONENT}") + else() + set(CMAKE_INSTALL_COMPONENT) + endif() +endif() + +# Install shared libraries without execute permission? +if(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) + set(CMAKE_INSTALL_SO_NO_EXE "1") +endif() + +# Is this installation the result of a crosscompile? +if(NOT DEFINED CMAKE_CROSSCOMPILING) + set(CMAKE_CROSSCOMPILING "FALSE") +endif() + +# Set default install directory permissions. +if(NOT DEFINED CMAKE_OBJDUMP) + set(CMAKE_OBJDUMP "/usr/bin/objdump") +endif() + diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/test_curvature_calculator.cpp b/scrum/planningDesign/shortHorzion/track_utils/test/test_curvature_calculator.cpp new file mode 100644 index 00000000..a5ed5d37 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/test_curvature_calculator.cpp @@ -0,0 +1,180 @@ +#include "track_utils/curvature_calculator.hpp" +#include +#include +#include + +int main() { + using geometry_msgs::msg::Point; + + std::cout << "=== Testing Curvature Calculation ===" << std::endl; + // Test for calculateCurvature (existing test case) + Point p1; + p1.x = 0.0; + p1.y = 0.0; + + Point p2; + p2.x = 1.0; + p2.y = 0.0; + + Point p3; + p3.x = 0.0; + p3.y = 1.0; + + auto curvature = track_utils::calculateCurvature(p1, p2, p3); + if (curvature.has_value()) { + std::cout << "Curvature: " << curvature.value() << std::endl; + } else { + std::cout << "Curvature: Undefined" << std::endl; + } + + std::cout << "\n=== Testing Bearing Difference ===" << std::endl; + // Test for calculateBearingDifference + Point left_cone; + left_cone.x = 0.0; + left_cone.y = 0.0; + + Point right_cone; + right_cone.x = 2.0; + right_cone.y = 0.0; + + Point current_position; + current_position.x = 1.0; + current_position.y = 1.0; + + Point previous_position; + previous_position.x = 0.0; + previous_position.y = 1.0; + + auto bearing_difference = track_utils::calculateBearingDifference( + left_cone, right_cone, current_position, previous_position); + + if (bearing_difference.has_value()) { + std::cout << "Bearing Difference: " << bearing_difference.value() << " degrees" << std::endl; + } else { + std::cout << "Bearing Difference: Undefined" << std::endl; + } + + std::cout << "\n=== Testing Corner Detection ===" << std::endl; + // Test cases for corner detection + std::vector track_points; + + // Test Case 1: Straight line followed by curve: + std::cout << "\nTest Case 1: Straight line followed by curve" << std::endl; + track_points.clear(); + + // Add straight line points + for (int i = 0; i < 3; ++i) { + Point p; + p.x = i * 1.0; + p.y = 0.0; + track_points.push_back(p); + std::cout << "Added straight point " << i << ": (" << p.x << ", " << p.y << ")" << std::endl; + } + + // Add curve points + Point curve1; + curve1.x = 3.0; + curve1.y = 0.5; + track_points.push_back(curve1); + std::cout << "Added curve point 1: (" << curve1.x << ", " << curve1.y << ")" << std::endl; + + Point curve2; + curve2.x = 3.5; + curve2.y = 1.0; + track_points.push_back(curve2); + std::cout << "Added curve point 2: (" << curve2.x << ", " << curve2.y << ")" << std::endl; + + std::cout << "Total points in track: " << track_points.size() << std::endl; + + // Check curvature values before corner detection + for (size_t i = 0; i < track_points.size() - 2; ++i) { + auto test_curvature = track_utils::calculateCurvature( + track_points[i], + track_points[i + 1], + track_points[i + 2] + ); + std::cout << "Curvature at point " << i << ": " + << (test_curvature.has_value() ? std::to_string(test_curvature.value()) : "undefined") + << std::endl; + } + + auto corner_start = track_utils::detectCornerStart(track_points); + if (corner_start.has_value()) { + std::cout << "Corner detected at point: (" + << corner_start.value().x << ", " + << corner_start.value().y << ")" << std::endl; + } else { + std::cout << "No corner detected" << std::endl; + } + + // Test Case 2: All straight line points (should not detect corner) + std::cout << "\nTest Case 2: Straight line only" << std::endl; + track_points.clear(); + for (int i = 0; i < 5; ++i) { + Point p; + p.x = i * 1.0; + p.y = 0.0; + track_points.push_back(p); + } + + corner_start = track_utils::detectCornerStart(track_points); + if (corner_start.has_value()) { + std::cout << "Corner detected at point: (" + << corner_start.value().x << ", " + << corner_start.value().y << ")" << std::endl; + } else { + std::cout << "No corner detected" << std::endl; + } + + // Test Case 3: Sharp corner (should definitely detect) + std::cout << "\nTest Case 3: Sharp corner" << std::endl; + track_points.clear(); + + // First two points straight + Point s1; + s1.x = 0.0; + s1.y = 0.0; + track_points.push_back(s1); + + Point s2; + s2.x = 1.0; + s2.y = 0.0; + track_points.push_back(s2); + + // Sharp turn + Point s3; + s3.x = 1.0; + s3.y = 1.0; + track_points.push_back(s3); + + Point s4; + s4.x = 0.0; + s4.y = 1.0; + track_points.push_back(s4); + + corner_start = track_utils::detectCornerStart(track_points); + if (corner_start.has_value()) { + std::cout << "Corner detected at point: (" + << corner_start.value().x << ", " + << corner_start.value().y << ")" << std::endl; + } else { + std::cout << "No corner detected" << std::endl; + } + + // Test Case 4: Insufficient points + std::cout << "\nTest Case 4: Insufficient points" << std::endl; + track_points.clear(); + track_points.push_back(s1); + track_points.push_back(s2); // Only 2 points + + corner_start = track_utils::detectCornerStart(track_points); + if (corner_start.has_value()) { + std::cout << "Corner detected at point: (" + << corner_start.value().x << ", " + << corner_start.value().y << ")" << std::endl; + } else { + std::cout << "No corner detected (expected for insufficient points)" << std::endl; + } + + return 0; +} \ No newline at end of file diff --git a/scrum/planningDesign/shortHorzion/track_utils/test/track_test.cpp b/scrum/planningDesign/shortHorzion/track_utils/test/track_test.cpp new file mode 100644 index 00000000..0f439b32 --- /dev/null +++ b/scrum/planningDesign/shortHorzion/track_utils/test/track_test.cpp @@ -0,0 +1,571 @@ +#define BOOST_TEST_MODULE MyTest +#define BOOST_TEST_LOG_LEVEL message +#include + +#include "util.hpp" +#include "track.hpp" +#include "accel_track.hpp" +#include "DataTypes.hpp" +#include "cone.hpp" +#include +#include +#include + +using namespace planning; + +// Helper function to initialize a track with cones and center points +std::shared_ptr setupTrack() { + auto track = std::make_shared(); + + // Add cones to the track + auto prop_fly_weight = std::make_shared(10); // Width of 10 + track->insertCone(Cone(Point{0, 0}, BIG_ORANGE, prop_fly_weight)); + track->insertCone(Cone(Point{10, 10}, BLUE, prop_fly_weight)); + track->insertCone(Cone(Point{20, 20}, YELLOW, prop_fly_weight)); + track->insertCone(Cone(Point{30, 30}, SMALL_ORANGE, prop_fly_weight)); + + // Add center points to the track + + track->initialiseCenterPoint({ + Point{1500,1600}, + Point{1700,1650}, + Point{1800,1550}, + Point{1850,1400}, + Point{1950,1250}, + Point{2100,1200}, + Point{2250,1400}, + Point{2200,1800}, + Point{2150,2200}, + Point{2200,2500}, + Point{2300,2700}, + Point{2550,2750}, + Point{2750,2700}, + Point{2800,2500}, + Point{2650,2250}, + Point{2650,2050}, + Point{2750,1750}, + Point{3000,1650}, + Point{3200,1700}, + Point{3300,1950}, + Point{3250,2200}, + Point{3200,2600}, + Point{3100,2800}, + Point{2900,3000}, + Point{2700,3200}, + Point{2400,3300}, + Point{2000,3300}, + Point{1700,3250}, + Point{1500,3300}, + Point{1250,3250}, + Point{1150,3000}, + Point{1150,2800}, + Point{1200,2600}, + Point{1150,2300}, + Point{1200,1950}, + Point{1200,1650} + }, true); + + return track; +} + +const double THRESHOLD = 0.00001; + +BOOST_AUTO_TEST_SUITE() + +BOOST_AUTO_TEST_CASE(first_test) +{ + using namespace planning; + int i = 1; + BOOST_TEST(i); + BOOST_TEST(i == 1); +} + + + +BOOST_AUTO_TEST_CASE(actual_test) +{ + using namespace planning; + auto prop_fly_weight_1 = IntrinsicConeProp(10);// width of 10 + + auto accel_track = AccelTrack(); + + Cone cone = Cone(Point{10,10}, BLUE, prop_fly_weight_1); + + accel_track.insertCone(cone); + + std::vector retrieved_cones = accel_track.getLocalCones({10,10}, 5);; + + BOOST_CHECK_EQUAL(retrieved_cones.size(), 1); + +} + +BOOST_AUTO_TEST_CASE(test_in_opposing_or_normal_direction) { + using namespace planning; + + // Case 1: Queried point is behind the line segment but fully along the same line + { + Point common_vertex(0, 0); + Point projection_head(1, 0); + Point queried_head(-1, 0); + bool result = in_opposing_or_normal_direction(common_vertex, projection_head, queried_head); + BOOST_TEST_MESSAGE("Test Case 1: Queried point is behind the line segment and colinear"); + BOOST_TEST_MESSAGE("Expected: true, Actual: " << std::boolalpha << result); + BOOST_CHECK(result); + } + + // Case 2: Queried point is in front of the line segment but fully along the same line + { + Point common_vertex(0, 0); + Point projection_head(1, 0); + Point queried_head(2, 0); + bool result = in_opposing_or_normal_direction(common_vertex, projection_head, queried_head); + BOOST_TEST_MESSAGE("Test Case 2: Queried point is in front of the line segment and colinear"); + BOOST_TEST_MESSAGE("Expected: false, Actual: " << std::boolalpha << result); + BOOST_CHECK(!result); + } + + // Case 3: Queried point is on the line segment but between teh other two points + { + Point common_vertex(0, 0); + Point projection_head(1, 0); + Point queried_head(0.5, 0); + bool result = in_opposing_or_normal_direction(common_vertex, projection_head, queried_head); + BOOST_TEST_MESSAGE("Test Case 3: Queried point is on the line segment but between the other two points"); + BOOST_TEST_MESSAGE("Expected: false, Actual: " << std::boolalpha << result); + BOOST_CHECK(!result); + } + + // Case 4: Queried point is on the line segment with magnitude less than the vector of the other points + { + Point common_vertex(0, 0); + Point projection_head(1, 0); + Point queried_head(-0.5, 0); + bool result = in_opposing_or_normal_direction(common_vertex, projection_head, queried_head); + BOOST_TEST_MESSAGE("Test Case 3: Queried point is on the line segment bwith magnitude less than the vector of the other point"); + BOOST_TEST_MESSAGE("Expected: true, Actual: " << std::boolalpha << result); + BOOST_CHECK(result); + } + + // Case 5: Queried point is perpendicular to the line segment + { + Point common_vertex(0, 0); + Point projection_head(1, 0); + Point queried_head(0, 1); + bool result = in_opposing_or_normal_direction(common_vertex, projection_head, queried_head); + BOOST_TEST_MESSAGE("Test Case 5: Queried point is perpendicular to the line segment"); + BOOST_TEST_MESSAGE("Expected: true, Actual: " << std::boolalpha << result); + BOOST_CHECK(result); + } + + // Case 6: Queried point is at the common vertex + { + Point common_vertex(0, 0); + Point projection_head(1, 0); + Point queried_head(0, 0); + bool result = in_opposing_or_normal_direction(common_vertex, projection_head, queried_head); + BOOST_TEST_MESSAGE("Test Case 6: Queried point is at the common vertex"); + BOOST_TEST_MESSAGE("Expected: true, Actual: " << std::boolalpha << result); + BOOST_CHECK(result); + } + + //Case 7 : Queried point is in front of the common vertex but not co-linear + { + Point common_vertex(0, 0); + Point projection_head(1, 0); + Point queried_head(0.3, 1.2); + bool result = in_opposing_or_normal_direction(common_vertex, projection_head, queried_head); + BOOST_TEST_MESSAGE("Test Case 7: Queried point is in front of the common vertex but not co-linear"); + BOOST_TEST_MESSAGE("Expected: false, Actual: " << std::boolalpha << result); + BOOST_CHECK(!result); + } + + //Case 8 : Queried point is behind the common vertex but not co-linear + { + Point common_vertex(0, 0); + Point projection_head(1, 0); + Point queried_head(-0.6, 0.45); + bool result = in_opposing_or_normal_direction(common_vertex, projection_head, queried_head); + BOOST_TEST_MESSAGE("Test Case 8: Queried point is in front of the common vertex but not co-linear"); + BOOST_TEST_MESSAGE("Expected: true, Actual: " << std::boolalpha << result); + BOOST_CHECK(result); + } + +} + +BOOST_AUTO_TEST_CASE(test_projection_magnitude) +{ + using namespace planning; + // Case 1: Normal projection + Point a(0, 0); + Point b(4, 0); + Point c(2, 3); + BOOST_CHECK_CLOSE(projection_magnitude(a, b, c), 2.0, 1e-6); + + // Case 2: Point C is on AB + Point d(3, 0); + BOOST_CHECK_CLOSE(projection_magnitude(a, b, d), 3.0, 1e-6); + + // Case 3: A and B are the same (degenerate case) + Point e(0, 0); + BOOST_CHECK_EQUAL(projection_magnitude(a, e, c), 0.0); + + // Case 4: Projection is negative (C before A) + Point f(-2, 1); + BOOST_CHECK_CLOSE(projection_magnitude(a, b, f), -2.0, 1e-6); + + //Case 5: B comes before A (reverse vector case) + BOOST_CHECK_CLOSE(projection_magnitude(b, a, c), 2.0, 1e-6); +} + +BOOST_AUTO_TEST_CASE(test_interpolate_curvature) +{ + using namespace planning; + + // Helper function to print test case details + auto print_test_case = [](int case_num, const std::string& description, double expected, double actual) { + BOOST_TEST_MESSAGE("Test Case " << case_num << ": " << description); + BOOST_TEST_MESSAGE("Expected: " << expected << ", Actual: " << actual); + }; + + // Case 1: Simple linear interpolation - midpoint + { + Point A(0, 0); + double kappa_A = 1.0; + Point B(10, 0); + double kappa_B = 2.0; + Point C(5, 0); // Midpoint + + double expected = 1.5; // Halfway between 1.0 and 2.0 + double result = interpolate_curvature(A, kappa_A, B, kappa_B, C); + print_test_case(1, "Simple linear interpolation - midpoint", expected, result); + BOOST_CHECK_CLOSE(result, expected, THRESHOLD); + } + + // Case 2: Simple linear interpolation - 75% of the way + { + Point A(0, 0); + double kappa_A = 1.0; + Point B(10, 0); + double kappa_B = 2.0; + Point C(7.5, 0); // 75% of the way from A to B + + double expected = 1.75; // 75% of the way from 1.0 to 2.0 + double result = interpolate_curvature(A, kappa_A, B, kappa_B, C); + print_test_case(2, "Simple linear interpolation - 75% of the way", expected, result); + BOOST_CHECK_CLOSE(result, expected, THRESHOLD); + } + + // Case 3: Interpolation with different curvature signs + { + Point A(0, 0); + double kappa_A = -1.0; + Point B(10, 0); + double kappa_B = 1.0; + Point C(5, 0); // Midpoint + + double expected = 0.0; // Halfway between -1.0 and 1.0 + double result = interpolate_curvature(A, kappa_A, B, kappa_B, C); + print_test_case(3, "Interpolation with different curvature signs", expected, result); + BOOST_CHECK_CLOSE(result, expected, THRESHOLD); + } + + // Case 4: Interpolation with extreme curvature values + { + Point A(0, 0); + double kappa_A = 0.0; + Point B(10, 0); + double kappa_B = 100.0; + Point C(2, 0); // 20% of the way from A to B + + double expected = 20.0; // 20% of the way from 0.0 to 100.0 + double result = interpolate_curvature(A, kappa_A, B, kappa_B, C); + print_test_case(4, "Interpolation with extreme curvature values", expected, result); + BOOST_CHECK_CLOSE(result, expected, THRESHOLD); + } + + // Case 5: Interpolation in 2D space + { + Point A(0, 0); + double kappa_A = 1.0; + Point B(10, 10); + double kappa_B = 3.0; + Point C(5, 5); // Midpoint in 2D space + + double expected = 2.0; // Halfway between 1.0 and 3.0 + double result = interpolate_curvature(A, kappa_A, B, kappa_B, C); + print_test_case(5, "Interpolation in 2D space", expected, result); + BOOST_CHECK_CLOSE(result, expected, THRESHOLD); + } + + // Case 6: Interpolation beyond point B + { + Point A(0, 0); + double kappa_A = 1.0; + Point B(10, 0); + double kappa_B = 2.0; + Point C(15, 0); // Beyond point B + + double expected = 2.5; // Extrapolated value + double result = interpolate_curvature(A, kappa_A, B, kappa_B, C); + print_test_case(6, "Interpolation beyond point B", expected, result); + BOOST_CHECK_CLOSE(result, expected, THRESHOLD); + } + + // Case 7: Interpolation before point A + { + Point A(0, 0); + double kappa_A = 1.0; + Point B(10, 0); + double kappa_B = 2.0; + Point C(-5, 0); // Before point A + + double expected = 0.5; // Extrapolated value + double result = interpolate_curvature(A, kappa_A, B, kappa_B, C); + print_test_case(7, "Interpolation before point A", expected, result); + BOOST_CHECK_CLOSE(result, expected, THRESHOLD); + } + + // Case 8: Special case - A and B are the same point + { + Point A(0, 0); + double kappa_A = 1.0; + Point B(0, 0); // Same as A + double kappa_B = 2.0; + Point C(5, 0); + + // When A and B are the same, the function should return kappa_A + double expected = kappa_A; + double result = interpolate_curvature(A, kappa_A, B, kappa_B, C); + print_test_case(8, "Special case - A and B are the same point", expected, result); + BOOST_CHECK_CLOSE(result, expected, THRESHOLD); + } +} + +BOOST_AUTO_TEST_CASE(test_getLocalCones) { + auto track = setupTrack(); + + // Test case 1: Retrieve cones within range of (10, 10) + std::vector cones1 = track->getLocalCones(Point{10, 10}, 15); // Range of 15 + BOOST_CHECK_EQUAL(cones1.size(), 3); // Expect 3 cones: (0, 0), (10, 10), and (20, 20) + + // Verify the positions of retrieved cones + std::vector expected_positions1 = {Point{0, 0}, Point{10, 10}, Point{20, 20}}; + for (const auto& cone : cones1) { + bool found = false; + for (const auto& expected_pos : expected_positions1) { + if (cone.getPos().distanceTo(expected_pos) < THRESHOLD) { + found = true; + break; + } + } + BOOST_CHECK(found); + } + + // Test case 2: Retrieve cones within range of (25, 25) + std::vector cones2 = track->getLocalCones(Point{25, 25}, 10); // Range of 10 + BOOST_CHECK_EQUAL(cones2.size(), 2); // Expect 2 cones: (20, 20) and (30, 30) + + // Verify the positions of retrieved cones + std::vector expected_positions2 = {Point{20, 20}, Point{30, 30}}; + for (const auto& cone : cones2) { + bool found = false; + for (const auto& expected_pos : expected_positions2) { + if (cone.getPos().distanceTo(expected_pos) < THRESHOLD) { + found = true; + break; + } + } + BOOST_CHECK(found); + } + + // Test case 3: Retrieve cones within range of (0, 0) + std::vector cones3 = track->getLocalCones(Point{0, 0}, 5); // Range of 5 + BOOST_CHECK_EQUAL(cones3.size(), 1); // Expect 1 cone: (0, 0) + BOOST_CHECK(cones3[0].getPos().distanceTo(Point{0, 0}) < THRESHOLD); + + // Test case 4: No cones within range + std::vector cones4 = track->getLocalCones(Point{100, 100}, 10); // Range of 10 + BOOST_CHECK_EQUAL(cones4.size(), 0); // Expect no cones +} + + +BOOST_AUTO_TEST_CASE(test_getLocalCenterPoints) { + auto track = setupTrack(); + + // Test case 1: Point exactly on a center point + Point testPoint1{2750, 1750}; // This point exists in our centerPoints + auto centerPoints1 = track->getLocalCenterPoints(testPoint1, 10); + BOOST_CHECK_EQUAL(centerPoints1.size(), 1); + BOOST_TEST_MESSAGE("Test case 1: Point exactly on center point (2750, 1750):"); + for (const auto& point : centerPoints1) { + BOOST_TEST_MESSAGE("Found point: (" << point.pos.x << ", " << point.pos.y << ")"); + } + + // Test case 2: Point between two center points with appropriate range + Point testPoint2{2875, 1700}; // Between (2750,1750) and (3000,1650) + auto centerPoints2 = track->getLocalCenterPoints(testPoint2, 200); // Range covers both points + BOOST_CHECK_EQUAL(centerPoints2.size(), 2); + BOOST_TEST_MESSAGE("Test case 2: Point between center points (2875, 1700):"); + for (const auto& point : centerPoints2) { + BOOST_TEST_MESSAGE("Found point: (" << point.pos.x << ", " << point.pos.y << ")"); + } + + // Test case 3: Point near track with small range (should find one point) + Point testPoint3{2760, 1760}; // Very close to (2750,1750) + auto centerPoints3 = track->getLocalCenterPoints(testPoint3, 20); + BOOST_CHECK_EQUAL(centerPoints3.size(), 1); + BOOST_TEST_MESSAGE("Test case 3: Point near track (2760, 1760):"); + for (const auto& point : centerPoints3) { + BOOST_TEST_MESSAGE("Found point: (" << point.pos.x << ", " << point.pos.y << ")"); + } + + // Test case 4: Point far from track (should find no points) + Point testPoint4{5000, 5000}; + auto centerPoints4 = track->getLocalCenterPoints(testPoint4, 100); + BOOST_CHECK_EQUAL(centerPoints4.size(), 0); + BOOST_TEST_MESSAGE("Test case 4: Point far from track (5000, 5000):"); + BOOST_TEST_MESSAGE("Found " << centerPoints4.size() << " points (expected 0)"); + + // Test case 5: Point with large range (should find multiple points) + Point testPoint5{2750, 1750}; + auto centerPoints5 = track->getLocalCenterPoints(testPoint5, 500); + BOOST_CHECK(centerPoints5.size() > 2); // Should find several points + BOOST_TEST_MESSAGE("Test case 5: Point with large range (2750, 1750):"); + BOOST_TEST_MESSAGE("Found " << centerPoints5.size() << " points"); + for (const auto& point : centerPoints5) { + BOOST_TEST_MESSAGE("Found point: (" << point.pos.x << ", " << point.pos.y << ")"); + } +} + +BOOST_AUTO_TEST_CASE(test_getNearestCenterPoints) { + // Helper function to format optional points for output + auto format_optional_point = [](const std::optional& opt) -> std::string { + if (opt.has_value()) { + return "Point(" + std::to_string(opt.value().pos.x) + ", " + std::to_string(opt.value().pos.y) + ")"; + } else { + return "nullopt"; + } + }; + + // Case 1: Test empty track + { + auto emptyTrack = std::make_shared(); + auto empty_result = emptyTrack->getNearestCenterPoints(Point{0, 0}); + BOOST_TEST_MESSAGE("Case 1: Test empty track"); + BOOST_TEST_MESSAGE("Expected: {nullopt, nullopt}, Actual: {" + << format_optional_point(empty_result.first) << ", " + << format_optional_point(empty_result.second) << "}"); + BOOST_CHECK(!empty_result.first.has_value()); + BOOST_CHECK(!empty_result.second.has_value()); + } + + // Create a track with three points + auto track = std::make_shared(); + std::vector threePoints = { + Point{1500, 1600}, + Point{1700, 1650}, + Point{1800, 1550} + }; + track->initialiseCenterPoint(std::move(threePoints), false); + + // Case 3: Open loop, queried point before first center point + { + auto before_start = track->getNearestCenterPoints(Point{1400, 1550}); + BOOST_TEST_MESSAGE("Case 3: Open loop, queried point before first center point"); + BOOST_TEST_MESSAGE("Expected: {nullopt, Point(1500, 1600)}, Actual: {" + << format_optional_point(before_start.first) << ", " + << format_optional_point(before_start.second) << "}"); + BOOST_CHECK(!before_start.first.has_value()); + BOOST_CHECK(before_start.second.has_value()); + if (before_start.second.has_value()) { // Add safety check + BOOST_CHECK_EQUAL(before_start.second.value().pos.x, 1500); + } + } + + // Case 4: Open loop, queried point between first and second center points + { + auto between_first_second = track->getNearestCenterPoints(Point{1600, 1625}); + BOOST_TEST_MESSAGE("Case 4: Open loop, queried point between first and second center points"); + BOOST_TEST_MESSAGE("Expected: {Point(1500, 1600), Point(1700, 1650)}, Actual: {" + << format_optional_point(between_first_second.first) << ", " + << format_optional_point(between_first_second.second) << "}"); + BOOST_CHECK(between_first_second.first.has_value()); + BOOST_CHECK(between_first_second.second.has_value()); + BOOST_CHECK_EQUAL(between_first_second.first.value().pos.x, 1500); + BOOST_CHECK_EQUAL(between_first_second.second.value().pos.x, 1700); + } + + // Case 5: Open loop, queried point between second and third center points + { + auto between_second_third = track->getNearestCenterPoints(Point{1750, 1600}); + BOOST_TEST_MESSAGE("Case 5: Open loop, queried point between second and third center points"); + BOOST_TEST_MESSAGE("Expected: {Point(1700, 1650), Point(1800, 1550)}, Actual: {" + << format_optional_point(between_second_third.first) << ", " + << format_optional_point(between_second_third.second) << "}"); + BOOST_CHECK(between_second_third.first.has_value()); + BOOST_CHECK(between_second_third.second.has_value()); + BOOST_CHECK_EQUAL(between_second_third.first.value().pos.x, 1700); + BOOST_CHECK_EQUAL(between_second_third.second.value().pos.x, 1800); + } + + // Case 6: Open loop, queried point after last center point + { + auto after_end = track->getNearestCenterPoints(Point{1850, 1500}); + BOOST_TEST_MESSAGE("Case 6: Open loop, queried point after last center point"); + BOOST_TEST_MESSAGE("Expected: {Point(1800, 1550), nullopt}, Actual: {" + << format_optional_point(after_end.first) << ", " + << format_optional_point(after_end.second) << "}"); + BOOST_CHECK(after_end.first.has_value()); + BOOST_CHECK(!after_end.second.has_value()); + BOOST_CHECK_EQUAL(after_end.first.value().pos.x, 1800); + } + + // Test closed loop cases + track->setClosedLoop(); + + // Case 7: Closed loop, queried point between last and first center points + { + auto between_last_first = track->getNearestCenterPoints(Point{1400, 1550}); + BOOST_TEST_MESSAGE("Case 7: Closed loop, queried point between last and first center points"); + BOOST_TEST_MESSAGE("Expected: {Point(1800, 1550), Point(1500, 1600)}, Actual: {" + << format_optional_point(between_last_first.first) << ", " + << format_optional_point(between_last_first.second) << "}"); + BOOST_CHECK(between_last_first.first.has_value()); + BOOST_CHECK(between_last_first.second.has_value()); + BOOST_CHECK_EQUAL(between_last_first.first.value().pos.x, 1800); + BOOST_CHECK_EQUAL(between_last_first.second.value().pos.x, 1500); + } + + // Case 8: Closed loop, queried point between first and second center points + { + auto closed_between_first_second = track->getNearestCenterPoints(Point{1600, 1625}); + BOOST_TEST_MESSAGE("Case 8: Closed loop, queried point between first and second center points"); + BOOST_TEST_MESSAGE("Expected: {Point(1500, 1600), Point(1700, 1650)}, Actual: {" + << format_optional_point(closed_between_first_second.first) << ", " + << format_optional_point(closed_between_first_second.second) << "}"); + BOOST_CHECK(closed_between_first_second.first.has_value()); + BOOST_CHECK(closed_between_first_second.second.has_value()); + BOOST_CHECK_EQUAL(closed_between_first_second.first.value().pos.x, 1500); + BOOST_CHECK_EQUAL(closed_between_first_second.second.value().pos.x, 1700); + } +} + +BOOST_AUTO_TEST_CASE(test_getCurvature_with_tuple_points) +{ + using namespace planning; + + Point a(10, 10); + Point b(10,11); + Point c(11,11); + + AccelTrack aTrack{}; + + auto res = aTrack.getCurvature({a,b,c}); + double expected = 1.41421; + + // getCurvature + BOOST_CHECK(res - expected <= THRESHOLD); + +} + +BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file diff --git a/scrum/planningDesign/trajectory_planner.cpp b/scrum/planningDesign/trajectory_planner.cpp new file mode 100644 index 00000000..996565be --- /dev/null +++ b/scrum/planningDesign/trajectory_planner.cpp @@ -0,0 +1,373 @@ +#include main.hpp + +// ros messages +#include "rclcpp/rclcpp.hpp" +#include "track_utils/src/cone.hpp" +#include "track_utils/src/track.hpp" +#include "track_utils/src/vehicle.hpp" + +// +#include "std_msgs/msg/string.hpp" +#include "std_msgs/msg/uint16.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose.hpp" + +// Zane + +// Jonty + +// Winola + +// Siva + +// Pang + +// Adrian + +// Mahmoud + +// Waldo + +namespace planning::ConeType = conetype; + +enum state_t +{ + UNINITIALIZED, + INITIALIZED, + TERMINATED +} + +enum event_type +{ + ACCEL, + SKIDPAD, + AUTOCROSS, +} + +enum side_t +{ + LEFT, + RIGHT +} + +struct current_event +{ // use case? + event_type event_type; +} + +struct CarHeuristics +{ // update car heuristics function/callback, use wall timer (e.g update interval). calls other helper funtcions to get these + double lateral_position; // helper function, finds the projection of the car onto the centerline + double car_position_bearing_difference; // write helper function that uses get difference in bearing + double car_velocity; // should be provided by a ros topic (find which one). +} + +// TODO: need separate function that uses car heuristics to generate the trajectory + +class TrajectoryPlanner(Node) +{ + using namespace std::chrono_literals; + + std::unique_ptr raceTrack; + state_t state = UNINITIALIZED; + + const std::map coneProp {{conetype::BIG_ORANGE, IntrinsicConeProp(0.285, 0.505)}, {conetype::SMALL_ORANGE, IntrinsicConeProp(0.228, 325)}, {conetype::BLUE, IntrinsicConeProp(0.228, 325)}, {conetype::YELLOW, IntrinsicConeProp(0.228, 325)}}; + CarHeuristics car_heuristics; + Vehicle vehicle; + + this->declare_parameter("autonomous_event_type", "accel"); // use lower case for param inputs + this->declare_parameter("enable_centerpoints", "false"); + + auto mAutonomousEventType = this->get_parameter("autonomous_event_type").as_string(); + +public: + TrajectoryPlanner() : Node("minimal_param_node") + { + // initialise raceTrack based on parameter input + this->update_race_event_type(); + } + +private: + // autonomous event type + + // car position and velocity subscribers + auto car_position_subscription = this->create_subscription<>; + + // subscriptions + auto car_pose_sub = this->create_subscription( + "/vehicle/pose", 10, // TODO: Change the topic as required, this is just a placeholder! + std::bind(&TrajectoryPlanner::carPoseCallback, this, _1);); + + auto car_velocity_sub = this->create_subscription( + "/vehicle/velocity", 10, // TODO: Change the topic as required, this is just a placeholder! + std::bind(&TrajectoryPlanner::carVelocityCallback, this, _1);); + + auto event_manager_subscription = this->create_subscription( + "", 10, std::bind(&TrajectoryPlanner::event_manager_subscription, this, _1)); + + // callback binding for perception cone information + auto left_track_subscription = this->create_subscription( + "", 10, std::bind(&TrajectoryPlanner::left_track_subscription, this, _1)); + + auto right_track_subscription = this->create_subscription( + "", 10, std::bind(&TrajectoryPlanner::right_track_subscription, this, _1)); + + auto pose_subscriber = this->create_subscription( + "/car_pose", 10, std::bind(&TrajectoryPlanner::poseCallback, this, std::placeholders::_1)); + + // publishers + auto trajectory_publisher = this->create_publisher("planned_trajectory", 10); + auto centerpoint_publisher = this->create_publisher("Centerpoints", 10); + + auto update_vehicle_heuristic_timer_ = this.create_wall_timer(500ms, std::bind(&TrajectoryPlanner::trajectory_publish_callback, this)); + auto centerpoints_timer_ = this.create_wall_timer(500ms, std::bind(&TrajectoryPlanner::centerpoints_publish_callback, this)); + + // TODO: callback binding for vehicle information + + // carPose callback that updates the + + // Car veholcity callback that updates the velocity of the vehicle object + + // TODO: publisher binding for the trajectory + // state machine based on track conditions + /** + * @brief Updates car heuristics based on current vehicle state and track + */ +void updateCarHeuristics() { + if (!raceTrack) { + RCLCPP_WARN(this->get_logger(), "Cannot update car heuristics: race track not initialized"); + return; + } + + // Get nearest center points + auto nearestPoints = raceTrack->getNearestCenterPoints(vehicle.pos); + + if (nearestPoints.first && nearestPoints.second) { + // Calculate lateral position (distance from centerline) + const Point& p1 = nearestPoints.first.value().pos; + const Point& p2 = nearestPoints.second.value().pos; + + // Vector from p1 to p2 (track direction) + double track_dx = p2.x - p1.x; + double track_dy = p2.y - p1.y; + double track_length = std::sqrt(track_dx * track_dx + track_dy * track_dy); + + if (track_length > 0) { + // Normalize track direction vector + double norm_track_dx = track_dx / track_length; + double norm_track_dy = track_dy / track_length; + + // Vector from p1 to vehicle + double vehicle_dx = vehicle.pos.x - p1.x; + double vehicle_dy = vehicle.pos.y - p1.y; + + // Project vehicle vector onto track direction + double proj = vehicle_dx * norm_track_dx + vehicle_dy * norm_track_dy; + + // Vector from track to vehicle (perpendicular component) + double perp_x = vehicle_dx - proj * norm_track_dx; + double perp_y = vehicle_dy - proj * norm_track_dy; + + // Lateral position is the length of the perpendicular vector + car_heuristics.lateral_position = std::sqrt(perp_x * perp_x + perp_y * perp_y); + + // Determine if vehicle is to the left or right of track + // Cross product sign determines which side + double cross = norm_track_dx * vehicle_dy - norm_track_dy * vehicle_dx; + if (cross < 0) { + car_heuristics.lateral_position = -car_heuristics.lateral_position; + } + } + + // Update bearing difference + auto bearingDiff = raceTrack->getDifferenceInBearing(vehicle); + if (bearingDiff) { + car_heuristics.car_position_bearing_difference = bearingDiff.value().getDegrees(); + } + } else { + RCLCPP_WARN(this->get_logger(), "Unable to find nearest center points for vehicle position"); + } + + // Velocity is already updated in carVelocityCallback +} + // center line following in straight areas of the track. + + // identifying a corner which needs special handling + + + // initiate a corner + void trajectory_publish_callback() + { + trajectory_helper(); + + moa_msg::trajectory; + trajectory_publisher.publish(); + } + + // TODO: helper to update heuristics about the car relative to the track. + void trajectory_helper(const planning::Track &track, const Vehicle &vehicle) + { + // update lateral position of the car in the track. Have a struct that + // stores that 'state data' + + // Get nearest center points on the track + auto nearestPoints = track.getNearestCenterPoints(vehicle.pos); + + if (!nearestPoints.first || !nearestPoints.second) + { + std::cerr << "Error: Unable to determine nearest center points.\n"; + return; + } + + // Extract the relevant points + planning::Point p1 = nearestPoints.first.value().pos; + planning::Point p2 = nearestPoints.second.value().pos; + + // Compute lateral position (perpendicular distance to track segment) + double dx = p2.x - p1.x; + double dy = p2.y - p1.y; + double lengthSquared = dx * dx + dy * dy; + + // angular displacement of the car's bearing to the track + + //subscribe to topic that gives the car's velocity + } + + // event_manager_subscription + // void event_manager_subscription(const moa_msgs::msg::current_event& msg) { + // switch (msg.event_type) + // { + // case ACCEL: + // raceTrack = std::make_unique(); + // break; + + // case SKIDPAD: + // raceTrack = std::make_unique(); + // /* code */ + // break; + + // case AUTOCROSS: + // raceTrack = std::make_unique(); + // /* code */ + // break; + + // default: + // break; + // } + // } + + // uses autonomous_event_type parameter to initialize Track raceTrack + event_type update_race_event_type() { + event_type event_type_from_param; + + switch (mAutonomousEventType) + { + case "accel": + event_type_from_param = ACCEL; + raceTrack = std::make_unique(); + break; + case "skidpad": + event_type_from_param = SKIDPAD; + raceTrack = std::make_unique(); + break; + case "autocross": + event_type_from_param = AUTOCROSS; + raceTrack = std::make_unique(); + break; + } + RCLCPP_INFO(get_logger(), "initalized %s track type", mAutonomousEventType.c_str()); + return event_type_from_param; + } + + //TODO: callback for perception cone information + void track_update_callback(const moa_msgs::msg::Track& msg, const side_t side) { + // Track: + // geometry_msgs/Point[] cones + + // geometry_msgs/Point: + // float64 x + // float64 y + // float64 z + + // left blue, right yellow, need to populate intrinsic cone prop, or write + // another constructor for planning::Cone without it + + // confirm cones would have ids, if existing id: updte coord, + // if else new id: insert new Cone to map + std::map cones(msg.cones.size()); + + void consume(const geometry_msgs::Point& cone) { + + auto match = cones.find(cone.id); + if (match != cones.end()) + { + match->second.setPos(planning::Point(cone.x, cone.y)); + } else { + if (side == LEFT) { + cones.insert({cone.id, Cone(planning::Point(cone.x, cone.y), planning::Conetype::BLUE)}); + } else { + cones.insert({cone.id, Cone(planning::Point(cone.x, cone.y), planning::Conetype::YELLOW)}); + } + } + } + + for (geometry_msgs::Point conePoint : msg) { + consume(conePoint); + } + } + + void lefttrack_subscription(const moa_msgs::msg::Track& msg) { + track_update_callback(msg, LEFT); + } + void right_track_subscription(const moa_msgs::msg::Track& msg) { + track_update_callback(msg, RIGHT); + } + + // callback for vehicle information (Jonty) : uopdating the struct using the + // 3 functions below + + //function for car position + + //function for car bearing differnce + double updateBearingDiff(vehicle){ + return track.getBearingDifference(vehicle); + } + + void centerpoints_publish_callback() { + if (this->get_parameter("enable_centerpoints").as_bool()){ + + //track msg to hold centerpoints + moa_msgs::msg::Track centerpoints_msg; + + //access racetrack centerpoints, + const auto& track_centerpoints = racetrack->getCenterPoints(); + + //populate msg with centerpoints + centerpoints_msg.cones.reserve(track_centerpoints.size()); + + for (const auto& point : track_centerpoints){ + moa_msgs::msg::Point center_point; + center_point.x = point.pos.x; + center_point.y = point.pos.y; + + centerpoints_msg.cones.push_back(center_point); + + //set header information + centerpoints_msg.header.stamp = this->now(); + centerpoints_msg.header.frame_id = "map"; + } + //Publish centerpoints + RCLCPP_INFO(this->get_logger(), "publishing %zu centerpoints", centerpoints_msg.cones.size()); + centerpoint_publisher->publish(centerpoints_msg); + } + } + +} //end of TrajectoryPlanner + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/scrum/planningDesign/trajectory_planner.hpp b/scrum/planningDesign/trajectory_planner.hpp new file mode 100644 index 00000000..0decf929 --- /dev/null +++ b/scrum/planningDesign/trajectory_planner.hpp @@ -0,0 +1,9 @@ +#pragma once + + +class TrajectoryPlanner(Node){ + +} // end of trajectory planner class + + + \ No newline at end of file diff --git a/src/perception/cone-detection/cone_detection/__init__.py b/src/action/acceleration/acceleration/__init__.py similarity index 100% rename from src/perception/cone-detection/cone_detection/__init__.py rename to src/action/acceleration/acceleration/__init__.py diff --git a/src/action/acceleration/acceleration/controller.py b/src/action/acceleration/acceleration/controller.py new file mode 100644 index 00000000..4f9d4503 --- /dev/null +++ b/src/action/acceleration/acceleration/controller.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python3 +# Python imports +import rclpy +from rclpy.node import Node +from moa_msgs.msg import ConeMap +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped +from std_msgs.msg import Header + + +class Acceleration_algorithm(Node): + def __init__(self): + super().__init__("Acceleration") + self.get_logger().info("Acceleration Started") + + # subscribe to Cone detection result + self.create_subscription(ConeMap, "cone_detection", self.main_hearback, 5) + + #self.drive_pub = self.create_publisher(AckermannDrive, "/drive", 5) + #self.drive_vis_pub = self.create_publisher(AckermannDrive, "/drive_vis", 5) + self.cmd_vel_pub = self.create_publisher(AckermannDriveStamped, "cmd_vel", 5) + self.create_subscription(AckermannDrive, "drive", self.get_steering_angle, 5) + + def get_steering_angle(self, msg: AckermannDrive): + self.steering_angle = msg.steering_angle + + def main_hearback(self, msg: ConeMap): + if len(msg.cones) < 2: + #No cones, make throttle to 0 + self.current_speed = 0.0 + self.publish_ackermann() + else: + #There is cones, adjust throttle to target speed + self.current_speed = 5/3.6 #In m/s + self.publish_ackermann() + + + def publish_ackermann(self): + if hasattr(self,"steering_angle"): + args1 = {"steering_angle": self.steering_angle, + "steering_angle_velocity": 0.0, + "speed": float(self.current_speed), + "acceleration": 0.0, + "jerk": 0.0} + msg1 = AckermannDrive(**args1) + + msg_cmd_vel = self.convert_to_stamped(msg1) + self.cmd_vel_pub.publish(msg_cmd_vel) + #self.drive_pub.publish(msg1) + #self.drive_vis_pub.publish(msg1) + + def convert_to_stamped(self, ackermann_msgs): + stamped_msg = AckermannDriveStamped() + stamped_msg.header = Header() + stamped_msg.header.stamp = self.get_clock().now().to_msg() # Set the current timestamp + stamped_msg.header.frame_id = '0' # Set the appropriate frame ID + stamped_msg.drive = ackermann_msgs + return stamped_msg + + +def main(args=None): + rclpy.init(args=args) + + accelrator = Acceleration_algorithm() + + rclpy.spin(accelrator) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + accelrator.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/action/acceleration/launch/acceleration_launch.py b/src/action/acceleration/launch/acceleration_launch.py new file mode 100644 index 00000000..751379db --- /dev/null +++ b/src/action/acceleration/launch/acceleration_launch.py @@ -0,0 +1,11 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='acceleration', + executable='controller', + name='Acceleration' + ), + ]) \ No newline at end of file diff --git a/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/package.xml b/src/action/acceleration/package.xml similarity index 74% rename from src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/package.xml rename to src/action/acceleration/package.xml index ce14b736..641d8f25 100644 --- a/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/package.xml +++ b/src/action/acceleration/package.xml @@ -1,12 +1,16 @@ - cone_map_foxglove_visualizer + acceleration 0.0.0 TODO: Package description - dyu056 + Yiyang Chen TODO: License declaration + moa_msgs + ackermann_msgs + std_msgs + ament_copyright ament_flake8 ament_pep257 diff --git a/src/perception/aruco_detection/resource/aruco_detection b/src/action/acceleration/resource/acceleration similarity index 100% rename from src/perception/aruco_detection/resource/aruco_detection rename to src/action/acceleration/resource/acceleration diff --git a/src/action/acceleration/setup.cfg b/src/action/acceleration/setup.cfg new file mode 100644 index 00000000..3427a7d3 --- /dev/null +++ b/src/action/acceleration/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/acceleration +[install] +install_scripts=$base/lib/acceleration diff --git a/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/setup.py b/src/action/acceleration/setup.py similarity index 73% rename from src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/setup.py rename to src/action/acceleration/setup.py index d5418fea..cdf48d43 100644 --- a/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/setup.py +++ b/src/action/acceleration/setup.py @@ -1,6 +1,9 @@ from setuptools import find_packages, setup -package_name = 'cone_map_foxglove_visualizer' +package_name = 'acceleration' + + + setup( name=package_name, @@ -13,14 +16,14 @@ ], install_requires=['setuptools'], zip_safe=True, - maintainer='dyu056', - maintainer_email='yudaniel888@hotmail.com', + maintainer='Yiyang Chen', + maintainer_email='yiyang030903@gmail.com', description='TODO: Package description', license='TODO: License declaration', tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'visualizer = cone_map_foxglove_visualizer.visualizer:main', + 'controller = acceleration.controller:main', ], }, ) diff --git a/src/perception/aruco_detection/test/test_copyright.py b/src/action/acceleration/test/test_copyright.py similarity index 100% rename from src/perception/aruco_detection/test/test_copyright.py rename to src/action/acceleration/test/test_copyright.py diff --git a/src/perception/aruco_detection/test/test_flake8.py b/src/action/acceleration/test/test_flake8.py similarity index 100% rename from src/perception/aruco_detection/test/test_flake8.py rename to src/action/acceleration/test/test_flake8.py diff --git a/src/perception/aruco_detection/test/test_pep257.py b/src/action/acceleration/test/test_pep257.py similarity index 100% rename from src/perception/aruco_detection/test/test_pep257.py rename to src/action/acceleration/test/test_pep257.py diff --git a/src/action/head_to_goal_control/head_to_goal_control/controller.py b/src/action/head_to_goal_control/head_to_goal_control/controller.py index 56618962..3c844de0 100644 --- a/src/action/head_to_goal_control/head_to_goal_control/controller.py +++ b/src/action/head_to_goal_control/head_to_goal_control/controller.py @@ -7,7 +7,6 @@ import math from geometry_msgs.msg import PoseArray from geometry_msgs.msg import Pose -from moa_msgs.msg import ConeMap from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped from std_msgs.msg import Header @@ -15,17 +14,17 @@ class head_to_goal_control_algorithm(Node): def __init__(self): super().__init__("Head_To_Goal_Controller") - self.get_logger().info("Head to Goal Controller Node v4 Started") + self.get_logger().info("Head to Goal Controller Node v5 Started") # Constant to tune (touch me please it makes me feel horny ahhhhhhh!) ## Tuning for look ahead distance self.look_up_distance = 1 - self.cancel_distance = 0.6 + self.cancel_distance = 0.7 ## Tuning for PID controller - self.P = 20 + self.P = 200 self.max_steering_angle = 20.0 #self.max_speed = 2.5 - self.max_speed = 2 + self.max_speed = 3.0 self.speed_adjuster_width = 15 ## Current speed setting self.current_speed = 0 @@ -63,7 +62,7 @@ def main_hearback(self, msg: Pose): else: self.steering_angle = 0 - self.get_logger().info("Warning: no trajectory found, will set steering angle to 0!!!!") + #self.get_logger().info("Warning: no trajectory found, will set steering angle to 0!!!!") # Experimental: speed adjuster # self.current_speed = self.steer_to_speed(self.steering_angle) @@ -92,7 +91,7 @@ def steer_to_speed(self, steering_angle): def apply_speed_decay(self): self.current_speed = (0.61 ** self.speed_decay_constant) * self.max_speed - self.get_logger().info(f"Speed decay applied: {self.speed_decay_constant}, set current speed to {self.current_speed}") + #self.get_logger().info(f"Speed decay applied: {self.speed_decay_constant}, set current speed to {self.current_speed}") # Coordinate tranformer def convert_to_transformation_matrix(self, x: float, y: float, theta: float) -> ( @@ -155,7 +154,7 @@ def get_track_point_in_global_frame(self, track_point_in_local_frame: Pose): def update_track_point(self, msg: PoseArray): #Main logic # Pick new tracking point if no tracking point is selected or old tracking point is no longer visible if self.need_new_track_point(): - self.get_logger().info("Update track point") + #self.get_logger().info("Updating track point, speed decay applied") self.Pose_to_track_in_global_frame = self.get_track_point_in_global_frame(msg) self.speed_decay_constant += 1 else: @@ -254,4 +253,4 @@ def main(args=None): if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/src/action/head_to_goal_control/head_to_goal_control/controller.py.save b/src/action/head_to_goal_control/head_to_goal_control/controller.py.save new file mode 100644 index 00000000..987445a9 --- /dev/null +++ b/src/action/head_to_goal_control/head_to_goal_control/controller.py.save @@ -0,0 +1,256 @@ +#!/usr/bin/env python3 +# Python imports +from typing import Optional +import rclpy +from rclpy.node import Node +import numpy as np +import math +from geometry_msgs.msg import PoseArray +from geometry_msgs.msg import Pose +from moa_msgs.msg import ConeMap +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped +from std_msgs.msg import Header + + +class head_to_goal_control_algorithm(Node): + def __init__(self): + super().__init__("Head_To_Goal_Controller") + self.get_logger().info("Head to Goal Controller Node v4 Started") + + # Constant to tune (touch me please it makes me feel horny ahhhhhhh!) + ## Tuning for look ahead distance + self.look_up_distance = 1 + self.cancel_distance = 0.6 + ## Tuning for PID controller + self.P = 20 + self.max_steering_angle = 20.0 + #self.max_speed = 2.5 + self.max_speed = 10 self.speed_adjuster_width = 15 + ## Current speed setting + self.current_speed = 0 + + # Initializer (normally don't touch) + self.steering_angle = 0 + self.pos = (0,0) + self.speed_decay_constant = 0 + + # subscribe to best trajectory + self.best_trajectory_sub = self.create_subscription(PoseArray, "moa/selected_trajectory", self.selected_trajectory_handler, 5) + #self.cone_map_sub = self.create_subscription(ConeMap, "cone_map", self.main_hearback, 5) + self.car_pos_sub = self.create_subscription(Pose, "car_position", self.main_hearback, 5) + + self.drive_pub = self.create_publisher(AckermannDrive, "/drive", 5) + self.drive_vis_pub = self.create_publisher(AckermannDrive, "/drive_vis", 5) + self.cmd_vel_pub = self.create_publisher(AckermannDriveStamped, "cmd_vel", 5) + + self.track_point_pub = self.create_publisher(Pose, "moa/track_point", 5) + + def main_hearback(self, msg: Pose): + # Update car's current location and update transformation matrix + #self.car_pose = msg.cones[0].pose.pose + self.car_pose = msg + self.position_vector, self.rotation_matrix_l2g, self.rotation_matrix_g2l = self.convert_to_transformation_matrix(self.car_pose.position.x, self.car_pose.position.y, self.car_pose.orientation.w) + + # Before proceed, check whether we have a trajectory input + if hasattr(self, "trajectory_in_global_frame"): + # Update destination point to track + self.update_track_point(self.trajectory_in_global_frame) + # Get expected steering angle to publish + self.steering_angle = self.get_steering_angle(self.Pose_to_track_in_global_frame) + self.steering_angle = self.saturating_steering(self.steering_angle) + # self.get_logger().info(f"Set steering angle to {self.steering_angle * self.P}") + + else: + self.steering_angle = 0 + self.get_logger().info("Warning: no trajectory found, will set steering angle to 0!!!!") + + # Experimental: speed adjuster + # self.current_speed = self.steer_to_speed(self.steering_angle) + # Adjust the speed base on the performance of the path planning (if takes long time to locate next destination then stop until refresh) + self.apply_speed_decay() + + # Publish command for velocity + self.publish_ackermann() + + def selected_trajectory_handler(self, msg: PoseArray): + self.trajectory_in_global_frame = msg + + def saturating_steering(self, steering_angle): + saturation = self.max_steering_angle + if steering_angle > saturation: + steering_angle = saturation + elif steering_angle < -1 * saturation: + steering_angle = -1 * saturation + + return steering_angle + + def steer_to_speed(self, steering_angle): + # RF with NN can be used here + speed = self.max_speed * np.exp(- (steering_angle ** 2) / (2 * (self.speed_adjuster_width ** 2))) + return speed + + def apply_speed_decay(self): + self.current_speed = (0.61 ** self.speed_decay_constant) * self.max_speed + self.get_logger().info(f"Speed decay applied: {self.speed_decay_constant}, set current speed to {self.current_speed}") + + # Coordinate tranformer + def convert_to_transformation_matrix(self, x: float, y: float, theta: float) -> ( + np.array, np.array, np.array): + '''Convert state and list_of_cones input into position vector, rotation matrix (DCM) and the matrix of list of cones + + Args: + x: x position specified in float + y: y position specified in float + theta: theta orientation specified in float + list_of_cones: np.array (numpy array) for matrix of cone positions in 2 by n matrix (n is number of cones recorded in input)) + + Returns: + position_vector: np.array of position vector of cart + rotation_matrix: np.array DCM matrix to convert reading from local frame into global frame + list_of_cones: np.array 2 x n matrix of the list of cones measured in global frame + + Raises: + None + ''' + position_vector = np.array([[x], [y]]) + rotation_matrix_l2g = np.array( + [[math.cos(theta), -math.sin(theta)], + [math.sin(theta), math.cos(theta)]]) # local coordinate to global coordinate matrix + rotation_matrix_g2l = np.array( + [[math.cos(theta), math.sin(theta)], + [-math.sin(theta), math.cos(theta)]]) # global coordinate to local coordinate matrix + + return position_vector, rotation_matrix_l2g, rotation_matrix_g2l + + + def get_track_point_in_local_frame(self, track_point_in_global_frame: Pose): + if (hasattr(self, "position_vector") and hasattr(self, "rotation_matrix_l2g") and hasattr(self, "rotation_matrix_g2l")): + track_point_in_local_frame = Pose() + position_input = np.array( + [[track_point_in_global_frame.position.x], [track_point_in_global_frame.position.y]]) + position_output = np.matmul(self.rotation_matrix_g2l, (position_input - self.position_vector)) + track_point_in_local_frame.position.x = float(position_output[0]) + track_point_in_local_frame.position.y = float(position_output[1]) + return track_point_in_local_frame + else: + self.get_logger.info("Warning: Local pose message not transformed to global frame") + return track_point_in_global_frame + + + def get_track_point_in_global_frame(self, track_point_in_local_frame: Pose): + if (hasattr(self, "position_vector") and hasattr(self, "rotation_matrix_l2g") and hasattr(self, "rotation_matrix_g2l")): + track_point_in_global_frame = Pose() + position_input = np.array( + [[track_point_in_local_frame.position.x], [track_point_in_local_frame.position.y]]) + position_output = np.matmul(self.rotation_matrix_l2g, position_input) + self.position_vector + track_point_in_global_frame.position.x = float(position_output[0]) + track_point_in_global_frame.position.y = float(position_output[1]) + return track_point_in_global_frame + else: + self.get_logger.info("Warning: Local pose message not transformed to global frame") + return track_point_in_local_frame + +# Picking and maintaining a track point + def update_track_point(self, msg: PoseArray): #Main logic + # Pick new tracking point if no tracking point is selected or old tracking point is no longer visible + if self.need_new_track_point(): + self.get_logger().info("Update track point") + self.Pose_to_track_in_global_frame = self.get_track_point_in_global_frame(msg) + self.speed_decay_constant += 1 + else: + self.speed_decay_constant = 0 + self.track_point_pub.publish(self.Pose_to_track_in_global_frame) + + def need_new_track_point(self): + if not(hasattr(self, "Pose_to_track_in_global_frame")): + return True + else: + Pose_to_track_in_local_frame = self.get_track_point_in_local_frame(self.Pose_to_track_in_global_frame) + if self.track_point_still_visible(Pose_to_track_in_local_frame): + if Pose_to_track_in_local_frame.position.y <= self.cancel_distance: + return True + else: + return False + else: + return True + + def get_track_point_in_global_frame(self, trajectory_in_global_frame: PoseArray): + if hasattr(self, "position_vector") and hasattr(self, "rotation_matrix_l2g") and hasattr(self, "rotation_matrix_g2l"): + sorted_trajectory_poses_in_global_frame = PoseArray() + sorted_trajectory_poses_in_global_frame.poses = sorted(trajectory_in_global_frame.poses, key=lambda item: self.get_distance(self.get_track_point_in_local_frame(item))) + for track_point_in_global_frame in sorted_trajectory_poses_in_global_frame.poses: + track_point_in_local_frame = self.get_track_point_in_local_frame(track_point_in_global_frame) + if self.track_point_distance_above_look_up_distance(track_point_in_local_frame): + return track_point_in_global_frame + return self.car_pose + + def track_point_still_visible(self, track_point_in_local_frame: Pose): + return track_point_in_local_frame.position.y >= 0 + + def track_point_distance_above_look_up_distance(self, track_point_in_local_frame: Pose): + return self.get_distance(track_point_in_local_frame) >= self.look_up_distance + + def get_distance(self, track_point_in_local_frame: Pose): + x = track_point_in_local_frame.position.x + y = track_point_in_local_frame.position.y + return abs(((x ** 2) + (y ** 2)) ** (1/2)) + +### Calculate Steering Angle + def get_steering_angle(self, Pose_to_track_in_global_frame: Pose): + Pose_to_track_in_local_frame = self.get_track_point_in_local_frame(Pose_to_track_in_global_frame); + error_in_x = Pose_to_track_in_local_frame.position.x + return error_in_x * self.P + + def lateral_distance(self): + return self.get_track_point_in_local_frame(self.Pose_to_track_in_global_frame).position.x + + def get_arc_radius(self, L, x): + return L ** 2 / (2 * abs(x)) + + def publish_ackermann(self): + + args1 = {"steering_angle": float(self.steering_angle), + "steering_angle_velocity": 0.0, + "speed": float(self.current_speed), + "acceleration": 0.0, + "jerk": 0.0} + msg1 = AckermannDrive(**args1) + + #print(msg1) + + args2 = {"steering_angle": float(self.steering_angle), + "steering_angle_velocity": 0.0, + "speed": float(self.current_speed), + "acceleration": 0.0, + "jerk": 0.0} + msg2 = AckermannDrive(**args2) + + msg_cmd_vel = self.convert_to_stamped(msg1) + self.cmd_vel_pub.publish(msg_cmd_vel) + self.drive_pub.publish(msg1) + self.drive_vis_pub.publish(msg2) + + def convert_to_stamped(self, ackermann_msgs): + stamped_msg = AckermannDriveStamped() + stamped_msg.header = Header() + stamped_msg.header.stamp = self.get_clock().now().to_msg() # Set the current timestamp + stamped_msg.header.frame_id = '0' # Set the appropriate frame ID + stamped_msg.drive = ackermann_msgs + return stamped_msg + +def main(args=None): + rclpy.init(args=args) + + pure_pursuiter = head_to_goal_control_algorithm() + + rclpy.spin(pure_pursuiter) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + pure_pursuiter.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/action/head_to_goal_control/head_to_goal_control/controller.py.save.1 b/src/action/head_to_goal_control/head_to_goal_control/controller.py.save.1 new file mode 100644 index 00000000..045b3196 --- /dev/null +++ b/src/action/head_to_goal_control/head_to_goal_control/controller.py.save.1 @@ -0,0 +1,257 @@ +#!/usr/bin/env python3 +# Python imports +from typing import Optional +import rclpy +from rclpy.node import Node +import numpy as np +import math +from geometry_msgs.msg import PoseArray +from geometry_msgs.msg import Pose +from moa_msgs.msg import ConeMap +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped +from std_msgs.msg import Header + + +class head_to_goal_control_algorithm(Node): + def __init__(self): + super().__init__("Head_To_Goal_Controller") + self.get_logger().info("Head to Goal Controller Node v4 Started") + + # Constant to tune (touch me please it makes me feel horny ahhhhhhh!) + ## Tuning for look ahead distance + self.look_up_distance = 2 + self.cancel_distance = 0.5 + ## Tuning for PID controller + self.P = 30 + self.max_steering_angle = 20.0 + #self.max_speed = 2.5 + self.max_speed = 4.0 + self.speed_adjuster_width = 15 + ## Current speed setting + self.current_speed = 0 + + # Initializer (normally don't touch) + self.steering_angle = 0 + self.pos = (0,0) + self.speed_decay_constant = 0 + + # subscribe to best trajectory + self.best_trajectory_sub = self.create_subscription(PoseArray, "moa/selected_trajectory", self.selected_trajectory_handler, 5) + #self.cone_map_sub = self.create_subscription(ConeMap, "cone_map", self.main_hearback, 5) + self.car_pos_sub = self.create_subscription(Pose, "car_position", self.main_hearback, 5) + + self.drive_pub = self.create_publisher(AckermannDrive, "/drive", 5) + self.drive_vis_pub = self.create_publisher(AckermannDrive, "/drive_vis", 5) + self.cmd_vel_pub = self.create_publisher(AckermannDriveStamped, "cmd_vel", 5) + + self.track_point_pub = self.create_publisher(Pose, "moa/track_point", 5) + + def main_hearback(self, msg: Pose): + # Update car's current location and update transformation matrix + #self.car_pose = msg.cones[0].pose.pose + self.car_pose = msg + self.position_vector, self.rotation_matrix_l2g, self.rotation_matrix_g2l = self.convert_to_transformation_matrix(self.car_pose.position.x, self.car_pose.position.y, self.car_pose.orientation.w) + + # Before proceed, check whether we have a trajectory input + if hasattr(self, "trajectory_in_global_frame"): + # Update destination point to track + self.update_track_point(self.trajectory_in_global_frame) + # Get expected steering angle to publish + self.steering_angle = self.get_steering_angle(self.Pose_to_track_in_global_frame) + self.steering_angle = self.saturating_steering(self.steering_angle) + # self.get_logger().info(f"Set steering angle to {self.steering_angle * self.P}") + + else: + self.steering_angle = 0 + #self.get_logger().info("Warning: no trajectory found, will set steering angle to 0!!!!") + + # Experimental: speed adjuster + # self.current_speed = self.steer_to_speed(self.steering_angle) + # Adjust the speed base on the performance of the path planning (if takes long time to locate next destination then stop until refresh) + self.apply_speed_decay() + + # Publish command for velocity + self.publish_ackermann() + + def selected_trajectory_handler(self, msg: PoseArray): + self.trajectory_in_global_frame = msg + + def saturating_steering(self, steering_angle): + saturation = self.max_steering_angle + if steering_angle > saturation: + steering_angle = saturation + elif steering_angle < -1 * saturation: + steering_angle = -1 * saturation + + return steering_angle + + def steer_to_speed(self, steering_angle): + # RF with NN can be used here + speed = self.max_speed * np.exp(- (steering_angle ** 2) / (2 * (self.speed_adjuster_width ** 2))) + return speed + + def apply_speed_decay(self): + self.current_speed = (0.61 ** self.speed_decay_constant) * self.max_speed + #self.get_logger().info(f"Speed decay applied: {self.speed_decay_constant}, set current speed to {self.current_speed}") + + # Coordinate tranformer + def convert_to_transformation_matrix(self, x: float, y: float, theta: float) -> ( + np.array, np.array, np.array): + '''Convert state and list_of_cones input into position vector, rotation matrix (DCM) and the matrix of list of cones + + Args: + x: x position specified in float + y: y position specified in float + theta: theta orientation specified in float + list_of_cones: np.array (numpy array) for matrix of cone positions in 2 by n matrix (n is number of cones recorded in input)) + + Returns: + position_vector: np.array of position vector of cart + rotation_matrix: np.array DCM matrix to convert reading from local frame into global frame + list_of_cones: np.array 2 x n matrix of the list of cones measured in global frame + + Raises: + None + ''' + position_vector = np.array([[x], [y]]) + rotation_matrix_l2g = np.array( + [[math.cos(theta), -math.sin(theta)], + [math.sin(theta), math.cos(theta)]]) # local coordinate to global coordinate matrix + rotation_matrix_g2l = np.array( + [[math.cos(theta), math.sin(theta)], + [-math.sin(theta), math.cos(theta)]]) # global coordinate to local coordinate matrix + + return position_vector, rotation_matrix_l2g, rotation_matrix_g2l + + + def get_track_point_in_local_frame(self, track_point_in_global_frame: Pose): + if (hasattr(self, "position_vector") and hasattr(self, "rotation_matrix_l2g") and hasattr(self, "rotation_matrix_g2l")): + track_point_in_local_frame = Pose() + position_input = np.array( + [[track_point_in_global_frame.position.x], [track_point_in_global_frame.position.y]]) + position_output = np.matmul(self.rotation_matrix_g2l, (position_input - self.position_vector)) + track_point_in_local_frame.position.x = float(position_output[0]) + track_point_in_local_frame.position.y = float(position_output[1]) + return track_point_in_local_frame + else: + self.get_logger.info("Warning: Local pose message not transformed to global frame") + return track_point_in_global_frame + + + def get_track_point_in_global_frame(self, track_point_in_local_frame: Pose): + if (hasattr(self, "position_vector") and hasattr(self, "rotation_matrix_l2g") and hasattr(self, "rotation_matrix_g2l")): + track_point_in_global_frame = Pose() + position_input = np.array( + [[track_point_in_local_frame.position.x], [track_point_in_local_frame.position.y]]) + position_output = np.matmul(self.rotation_matrix_l2g, position_input) + self.position_vector + track_point_in_global_frame.position.x = float(position_output[0]) + track_point_in_global_frame.position.y = float(position_output[1]) + return track_point_in_global_frame + else: + self.get_logger.info("Warning: Local pose message not transformed to global frame") + return track_point_in_local_frame + +# Picking and maintaining a track point + def update_track_point(self, msg: PoseArray): #Main logic + # Pick new tracking point if no tracking point is selected or old tracking point is no longer visible + if self.need_new_track_point(): + #self.get_logger().info("Updating track point, speed decay applied") + self.Pose_to_track_in_global_frame = self.get_track_point_in_global_frame(msg) + self.speed_decay_constant += 1 + else: + self.speed_decay_constant = 0 + self.track_point_pub.publish(self.Pose_to_track_in_global_frame) + + def need_new_track_point(self): + if not(hasattr(self, "Pose_to_track_in_global_frame")): + return True + else: + Pose_to_track_in_local_frame = self.get_track_point_in_local_frame(self.Pose_to_track_in_global_frame) + if self.track_point_still_visible(Pose_to_track_in_local_frame): + if Pose_to_track_in_local_frame.position.y <= self.cancel_distance: + return True + else: + return False + else: + return True + + def get_track_point_in_global_frame(self, trajectory_in_global_frame: PoseArray): + if hasattr(self, "position_vector") and hasattr(self, "rotation_matrix_l2g") and hasattr(self, "rotation_matrix_g2l"): + sorted_trajectory_poses_in_global_frame = PoseArray() + sorted_trajectory_poses_in_global_frame.poses = sorted(trajectory_in_global_frame.poses, key=lambda item: self.get_distance(self.get_track_point_in_local_frame(item))) + for track_point_in_global_frame in sorted_trajectory_poses_in_global_frame.poses: + track_point_in_local_frame = self.get_track_point_in_local_frame(track_point_in_global_frame) + if self.track_point_distance_above_look_up_distance(track_point_in_local_frame): + return track_point_in_global_frame + return self.car_pose + + def track_point_still_visible(self, track_point_in_local_frame: Pose): + return track_point_in_local_frame.position.y >= 0 + + def track_point_distance_above_look_up_distance(self, track_point_in_local_frame: Pose): + return self.get_distance(track_point_in_local_frame) >= self.look_up_distance + + def get_distance(self, track_point_in_local_frame: Pose): + x = track_point_in_local_frame.position.x + y = track_point_in_local_frame.position.y + return abs(((x ** 2) + (y ** 2)) ** (1/2)) + +### Calculate Steering Angle + def get_steering_angle(self, Pose_to_track_in_global_frame: Pose): + Pose_to_track_in_local_frame = self.get_track_point_in_local_frame(Pose_to_track_in_global_frame); + error_in_x = Pose_to_track_in_local_frame.position.x + return error_in_x * self.P + + def lateral_distance(self): + return self.get_track_point_in_local_frame(self.Pose_to_track_in_global_frame).position.x + + def get_arc_radius(self, L, x): + return L ** 2 / (2 * abs(x)) + + def publish_ackermann(self): + + args1 = {"steering_angle": float(self.steering_angle), + "steering_angle_velocity": 0.0, + "speed": float(self.current_speed), + "acceleration": 0.0, + "jerk": 0.0} + msg1 = AckermannDrive(**args1) + + #print(msg1) + + args2 = {"steering_angle": float(self.steering_angle), + "steering_angle_velocity": 0.0, + "speed": float(self.current_speed), + "acceleration": 0.0, + "jerk": 0.0} + msg2 = AckermannDrive(**args2) + + msg_cmd_vel = self.convert_to_stamped(msg1) + self.cmd_vel_pub.publish(msg_cmd_vel) + self.drive_pub.publish(msg1) + self.drive_vis_pub.publish(msg2) + + def convert_to_stamped(self, ackermann_msgs): + stamped_msg = AckermannDriveStamped() + stamped_msg.header = Header() + stamped_msg.header.stamp = self.get_clock().now().to_msg() # Set the current timestamp + stamped_msg.header.frame_id = '0' # Set the appropriate frame ID + stamped_msg.drive = ackermann_msgs + return stamped_msg + +def main(args=None): + rclpy.init(args=args) + + pure_pursuiter = head_to_goal_control_algorithm() + + rclpy.spin(pure_pursuiter) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + pure_pursuiter.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/action/head_to_goal_control/launch/head_to_goal_launch.py b/src/action/head_to_goal_control/launch/head_to_goal_launch.py new file mode 100644 index 00000000..32f5d289 --- /dev/null +++ b/src/action/head_to_goal_control/launch/head_to_goal_launch.py @@ -0,0 +1,17 @@ +#!/usr/bin/env python3 + + +# no parameters, run ros2 launch head_to_goal_control head_to_goal_launch.py + +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='head_to_goal_control', + executable='controller', + name='Head_To_Goal_Controller', + output='screen' + ), + ]) diff --git a/src/action/head_to_goal_control/package.xml b/src/action/head_to_goal_control/package.xml index 99c2dd2f..d261acc5 100644 --- a/src/action/head_to_goal_control/package.xml +++ b/src/action/head_to_goal_control/package.xml @@ -7,6 +7,13 @@ dyu056 TODO: License declaration + ackermann_msgs + geometry_msgs + std_msgs + moa_msgs + + python3-numpy + ament_copyright ament_flake8 ament_pep257 diff --git a/src/action/head_to_goal_control/setup.py b/src/action/head_to_goal_control/setup.py index c772895d..913eb163 100644 --- a/src/action/head_to_goal_control/setup.py +++ b/src/action/head_to_goal_control/setup.py @@ -1,4 +1,6 @@ from setuptools import find_packages, setup +import os +from glob import glob package_name = 'head_to_goal_control' @@ -10,6 +12,8 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + # include the launch directory + (os.path.join('share', package_name, 'launch'), glob('launch/*.py')), ], install_requires=['setuptools'], zip_safe=True, diff --git a/src/action/pure_pursuit/launch/pure_pursuit_launch.py b/src/action/pure_pursuit/launch/pure_pursuit_launch.py new file mode 100644 index 00000000..1ce0739f --- /dev/null +++ b/src/action/pure_pursuit/launch/pure_pursuit_launch.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python3 + +# to launch +# ros2 launch pure_pursuit pure_pursuit_launch.py + + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ament_index_python import get_package_prefix +from launch.actions import OpaqueFunction +import os + + +def generate_launch_description(): + return LaunchDescription([ + # Declare the 'vis' parameter + DeclareLaunchArgument( + 'vis', + default_value='false', + description='Enable visualization?' + ), + + Node( + package='pure_pursuit', + executable='pure_pursuit', + name='pure_pursuit_controller', + parameters=[{'vis': LaunchConfiguration('vis')}], + output='screen', + ), + ]) \ No newline at end of file diff --git a/src/action/pure_pursuit/pure_pursuit/pure_pursuit.py b/src/action/pure_pursuit/pure_pursuit/pure_pursuit.py index 5f61e735..7259f618 100644 --- a/src/action/pure_pursuit/pure_pursuit/pure_pursuit.py +++ b/src/action/pure_pursuit/pure_pursuit/pure_pursuit.py @@ -1,14 +1,14 @@ #!/usr/bin/env python3 # Python imports -from typing import Optional import rclpy from rclpy.node import Node import numpy as np import math -from geometry_msgs.msg import PoseArray -from geometry_msgs.msg import Pose -from moa_msgs.msg import ConeMap +from geometry_msgs.msg import PoseArray, Pose, Point, Quaternion, Vector3 from ackermann_msgs.msg import AckermannDrive +from foxglove_msgs.msg import LinePrimitive, Color, SceneEntity, SceneUpdate, ArrowPrimitive, SpherePrimitive, PoseInFrame, PosesInFrame +from builtin_interfaces.msg import Time, Duration +from moa_msgs.msg import Track @@ -27,18 +27,39 @@ def __init__(self): # Initializer (normally don't touch) self.steering_angle = 0 - self.pos = (0,0) + self.pos = (0,0) + + # visualisation parameters + self.declare_parameter('vis', False) # Enable/Disable visualization + self.visualization_enabled = self.get_parameter('vis').get_parameter_value().bool_value # subscribe to best trajectory self.best_trajectory_sub = self.create_subscription(PoseArray, "moa/selected_trajectory", self.selected_trajectory_handler, 5) - self.cone_map_sub = self.create_subscription(ConeMap, "cone_map", self.main_hearback, 5) + + self.cone_map_sub = self.create_subscription(Track, "cone_map", self.main_hearback, 5) + + self.create_subscription(Pose, "car_position", self.get_car_position, 5) self.cmd_vel_pub = self.create_publisher(AckermannDrive, "/drive", 5) self.cmd_vis_pub = self.create_publisher(AckermannDrive, "/drive_vis", 5) self.track_point_pub = self.create_publisher(Pose, "moa/track_point", 5) - def main_hearback(self, msg: ConeMap): + if self.visualization_enabled: + self.viz_pub = self.create_publisher(SceneUpdate, 'control_visualization', 5) + # selected path + self.chosen_path = self.create_subscription(AckermannDrive, "/drive_vis", self.show_drive_path, 5) + self.next_destination = self.create_subscription(Pose, "moa/track_point", self.save_pursue_destination, 5) + self.cone_map_sub = self.create_subscription(Pose, "car_position", self.get_transformations, 5) + + self.id = 1 + + else: + self.get_logger().info("Visualization is disabled") + + self.get_logger().info("Pure Pursuit Node Started") + + def main_hearback(self, msg: Track): # Update car's current location and update transformation matrix - self.car_pose = msg.cones[0].pose.pose + self.car_pose = self.car_position_pose self.position_vector, self.rotation_matrix_l2g, self.rotation_matrix_g2l = self.convert_to_transformation_matrix(self.car_pose.position.x, self.car_pose.position.y, self.car_pose.orientation.w) # Before proceed, check whether we have a trajectory input @@ -70,7 +91,35 @@ def saturating_steering(self, steering_angle): steering_angle = -1 * saturation return steering_angle - + + def single_trajectory_generator(self, steering_radius): + # https://dingyan89.medium.com/simple-understanding-of-kinematic-bicycle-model-81cac6420357 is used for + # bicycle steering + R = steering_radius + t_range = np.arange(0, np.pi/2, 0.01); + trajectory_output = PoseArray(); + if hasattr(self, "position_vector") and hasattr(self, "rotation_matrix_l2g") and hasattr(self, "rotation_matrix_g2l"): + for individual_t in t_range: + pose_input = Pose(); + x_pre_trans = np.cos(individual_t) * R - R + y_pre_trans = abs(np.sin(individual_t) * R) + post_trans_point = self.apply_transformation(self.position_vector, self.rotation_matrix_l2g, x_pre_trans, y_pre_trans); + pose_input.position.x = post_trans_point[0][0] + pose_input.position.y = post_trans_point[1][0] + trajectory_output.poses.append(pose_input) + else: + self.get_logger().info("Warning: Transformation data not acquired, no trajectory produced") + return trajectory_output + + def apply_transformation(self, position_vector, rotation_matrix, point_x, point_y): + point = np.array([[point_x], [point_y]]) + transformed_point = np.matmul(rotation_matrix, point) + position_vector + return transformed_point + + def get_transformations(self, msg: Pose): + # Update car's current location and update transformation matrix + self.car_pose = msg + self.position_vector, self.rotation_matrix_l2g, self.rotation_matrix_g2l = self.convert_to_transformation_matrix(self.car_pose.position.x, self.car_pose.position.y, self.car_pose.orientation.w) # Coordinate tranformer def convert_to_transformation_matrix(self, x: float, y: float, theta: float) -> ( @@ -129,6 +178,57 @@ def get_track_point_in_global_frame(self, track_point_in_local_frame: Pose): self.get_logger.info("Warning: Local pose message not transformed to global frame") return track_point_in_local_frame + def show_chosen_path(self, msg: PoseArray): + tcols = Color(r=0.0, g=255.0, b=0.0, a=1.0) + pts = [] + line_list = []; + for j in range(len(msg.poses)): + # get a particular pose + _ = msg.poses[j].position + pts.append(_) + args = {'type': LinePrimitive.LINE_STRIP, + 'pose': Pose(position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=0.0)), + 'thickness': 2.0, + 'scale_invariant': True, + 'points': pts, + 'color': tcols} + #line_list.append(LinePrimitive(**args)) + + # arrow primitive code if needed + # args = {'pose': Pose(position=Point(x=1.0,y=0.0,z=0.0), orientation=Quaternion(x=0.0,y=0.0,z=0.0,w=0.0)), + # 'shaft_length': 1.0, + # 'shaft_diameter': 0.1, + # 'head_length': 2.5, + # 'head_diameter': 0.5, + # 'color': Color(r=67.0,g=125.0,b=100.0,a=1.0)} + # msg = ArrowPrimitive(**args) + + # scene entity encapsulates these primitive objects + sargs = {'timestamp': Time(sec=0,nanosec=0), + 'frame_id': 'global_frame', + 'id': f'{self.id}', + 'lifetime': Duration(sec=3,nanosec=0), + 'frame_locked': False, + 'lines': line_list, + 'spheres': self.next_destination_vis} + + # scene update is a wrapper for scene entity + scene_update_msg = SceneUpdate(entities=[SceneEntity(**sargs)]) + + self.pubviz.publish(scene_update_msg) + #self.get_logger().info("Published msg") + + self.id += 1 + + def show_drive_path(self, msg: AckermannDrive): + steering_angle = msg.steering_angle + if steering_angle == 0: + steering_angle = 1e-9 + steering_radius = 1 / steering_angle + trajectory_from_steer = self.single_trajectory_generator(steering_radius) + self.show_chosen_path(trajectory_from_steer) + # Picking and maintaining a track point def update_track_point(self, msg: PoseArray): #Main logic # Pick new tracking point if no tracking point is selected or old tracking point is no longer visible @@ -206,6 +306,35 @@ def publish_ackermann(self): msg2 = AckermannDrive(**args2) self.cmd_vel_pub.publish(msg1) self.cmd_vis_pub.publish(msg2) + + def get_car_position(self, msg:Pose): self.car_position_pose = msg + + + def visualize_trajectory(self): + tcols = Color(r=0.0, g=255.0, b=0.0, a=1.0) # Green trajectory + if hasattr(self, "trajectory_in_global_frame"): + pts = [pose.position for pose in self.trajectory_in_global_frame.poses] + line = LinePrimitive(type=LinePrimitive.LINE_STRIP, points=pts, color=tcols, thickness=0.2) + scene_msg = SceneUpdate( + entities=[SceneEntity( + id=f'trajectory_{self.id_counter}', + lines=[line], + timestamp=Time() + )] + ) + self.viz_pub.publish(scene_msg) + self.id_counter += 1 + + + def save_pursue_destination(self, msg : Pose): + tcols = Color(r=255.0, g=255.0, b=0.0, a=1.0) + args = {'pose': msg, + 'size': Vector3(x=1.0, y=1.0, z=1.0), + 'color': tcols} + if len(self.next_destination_vis) == 0: + self.next_destination_vis.append(SpherePrimitive(**args)) + else: + self.next_destination_vis[0] = SpherePrimitive(**args) def main(args=None): rclpy.init(args=args) diff --git a/src/action/pure_pursuit/setup.py b/src/action/pure_pursuit/setup.py index d61a0d9b..430ab23e 100644 --- a/src/action/pure_pursuit/setup.py +++ b/src/action/pure_pursuit/setup.py @@ -1,4 +1,6 @@ from setuptools import find_packages, setup +import os +from glob import glob package_name = 'pure_pursuit' @@ -10,6 +12,8 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + # include the launch directory + (os.path.join('share', package_name, 'pure_pursuit'), glob('launch/*.py')), ], install_requires=['setuptools'], zip_safe=True, diff --git a/src/action/stanley_controller/package.xml b/src/action/stanley_controller/package.xml index 1c3a5a92..4f0c7013 100644 --- a/src/action/stanley_controller/package.xml +++ b/src/action/stanley_controller/package.xml @@ -7,6 +7,14 @@ yiyang Apache-2.0 + ackermann_msgs + geometry_msgs + std_msgs + moa_msgs + + python3-numpy + python3-matplotlib + ament_copyright ament_flake8 ament_pep257 diff --git a/src/action/stanley_controller/stanley_controller/stanley_controller.py b/src/action/stanley_controller/stanley_controller/stanley_controller.py index 35e5601c..2b8b625a 100644 --- a/src/action/stanley_controller/stanley_controller/stanley_controller.py +++ b/src/action/stanley_controller/stanley_controller/stanley_controller.py @@ -4,8 +4,8 @@ import math from geometry_msgs.msg import PoseArray from geometry_msgs.msg import Pose -from moa_msgs.msg import ConeMap -from ackermann_msgs.msg import AckermannDrive +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped +from std_msgs.msg import Header def angle_mod(x, zero_2_2pi=False, degree=False): """ @@ -72,45 +72,74 @@ def __init__(self): super().__init__('Stanley_Controller') self.get_logger().info("Stanley Controller Node Started") + self.declare_parameters( + namespace='', + parameters=[ + ('vel', 8.0), + ] + ) + #Constants self.k_stanley = 5.0 #stanley Controller gain self.k_speed = 1.0 #speed Controller gain self.cam_fron_axle_dist= 1 #[m] Wheel base of vehicle - self.max_steer = np.radians(27.0) # [rad] max steering angle - self.target_speed = 150/3.6 #[m/s] + self.max_steer = 30.0 # [degrees] max steering angle + self.target_speed = self.get_parameter('vel').get_parameter_value() #[m/s] + + self.trajectory_in_global_frame = PoseArray() + self.tx = [] + self.ty = [] + + # The max steering angle is now +-30 #Subscribe for car pose and track - self.create_subscription(PoseArray, "moa/selected_trajectory", self.selected_trajectory_handler, 5) - self.create_subscription(ConeMap, "cone_map", self.main_hearback, 5) + self.create_subscription(PoseArray, "selected_trajectory", self.selected_trajectory_handler, 5) + self.create_subscription(Pose, "car_position", self.main_hearback, 5) #Publish result - self.cmd_vel_pub = self.create_publisher(AckermannDrive, "/drive", 5) - self.cmd_vis_pub = self.create_publisher(AckermannDrive, "/drive_vis", 5) - self.create_publisher(Pose, "moa/track_point", 5) + self.cmd_drive_pub = self.create_publisher(AckermannDrive, "drive", 5) + self.cmd_vis_pub = self.create_publisher(AckermannDrive, "drive_vis", 5) + self.cmd_vel_pub = self.create_publisher(AckermannDriveStamped, "cmd_vel", 5) + self.create_publisher(Pose, "track_point", 5) def main_hearback(self, msg): - car_pose = msg.cones[0].pose.pose + car_pose = msg camera_position = [car_pose.position.x,car_pose.position.y] car_yaw = car_pose.orientation.w + self.car_yaw_corrected = self.normalize_angle(car_yaw-4.71) + + # self.target_speed = 10.0 + self.target_speed = self.get_parameter('vel').get_parameter_value().double_value + + self.get_logger().info(f"car_yaw: {car_yaw}, car_yaw_corrected: {self.car_yaw_corrected}") - if hasattr(self, "trajectory_in_global_frame"): + if len(self.tx) > 1 and len(self.ty) > 1: #Get Car front axle center position - axle_pos = self.get_front_axle_position(camera_position,car_yaw) + axle_pos = self.get_front_axle_position(camera_position,self.car_yaw_corrected) #Get closest point on track and distance error - cls_point,error_front_axle = self.get_closest_track_point(axle_pos,car_yaw) - #Compute target yaw + cls_point,error_front_axle = self.get_closest_track_point(axle_pos,self.car_yaw_corrected) + #Compute target yaw - Angle from positive x in radians target_yaw = self.cal_target_yaw(cls_point) #Compute steering angle - theta_e = self.normalize_angle(target_yaw-car_yaw) - theta_d = np.arctan2(self.k_stanley * error_front_axle, self.target_speed) - delta = theta_e + theta_d + theta_e = -self.circular_diff(target_yaw, self.car_yaw_corrected) + theta_d = -(np.arctan2(self.k_stanley * error_front_axle, self.target_speed)) + + self.get_logger().info(f"target_yaw: {target_yaw}") + + delta = math.degrees(theta_e + theta_d) delta = np.clip(delta, -self.max_steer, self.max_steer) + + self.get_logger().info(f"steering angle: {delta}") + self.steering_angle = delta + self.target_speed = self.target_speed else: self.steering_angle = 0 - self.get_logger().info("Warning: no trajectory found, will set steering angle to 0!!!!") + self.target_speed = 0 + self.get_logger().warn("Warning: no trajectory found, will set steering angle to 0!!!!") # Publish command for velocity self.publish_ackermann() + def selected_trajectory_handler(self, msg): self.trajectory_in_global_frame = msg @@ -136,8 +165,16 @@ def publish_ackermann(self): "acceleration": 0.0, "jerk": 0.0} msg2 = AckermannDrive(**args2) - self.cmd_vel_pub.publish(msg1) + + args3 = {"header": Header(stamp=self.get_clock().now().to_msg(),frame_id="stanley_controller"), + "drive": msg1} + msg3 = AckermannDriveStamped(**args3) + + self.get_logger().warn('Sending Angle: ' + str(self.steering_angle)) + + self.cmd_drive_pub.publish(msg1) self.cmd_vis_pub.publish(msg2) + self.cmd_vel_pub.publish(msg3) def get_front_axle_position(self,cam_pos,car_yaw): @@ -145,6 +182,7 @@ def get_front_axle_position(self,cam_pos,car_yaw): return axle_pos def get_closest_track_point(self,axle_pos,car_yaw): + # self.get_logger().info(f"target position: {self.tx}, {self.ty}") dx = list(axle_pos[0]-np.asarray(self.tx)) dy = list(axle_pos[1]-np.asarray(self.ty)) d = np.hypot(dx, dy) @@ -156,13 +194,35 @@ def get_closest_track_point(self,axle_pos,car_yaw): return target_idx, error_front_axle def cal_target_yaw(self,cls_point): + if(cls_point==(len(self.ty)-1)): + dy = self.ty[cls_point]-self.ty[cls_point-1] + dx = self.tx[cls_point]-self.tx[cls_point-1] + else: + dy = self.ty[cls_point+1]-self.ty[cls_point] + dx = self.tx[cls_point+1]-self.tx[cls_point] + + target_yaw_op1 = self.normalize_angle(np.arctan2(dy,dx)) + target_yaw_op2 = self.normalize_angle(target_yaw_op1 + np.pi) + + yaw_diff_1 = abs(self.circular_diff(target_yaw_op1, self.car_yaw_corrected)) + yaw_diff_2 = abs(self.circular_diff(target_yaw_op2, self.car_yaw_corrected)) + target_yaw2 = target_yaw_op1 if yaw_diff_1rclpy std_msgs + ackermann_msgs ament_copyright ament_flake8 diff --git a/src/action/steer_torque_from_ackermann/setup.py b/src/action/steer_torque_from_ackermann/setup.py index 68f05ce7..39c3af03 100644 --- a/src/action/steer_torque_from_ackermann/setup.py +++ b/src/action/steer_torque_from_ackermann/setup.py @@ -1,4 +1,6 @@ from setuptools import find_packages, setup +import os +from glob import glob package_name = 'steer_torque_from_ackermann' @@ -10,11 +12,13 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + # launch files + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.py'))) ], install_requires=['setuptools'], zip_safe=True, - maintainer='Daniel Yu', - maintainer_email='daniel.yu@fsae.co.nz', + maintainer='Jonty Clark', + maintainer_email='jonty.clark@fsae.co.nz', description='Controller node for simulation car to control its speed and steering angle', license='TODO: License declaration', tests_require=['pytest'], diff --git a/src/action/steer_torque_from_ackermann/steer_torque_from_ackermann/converter.py.save b/src/action/steer_torque_from_ackermann/steer_torque_from_ackermann/converter.py.save new file mode 100644 index 00000000..57e9cdaf --- /dev/null +++ b/src/action/steer_torque_from_ackermann/steer_torque_from_ackermann/converter.py.save @@ -0,0 +1,115 @@ +# Copyright 2016 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. + +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Float32, Float64 +from ackermann_msgs.msg import AckermannDrive + + +class ackermann_to_steer(Node): + + def __init__(self): + super().__init__('ackermann_to_steer_torque_angle') + + # Declare a parameter with a default value + self.declare_parameter('car_name', 'test') + car_name = self.get_parameter('car_name').get_parameter_value().string_value + + self.Integral_speed = 0 + self.Integral_angle = 0 + + self.speed_ref = 0 + self.steering_angle_ref = 0 + + # Publishing topics + steering_topic = "/" + car_name + "/cmd_steering" + torque_topic = "/" + car_name + "/cmd_throttle" + speed_topic = "/" + car_name + "/speed" + + self.steering_publisher = self.create_publisher(Float32, steering_topic, 10) + self.torque_publisher = self.create_publisher(Float32, torque_topic, 10) + + # Subscribing topics + self.ackermann_subscribe = self.create_subscription( + AckermannDrive, + '/drive', + self.set_reference, + 10) + self.ackermann_subscribe # prevent unused variable warning + + self.feedback_subscribe = self.create_subscription( + Float64, + speed_topic, + self.PI_controller, + 10) + self.feedback_subscribe # prevent unused variable warning + + # Tune the following parameter for PI controller + # Only turn on I if P term cant make turn angle and speed converge + self.P_speed = 1000 + self.I_speed = + + # Tune these two only if need + self.abs_integral_speed_max = 400 #400 before change + self.abs_integral_angle_max = 20.000 #20.000 before change + + print("Initialized") + + def PI_controller(self, msg): + current_speed = msg.data + + error_speed = self.speed_ref - current_speed + + if abs(self.Integral_speed) <= self.abs_integral_speed_max: + self.Integral_speed += current_speed + + output_torque = error_speed * self.P_speed + self.Integral_speed * self.I_speed + + msg_torque = Float32() + + msg_torque.data = output_torque + #print(current_speed) + self.torque_publisher.publish(msg_torque) + + def set_reference(self, msg): + self.speed_ref = msg.speed + target_steer = msg.steering_angle + + msg_steer = Float32() + if target_steer > self.abs_integral_angle_max: + target_steer = float(self.abs_integral_angle_max) + elif target_steer < -1 * self.abs_integral_angle_max: + target_steer = float(-1 * self.abs_integral_angle_max) + + msg_steer.data = target_steer + self.steering_publisher.publish(msg_steer) + +def main(args=None): + rclpy.init(args=args) + + ackermann_to_steer_node = ackermann_to_steer() + + rclpy.spin(ackermann_to_steer_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + ackermann_to_steer_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/driverless_sim/simulator/launch/simulator.launch.py b/src/driverless_sim/simulator/launch/simulator.launch.py new file mode 100644 index 00000000..6b0cd018 --- /dev/null +++ b/src/driverless_sim/simulator/launch/simulator.launch.py @@ -0,0 +1,53 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ament_index_python import get_package_prefix +from launch.actions import OpaqueFunction +import os + +# Launches nodes within simulator package where get_cones, get_car_position and set_car_controls nodes are ran by default +def generate_launch_description() -> LaunchDescription: + package_name = 'simulator' + package_dir = os.path.join(get_package_prefix(package_name), 'lib', package_name) # directory of installed node names + installed_nodes = os.listdir(package_dir) # retrieves installed node names + + launch_descriptions = [ + # launch arguments + DeclareLaunchArgument( + 'node_name', + default_value='get_cones get_car_position set_car_controls', + description='which node(s) from this package to launch' + ), + DeclareLaunchArgument( + 'viz', + default_value='True', + description='visualise node output?' + ) + ] + + nodes = OpaqueFunction(function=get_nodes, args=[package_name, installed_nodes]) # get a list of actions + launch_descriptions.append(nodes) + + return LaunchDescription(launch_descriptions) + +def get_nodes(context, package_name, installed_nodes): + NODES = [] + nodes_2_run = LaunchConfiguration('node_name').perform(context) # get runtime value of argument + + for node_name in installed_nodes: + if node_name in nodes_2_run: + _ = Node( + package=package_name, + executable=node_name, + name=node_name, + parameters=[{'viz': LaunchConfiguration('viz')}] + ) + + NODES.append(_) + + if len(NODES) == 0: + raise Exception(f"selected node(s) {nodes_2_run} does not exist!") + + return NODES + diff --git a/src/driverless_sim/simulator/package.xml b/src/driverless_sim/simulator/package.xml new file mode 100644 index 00000000..1afae563 --- /dev/null +++ b/src/driverless_sim/simulator/package.xml @@ -0,0 +1,28 @@ + + + + simulator + 0.0.0 + TODO: Package description + tanish + TODO: License declaration + + ackermann_msgs + geometry_msgs + std_msgs + moa_msgs + + python3-numpy + python3-matplotlib + + rclpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/perception/cone-detection/resource/cone_detection b/src/driverless_sim/simulator/resource/simulator similarity index 100% rename from src/perception/cone-detection/resource/cone_detection rename to src/driverless_sim/simulator/resource/simulator diff --git a/src/driverless_sim/simulator/setup.cfg b/src/driverless_sim/simulator/setup.cfg new file mode 100644 index 00000000..abfbe9c1 --- /dev/null +++ b/src/driverless_sim/simulator/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/simulator +[install] +install_scripts=$base/lib/simulator diff --git a/src/driverless_sim/simulator/setup.py b/src/driverless_sim/simulator/setup.py new file mode 100644 index 00000000..14dd9559 --- /dev/null +++ b/src/driverless_sim/simulator/setup.py @@ -0,0 +1,32 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = 'simulator' + +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']), + # launch files + (os.path.join('share',package_name,'launch'), glob(os.path.join('launch', '*.py*'))) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='tanish', + maintainer_email='Tanish.Bhatt@fsae.co.nz', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'get_cones = simulator.get_cones:main', + 'get_car_position = simulator.get_car_position:main', + 'set_car_controls = simulator.set_car_controls:main', + ], + }, +) diff --git a/src/perception/cone_mapping/cone_mapping/cone_mapping/__init__.py b/src/driverless_sim/simulator/simulator/__init__.py similarity index 100% rename from src/perception/cone_mapping/cone_mapping/cone_mapping/__init__.py rename to src/driverless_sim/simulator/simulator/__init__.py diff --git a/src/driverless_sim/simulator/simulator/get_car_position.py b/src/driverless_sim/simulator/simulator/get_car_position.py new file mode 100644 index 00000000..fb7b16f0 --- /dev/null +++ b/src/driverless_sim/simulator/simulator/get_car_position.py @@ -0,0 +1,61 @@ +#!/usr/bin/python3 +import rclpy +from rclpy.node import Node + +from geometry_msgs.msg import Pose, Point, Quaternion + +import os +import sys +import matplotlib.pyplot as plt +import numpy as np + +## adds the fsds package located the parent directory to the pyhthon path +path = os.path.abspath(os.path.join('Formula-Student-Driverless-Simulator', 'python')) +sys.path.insert(0, path) +# sys.path.append('/home/Formula-Student-Driverless-Simulator/python') +# print(sys.path) +import fsds + +class get_car_position(Node): + def __init__(self): + super().__init__("get_car_position") + + # connect to the simulator + self.client = fsds.FSDSClient() + # Check network connection, exit if not connected + self.client.confirmConnection() + # After enabling setting trajectory setpoints via the api. + self.client.enableApiControl(False) + # create publisher + self.sim_car_pub = self.create_publisher(Pose, 'car_position', 10) + + self.create_timer(1.0, self.get_car_position) + + def get_car_position(self): + """publishes current position of the simulator car""" + state = self.client.getCarState() + position = state.kinematics_estimated.position # car kinetmatics in ENU coordinates + orientation = state.kinematics_estimated.orientation + + msg = Pose() + msg.position = Point(x=position.x_val, y=position.y_val, z=0.0) # add position information to Pose msg + msg.orientation = Quaternion(x=orientation.x_val,y=orientation.y_val,z=orientation.z_val,w=orientation.w_val,) + + self.sim_car_pub.publish(msg) # publish msg + +def main(args=None): + rclpy.init(args=args) + + simulator_car_position = get_car_position() + + rclpy.spin(simulator_car_position) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + simulator_car_position.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/driverless_sim/simulator/simulator/get_cones.py b/src/driverless_sim/simulator/simulator/get_cones.py new file mode 100644 index 00000000..6fcd737b --- /dev/null +++ b/src/driverless_sim/simulator/simulator/get_cones.py @@ -0,0 +1,163 @@ +#!/usr/bin/python3 +import rclpy +from rclpy.node import Node + +from geometry_msgs.msg import Point +from moa_msgs.msg import ConeMap + +import os +import sys +import matplotlib.pyplot as plt +import numpy as np + +## adds the fsds package located the parent directory to the pyhthon path +path = os.path.abspath(os.path.join('Formula-Student-Driverless-Simulator', 'python')) +sys.path.insert(0, path) +# sys.path.append('/home/Formula-Student-Driverless-Simulator/python') +# print(sys.path) +import fsds + +class get_cones(Node): + def __init__(self): + super().__init__("get_cones") + + self.plot = False + # connect to the simulator + self.client = fsds.FSDSClient() + # Check network connection, exit if not connected + self.client.confirmConnection() + # After enabling setting trajectory setpoints via the api. + self.client.enableApiControl(False) + # create publisher + self.sim_cone_pub = self.create_publisher(ConeMap, "cone_map", 10) + + self.create_timer(1.0, self.get_cones_from_simulator) + + + def get_cones_from_simulator(self): + lb, rb, start_end = self.find_cones() # detect cones in simulation + + msg = self.get_cone_map_msg(lb.copy(),rb.copy()) # convert the boundaries to point list + + self.sim_cone_pub.publish(msg) # publish + + if self.plot: + plt.show() + while True: + plt.pause(0.05) + plt.clf() + + # get current position + x,y,z = self.get_car_position() + + # plot + plt.plot([P[0] for P in lb], [P[1] for P in lb], ".b", label="left") + plt.plot([P[0] for P in rb], [P[1] for P in rb], ".y", label="right") + plt.plot([P[0] for P in start_end], [P[1] for P in start_end], "*k", label='start/end') + plt.plot(x,y,".r",label="car") + # for i in range(len(lb)): + # plt.annotate(f"{i}",(lb[i][0], lb[i][1])) + # for i in range(len(rb)): + # plt.annotate(f"{i}",(rb[i][0], rb[i][1])) + + plt.legend() + # plt.gca().invert_yaxis() + + + def get_car_position(self): + """returns current position of the simulator car""" + position = self.client.getCarState().kinematics_estimated.position # car kinetmatics in ENU coordinates + + return position.x_val, position.y_val, 0.0 + + + def find_cones(self): + """ Detects cones in the simulator + HOWEVER at the moment the exact positions of the cones is requested from API (change asap)""" + # referee state + ref_state = self.client.getRefereeState() + + cones = ref_state.cones # list of cones where each cone is in a dictionary format e.g., [{cone1}, {cone2}, {cone3}] + leftboundary = [] + rightboundary = [] + start_end = [] + + for cone_dict in cones: + color = cone_dict['color'] + x = cone_dict['x'] + y = cone_dict['y'] + z = 0.0 # z value is currently not provided + + x,y,z = self.get_local_ENU([x,y,z]) # transform from UU to ENU coordinates + + if color == 0: # type 0 color cone is blue + rightboundary.append([x,y,z]) + elif color == 1: # type 1 color cone is yellow + leftboundary.append([x,y,z]) + elif color == 2: # orange start/end cones + start_end.append([x,y,z]) + + return leftboundary, rightboundary, start_end + + def get_local_ENU(self,position, world_to_meters=100): + """Transform Unreal engine coordinates (UU) to AirSim coordiantes (ENU) - currently there is translation and scale (cm to m) I think not 100% sure + For more details please see the below documents + 1. https://fs-driverless.github.io/Formula-Student-Driverless-Simulator/v2.2.0/coordinate-frames/#unreal-engine + 2. https://github.com/FS-Driverless/Formula-Student-Driverless-Simulator/blob/master/UE4Project/Plugins/AirSim/Source/CoordFrameTransformer.cpp + """ + local_offset = np.array([4575.15,8577.82,0]) # translation vector from UU to ENU (fixed for now) + return self.get_vector3r(position - local_offset, 1/world_to_meters) + + + def get_vector3r(self, vec, scale): + """Scales the given vector using the given real scalar""" + return [vec[0]*scale, -vec[1]*scale, vec[2]*scale] + + + def get_cone_map_msg(self, lb, rb): + try: + assert len(lb) == len(rb) + except Exception as e: + self.get_logger().error(e) + finally: + for i in range(len(lb)): + lb[i] = Point(x=lb[i][0],y=lb[i][1],z=lb[i][2]) + rb[i] = Point(x=rb[i][0],y=rb[i][1],z=rb[i][2]) + + return ConeMap(left_cones=lb, right_cones=rb) + + + # DEPRECATED/NOT USED + # def get_transformed_point(self, x, y): + # translation_matrix = np.array([ + # [1,0,4575.15], + # [0,1,8577.81], + # [0,0,1] + # ]) + # rotation_matrix = np.array([ + # [1,0,0], + # [0,-1,0], + # [0,0,-1] + # ]) + # P = np.array([[x],[y],[1]]) + # transformation_matrix = translation_matrix + + # return np.linalg.inv(transformation_matrix) @ P + + +def main(args=None): + rclpy.init(args=args) + + simulator_cones = get_cones() + + rclpy.spin(simulator_cones) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + simulator_cones.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/driverless_sim/simulator/simulator/set_car_controls.py b/src/driverless_sim/simulator/simulator/set_car_controls.py new file mode 100644 index 00000000..20723034 --- /dev/null +++ b/src/driverless_sim/simulator/simulator/set_car_controls.py @@ -0,0 +1,66 @@ +#!/usr/bin/python3 +import rclpy +from rclpy.node import Node + +from ackermann_msgs.msg import AckermannDrive + +import os +import sys +import numpy as np + +## adds the fsds package located the parent directory to the pyhthon path +path = os.path.abspath(os.path.join('Formula-Student-Driverless-Simulator', 'python')) +sys.path.insert(0, path) +# sys.path.append('/home/Formula-Student-Driverless-Simulator/python') +# print(sys.path) +import fsds + +class set_car_controls(Node): + def __init__(self): + super().__init__("set_car_controls") + + self.max_throttle = 21 # m/s + self.max_steering = 25 # degrees + # connect to the simulator + self.client = fsds.FSDSClient() + # Check network connection, exit if not connected + self.client.confirmConnection() + # After enabling setting trajectory setpoints via the api. + self.client.enableApiControl(True) + # create publisher + self.create_subscription(AckermannDrive, 'drive', self.callback, 10) + + + def callback(self, msg:AckermannDrive): + """Set the car controls in the simulator using the given ackerman msg""" + steering = msg.steering_angle + speed = msg.speed + + steering, throttle = self.get_simulator_controls(steering, speed) + + # class is part of types.py file in sim repo + self.client.setCarControls(fsds.CarControls(throttle=throttle,steering=steering,brake=0.0)) + + def get_simulator_controls(self, steering, speed): + throttle = speed/self.max_throttle # max throttle is 1 in sim where the max speed is ~20m/s + steering = steering/self.max_steering # max absolute steering is 1 in sim where the max steering angle is 25 degrees by default + + return steering, throttle + + +def main(args=None): + rclpy.init(args=args) + + simulator_car_controls = set_car_controls() + + rclpy.spin(simulator_car_controls) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + simulator_car_controls.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/perception/cone-detection/test/test_copyright.py b/src/driverless_sim/simulator/test/test_copyright.py similarity index 100% rename from src/perception/cone-detection/test/test_copyright.py rename to src/driverless_sim/simulator/test/test_copyright.py diff --git a/src/perception/cone-detection/test/test_flake8.py b/src/driverless_sim/simulator/test/test_flake8.py similarity index 100% rename from src/perception/cone-detection/test/test_flake8.py rename to src/driverless_sim/simulator/test/test_flake8.py diff --git a/src/perception/cone-detection/test/test_pep257.py b/src/driverless_sim/simulator/test/test_pep257.py similarity index 100% rename from src/perception/cone-detection/test/test_pep257.py rename to src/driverless_sim/simulator/test/test_pep257.py diff --git a/src/hardware_drivers/CanTalk b/src/hardware_drivers/CanTalk index f887c380..78c54484 160000 --- a/src/hardware_drivers/CanTalk +++ b/src/hardware_drivers/CanTalk @@ -1 +1 @@ -Subproject commit f887c38038fd4ba4a87c7075ca9f883bd60670bb +Subproject commit 78c5448404859899a7340ca71cf051b7556278bf diff --git a/src/hardware_drivers/zed-ros2-wrapper b/src/hardware_drivers/zed-ros2-wrapper deleted file mode 160000 index 9fd5fd38..00000000 --- a/src/hardware_drivers/zed-ros2-wrapper +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 9fd5fd388e8dd57f9719d3f2e7061c3b69d58b52 diff --git a/src/perception/aruco_detection/LICENSE b/src/kart_lap_attempt/LICENSE similarity index 100% rename from src/perception/aruco_detection/LICENSE rename to src/kart_lap_attempt/LICENSE diff --git a/src/perception/localization/localization/__init__.py b/src/kart_lap_attempt/kart_lap_attempt/__init__.py similarity index 100% rename from src/perception/localization/localization/__init__.py rename to src/kart_lap_attempt/kart_lap_attempt/__init__.py diff --git a/src/kart_lap_attempt/launch/crown_launch.py b/src/kart_lap_attempt/launch/crown_launch.py new file mode 100644 index 00000000..3326b081 --- /dev/null +++ b/src/kart_lap_attempt/launch/crown_launch.py @@ -0,0 +1,92 @@ +import launch +import launch.actions +import launch_ros.actions +from launch import LaunchDescription +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + invert_cones = True + visualize = False + + base_description = [ + DeclareLaunchArgument( + 'can_id', + default_value='0x300', + description='The frame ID for the CAN messages containing Ackermann commands that are sent to the car' + ), + Node( + namespace='moa', + package='zed_launch', + executable='zed_launch_node', + name='perception' + ), + Node( + namespace='moa', + package='cone_mapping', + executable='kalman_filter', + name='kalman_filter', + parameters=[ + {'invert_cones': invert_cones}, + ], + ), + Node( + namespace='moa', + package='path_planning', + executable='fasttube', + name='centerline_planner', + parameters=[ + {'invert_cones': invert_cones}, + ], + ), + Node( + namespace='moa', + package='stanley_controller', + executable='controller', + name='stanley_controller', + parameters=[ + {'vel': 9.0}, + ], + ), + launch_ros.actions.Node( + namespace='moa', + package="CanTalk", + executable="candapter_node", + output="screen" + ), + launch_ros.actions.Node( + namespace='moa', + package="moa_controllers", + executable="ack_to_can_node", + name='ack_to_can', + parameters=[{'can_id': LaunchConfiguration('can_id')}], + output="screen" + ), + # visualisation + ] + visual_description = [ + launch_ros.actions.Node( + namespace='moa', + package="path_planning_visualiser", + executable="image_throttler", + name='image_throttler', + output="screen", + ), + launch_ros.actions.Node( + package='foxglove_bridge', + executable='foxglove_bridge', + name='foxglove_bridge', + # parameters=[{'port':8765, 'topic_whitelist': + # ["/moa/image_throttled", "/moa/cmd_vel", + # "/moa/selected_trajectory", "/moa/car_position", + # "/moa/cone_detection", "/moa/times_modified"]}] + parameters=[{'port':8765, 'topic_whitelist': ["/moa/image_throttled"]}] + ) + ] + description = base_description + + if (visualize): + description.extend(visual_description) + + return LaunchDescription(description) diff --git a/src/kart_lap_attempt/launch/tech_night_launch.py b/src/kart_lap_attempt/launch/tech_night_launch.py new file mode 100644 index 00000000..4e7322a7 --- /dev/null +++ b/src/kart_lap_attempt/launch/tech_night_launch.py @@ -0,0 +1,65 @@ +import launch +import launch.actions +import launch_ros.actions +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'can_id', + default_value='0x300', + description='The frame ID for the CAN messages containing Ackermann commands that are sent to the car' + ), + Node( + namespace='moa', + package='zed_launch', + executable='zed_launch_node', + name='perception' + ), + Node( + namespace='moa', + package='path_planning', + executable='fasttube_without_kalman', + name='centerline_planner' + ), + Node( + namespace='moa', + package='stanley_controller', + executable='controller', + name='stanley_controller', + parameters=[ + {'vel': 10.0}, + ], + ), + launch_ros.actions.Node( + namespace='moa', + package="CanTalk", + executable="candapter_node", + output="screen" + ), + launch_ros.actions.Node( + namespace='moa', + package="moa_controllers", + executable="ack_to_can_node", + name='ack_to_can', + parameters=[{'can_id': LaunchConfiguration('can_id')}], + output="screen" + ), + # visualisation + launch_ros.actions.Node( + namespace='moa', + package="path_planning_visualiser", + executable="image_throttler", + name='image_throttler', + output="screen", + ), + launch_ros.actions.Node( + package='foxglove_bridge', + executable='foxglove_bridge', + name='foxglove_bridge', + parameters=[{'port':8765, 'topic_whitelist': ["/moa/image_throttled", "/moa/cmd_vel", "/moa/selected_trajectory", "/moa/car_position", "/moa/cone_detection", "/moa/times_modified"]}] + ), + ]) diff --git a/src/kart_lap_attempt/package.xml b/src/kart_lap_attempt/package.xml new file mode 100644 index 00000000..32d9bf6f --- /dev/null +++ b/src/kart_lap_attempt/package.xml @@ -0,0 +1,34 @@ + + + + kart_lap_attempt + 0.0.1 + Autonomous kart lap attempt package + sivasriram + Apache-2.0 + + + rclpy + launch + launch_ros + zed_launch + path_planning + stanley_controller + moa_controllers + CanTalk + cone_mapping + ros2launch + path_planning_visualiser + + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + + ament_python + + diff --git a/src/perception/cone_mapping/cone_mapping/resource/cone_mapping b/src/kart_lap_attempt/resource/kart_lap_attempt similarity index 100% rename from src/perception/cone_mapping/cone_mapping/resource/cone_mapping rename to src/kart_lap_attempt/resource/kart_lap_attempt diff --git a/src/kart_lap_attempt/setup.cfg b/src/kart_lap_attempt/setup.cfg new file mode 100644 index 00000000..d213e7fa --- /dev/null +++ b/src/kart_lap_attempt/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/kart_lap_attempt +[install] +install_scripts=$base/lib/kart_lap_attempt diff --git a/src/kart_lap_attempt/setup.py b/src/kart_lap_attempt/setup.py new file mode 100644 index 00000000..93657a62 --- /dev/null +++ b/src/kart_lap_attempt/setup.py @@ -0,0 +1,29 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = 'kart_lap_attempt' + +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']), + # Include all launch files + (os.path.join('share', package_name, 'launch'), glob('launch/*.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='sivasriram', + maintainer_email='siva.sriram1604@gmail.com', + description='TODO: Package description', + license='Apache-2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) \ No newline at end of file diff --git a/src/perception/cone_mapping/cone_mapping/test/test_copyright.py b/src/kart_lap_attempt/test/test_copyright.py similarity index 100% rename from src/perception/cone_mapping/cone_mapping/test/test_copyright.py rename to src/kart_lap_attempt/test/test_copyright.py diff --git a/src/perception/cone_mapping/cone_mapping/test/test_flake8.py b/src/kart_lap_attempt/test/test_flake8.py similarity index 100% rename from src/perception/cone_mapping/cone_mapping/test/test_flake8.py rename to src/kart_lap_attempt/test/test_flake8.py diff --git a/src/perception/cone_mapping/cone_mapping/test/test_pep257.py b/src/kart_lap_attempt/test/test_pep257.py similarity index 100% rename from src/perception/cone_mapping/cone_mapping/test/test_pep257.py rename to src/kart_lap_attempt/test/test_pep257.py diff --git a/src/visualization/foxglove_visualizer/base_tf/base_tf/__init__.py b/src/moa/cone-detection/cone_detection/__init__.py similarity index 100% rename from src/visualization/foxglove_visualizer/base_tf/base_tf/__init__.py rename to src/moa/cone-detection/cone_detection/__init__.py diff --git a/src/moa/moa_bringup/launch/base.launch.py b/src/moa/moa_bringup/launch/base.launch.py new file mode 100644 index 00000000..07f9dcbd --- /dev/null +++ b/src/moa/moa_bringup/launch/base.launch.py @@ -0,0 +1,53 @@ +import launch +import launch_ros.actions +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python import get_package_share_directory +import os + +def generate_launch_description(): + return launch.LaunchDescription([ + DeclareLaunchArgument( + 'can_id', + default_value='0x300', + description='The frame ID for the CAN messages containing Ackermann commands that are sent to the car' + ), + + DeclareLaunchArgument( + 'candapter_topic', + default_value='pub_raw_can', + description='The subscriber and publisher topic for the Can Adapter node' + ), + + launch_ros.actions.Node( + package='moa_controllers', + executable='ack_to_can_node', + name='ack_to_can_node', + parameters=[{'can_id': launch.substitutions.LaunchConfiguration('can_id')}], + ), + + # # uncomment when CAN interface is completed + # launch_ros.actions.Node( + # package='moa_driver', + # executable='can_interface_jnano', + # name='can_interface_jnano'), + + # IncludeLaunchDescription( + # PythonLaunchDescriptionSource([os.path.join( + # get_package_share_directory('moa_description'), 'launch'), + # '/urdf_model.py'])), + + launch_ros.actions.Node( + package='moa_controllers', + executable='as_status_node', + name='as_status_node', + ), + + launch_ros.actions.Node( + package='CanTalk', + executable='candapter_node', + name='candapter_node', + remappings=[('can',launch.substitutions.LaunchConfiguration('candapter_topic'))], + ), + ]) diff --git a/src/moa/moa_bringup/launch/fs_sim_launch.launch.py b/src/moa/moa_bringup/launch/fs_sim_launch.launch.py new file mode 100644 index 00000000..f4b78d51 --- /dev/null +++ b/src/moa/moa_bringup/launch/fs_sim_launch.launch.py @@ -0,0 +1,61 @@ +import launch +import launch_ros.actions + +def generate_launch_description(): + return launch.LaunchDescription([ + # get cones + launch_ros.actions.Node( + package='simulator', + executable='get_cones', + name='get_cones', + ), + + # car position + launch_ros.actions.Node( + package='simulator', + executable='get_car_position', + name='get_car_position', + ), + + # path planning + launch_ros.actions.Node( + package='path_planning', + executable='centerline_planner', + name='centerline_planner', + ), + + # controller + launch_ros.actions.Node( + package='head_to_goal_control', + executable='controller', + name='controller', + ), + + # simulator controller + launch_ros.actions.Node( + package='simulator', + executable='set_car_controls', + name='set_car_controls' + ), + + # steer torque + # launch_ros.actions.Node( + # package='steer_torque_from_ackermann', + # executable='steer_torque_from_ackermann', + # name='steer_torque_from_ackermann', + # ), + + # path viz + launch_ros.actions.Node( + package='path_planning_visualization', + executable='visualize', + name='path_viz', + ), + + # track viz + launch_ros.actions.Node( + package='cone_map_foxglove_visualizer', + executable='visualizer', + name='track_viz', + ), + ]) diff --git a/src/moa/moa_bringup/launch/joystick_teleop.launch.py b/src/moa/moa_bringup/launch/joystick_teleop.launch.py new file mode 100644 index 00000000..259f0d34 --- /dev/null +++ b/src/moa/moa_bringup/launch/joystick_teleop.launch.py @@ -0,0 +1,54 @@ +import launch +import launch_ros.actions +from launch_ros.actions import Node +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + return launch.LaunchDescription([ + # launch file arguments + DeclareLaunchArgument( + 'max_speed', + default_value="2.0", + description='maximum speed of the vehicle when R2 is fully pressed on the joystick' + ), + + DeclareLaunchArgument( + 'max_angle', + default_value="25.0", + description='maximum angle of the vehicle when L3 is fully pressed to the right/left on the joystick' + ), + + DeclareLaunchArgument( + 'use_ds4drv', + default_value="True", + description='Its a type of connection, whether to use it or not. Recommended' + ), + + DeclareLaunchArgument( + 'verbose', + default_value="False", + description='Whether to print output' + ), + + # add base launch file + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + FindPackageShare("moa_bringup"),'/launch','/base.launch.py' + ]) + ), + + # joystick teleoperation node + Node( + package='moa_controllers', + executable='joystick_teleop', + name='joystick_teleop', + parameters=[{'max_speed', LaunchConfiguration("max_speed")}, + {'max_angle', LaunchConfiguration("max_angle")}, + {'use_ds4drv', LaunchConfiguration("use_ds4drv")}, + {'verbose', LaunchConfiguration("verbose")}] + ), + ]) diff --git a/src/moa/moa_bringup/launch/scrutineering.py b/src/moa/moa_bringup/launch/scrutineering.launch.py similarity index 100% rename from src/moa/moa_bringup/launch/scrutineering.py rename to src/moa/moa_bringup/launch/scrutineering.launch.py diff --git a/src/moa/moa_bringup/launch/sim_launch.py b/src/moa/moa_bringup/launch/sim_launch.py deleted file mode 100644 index 034c04ef..00000000 --- a/src/moa/moa_bringup/launch/sim_launch.py +++ /dev/null @@ -1,50 +0,0 @@ -import launch -import launch_ros.actions - -def generate_launch_description(): - return launch.LaunchDescription([ - # cone detect - launch_ros.actions.Node( - package='cone_mapping', - executable='listener', - name='listener', - ), - - # path generation - launch_ros.actions.Node( - package='path_planning', - executable='trajectory_generation', - name='trajectory_generation', - parameters=[{'debug': True, - 'timer':0.1}], - ), - - # path optimization - launch_ros.actions.Node( - package='path_planning', - executable='trajectory_optimisation', - name='trajectory_optimisation', - parameters=[{'debug': True}], - ), - - # controller - launch_ros.actions.Node( - package='moa_controllers', - executable='trajectory_follower', - name='trajectory_follower', - ), - - # path viz - launch_ros.actions.Node( - package='path_planning_visualization', - executable='visualize2', - name='path_viz', - ), - - # track viz - launch_ros.actions.Node( - package='cone_map_foxglove_visualizer', - executable='visualizer', - name='track_viz', - ), - ]) \ No newline at end of file diff --git a/src/moa/moa_bringup/launch/test_aruco.launch.py b/src/moa/moa_bringup/launch/test_aruco.launch.py new file mode 100644 index 00000000..d0b5e589 --- /dev/null +++ b/src/moa/moa_bringup/launch/test_aruco.launch.py @@ -0,0 +1,24 @@ +from launch import LaunchDescription +import launch_ros +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python import get_package_share_directory +import os + + +def generate_launch_description(): + zed_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('zed_wrapper'), 'launch'), + '/zed2i.launch.py']) + ) + + aruco = launch_ros.actions.Node( + package='test_plan_act_algorithems', + executable='aruco_triangle', + name='aruco_triangle') + + return LaunchDescription([ + zed_launch, + aruco + ]) \ No newline at end of file diff --git a/src/moa/moa_bringup/launch/unity_sim_launch.launch.py b/src/moa/moa_bringup/launch/unity_sim_launch.launch.py new file mode 100644 index 00000000..82a369a7 --- /dev/null +++ b/src/moa/moa_bringup/launch/unity_sim_launch.launch.py @@ -0,0 +1,79 @@ +import launch +import launch_ros.actions + +def generate_launch_description(): + return launch.LaunchDescription([ + # acceleration + launch_ros.actions.Node( + package="acceleration", + executable="controller", + name="acceleration" + ), + + # cone map + launch_ros.actions.Node( + package='aruco_detection', + executable='aruco_detection', + name='aruco_detection' + ), + + # cone mapping + launch_ros.actions.Node( + package='cone_mapping', + executable='dbscan', + name='dbscan', + ), + + # car position + launch_ros.actions.Node( + package='cone_mapping', + executable='car_position', + name='car_position', + ), + + # path generation + launch_ros.actions.Node( + package='path_planning', + executable='trajectory_generation', + name='trajectory_generation', + parameters=[{'debug': True, + 'timer': 1.0}], + ), + + # path optimization + launch_ros.actions.Node( + package='path_planning', + executable='trajectory_optimisation', + name='trajectory_optimisation', + parameters=[{'delete': False, + 'interpolate': False}], + ), + + # controller + launch_ros.actions.Node( + package='stanley_controller', + executable='controller', + name='controller', + ), + + # steer torque + # launch_ros.actions.Node( + # package='steer_torque_from_ackermann', + # executable='steer_torque_from_ackermann', + # name='steer_torque_from_ackermann', + # ), + + # path viz + launch_ros.actions.Node( + package='path_planning_visualization', + executable='visualize', + name='path_viz', + ), + + # track viz + launch_ros.actions.Node( + package='cone_map_foxglove_visualizer', + executable='visualizer', + name='track_viz', + ), + ]) diff --git a/src/moa/moa_bringup/package.xml b/src/moa/moa_bringup/package.xml index b225f8ef..a1d71f94 100644 --- a/src/moa/moa_bringup/package.xml +++ b/src/moa/moa_bringup/package.xml @@ -14,7 +14,7 @@ ros2launch scrutineering - + ament_python diff --git a/src/moa/moa_bringup/setup.py b/src/moa/moa_bringup/setup.py index f03dee3c..dd13fe3d 100644 --- a/src/moa/moa_bringup/setup.py +++ b/src/moa/moa_bringup/setup.py @@ -12,6 +12,7 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + #(join('share', package_name), glob('launch/*launch.[pxy][yma]*')), # launch files (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.py*'))), @@ -20,8 +21,8 @@ zip_safe=True, maintainer='chris', maintainer_email='chrisgraham908@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', + description='All the bring up and startup files for the autonomus car for the uoa fsae team', + license='apache 2.0', tests_require=['pytest'], entry_points={ 'console_scripts': [ diff --git a/src/moa/moa_controllers/launch/camera_stimulus_test_launch.py b/src/moa/moa_controllers/launch/camera_stimulus_test_launch.py new file mode 100644 index 00000000..b30793be --- /dev/null +++ b/src/moa/moa_controllers/launch/camera_stimulus_test_launch.py @@ -0,0 +1,70 @@ +from launch_ros.actions import Node +from launch import LaunchDescription +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +from ament_index_python import get_package_share_directory +import os + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'can_id', + default_value='0x300', + description='The frame ID for the CAN messages containing Ackermann commands that are sent to the car' + ), + + DeclareLaunchArgument( + 'candapter_topic', + default_value='pub_raw_can', + description='The subscriber and publisher topic for the Can Adapter node' + ), + + Node( + package='moa_controllers', + executable='ack_to_can_node', + name='ack_to_can_node', + parameters=[{'can_id': LaunchConfiguration('can_id')}], + ), + + Node( + namespace='moa', + package='zed_launch', + executable='zed_launch_node', + name='perception' + ), + + Node( + namespace='moa', + package='cone_mapping', + executable='kalman_filter', + name='kalman_filter' + ), + + Node( + namespace='moa', + package='path_planning', + executable='fasttube', + name='centerline_planner' + ), + + # # uncomment when CAN interface is completed + # Node( + # package='moa_driver', + # executable='can_interface_jnano', + # name='can_interface_jnano'), + + Node( + package='CanTalk', + executable='candapter_node', + name='candapter_node', + remappings=[('can', LaunchConfiguration('candapter_topic'))], + ), + Node( + package='moa_controllers', + executable='mock_stimulus', + name='mock_stimulus', + ), + ]) diff --git a/src/moa/moa_controllers/launch/control.py b/src/moa/moa_controllers/launch/control.py index 1f4f3998..1aad946b 100644 --- a/src/moa/moa_controllers/launch/control.py +++ b/src/moa/moa_controllers/launch/control.py @@ -36,13 +36,13 @@ def generate_launch_description(): remappings=[('can',launch.substitutions.LaunchConfiguration('candapter_topic'))], ), - # foxglove - launch_ros.actions.Node( - package='foxglove_bridge', - executable='foxglove_bridge', - name='foxglove_bridge', - parameters=[{'port':8765}], - ), + # # foxglove + # launch_ros.actions.Node( + # package='foxglove_bridge', + # executable='foxglove_bridge', + # name='foxglove_bridge', + # parameters=[{'port':8765}], + # ), # cone detection - aruco detection (ANY) launch_ros.actions.Node( @@ -54,27 +54,51 @@ def generate_launch_description(): # cone map launch_ros.actions.Node( package='cone_mapping', - executable='listener', - name='cone_map' + executable='dbscan', + name='listener', ), - # path planning algorithm - center line (ANY) + # path generation launch_ros.actions.Node( package='path_planning', - executable='center_line', - name='center_line', + executable='trajectory_generation', + name='trajectory_generation', + parameters=[{'debug': True, + 'timer': 1.0}], ), - # path planning controller - head to goal (ANY) + # path optimization launch_ros.actions.Node( - package='head_to_goal_control', - executable='controller', - name='head_to_goal_controller' + package='path_planning', + executable='trajectory_optimisation', + name='trajectory_optimisation', + parameters=[{'debug': True}], ), + # controller launch_ros.actions.Node( package='moa_controllers', - executable='as_status_node', - name='as_status_node', + executable='trajectory_follower', + name='trajectory_follower', + ), + + # path viz + launch_ros.actions.Node( + package='path_planning_visualization', + executable='visualize2', + name='path_viz', ), + + # track viz + launch_ros.actions.Node( + package='cone_map_foxglove_visualizer', + executable='visualizer', + name='track_viz', + ), + + # launch_ros.actions.Node( + # package='moa_controllers', + # executable='as_status_node', + # name='as_status_node', + # ), ]) diff --git a/src/moa/moa_controllers/launch/motec_mock_controller_test_launch.py b/src/moa/moa_controllers/launch/motec_mock_controller_test_launch.py new file mode 100644 index 00000000..55631ab3 --- /dev/null +++ b/src/moa/moa_controllers/launch/motec_mock_controller_test_launch.py @@ -0,0 +1,49 @@ +from launch_ros.actions import Node +from launch import LaunchDescription +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +from ament_index_python import get_package_share_directory +import os + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'can_id', + default_value='0x300', + description='The frame ID for the CAN messages containing Ackermann commands that are sent to the car' + ), + + DeclareLaunchArgument( + 'candapter_topic', + default_value='pub_raw_can', + description='The subscriber and publisher topic for the Can Adapter node' + ), + + Node( + package='moa_controllers', + executable='ack_to_can_node', + name='ack_to_can_node', + parameters=[{'can_id': LaunchConfiguration('can_id')}], + ), + + # # uncomment when CAN interface is completed + # Node( + # package='moa_driver', + # executable='can_interface_jnano', + # name='can_interface_jnano'), + + Node( + package='CanTalk', + executable='candapter_node', + name='candapter_node', + remappings=[('can', LaunchConfiguration('candapter_topic'))], + ), + Node( + package='moa_controllers', + executable='mock_stimulus', + name='mock_stimulus', + ), + ]) diff --git a/src/moa/moa_controllers/moa_controllers/ack_to_can.py b/src/moa/moa_controllers/moa_controllers/ack_to_can.py index 0a67fe60..cd3bd3a1 100644 --- a/src/moa/moa_controllers/moa_controllers/ack_to_can.py +++ b/src/moa/moa_controllers/moa_controllers/ack_to_can.py @@ -11,10 +11,42 @@ from ackermann_msgs.msg import AckermannDriveStamped from moa_msgs.msg import CANStamped - -class ack_to_can(Node): - def __init__(self): - super().__init__('ackermann_to_can') # node name (NB: MoTec listens to this) +def can_data_is_valid(data): + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + return ( + (isinstance(data, Sequence) or + isinstance(data, Set) or + isinstance(data, UserList)) and + not isinstance(data, str) and + not isinstance(data, UserString) and + all(isinstance(v, int) for v in data) and + all(val >= 0 and val < 256 for val in data) + ) + +def get_can_data_zero(): + ackermann_vals = np.array([ + int(0), # velocity + int(0), # acceleration + int(0), # jerk + int(0), # steering_angle + int(0), # reserved + int(0), # angular velocity + int(0), # reserved + int(0) # reserved + ], + dtype=np.uint8 + ) + return ackermann_vals.tolist() + +# todo create a CAN message class wrapper + + +class AckToCan(Node): + def __init__(self, *vargs, **kwargs): + super().__init__('ackermann_to_can', *vargs, **kwargs) # node name (NB: MoTec listens to this) # init ros_arg parameters self.declare_parameter('can_id', @@ -28,7 +60,7 @@ def __init__(self): self.subscription = self.create_subscription( AckermannDriveStamped, # msg type 'cmd_vel', # topic receiving from - self.ack_to_can_publish_callback, #callback function + self.AckToCan_publish_callback, #callback function 10 # qos profile ) @@ -38,7 +70,50 @@ def __init__(self): 'pub_raw_can', 10 ) + + self.create_timer(1.0, self.feedback) # used to wake the thread occassionally to handle sigint signals + self.on_start() + + def feedback(self): + pass + # print("Hello world") + + def on_start(self): + # zero all control signals + can_msg = CANStamped() + + # configure header + can_msg.header.frame_id = 'ackermann_to_can' + + # set CAN id/data + can_msg.can.id = self.can_id + print(can_data_is_valid(get_can_data_zero())) + can_msg.can.data = get_can_data_zero() + + # log + self.get_logger().info(f"INITIALIZING CONTROL SIGNALS ") + + # publish CAN to topic + self.can_pub.publish(can_msg) + + def on_shutdown(self): + # zero all control signals + can_msg = CANStamped() + + # configure header + can_msg.header.frame_id = 'ackermann_to_can' + # set CAN id/data + can_msg.can.id = self.can_id + can_msg.can.data = get_can_data_zero() + + # log + self.get_logger().info(f"SHUTTING DOWN - ZEROING CONTROL SIGNALS") + + # publish CAN to topic + self.can_pub.publish(can_msg) + + def ackermann_to_can_parser(self, ack_msg: AckermannDriveStamped) -> Optional[CANStamped]: """ Parses an AckermannDriveStamped message into a list of CAN data bytes. @@ -69,56 +144,66 @@ def ackermann_to_can_parser(self, ack_msg: AckermannDriveStamped) -> Optional[CA """ # checks before sending Ackermann - if 0 > ack_msg.drive.speed or ack_msg.drive.speed > 120/3.6: # m/s - self.get_logger().warn('ackermann drive SPEED out of bounds') + if 0 > ack_msg.drive.speed or ack_msg.drive.speed > 255: # m/s + self.get_logger().warn('ackermann drive SPEED out of bounds: ' + str(ack_msg.drive.speed)) return None elif 0 > ack_msg.drive.acceleration or ack_msg.drive.acceleration > 255: # m/s^2 - self.get_logger().warn('ackermann drive ACCLERATION out of bounds') + self.get_logger().warn('ackermann drive ACCLERATION out of bounds: ' + str(ack_msg.drive.acceleration)) return None elif 0 > ack_msg.drive.jerk or ack_msg.drive.jerk > 1: # m/s^3 # unsure of upper limit # not too fussed about assign 1 byte - self.get_logger().warn('ackermann drive JERK out of bounds') + self.get_logger().warn('ackermann drive JERK out of bounds: ' + str(ack_msg.drive.jerk)) return None - elif -45 > ack_msg.drive.steering_angle or ack_msg.drive.steering_angle > 45: # radians - self.get_logger().warn('ackermann drive STEERING_ANGLE out of bounds') + elif ack_msg.drive.steering_angle < -30 or 30 < ack_msg.drive.steering_angle: # degrees + self.get_logger().warn('ackermann drive STEERING_ANGLE out of bounds: ' + str(ack_msg.drive.steering_angle)) return None elif 0 > ack_msg.drive.steering_angle_velocity or ack_msg.drive.steering_angle_velocity > 1: # radians/s # unsure of upper limit definitely dont need more than 1 - self.get_logger().warn('ackermann drive STEERING_ANGLE_VELOCITY out of bounds') + self.get_logger().warn('ackermann drive STEERING_ANGLE_VELOCITY out of bounds: ' + str(ack_msg.drive.steering_angle_velocity)) return None # format values of Ackermann speed = ack_msg.drive.speed acceleration = ack_msg.drive.acceleration jerk = ack_msg.drive.jerk*100 - steering_angle = np.float16(ack_msg.drive.steering_angle).tobytes() + steering_angle = ack_msg.drive.steering_angle *4 steering_angle_vel = ack_msg.drive.steering_angle_velocity*100 + + # convert fro 2's compliment to signed magnitude + if (steering_angle < 0): + steering_angle = int(-steering_angle) | 0x80 + + + # Float format for steering # separator for steering_angle - s_a_size = len(steering_angle) - - # Compose CAN data packet + # steering_angle = np.float16(ack_msg.drive.steering_angle).tobytes() + # s_a_size = len(steering_angle) + # steering_angle_lower = int.from_bytes(steering_angle[:s_a_size//2], 'big'), + # steering_angle_upper = int.from_bytes(steering_angle[s_a_size//2:], 'big'), + ackermann_vals = np.array([ - speed, - acceleration, - jerk, - int.from_bytes(steering_angle[:s_a_size//2], 'big'), - int.from_bytes(steering_angle[s_a_size//2:], 'big'), - steering_angle_vel, - 0, - 0], + int(speed), + int(acceleration), + int(jerk), + int(steering_angle), + int(0.0), # reserved + int(steering_angle_vel), + int(0), # reserved + int(0) # reserved + ], dtype=np.uint8 ) - return ackermann_vals + return ackermann_vals.tolist() - def ack_to_can_publish_callback(self, ack_msg: AckermannDriveStamped): + def AckToCan_publish_callback(self, ack_msg: AckermannDriveStamped): can_msg = CANStamped() # configure header @@ -126,21 +211,42 @@ def ack_to_can_publish_callback(self, ack_msg: AckermannDriveStamped): # set CAN header/data/id can_msg.can.id = self.can_id - can_msg.can.data = self.ackermann_to_can_parser(ack_msg) + data = self.ackermann_to_can_parser(ack_msg) - if can_msg.can.data is not None: + if data is not None: + self.get_logger().info(f"DATA IS = {data}") + can_msg.can.data = data # publish CAN to topic self.can_pub.publish(can_msg) -def main(args=None): - rclpy.init(args=args) +def shutdown_cb(): + print("[from callback] - shutting down ") - node = ack_to_can() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() +from rclpy.context import Context +from rclpy.executors import SingleThreadedExecutor + + +def main(args=None): + + # set up a cucstom context to handle init-shutdown cycle + context = Context() + rclpy.init(args=args, context=context) + + ack_to_can_node = AckToCan(context=context) + executor = SingleThreadedExecutor(context=context) + executor.add_node(ack_to_can_node) + + try: + executor.spin() + except KeyboardInterrupt: + print("\nkeyboard interrupt signal intercepted") + ack_to_can_node.on_shutdown() + finally: + print("\nshutting down") + ack_to_can_node.destroy_node() + context.shutdown() if __name__ == '__main__': diff --git a/src/moa/moa_controllers/moa_controllers/ack_to_can_test.py b/src/moa/moa_controllers/moa_controllers/ack_to_can_test.py index fe8de585..75048636 100644 --- a/src/moa/moa_controllers/moa_controllers/ack_to_can_test.py +++ b/src/moa/moa_controllers/moa_controllers/ack_to_can_test.py @@ -10,12 +10,12 @@ class PublishAckermannMsg(Node): def __init__(self): super().__init__("ackermann_publiser_node") self.ackerman_publisher = self.create_publisher(AckermannDriveStamped, "cmd_vel", 10) - self.create_timer(5, self.publish_msg) + self.create_timer(0.5, self.publish_msg) def publish_msg(self): - args = {"steering_angle": uniform(-45,45), + args = {"steering_angle": 0.0, "steering_angle_velocity": random(), - "speed": float(randint(0,30)), + "speed": 0.0, "acceleration": float(randint(0,255)), "jerk": random()} diff --git a/src/moa/moa_controllers/moa_controllers/event_controller_TEMPLATE.py b/src/moa/moa_controllers/moa_controllers/event_controller_TEMPLATE.py new file mode 100644 index 00000000..5475083c --- /dev/null +++ b/src/moa/moa_controllers/moa_controllers/event_controller_TEMPLATE.py @@ -0,0 +1,85 @@ +import rclpy +from rclpy.node import Node +from moa_msgs.msg import Pulse +from std_msgs.msg import UInt8, String + +MAXLAPS = 3 + +START_DESCRIPTION = "" +END_DESCRIPTION = "" + +class EventController(Node): + def __init__(self): + super().__init__('event_controller') + + self.start_end_subsc = self.create_subscription( + Pulse, + 'start_end_detection', + self.pulse_callback, + 10) + self.at_start = False + self.at_finish = False + + self.round = 0 + + self.assi_finished_pub = self.create_publisher( + Pulse, + 'mission_finished', + 10, + ) + + + self.start_desc_pub = self.create_publisher( + String, + 'start_description', + 10, + ) + + self.end_desc_pub = self.create_publisher( + String, + 'end_description', + 10, + ) + + self.timer1 = self.create_timer(1, self.publish_start_description) + self.timer2 = self.create_timer(1, self.publish_end_description) + + def publish_start_description(self): + start_desc_msg = String() + start_desc_msg.data = START_DESCRIPTION + self.start_desc_pub.publish(start_desc_msg) + + def publish_end_description(self): + end_desc_msg = String() + end_desc_msg.data = END_DESCRIPTION + self.end_desc_pub.publish(end_desc_msg) + + def pulse_callback(self, msg): + if msg.value: + msgToSend = Pulse() + msgToSend.value = False + # If pulse is True, it means the event has occurred + if msg.event_type == 'start': + self.at_start = True + self.at_finish = False + + elif msg.event_type == 'end': + if self.round < MAXLAPS: + self.round += 1 + else: + self.at_finish = True + self.at_start = False + msgToSend.value = True + msgToSend.event_type = 'finish' + + self.self.assi_finished_pub.publish(msg) + +def main(args=None): + rclpy.init(args=args) + self_driving_car = SelfDrivingCar() + rclpy.spin(self_driving_car) + self_driving_car.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/moa/moa_controllers/moa_controllers/joystick_teleop.py b/src/moa/moa_controllers/moa_controllers/joystick_teleop.py new file mode 100644 index 00000000..c9e4c5ce --- /dev/null +++ b/src/moa/moa_controllers/moa_controllers/joystick_teleop.py @@ -0,0 +1,357 @@ +from pyPS4Controller.controller import Controller +import rclpy +from rclpy.node import Node +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped +from std_msgs.msg import Header +import numpy as np + +# R3 right and left buttons max is 32767 when pressed and -32767 when fully released +# L3/left joystick goes up,down when fully pushed up is -32767 and positive when down +# L3/left joystick goes right and down when fully pushed is 32767 when pushed fully right and negative when fully left +class joystick_teleop(Node): + def __init__(self): + # publisher + super().__init__("joystick_teleop") + self.get_logger().info("joystick teleoperation node started") + + self.declare_parameters( + namespace='', + parameters=[ + ('max_speed', 2.0), + ('max_angle', 22.0), + ('use_ds4drv', True), + ('verbose', False), + ] + ) + + # get parameter values + max_speed = self.get_parameter("max_speed").get_parameter_value().double_value + max_angle = self.get_parameter("max_angle").get_parameter_value().double_value + use_ds4drv = self.get_parameter("use_ds4drv").get_parameter_value().bool_value + verbose = self.get_parameter("verbose").get_parameter_value().bool_value + # now make sure the controller is paired over the Bluetooth and turn on the listener + joystick = MyController(max_speed = max_speed, + min_speed = 0, + max_angle = max_angle, + min = -32767, + max = 32767, + speed = 0, + previous_value = -32767, + previous_value2 = -32767, + publisher=self.create_publisher(AckermannDriveStamped, "cmd_vel", 10), + use_ds4drv=use_ds4drv, + verbose=verbose, + interface="/dev/input/js0", + connecting_using_ds4drv=use_ds4drv + ) + joystick.listen() + +class MyController(Controller): # create a custom class for your controller and subclass Controller + """ + If we want to bind an action to the X button on the controller, we need to override its respective methods. + + Some of the buttons have a binary On/Off state. For example the X, Circle, Square, and Triangle buttons. + When overriding their respective methods there are no args in the function signature. + + Some controls like the L2, L3, R2 and R3 have a variable On state. + When overriding their respective method, there is a value argument in the function signature + which indicates the degree of the input. + + You can put any custom code inside the functions bellow. I have put print statements in there just so you + can copy/paste the code, connect controller, play with the inputs and see the result. + + All of the functions that you can override are listed in this script. + """ + def __init__(self, max_speed, min_speed, max_angle, min, max, speed, previous_value, previous_value2, + publisher, use_ds4drv, verbose, **kwargs): + Controller.__init__(self, **kwargs) + self.max_speed = max_speed + self.min_speed = min_speed + self.max_angle = max_angle + self.min = min + self.max = max + self.speed = speed + self.angle = 0 + self.joystick_teleop_pub = publisher + self.ds4drv = use_ds4drv + self.verbose = verbose + + def get_ackerman(self): + args = { + 'steering_angle': float(np.round(self.angle,2)), + 'steering_angle_velocity': 0.0, + 'speed': float(np.round(self.speed,2)), + 'acceleration': 0.0, + 'jerk': 0.0, + } + + return AckermannDrive(**args) + + def get_ackerman_stamped(self, ackerman_msg): + args = {"header": Header(stamp=Node("joystick_teleop").get_clock().now().to_msg(), + frame_id="joystick_teleop"), + "drive": ackerman_msg} + + return AckermannDriveStamped(**args) + + def on_L3_left(self, value): + """steer left""" + self.angle = (value/self.max) * self.max_angle + # publish msg + msg = self.get_ackerman_stamped(self.get_ackerman()) + self.joystick_teleop_pub.publish(msg) + + if self.verbose: print(f"angle = {self.angle}") + + def on_L3_right(self, value): + """steer right""" + self.angle = (value/self.max) * self.max_angle + # publish msg + msg = self.get_ackerman_stamped(self.get_ackerman()) + self.joystick_teleop_pub.publish(msg) + + if self.verbose: print(f"angle = {self.angle}") + + def on_L3_at_rest(self): + """reset steering to 0""" + self.angle = 0 + # publish msg + msg = self.get_ackerman_stamped(self.get_ackerman()) + self.joystick_teleop_pub.publish(msg) + + if self.verbose: print(f"angle = {self.angle}") + + def on_R2_press(self, value): + if not self.ds4drv: return + + """acceleration""" + self.speed = (value-self.min)/(self.max-self.min) * (self.max_speed-self.min_speed) + # publish msg + msg = self.get_ackerman_stamped(self.get_ackerman()) + self.joystick_teleop_pub.publish(msg) + + if self.verbose: print(f"speed = {self.speed}") + + def on_R2_release(self): + if not self.ds4drv: return + + """acceleration released""" + self.speed = 0 + # publish msg + msg = self.get_ackerman_stamped(self.get_ackerman()) + self.joystick_teleop_pub.publish(msg) + + if self.verbose: print(f"speed = {self.speed}") + + def on_L2_press(self, value): + if not self.ds4drv: return + + """deceleration""" + self.speed = (-value-self.min)/(self.max-self.min) * (self.speed-self.min_speed) + # publish msg + msg = self.get_ackerman_stamped(self.get_ackerman()) + self.joystick_teleop_pub.publish(msg) + + if self.verbose: print(f"speed = {self.speed}") + + + ## if ds4drv is False + """acceleration""" + def on_R3_up(self, value): + if self.ds4drv: return + + range = (self.max_speed - self.min_speed)/2 + proportion = 1 - (value/self.min) + self.speed = proportion * range + # publish msg + msg = self.get_ackerman_stamped(self.get_ackerman()) + self.joystick_teleop_pub.publish(msg) + + if self.verbose: print(f"speed = {self.speed}") + + def on_R3_down(self, value): + if self.ds4drv: return + + range = (self.max_speed - self.min_speed)/2 + proportion = value/self.max + self.speed = (self.max_speed/2) + (proportion * range) + # publish msg + msg = self.get_ackerman_stamped(self.get_ackerman()) + self.joystick_teleop_pub.publish(msg) + + if self.verbose: print(f"speed = {self.speed}") + + """deceleration""" + def on_R3_left(self, value): + if self.ds4drv: return + + range = (self.speed - self.min_speed)/2 + proportion = value/self.min + self.speed = max(0,self.speed - (proportion * range)) + # publish msg + msg = self.get_ackerman_stamped(self.get_ackerman()) + self.joystick_teleop_pub.publish(msg) + + if self.verbose: print(f"speed = {self.speed}") + + def on_R3_right(self, value): + if self.ds4drv: return + + range = (self.max_speed - self.min_speed)/2 + proportion = value/self.max + self.speed = max(0,self.speed - (proportion * range)) + # publish msg + msg = self.get_ackerman_stamped(self.get_ackerman()) + self.joystick_teleop_pub.publish(msg) + + if self.verbose: print(f"speed = {self.speed}") + + # def on_R3_left(self, value): + # if value > self.previous_value2: + # self.speed = (1-((value+self.max)/(self.max*2)))*self.speed + # self.previous_value = (self.speed*(self.max-self.min))/(self.max_speed-self.min_speed) + self.min + # self.previous_value2 = value + # print(f"speed = {self.speed}") + # # publish msg + # msg = self.get_ackerman_stamped(self.get_ackerman()) + # self.joystick_teleop_pub.publish(msg) + + # def on_R3_right(self, value): + # if value > self.previous_value2: + # self.speed = (1-((value+self.max)/(self.max*2)))*self.speed + # self.previous_value = (self.speed*(self.max-self.min))/(self.max_speed-self.min_speed) + self.min + # self.previous_value2 = value + # print(f"speed = {self.speed}") + # # publish msg + # msg = self.get_ackerman_stamped(self.get_ackerman()) + # self.joystick_teleop_pub.publish(msg) + + # def on_L3_up(self, value): + # print(f"on_L3_up {value}") + + # def on_L3_down(self, value): + # print(f"on_L3_down {value}") + + # def on_L3_at_rest(self): + # """R3 joystick is at rest after the joystick was moved and let go off""" + # print("on_L3_at_rest") + + # def on_L3_press(self): + # """R3 joystick is clicked""" + # print("on_L3_press") + + # def on_L3_release(self): + # """R3 joystick is released after the click""" + # print("on_L3_release") + + # def on_x_press(self): + # print("on_x_press") + + # def on_x_release(self): + # print("on_x_release") + + # def on_triangle_press(self): + # print("on_triangle_press") + + # def on_triangle_release(self): + # print("on_triangle_release") + + # def on_circle_press(self): + # print("on_circle_press") + + # def on_circle_release(self): + # print("on_circle_release") + + # def on_square_press(self): + # print("on_square_press") + + # def on_square_release(self): + # print("on_square_release") + + # def on_L1_press(self): + # print("on_L1_press") + + # def on_L1_release(self): + # print("on_L1_release") + + # def on_L2_press(self, value): + # print("on_L2_press") + + # def on_L2_release(self): + # print("on_L2_release") + + # def on_R1_press(self): + # print("on_R1_press") + + # def on_R1_release(self): + # print("on_R1_release") + + # def on_R2_press(self, value): + # print("on_R2_press") + + # def on_R2_release(self): + # print("on_R2_release") + + # def on_up_arrow_press(self): + # print("on_up_arrow_press") + + # def on_up_down_arrow_release(self): + # print("on_up_down_arrow_release") + + # def on_down_arrow_press(self): + # print("on_down_arrow_press") + + # def on_left_arrow_press(self): + # print("on_left_arrow_press") + + # def on_left_right_arrow_release(self): + # print("on_left_right_arrow_release") + + # def on_right_arrow_press(self): + # print("on_right_arrow_press") + + # def on_R3_at_rest(self): + # """R3 joystick is at rest after the joystick was moved and let go off""" + # print("on_R3_at_rest") + + # def on_R3_press(self): + # """R3 joystick is clicked. This event is only detected when connecting without ds4drv""" + # print("on_R3_press") + + # def on_R3_release(self): + # """R3 joystick is released after the click. This event is only detected when connecting without ds4drv""" + # print("on_R3_release") + + # def on_options_press(self): + # print("on_options_press") + + # def on_options_release(self): + # print("on_options_release") + + # def on_share_press(self): + # """this event is only detected when connecting without ds4drv""" + # print("on_share_press") + + # def on_share_release(self): + # """this event is only detected when connecting without ds4drv""" + # print("on_share_release") + + # def on_playstation_button_press(self): + # """this event is only detected when connecting without ds4drv""" + # print("on_playstation_button_press") + + # def on_playstation_button_release(self): + # """this event is only detected when connecting without ds4drv""" + # print("on_playstation_button_release") + +def main(args=None): + rclpy.init(args=args) + + node = joystick_teleop() + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/src/moa/moa_controllers/moa_controllers/keyboard_teleop.py b/src/moa/moa_controllers/moa_controllers/keyboard_teleop.py new file mode 100755 index 00000000..fa0c6fe1 --- /dev/null +++ b/src/moa/moa_controllers/moa_controllers/keyboard_teleop.py @@ -0,0 +1,98 @@ +#!/usr/bin/python3 +from timeit import default_timer +from sshkeyboard import listen_keyboard +import rclpy +from rclpy.node import Node +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped +from std_msgs.msg import Header + +class keyboard_teleop(Node): + def __init__(self, update): + super().__init__("keyboard_teleop") + self.steering_angle = 0 + self.speed = 0 + self.top_speed = 10 # m/s + self.update = update + self.up_key_pressed = False + self.duration = 1000 + + # log control msg + txt = """ + CONTROL THE GOKART USING ARROWS KEYS! + RIGHT ARROW = RIGHT STEER + LEFT ARROW = LEFT STEER + UP ARROW = INCREASE SPEED + DOWN ARROW = DECREASE SPEED + + nb: keys have to be pressed one by one two different keys pressed simulatenously doesn't work + """ + self.get_logger().info(txt) + + # publisher + self.keyboard_teleop_pub = self.create_publisher(AckermannDriveStamped, "cmd_vel", 10) + listen_keyboard(on_press=self.key_pressed, on_release=self.key_released,delay_second_char=0.01,delay_other_chars=0.01) + + def key_pressed(self,key): + if key == "left": + self.steering_angle = max(self.steering_angle-self.update, -25) + print(self.steering_angle) + if key == "right": + self.steering_angle = min(self.steering_angle+self.update, 25) + print(self.steering_angle) + if key == "up": + self.up_key_pressed = True + self.speed = min(self.speed+self.update, self.top_speed) + print(self.speed) + if key == "down": + self.speed = max(self.speed-2*self.update, 0) + print(self.speed) + if key == "z": + self.speed = 0 + print(self.speed) + + # publish msg + msg = self.get_ackerman_stamped(self.get_ackerman()) + self.keyboard_teleop_pub.publish(msg) + + def key_released(self,key): + if key == "up": + self.up_key_pressed = False + self.released_up_time = default_timer() + + # def slowcardown(self): + # print('hello') + # duration = default_timer() - self.released_up_time + # if duration > 1 and self.speed != 0 and not self.up_key_pressed: + # self.speed = max(self.speed-1, 0) + # print(self.speed) + + def get_ackerman(self): + args = { + 'steering_angle': float(self.steering_angle), + 'steering_angle_velocity': 0.0, + 'speed': float(self.speed), + 'acceleration': 0.0, + 'jerk': 0.0, + } + + return AckermannDrive(**args) + + def get_ackerman_stamped(self, ackerman_msg): + args = {"header": Header(stamp=self.get_clock().now().to_msg(), + frame_id="keyboard_teleop"), + "drive": ackerman_msg} + + return AckermannDriveStamped(**args) + +def main(args=None): + rclpy.init(args=args) + + node = keyboard_teleop(update=1.0) + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/moa/moa_controllers/moa_controllers/mock_stimulus.py b/src/moa/moa_controllers/moa_controllers/mock_stimulus.py new file mode 100644 index 00000000..938e24a1 --- /dev/null +++ b/src/moa/moa_controllers/moa_controllers/mock_stimulus.py @@ -0,0 +1,82 @@ +import rclpy +from rclpy.node import Node +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped +from std_msgs.msg import Header +from rclpy.task import Future + + +class mock_stimulus_node(Node): + + def __init__(self, *vargs, **kwargs): + super().__init__("stimulus_node", *vargs, **kwargs) + + self.shutdown_future = Future() + + # real angle is 1.395 the magnitude of the input angle + self.declare_parameters( + namespace='', + parameters=[ + ('vel', 0.0), + ('accel', 0.0), + ('angle', 0.0), + ('angular_vel', 0.0), + ('verbose', False), + ] + ) + + timer_period = 0.1 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + + self.cmd_vel_pub = self.create_publisher(AckermannDriveStamped, "cmd_vel", 10) + + def update_params(self): + self.vel = self.get_parameter("vel").get_parameter_value().double_value + self.accel = self.get_parameter("accel").get_parameter_value().double_value + self.angle = self.get_parameter("angle").get_parameter_value().double_value + self.angular_vel = self.get_parameter("angular_vel").get_parameter_value().double_value + self.verbose = self.get_parameter("verbose").get_parameter_value().bool_value + + def timer_callback(self): + self.update_params() + + ackerman_msg = AckermannDrive( + steering_angle = self.angle, + steering_angle_velocity = self.angular_vel, + speed = self.vel, + acceleration = self.accel, + jerk = 0.0, + ) + + ackermannHeader = Header(stamp=self.get_clock().now().to_msg(), + frame_id="gokart") + + msg = AckermannDriveStamped( + header = ackermannHeader, + drive = ackerman_msg + ) + + self.cmd_vel_pub.publish(msg) + + +import signal + +def main(args=None): + + # Handle shutdown via Ctrl+C + def sigint_handler(signum, frame): + global shutdown_requested + node.get_logger().info("handling SIGINT - triggering shut down") + if not node.shutdown_future.done(): + node.shutdown_future.set_result(None) + signal.signal(signal.SIGINT, sigint_handler) + rclpy.init(args=args, signal_handler_options=rclpy.signals.SignalHandlerOptions.NO) + + node = mock_stimulus_node() + + rclpy.spin_until_future_complete(node, node.shutdown_future) + node.get_logger().info("shutting down") + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/src/moa/moa_controllers/moa_controllers/pure_pursuit.py b/src/moa/moa_controllers/moa_controllers/pure_pursuit.py new file mode 100644 index 00000000..c801436a --- /dev/null +++ b/src/moa/moa_controllers/moa_controllers/pure_pursuit.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 +# Python imports +from typing import Optional +import rclpy +from rclpy.node import Node +import numpy as np +from math import pi +from math import sqrt + +from geometry_msgs.msg import PoseArray, Pose +from moa_msg.msg import ConeMap, Cone +from ackermann_msgs.msg import AckermannDrive + + + +class PurePursuitController(Node): + def __init__(self): + super().__init__("Pure_Pursuit_Controller") + self.get_logger().info("Path Planning Node Started") + + self.steering_angle = 0 + self.look_ahead = 100 + self.pos = None + self.way_point = None + # subscribe to best trajectory + self.best_trajectory_sub = self.create_subscription(PoseArray, "moa/selected_trajectory", self.selected_trajectory_handler, 5) + self.cmd_vel_pub = self.create_publisher(AckermannDrive, "cmd/vel", 5) + + self.timer = self.create_timer(5, self.publish_ackermann) + + def selected_trajectory_handler(self, poseArray: PoseArray) -> None: + # The first marker location is the car + # TODO separate into its own topic + self.car_pose = poseArray[0].position + # The cars orientation is stored as radians from the car's initial + # orientation (following right hand rule for positive and negative + # direction) as the manitude of the quaternion (w). + self.car_orientation = poseArray[0].orientation.w + + cones = poseArray[1:] + self.way_point = self.way_point_look_ahead(cones) + + + self.set_steering_angle(self.curvator()) + + self.cmd_vel_pub.publish() + + def extract_cones(self, coneMap: ConeMap) -> list[Cone]: + return coneMap[1:] + + def way_point_look_ahead(self, cones: ConeMap) -> Pose: + way_point = cones[0].pose + distance = float('inf') + for cone in cones: + distance_from_look_ahead_radius = self.distance_from_look_ahead_radius(cone.pose.position) + if distance_from_look_ahead_radius < distance: + way_point = cone.pose + distance = distance_from_look_ahead_radius + return way_point + + def distance_from_look_ahead_radius(self, pos): + return abs(self.distanceTo(pos) - self.look_ahead) + + def distanceTo(self, pos): + return sqrt(pow((self.pose.postion.x - pos.x),2) + pow((self.pose.postion.y - pos.y),2)) + + def publish_ackermann(self) -> None: + + if (self.steering_angle == None): return + + args = {"steering_angle": self.steering_angle, + "steering_angle_velocity": 0.0, + "speed": self.current_speed, + "acceleration": 0.0, + "jerk": 0.0} + msg = AckermannDrive(**args) + self.cmd_vel_pub.publish(msg) + + def lateral_distance(): + pass + + + def arc_radius(self): + y = self.lateral_distance(self.way_point) + pow(self.look_ahead, 2) / 2 * y + + def curvator(self): + return 1/self.arc_radius() + + def set_steering_angle(self, angle): + self.steering_angle = angle + + + +def main(): + rclpy.init() + node = PurePursuitController() + try: + rclpy.spin(node) + except Exception as e: + print(f"node spin error: {e}") + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/src/moa/moa_controllers/moa_controllers/trajectory_follower_p_controller.py b/src/moa/moa_controllers/moa_controllers/trajectory_follower_p_controller.py index b7b04a8f..364f279d 100644 --- a/src/moa/moa_controllers/moa_controllers/trajectory_follower_p_controller.py +++ b/src/moa/moa_controllers/moa_controllers/trajectory_follower_p_controller.py @@ -1,90 +1,191 @@ #!/usr/bin/python3 import rclpy from rclpy.node import Node -from ackermann_msgs.msg import AckermannDrive +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy + +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped from rclpy.executors import SingleThreadedExecutor from std_msgs.msg import Float32, Float64 + +from geometry_msgs.msg import PoseArray, Pose +from moa_msgs.msg import ConeMap + +from std_msgs.msg import Header +from builtin_interfaces.msg import Time import numpy as np class trajectory_following(Node): def __init__(self): + + self._car_position: Pose super().__init__("Trajectory_Following") self.get_logger().info("Trajectory Following Node Started") - self.declare_parameters( - namespace='', - parameters=[ - ('debug', False) - ] + # self.declare_parameters( + # namespace='', + # parameters=[ + # ('debug', False) + # ] + # ) + self._distance_to_front = 0.9 + + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10 ) - - self.debug = self.get_parameter('debug').get_parameter_value().bool_value - - # publish p-controlled trajectory - # self.p_controlled_pub = self.create_publisher(AckermannDrive, "cmd_vel", 5) - # subscribe to best trajectory - # self.best_traj_sub = self.create_subscription(PoseArray, "moa/selected_trajectory", self.get_best_state, 5) - self.create_subscription(Float32, "moa/selected_steering_angle", self.get_best_state, 10) - # self.current_states_sub = self.create_subscription(AckermannDrive, "moa/cur_vel", self.get_current_states, 5) - self.declare_parameter('car_name', 'test') car_name = self.get_parameter('car_name').get_parameter_value().string_value steering_topic = "/" + car_name + "/cmd_steering" torque_topic = "/" + car_name + "/cmd_throttle" - speed_topic = "/" + car_name + "/speed" - self.steering_publisher = self.create_publisher(Float32, steering_topic, 10) - # self.torque_publisher = self.create_publisher(Float32, torque_topic, 10) - self.feedback_subscribe = self.create_subscription(Float64,speed_topic,self.set_speed,10) + # speed_topic = "/" + car_name + "/speed" + + # subscribers + self.create_subscription(PoseArray, "moa/selected_trajectory", self.get_desired_pose, qos_profile) + self.create_subscription(Float32, "moa/selected_steering_angle", self.get_steering_angle, qos_profile) + self.create_subscription(ConeMap, "cone_map", self.callback, qos_profile) + self.create_subscription(ConeMap, "car_position", self.car_pos_cb) + + # publishers (including simulation) + self.moa_steering_pub = self.create_publisher(AckermannDriveStamped, "cmd_vel", 10) + self.sim_steering_pub = self.create_publisher(Float32, steering_topic, 10) + # self.feedback_subscribe = self.create_subscription(Float64,speed_topic,self.set_speed,10) - def set_speed(self, msg:Float64): - self.current_speed = msg.data - - def get_current_states(self, msg: AckermannDrive) -> None: - self.current_speed = msg.speed - self.current_angle = msg.steering_angle - - def get_control_error(self, csa, dsa): return dsa-csa - - def get_best_state(self, msg: Float32): - p_gain = 1 - steering_angle_deg = (msg.data*180) / np.pi * p_gain - # if steering_angle_deg < -9.0: - # steering_angle_deg = -30.0 - # elif steering_angle_deg > 9.0: - # steering_angle_deg = 30.0 - self.get_logger().info(f"before and after gain: {steering_angle_deg/p_gain}, {steering_angle_deg}") - self.steering_publisher.publish(Float32(data=steering_angle_deg)) - # self.get_logger().info(f"chosen recieved state is = {msg.steering_angle}") - - # if self.debug: - # self.current_angle = self.current_speed = 0.0 - - # if hasattr(self,"current_speed") and hasattr(self,"current_angle"): - # # error - # error = self.get_control_error(self.current_angle, msg.steering_angle) + def get_steering_angle(self, msg:Float32): self._steering_angle = msg.data # radians + + def get_desired_pose(self, msg:PoseArray): self._desired_pose = msg.poses[-1] + + def get_car_position(self, pose:Pose): self._car_position = pose + + + + def callback(self, msg:ConeMap): + if hasattr(self, "_steering_angle"): + # get current and desired points + car_position_orientation = self.get_position_of_cart(msg) + car_position, rotation_matrix = self.get_transformation_matrix(car_position_orientation) + local_origin = msg.cones[0].pose.pose.position + post_trans_local = self.apply_transformation(car_position, rotation_matrix, local_origin.x, local_origin.y) # local frame distance to front of car + current = np.array([post_trans_local[0][0], post_trans_local[1][0] + self._distance_to_front]) + + # boundaries + innerboundary, outerboundary = self.get_boundaries(msg.cones) + desired = self.get_center_points(car_position, rotation_matrix, innerboundary, outerboundary) # local frame + + # compute the distance between the desired and current point + error = self.get_control_error(current, desired) + # get p-gain + track_width = self.get_track_width(innerboundary, outerboundary) + p_gain = self.get_gain(min(error), track_width) + + # compute new angle in degrees + steering_angle_rad = p_gain * self._steering_angle + + + self.get_logger().info(f"before and after gain: {steering_angle_rad/p_gain}, {steering_angle_rad}") + + steering_angle_deg = self._steering_angle * 180 / np.pi + + # publish msgs + args = {"steering_angle": float(steering_angle_deg), + "steering_angle_velocity": 0.0, + "speed": 3.0, + "acceleration": 0.0, + "jerk": 0.0} - # # constant gain multiplier - # p_gain = 0.5 - - # # new steering angle output - # chosen_state = self.current_angle + error * p_gain - - # # publish msg to /moa/drive - # args = {"steering_angle": float(chosen_state), - # "steering_angle_velocity": 0.0, - # "speed": self.current_speed, - # "acceleration": 0.0, - # "jerk": 0.0} + args2 = {'stamp':Time(sec=1,nanosec=2), + 'frame_id':'ack_to_can_test'} - # msg = AckermannDrive(**args) - # self.p_controlled_pub.publish(msg) - # self.get_logger().info(f"P-controlled state published = {chosen_state}") + args3 = {'header': Header(**args2), + 'drive':AckermannDrive(**args)} - # return + self.moa_steering_pub.publish(AckermannDriveStamped(**args3)) + self.sim_steering_pub.publish(Float32(data=steering_angle_deg)) + + return - # self.get_logger().info("Attributes current speed/angle not initialised") - # return + self.get_logger().info("waiting for moa/selected_trjectory topic") + + return + + + def get_control_error(self, current:np.array, desired:np.array): return np.linalg.norm(current-desired, axis=1) + + def get_gain(self, distance, center_to_boundary_distance): + """return gain on steering angle based on error - distance to center line currently""" + self.get_logger().info(f"DISTANCE = {distance}") + + return (distance/center_to_boundary_distance) + + def get_center_points(self, car_position, rotation_matrix, innerboundary, outerboundary): + """get the center points""" + up_to = min(len(innerboundary), len(outerboundary)) + center_points = [] + for i in range(up_to): + x1, y1 = innerboundary[i] + x2, y2 = outerboundary[i] + x, y = self.get_average_point(x1,y1,x2,y2) + # get transformed point + post_trans_point = self.apply_transformation(car_position, rotation_matrix, x, y) + x = post_trans_point[0][0] + y = post_trans_point[1][0] + center_points.append([x,y]) + + return center_points + + def get_track_width(self, innerboundary, outerboundary): + firstInner = np.array(innerboundary[0]) + firstOuter = np.array(outerboundary[0]) + width = self.get_distance(firstInner, firstOuter) + + return width + + def get_boundaries(self, cones): + innerboundary = [] + outerboundary = [] + for i in range(len(cones)): + if i != 0: + x = cones[i].pose.pose.position.x + y = cones[i].pose.pose.position.y + # blue - inner + if cones[i].colour == 0: + innerboundary.append([x,y]) + elif cones[i].colour == 2: + outerboundary.append([x,y]) + + return innerboundary, outerboundary + + def get_average_point(self, x1,y1,x2,y2): return (x1+x2)/2, (y1+y2)/2 + + def get_distance(self, p1:np.array, p2:np.array): return np.sqrt(sum((p2-p1)**2)) + + def get_position_of_cart(self, cone_map): + # first cone + localization_data = cone_map.cones[0] + x = localization_data.pose.pose.position.x + y = localization_data.pose.pose.position.y + theta = localization_data.pose.pose.orientation.w + return x, y, theta + + def get_transformation_matrix(self, position_and_orientation): + # theta = position_and_orientation[2] - np.pi/2 + cart_x = position_and_orientation[0] + cart_y = position_and_orientation[1] + theta = position_and_orientation[2] + # 2d trasformation matrix + rotation_matrix = np.array([[np.cos(theta), -np.sin(theta)],[np.sin(theta), np.cos(theta)]]) + position_vector = np.array([[cart_x], [cart_y]]) + + return position_vector, rotation_matrix + + def apply_transformation(self, position_vector, rotation_matrix, point_x, point_y): + point = np.array([[point_x], [point_y]]) + # matrix multiplication for rotation then translate from car position + transformed_point = np.matmul(rotation_matrix, point) + position_vector + return transformed_point + + def main(): rclpy.init() diff --git a/src/moa/moa_controllers/package.xml b/src/moa/moa_controllers/package.xml index af40df55..95006cfb 100644 --- a/src/moa/moa_controllers/package.xml +++ b/src/moa/moa_controllers/package.xml @@ -15,6 +15,7 @@ python3-numpy + ackermann_msgs diff --git a/src/moa/moa_controllers/setup.py b/src/moa/moa_controllers/setup.py index 0a5bfd4c..ca6e9c9e 100644 --- a/src/moa/moa_controllers/setup.py +++ b/src/moa/moa_controllers/setup.py @@ -26,7 +26,9 @@ 'console_scripts': [ 'ack_to_can_node = moa_controllers.ack_to_can:main', 'as_status_node = moa_controllers.sys_status:main', - 'trajectory_follower = moa_controllers.trajectory_follower_p_controller:main' + 'trajectory_follower = moa_controllers.trajectory_follower_p_controller:main', + 'joystick_teleop = moa_controllers.joystick_teleop:main', + 'mock_stimulus = moa_controllers.mock_stimulus:main' ], }, ) diff --git a/src/moa/moa_description/launch/urdf_model.py b/src/moa/moa_description/launch/urdf_model.py index 6308a345..03b8a0b7 100644 --- a/src/moa/moa_description/launch/urdf_model.py +++ b/src/moa/moa_description/launch/urdf_model.py @@ -1,14 +1,26 @@ import os -from ament_index_python import get_package_share_directory +from ament_index_python import get_package_share_directory, get_package_prefix from launch import LaunchDescription from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import SetEnvironmentVariable import xacro def generate_launch_description(): + # pkg_install_path = get_package_share_directory('moa_description') + + # if 'GAZEBO_MODEL_PATH' in os.environ: + # model_path = os.environ['GAZEBO_MODEL_PATH'] + ':' + pkg_install_path + # else: + # model_path = pkg_install_path + + # gazebo_env = SetEnvironmentVariable("GAZEBO_MODEL_PATH", model_path) + # get urdf file path path = os.path.join(get_package_share_directory('moa_description')) xacro_file = os.path.join(path,'urdf','moa_robot.urdf.xacro') @@ -20,15 +32,34 @@ def generate_launch_description(): package='robot_state_publisher', executable='robot_state_publisher', output='screen', - parameters=[{'robot_description':robot_description.toxml()}] + parameters=[{'robot_description':robot_description.toxml(), + 'use_sim_time':True}] ) # joint state publisher node - joint_state_pub_node = Node( - package='joint_state_publisher', - executable='joint_state_publisher', - remappings=[('joint_states','/joint_states')], - parameters=[{'robot_description':str(robot_description.toxml())}] + # joint_state_pub_node = Node( + # package='joint_state_publisher', + # executable='joint_state_publisher', + # remappings=[('joint_states','/joint_states')], + # parameters=[{'robot_description':str(robot_description.toxml())}] + # ) + + # start gazebo + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('gazebo_ros'),'launch'), '/gazebo.launch.py']), ) + + # spawn entity + spawn_entity = Node( + package='gazebo_ros', + executable='spawn_entity.py', + arguments=['-topic','robot_description', + '-entity','my_bot', + '-x', '0', + '-y', '0', + '-z', '1'], + output='screen' + ) - return LaunchDescription([robot_state_pub_node, joint_state_pub_node]) \ No newline at end of file + return LaunchDescription([robot_state_pub_node, gazebo, spawn_entity]) \ No newline at end of file diff --git a/src/moa/moa_description/urdf/gazebo.xacro b/src/moa/moa_description/urdf/gazebo.xacro new file mode 100644 index 00000000..a67cff00 --- /dev/null +++ b/src/moa/moa_description/urdf/gazebo.xacro @@ -0,0 +1,16 @@ + + + + + 20 + world_to_chassis_joint + chassis_to_kingpin1_joint + chassis_to_kingpin2_joint + kingpin1_to_front_right_wheel + kingpin2_to_front_left_wheel + chassis_to_rear_right_wheel + chassis_to_rear_left_wheel + + + \ No newline at end of file diff --git a/src/moa/moa_description/urdf/moa_joints.xacro b/src/moa/moa_description/urdf/moa_joints.xacro index 6620a0af..b82342c0 100644 --- a/src/moa/moa_description/urdf/moa_joints.xacro +++ b/src/moa/moa_description/urdf/moa_joints.xacro @@ -6,6 +6,7 @@ + @@ -14,7 +15,8 @@ - + + @@ -23,7 +25,8 @@ - + + @@ -32,6 +35,7 @@ + @@ -40,6 +44,7 @@ + @@ -48,6 +53,7 @@ + @@ -56,6 +62,7 @@ + diff --git a/src/moa/moa_description/urdf/moa_links.xacro b/src/moa/moa_description/urdf/moa_links.xacro index 9934b409..95ba481c 100644 --- a/src/moa/moa_description/urdf/moa_links.xacro +++ b/src/moa/moa_description/urdf/moa_links.xacro @@ -7,31 +7,47 @@ + - + + + + + + + + + + + + + + + + - + - + - + - + diff --git a/src/moa/moa_description/urdf/moa_macro.xacro b/src/moa/moa_description/urdf/moa_macro.xacro index 829ce4cc..11c92dc3 100644 --- a/src/moa/moa_description/urdf/moa_macro.xacro +++ b/src/moa/moa_description/urdf/moa_macro.xacro @@ -7,12 +7,27 @@ - + + + + + + + + + + + + + + + + @@ -25,6 +40,20 @@ + + + + + + + + + + + + + + diff --git a/src/moa/moa_description/urdf/moa_robot.urdf.xacro b/src/moa/moa_description/urdf/moa_robot.urdf.xacro index c543a2b6..7192c102 100644 --- a/src/moa/moa_description/urdf/moa_robot.urdf.xacro +++ b/src/moa/moa_description/urdf/moa_robot.urdf.xacro @@ -12,4 +12,7 @@ + + + \ No newline at end of file diff --git a/src/moa/moa_description/urdf/zed_descr.urdf.xacro b/src/moa/moa_description/urdf/zed_descr.urdf.xacro index 0ed199b1..5ab822b7 100644 --- a/src/moa/moa_description/urdf/zed_descr.urdf.xacro +++ b/src/moa/moa_description/urdf/zed_descr.urdf.xacro @@ -23,7 +23,7 @@ - + diff --git a/src/moa/moa_driver/launch/moa_driver.launch.py b/src/moa/moa_driver/launch/moa_driver.launch.py new file mode 100644 index 00000000..ace28360 --- /dev/null +++ b/src/moa/moa_driver/launch/moa_driver.launch.py @@ -0,0 +1,22 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + can_decoder = Node( + package='moa_driver', + executable='can_decoder_jnano', + name='can_decoder_node', + output='screen' + ) + + can_interface = Node( + package='moa_driver', + executable='can_interface_jnano', + name='can_interface_node', + output='screen' + ) + + return LaunchDescription([ + can_decoder, + can_interface + ]) \ No newline at end of file diff --git a/src/moa/moa_driver/moa_driver/can_decoder_jnano.py b/src/moa/moa_driver/moa_driver/can_decoder_jnano.py new file mode 100644 index 00000000..dbb62134 --- /dev/null +++ b/src/moa/moa_driver/moa_driver/can_decoder_jnano.py @@ -0,0 +1,117 @@ +import rclpy +from rclpy.node import Node + +# import all msg types needed +from moa_msgs.msg import CANStamped +from sensor_msgs.msg import BatteryState # https://docs.ros2.org/foxy/api/sensor_msgs/msg/BatteryState.html +from ackermann_msgs.msg import AckermannStamped # http://docs.ros.org/en/api/ackermann_msgs/html/msg/AckermannDriveStamped.html + + +class CANDecoderJNano(Node): + def __init__(self) -> None: + super().__init__("CAN_decoder") + + self.battery_count = 0 + self.drive_count = 0 + + # subscribe to raw can data from CAN_interface + self.can_sub = self.create_subscription( + CANStamped, + "/raw_can", + self.callback_can_data, + 10 + ) + + # publish different status msgs + self.battery_status_pub = self.create_publisher(BatteryState, "/battery_state", 10) + self.drive_status_pub = self.create_publisher(AckermannStamped, "/drive_status", 10) + # self.steering_status_pub = self.create_publisher() + self.glv_status_pub = self.create_publisher(BatteryState, "/glv_state", 10) + # self.motor_temp_pub = self.create_publisher(BatteryState, "/glv_state", 10) + + def callback_can_data(self, msg: CANStamped): + + # it is assumed that different can msgs are published seperatly + # however a better design strategy would be for it to be published as an array + + # switch case for different can ids + # TODO make sure that the can id is accessed correctly + # TODO make sure high byte + low byte done correctly + + # BATTERY STATUS + + # need to make sure all battery msgs are received before publishing + if self.battery_count == 0 and (msg.can.id == 0x6b0 or msg.can.id == 0x6b3 or msg.can.id == 0x6b4): + self.battery_msg = BatteryState() + self.battery_count += 1 + + if self.battery_count != 0: + if msg.can.id == 0x6b0: + self.battery_msg.present = True + self.battery_msg.current = float(msg.can.data[0] + msg.can.data[1]) + self.battery_msg.voltage = float(msg.can.data[2] + msg.can.data[3]) + self.battery_count += 1 + + if msg.can.id == 0x6b3: + self.battery_msg.cell_voltage = [float('nan') for _ in range(int(msg.can.data[6]))] + self.battery_msg.cell_temperature = [float('nan') for _ in range(int(msg.can.data[6]))] + self.battery_count += 1 + + if msg.can.id == 0x6b4: + self.battery_msg.temperature = float(msg.can.data[2] + msg.can.data[3]) + self.battery_count += 1 + + if self.battery_count == 4: # dont know which order it gets published + self.battery_msg.header.stamp = self.get_clock().now().to_msg() # not sure if this is needed + self.battery_status_pub.publish(self.battery_msg) + self.battery_count = 0 + + + # DRIVE STATUS + if self.drive_count == 0 and (msg.can.id == 0x604 or msg.can.id == 0x605): + self.drive_status_msg = AckermannStamped() + self.drive_count += 1 + + if self.drive_count != 0: + if msg.can.id == 0x604: + self.drive_status_msg.drive.speed = float(msg.can.data[4]) # are we sure it is in binary + self.drive_count += 1 + + if msg.can.id == 0x605: + self.drive_status_msg.drive.steering_angle = float(msg.can.data[0]) + self.drive_count += 1 + + if self.drive_count == 3: + self.drive_status_msg.header.stamp = self.get_clock().now().to_msg() # not sure if this is needed + self.drive_status_pub.publish(self.drive_status_msg) + self.drive_count = 0 + + # GLV STATUS + if msg.can.id == 0x602: + glv_status_msg = BatteryState() + glv_status_msg.header.stamp = self.get_clock().now().to_msg() + glv_status_msg.present = True + glv_status_msg.percentage = float(msg.can.data[0]) # make sure SOC is stored as percentage 0 - 1. + # also do we need this in the battery status + glv_status_msg.voltage = float(msg.can.data[2] + msg.can.data[1]) + glv_status_msg.current = float(msg.can.data[4] + msg.can.data[3]) + self.glv_status_pub.publish(glv_status_msg) + + + # MOTOR STATUS + + + # ERROR STATUS + +def main(args=None): + rclpy.init(args=args) + + CAN_decoder = CANDecoderJNano() + + rclpy.spin(CAN_decoder) + CAN_decoder.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/moa/moa_driver/package.xml b/src/moa/moa_driver/package.xml index 559d60e3..1f5436b6 100644 --- a/src/moa/moa_driver/package.xml +++ b/src/moa/moa_driver/package.xml @@ -13,6 +13,8 @@ python3-pytest moa_msgs + sensor_msgs + ackermann_msgs ament_python diff --git a/src/moa/moa_driver/setup.py b/src/moa/moa_driver/setup.py index f14a8154..0b169d71 100644 --- a/src/moa/moa_driver/setup.py +++ b/src/moa/moa_driver/setup.py @@ -20,7 +20,8 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'can_interface_jnano = moa_driver.can_interface_jnano:main' + 'can_decoder_jnano = moa_driver.can_decoder_jnano:main', + 'can_interface_jnano = moa_driver.can_interface_jnano:main', ] }, ) diff --git a/src/moa/moa_msgs/CMakeLists.txt b/src/moa/moa_msgs/CMakeLists.txt index a2fcaf0f..3e21a158 100644 --- a/src/moa/moa_msgs/CMakeLists.txt +++ b/src/moa/moa_msgs/CMakeLists.txt @@ -17,10 +17,8 @@ find_package(rosidl_default_generators REQUIRED) set(msg_files "msg/CAN.msg" "msg/CANStamped.msg" - "msg/Cone.msg" - "msg/ConeStamped.msg" - "msg/ConeMap.msg" - "msg/ConeMapStamped.msg" + "msg/Detections.msg" + "msg/Track.msg" "msg/BoundaryStamped.msg" "msg/AllTrajectories.msg" "msg/HardwareStates.msg" @@ -28,6 +26,7 @@ set(msg_files "msg/MissionStates.msg" "msg/MissionStatesStamped.msg" "msg/AllStates.msg" + ) set(srv_files "srv/CANSendReq.srv" diff --git a/src/moa/moa_msgs/msg/AllStates.msg b/src/moa/moa_msgs/msg/AllStates.msg deleted file mode 100644 index ceef45cb..00000000 --- a/src/moa/moa_msgs/msg/AllStates.msg +++ /dev/null @@ -1,3 +0,0 @@ -uint16 id - -ackermann_msgs/AckermannDrive[] states \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/AllTrajectories.msg b/src/moa/moa_msgs/msg/AllTrajectories.msg deleted file mode 100644 index 5cce2878..00000000 --- a/src/moa/moa_msgs/msg/AllTrajectories.msg +++ /dev/null @@ -1,3 +0,0 @@ -uint16 id - -geometry_msgs/PoseArray[] trajectories \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/BoundaryStamped.msg b/src/moa/moa_msgs/msg/BoundaryStamped.msg deleted file mode 100644 index 6a204623..00000000 --- a/src/moa/moa_msgs/msg/BoundaryStamped.msg +++ /dev/null @@ -1,3 +0,0 @@ -std_msgs/Header header - -float32[] coords \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/CAN.msg b/src/moa/moa_msgs/msg/CAN.msg deleted file mode 100644 index 44dd87a5..00000000 --- a/src/moa/moa_msgs/msg/CAN.msg +++ /dev/null @@ -1,4 +0,0 @@ -uint16 id - -bool is_rtr false -uint8[8] data [0, 0, 0, 0, 0, 0, 0, 0] \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/CANStamped.msg b/src/moa/moa_msgs/msg/CANStamped.msg deleted file mode 100644 index c7db3c82..00000000 --- a/src/moa/moa_msgs/msg/CANStamped.msg +++ /dev/null @@ -1,3 +0,0 @@ -std_msgs/Header header - -CAN can \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/CandidateState.msg b/src/moa/moa_msgs/msg/CandidateState.msg deleted file mode 100644 index 0d39ed95..00000000 --- a/src/moa/moa_msgs/msg/CandidateState.msg +++ /dev/null @@ -1,3 +0,0 @@ -Ackermann ackermann -float32 lateralVel - diff --git a/src/moa/moa_msgs/msg/CandidateStates.msg b/src/moa/moa_msgs/msg/CandidateStates.msg deleted file mode 100644 index 2c1ea5e5..00000000 --- a/src/moa/moa_msgs/msg/CandidateStates.msg +++ /dev/null @@ -1 +0,0 @@ -CandidateState[] candidateState \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/Cone.msg b/src/moa/moa_msgs/msg/Cone.msg deleted file mode 100644 index 32c1ef8a..00000000 --- a/src/moa/moa_msgs/msg/Cone.msg +++ /dev/null @@ -1,10 +0,0 @@ -uint32 id 0 # ID value of cone (optional, defaults to 0 if not given) - -float32 confidence 0.0 # The confidence that this is the cone, a percentage value from 0 to 100 (defaults to 0.0) - -uint8 colour # The color of the cone: 0 for Blue, 1 for Orange, 2 for Yellow, and 3 for Other - -geometry_msgs/PoseWithCovariance pose # Position of cone (x, y, z) and rotation - -float32 radius 0.0 # Size of cone, radius (defaults to 0.0) -float32 height 0.0 # Size of cone, height (defaults to 0.0) diff --git a/src/moa/moa_msgs/msg/ConeMap.msg b/src/moa/moa_msgs/msg/ConeMap.msg deleted file mode 100644 index 16a9bb90..00000000 --- a/src/moa/moa_msgs/msg/ConeMap.msg +++ /dev/null @@ -1,2 +0,0 @@ -# List of cones detected and where they are -Cone[] cones diff --git a/src/moa/moa_msgs/msg/ConeMapStamped.msg b/src/moa/moa_msgs/msg/ConeMapStamped.msg deleted file mode 100644 index 6079a946..00000000 --- a/src/moa/moa_msgs/msg/ConeMapStamped.msg +++ /dev/null @@ -1,3 +0,0 @@ -std_msgs/Header header - -ConeMap cone_map \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/ConeStamped.msg b/src/moa/moa_msgs/msg/ConeStamped.msg deleted file mode 100644 index f1f4a3c9..00000000 --- a/src/moa/moa_msgs/msg/ConeStamped.msg +++ /dev/null @@ -1,3 +0,0 @@ -std_msgs/Header header - -Cone cone \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/HardwareStates.msg b/src/moa/moa_msgs/msg/HardwareStates.msg deleted file mode 100644 index 412b20ff..00000000 --- a/src/moa/moa_msgs/msg/HardwareStates.msg +++ /dev/null @@ -1,6 +0,0 @@ -uint8 ebs_active -uint8 ts_active -uint8 in_gear -uint8 master_switch_on -uint8 asb_ready -uint8 brakes_engaged \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/HardwareStatesStamped.msg b/src/moa/moa_msgs/msg/HardwareStatesStamped.msg deleted file mode 100644 index 9737ceb4..00000000 --- a/src/moa/moa_msgs/msg/HardwareStatesStamped.msg +++ /dev/null @@ -1,3 +0,0 @@ -std_msgs/Header header - -HardwareStates hardware_states \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/MissionStates.msg b/src/moa/moa_msgs/msg/MissionStates.msg deleted file mode 100644 index 1237fc97..00000000 --- a/src/moa/moa_msgs/msg/MissionStates.msg +++ /dev/null @@ -1,2 +0,0 @@ -uint8 mission_selected -uint8 mission_finished \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/MissionStatesStamped.msg b/src/moa/moa_msgs/msg/MissionStatesStamped.msg deleted file mode 100644 index 11f709e2..00000000 --- a/src/moa/moa_msgs/msg/MissionStatesStamped.msg +++ /dev/null @@ -1,5 +0,0 @@ -std_msgs/Header header - -MissionStates mission_states - - diff --git a/src/moa/moa_msgs/msg/OccupancyGrid.msg b/src/moa/moa_msgs/msg/OccupancyGrid.msg deleted file mode 100644 index a962b103..00000000 --- a/src/moa/moa_msgs/msg/OccupancyGrid.msg +++ /dev/null @@ -1,4 +0,0 @@ -std_msgs/Header header - -uint8[][] occupancyGrid -float32 resolution \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/Pulse.msg b/src/moa/moa_msgs/msg/Pulse.msg new file mode 100644 index 00000000..234ce347 --- /dev/null +++ b/src/moa/moa_msgs/msg/Pulse.msg @@ -0,0 +1,2 @@ +bool value +string event_type diff --git a/src/moa/moa_msgs/srv/CANSendReq.srv b/src/moa/moa_msgs/srv/CANSendReq.srv deleted file mode 100644 index 253902c9..00000000 --- a/src/moa/moa_msgs/srv/CANSendReq.srv +++ /dev/null @@ -1,3 +0,0 @@ -CAN can ---- -bool sent \ No newline at end of file diff --git a/src/perception/localization/LICENSE b/src/perception/aquisition/aruco_detection/LICENSE similarity index 100% rename from src/perception/localization/LICENSE rename to src/perception/aquisition/aruco_detection/LICENSE diff --git a/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/cone_map_foxglove_visualizer/__init__.py b/src/perception/aquisition/aruco_detection/aruco_detection/__init__.py similarity index 100% rename from src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/cone_map_foxglove_visualizer/__init__.py rename to src/perception/aquisition/aruco_detection/aruco_detection/__init__.py diff --git a/src/perception/aruco_detection/aruco_detection/aruco_detection.py b/src/perception/aquisition/aruco_detection/aruco_detection/aruco_detection.py similarity index 81% rename from src/perception/aruco_detection/aruco_detection/aruco_detection.py rename to src/perception/aquisition/aruco_detection/aruco_detection/aruco_detection.py index 631e2fc3..fb28a932 100644 --- a/src/perception/aruco_detection/aruco_detection/aruco_detection.py +++ b/src/perception/aquisition/aruco_detection/aruco_detection/aruco_detection.py @@ -9,8 +9,7 @@ import cv2.aruco as aruco import numpy as np -from moa_msgs.msg import ConeMap -from moa_msgs.msg import Cone +from moa_msgs.msg import Cones, Cone class ArucoDetectionNode(Node): @@ -21,7 +20,7 @@ def __init__(self): self.create_subscription(CameraInfo,'zed/zed_node/rgb/camera_info',self.camera_callback,10) self.create_subscription(PoseStamped,'zed/zed_node/pose',self.pose_callback,10) self.create_subscription(Image,'/zed/zed_node/rgb/image_rect_color',self.image_callback,10) - self.publisher = self.create_publisher(ConeMap, 'cone_detection', 10) + self.publisher = self.create_publisher(Cones, 'cone_detection', 10) self.localization_publisher = self.create_publisher(Pose, 'car_position', 5) self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) @@ -37,7 +36,7 @@ def __init__(self): self.get_logger().info("class initialized") def image_callback(self, msg): - self.get_logger().info("publishing cone map soon") + #self.get_logger().info("publishing cone map soon") cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') corners, ids, _ = self.detector.detectMarkers(cv_image) @@ -55,38 +54,33 @@ def image_callback(self, msg): idc = id[0] _,rvec,tvec = cv2.solvePnP(self.marker_points,corners[indx],self.camera_matrix,self.dist_coeffs) - single_cone.id = indx single_cone.confidence = 100.0 # assuming id 1 is blue cone and id 2 is yellow cone - single_cone.colour = 0 if idc==1 else 2 if idc==2 else 3 + single_cone.type = 0 if idc==1 else 1 if idc==2 else 4 # x-axis is forward/backward - single_cone.pose.pose.position.x = float(tvec[0][0]) + single_cone.position.x = float(tvec[0][0]) # y-axis is left/right - single_cone.pose.pose.position.y = float(tvec[2][0]) + single_cone.position.y = float(tvec[2][0]) # z-axis is up/down - single_cone.pose.pose.position.z = float(tvec[1][0]) + single_cone.position.z = float(tvec[1][0]) single_cone.radius = 1.0 single_cone.height = 1.0 # Only sending blue cone and yellow cone - if single_cone.colour == 0 or single_cone.colour == 2: + if single_cone.type == 0 or single_cone.type == 1: self.aruco_msg.cones.append(single_cone) - self.get_logger().info("POTENTIAL MARKERS DETECTED!!") + #self.get_logger().info("POTENTIAL MARKERS DETECTED!!") self.publisher.publish(self.aruco_msg) self.edit_msg = False def pose_callback(self,msg): - self.aruco_msg = ConeMap() - localization_cone = Cone() + self.aruco_msg = Cones() localization_pose = Pose() - localization_cone.id = 99999; euler = self.convert_quaternion_to_euler(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w) localization_pose.position.x = -msg.pose.position.y localization_pose.position.y = msg.pose.position.x localization_pose.orientation.w = euler[1] - localization_cone.pose.pose = localization_pose - self.aruco_msg.cones.append(localization_cone) self.edit_msg = True self.localization_publisher.publish(localization_pose) diff --git a/src/perception/aquisition/aruco_detection/launch/aruco_detection.launch.py b/src/perception/aquisition/aruco_detection/launch/aruco_detection.launch.py new file mode 100644 index 00000000..fbd380d3 --- /dev/null +++ b/src/perception/aquisition/aruco_detection/launch/aruco_detection.launch.py @@ -0,0 +1,16 @@ +from launch_ros.actions import Node +from launch import LaunchDescription + +def generate_launch_description(): + package_name = 'aruco_detection' + + launch_descriptions = [ + Node( + package=package_name, + executable='aruco_detection', + name=package_name, + ) + ] + + + return LaunchDescription(launch_descriptions) \ No newline at end of file diff --git a/src/perception/aruco_detection/package.xml b/src/perception/aquisition/aruco_detection/package.xml similarity index 54% rename from src/perception/aruco_detection/package.xml rename to src/perception/aquisition/aruco_detection/package.xml index 2d6e8e43..3e90402e 100644 --- a/src/perception/aruco_detection/package.xml +++ b/src/perception/aquisition/aruco_detection/package.xml @@ -7,26 +7,15 @@ yiyang Apache License 2.0 - ament_python - rclpy - std_msgs - moa_msgs - sys - numpy - opencv-python - opencv-contrib-python - - - rclpy std_msgs moa_msgs - sys - numpy - argparse - torch - cv2 - pyzed - cv_bridge + sensor_msgs + geometry_msgs + + python3-opencv + python3-numpy + python-argparse + python3-torch ament_copyright diff --git a/src/perception/localization/resource/localization b/src/perception/aquisition/aruco_detection/resource/aruco_detection similarity index 100% rename from src/perception/localization/resource/localization rename to src/perception/aquisition/aruco_detection/resource/aruco_detection diff --git a/src/perception/aruco_detection/setup.cfg b/src/perception/aquisition/aruco_detection/setup.cfg similarity index 100% rename from src/perception/aruco_detection/setup.cfg rename to src/perception/aquisition/aruco_detection/setup.cfg diff --git a/src/perception/aruco_detection/setup.py b/src/perception/aquisition/aruco_detection/setup.py similarity index 82% rename from src/perception/aruco_detection/setup.py rename to src/perception/aquisition/aruco_detection/setup.py index da6d90a3..56c5a178 100644 --- a/src/perception/aruco_detection/setup.py +++ b/src/perception/aquisition/aruco_detection/setup.py @@ -1,4 +1,6 @@ from setuptools import find_packages, setup +import os +from glob import glob package_name = 'aruco_detection' @@ -10,6 +12,8 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + # launch files + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.py*'))), ], install_requires=['setuptools'], zip_safe=True, diff --git a/src/perception/localization/test/test_copyright.py b/src/perception/aquisition/aruco_detection/test/test_copyright.py similarity index 100% rename from src/perception/localization/test/test_copyright.py rename to src/perception/aquisition/aruco_detection/test/test_copyright.py diff --git a/src/perception/localization/test/test_flake8.py b/src/perception/aquisition/aruco_detection/test/test_flake8.py similarity index 100% rename from src/perception/localization/test/test_flake8.py rename to src/perception/aquisition/aruco_detection/test/test_flake8.py diff --git a/src/perception/localization/test/test_pep257.py b/src/perception/aquisition/aruco_detection/test/test_pep257.py similarity index 100% rename from src/perception/localization/test/test_pep257.py rename to src/perception/aquisition/aruco_detection/test/test_pep257.py diff --git a/src/perception/cone-detection/cone_detection/README.md b/src/perception/aquisition/cone_detection_python/cone_detection_python/README.md similarity index 100% rename from src/perception/cone-detection/cone_detection/README.md rename to src/perception/aquisition/cone_detection_python/cone_detection_python/README.md diff --git a/src/visualization/path_planning_visualization/path_planning_visualization/__init__.py b/src/perception/aquisition/cone_detection_python/cone_detection_python/__init__.py similarity index 100% rename from src/visualization/path_planning_visualization/path_planning_visualization/__init__.py rename to src/perception/aquisition/cone_detection_python/cone_detection_python/__init__.py diff --git a/src/perception/cone-detection/cone_detection/detection_final.py b/src/perception/aquisition/cone_detection_python/cone_detection_python/detection_final.py similarity index 90% rename from src/perception/cone-detection/cone_detection/detection_final.py rename to src/perception/aquisition/cone_detection_python/cone_detection_python/detection_final.py index a99ba7a9..51fce650 100644 --- a/src/perception/cone-detection/cone_detection/detection_final.py +++ b/src/perception/aquisition/cone_detection_python/cone_detection_python/detection_final.py @@ -15,7 +15,7 @@ import pyzed.sl as sl import torch.backends.cudnn as cudnn -sys.path.insert(0, './yolov7') +sys.path.insert(0, './yolov8') from models.experimental import attempt_load from utils.general import check_img_size, non_max_suppression, scale_coords, xyxy2xywh from utils.torch_utils import select_device @@ -24,10 +24,12 @@ from threading import Lock, Thread from time import sleep +from ultralytics import YOLO + global exit_signal,detections, weights, img_size, conf_thres #Basic arguments of the scripts -weights = "yolov7m.pt" +weights = "yolov8m.pt" img_size = 416 conf_thres = 0.4 @@ -39,7 +41,7 @@ def __init__(self): self.run_signal = False super().__init__('detector') - # Initialize ZED camera and YOLOv7 + # Initialize ZED camera and YOLOv8 capture_thread = Thread(target=self.torch_thread,kwargs={'weights': weights, 'img_size': img_size, "conf_thres": conf_thres}) capture_thread.start() @@ -83,7 +85,7 @@ def __init__(self): self.py_translation = sl.Translation() self.obj_runtime_param = sl.ObjectDetectionRuntimeParameters() - # ... [Initialize the ROS 2 publisher for DetectedObject message] + # ... [Initialize the ROS2 publisher for DetectedObject message] #self.publisher = self.create_publisher(ConeMap, 'cone_detection', 10) self.publisher = self.create_publisher(ConeMap, 'cone_map', 10) self.timer = self.create_timer(0.1, self.run_detection) @@ -158,25 +160,29 @@ def torch_thread(self, weights, img_size, conf_thres=0.2, iou_thres=0.45): imgsz = img_size # Load model - model = attempt_load(weights, map_location=device) # load FP32 - stride = int(model.stride.max()) # model stride - imgsz = check_img_size(imgsz, s=stride) # check img_size - if half: - model.half() # to FP16 - cudnn.benchmark = True - - # Run inference - if device.type != 'cpu': - model(torch.zeros(1, 3, imgsz, imgsz).to(device).type_as(next(model.parameters()))) # run once + + #model = attempt_load(weights, map_location=device) # load FP32 + model = YOLO(weights) + #stride = int(model.stride.max()) # model stride + #imgsz = check_img_size(imgsz, s=stride) # check img_size + # if half: + # model.half() # to FP16 + # cudnn.benchmark = True + # + # # Run inference + # if device.type != 'cpu': + # model(torch.zeros(1, 3, imgsz, imgsz).to(device).type_as(next(model.parameters()))) # run once while not exit_signal: #print("looping in another thread") if self.run_signal: lock.acquire() - img, ratio, pad = self.img_preprocess(self.image_net, device, half, imgsz) - pred = model(img)[0] - det = non_max_suppression(pred, conf_thres, iou_thres) + #img, ratio, pad = self.img_preprocess(self.image_net, device, half, imgsz) + img = cv2.cvtColor(image_net, cv2.COLOR_BGRA2BGR) + #pred = model(img)[0] + #det = non_max_suppression(pred, conf_thres, iou_thres) + det = model.predict(img, save=False, imsize=imgsz, conf=conf_thres, iou=iou_thres)[0].cpu().numpy.boxes # ZED CustomBox format (with inverse letterboxing tf applied) detections = self.detections_to_custom_box(det, img, self.image_net) @@ -283,8 +289,6 @@ def print_cone_information(self, cone_input, is_first): print(height_message); - - def main(args=None): rclpy.init(args=args) cone_detection = detection() @@ -293,5 +297,6 @@ def main(args=None): cone_detection.destroy_node() rclpy.shutdown() + if __name__ == '__main__': main() diff --git a/src/perception/aquisition/cone_detection_python/cone_detection_python/detector-clean-worked.py b/src/perception/aquisition/cone_detection_python/cone_detection_python/detector-clean-worked.py new file mode 100644 index 00000000..126a1b8e --- /dev/null +++ b/src/perception/aquisition/cone_detection_python/cone_detection_python/detector-clean-worked.py @@ -0,0 +1,173 @@ +#!/usr/bin/env python3 + +import sys +import numpy as np + +import argparse +import torch +import cv2 +import pyzed.sl as sl +from ultralytics import YOLO + +from threading import Lock, Thread +from time import sleep + +lock = Lock() +run_signal = False +exit_signal = False + + +def xywh2abcd(xywh, im_shape): + output = np.zeros((4, 2)) + + # Center / Width / Height -> BBox corners coordinates + x_min = (xywh[0] - 0.5*xywh[2]) #* im_shape[1] + x_max = (xywh[0] + 0.5*xywh[2]) #* im_shape[1] + y_min = (xywh[1] - 0.5*xywh[3]) #* im_shape[0] + y_max = (xywh[1] + 0.5*xywh[3]) #* im_shape[0] + + # A ------ B + # | Object | + # D ------ C + + output[0][0] = x_min + output[0][1] = y_min + + output[1][0] = x_max + output[1][1] = y_min + + output[2][0] = x_max + output[2][1] = y_max + + output[3][0] = x_min + output[3][1] = y_max + return output + +def detections_to_custom_box(detections, im0): + output = [] + for i, det in enumerate(detections): + xywh = det.xywh[0] + + # Creating ingestable objects for the ZED SDK + obj = sl.CustomBoxObjectData() + obj.bounding_box_2d = xywh2abcd(xywh, im0.shape) + obj.label = det.cls + obj.probability = det.conf + obj.is_grounded = False + output.append(obj) + return output + + +def torch_thread(weights, img_size, conf_thres=0.2, iou_thres=0.45): + global image_net, exit_signal, run_signal, detections + + print("Intializing Network...") + + model = YOLO(weights) + + while not exit_signal: + if run_signal: + lock.acquire() + + img = cv2.cvtColor(image_net, cv2.COLOR_BGRA2BGR) + # https://docs.ultralytics.com/modes/predict/#video-suffixes + det = model.predict(img, save=False, imgsz=img_size, conf=conf_thres, iou=iou_thres)[0].cpu().numpy().boxes + + # ZED CustomBox format (with inverse letterboxing tf applied) + detections = detections_to_custom_box(det, image_net) + lock.release() + run_signal = False + sleep(0.01) + + +def main(): + global image_net, exit_signal, run_signal, detections + + capture_thread = Thread(target=torch_thread, kwargs={'weights': opt.weights, 'img_size': opt.img_size, "conf_thres": opt.conf_thres}) + capture_thread.start() + + print("Initializing Camera...") + + zed = sl.Camera() + + input_type = sl.InputType() + if opt.svo is not None: + input_type.set_from_svo_file(opt.svo) + + # Create a InitParameters object and set configuration parameters + init_params = sl.InitParameters(input_t=input_type, svo_real_time_mode=True) + init_params.camera_resolution = sl.RESOLUTION.HD720 + init_params.coordinate_units = sl.UNIT.METER + init_params.depth_mode = sl.DEPTH_MODE.ULTRA # QUALITY + init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP + init_params.depth_maximum_distance = 50 + + runtime_params = sl.RuntimeParameters() + status = zed.open(init_params) + + if status != sl.ERROR_CODE.SUCCESS: + print(repr(status)) + exit() + + image_left_tmp = sl.Mat() + + print("Initialized Camera") + + py_transform = sl.Transform() + positional_tracking_parameters = sl.PositionalTrackingParameters(_init_pos=py_transform) + # If the camera is static, uncomment the following line to have better performances and boxes sticked to the ground. + # positional_tracking_parameters.set_as_static = True + zed.enable_positional_tracking(positional_tracking_parameters) + + obj_param = sl.ObjectDetectionParameters() + obj_param.detection_model = sl.OBJECT_DETECTION_MODEL.CUSTOM_BOX_OBJECTS + obj_param.enable_tracking = True + zed.enable_object_detection(obj_param) + + objects = sl.Objects() + obj_runtime_param = sl.ObjectDetectionRuntimeParameters() + + cam_w_pose = sl.Pose() + + while not exit_signal: + if zed.grab(runtime_params) == sl.ERROR_CODE.SUCCESS: + # -- Get the image + lock.acquire() + zed.retrieve_image(image_left_tmp, sl.VIEW.LEFT) + image_net = image_left_tmp.get_data() + lock.release() + run_signal = True + + # -- Detection running on the other thread + while run_signal: + sleep(0.001) + + # Wait for detections + lock.acquire() + # -- Ingest detections + zed.ingest_custom_box_objects(detections) + lock.release() + zed.retrieve_objects(objects, obj_runtime_param) + + # -- Display + # Retrieve display data + zed.get_position(cam_w_pose, sl.REFERENCE_FRAME.WORLD) + for object in objects.object_list: + print("{} {} {}".format(object.id, object.raw_label, object.position)) + else: + exit_signal = True + + exit_signal = True + zed.close() + + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('--weights', type=str, default='yolov8m.pt', help='model.pt path(s)') + parser.add_argument('--svo', type=str, default=None, help='optional svo file, if not passed, use the plugged camera instead') + parser.add_argument('--img_size', type=int, default=416, help='inference size (pixels)') + parser.add_argument('--conf_thres', type=float, default=0.4, help='object confidence threshold') + opt = parser.parse_args() + + with torch.no_grad(): + main() \ No newline at end of file diff --git a/src/perception/cone-detection/package.xml b/src/perception/aquisition/cone_detection_python/package.xml similarity index 97% rename from src/perception/cone-detection/package.xml rename to src/perception/aquisition/cone_detection_python/package.xml index 4ba2bf69..902c52fc 100644 --- a/src/perception/cone-detection/package.xml +++ b/src/perception/aquisition/cone_detection_python/package.xml @@ -1,7 +1,7 @@ - cone_detection + cone_detection_python 0.0.0 THis package connect to ZED and implement cone detection yiyang diff --git a/src/visualization/foxglove_visualizer/base_tf/resource/base_tf b/src/perception/aquisition/cone_detection_python/resource/cone_detection_python similarity index 100% rename from src/visualization/foxglove_visualizer/base_tf/resource/base_tf rename to src/perception/aquisition/cone_detection_python/resource/cone_detection_python diff --git a/src/perception/aquisition/cone_detection_python/setup.cfg b/src/perception/aquisition/cone_detection_python/setup.cfg new file mode 100644 index 00000000..24fd86c3 --- /dev/null +++ b/src/perception/aquisition/cone_detection_python/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/cone_detection_python +[install] +install_scripts=$base/lib/cone_detection_python diff --git a/src/perception/cone-detection/setup.py b/src/perception/aquisition/cone_detection_python/setup.py similarity index 84% rename from src/perception/cone-detection/setup.py rename to src/perception/aquisition/cone_detection_python/setup.py index 1281d135..37fe5926 100644 --- a/src/perception/cone-detection/setup.py +++ b/src/perception/aquisition/cone_detection_python/setup.py @@ -1,6 +1,6 @@ from setuptools import setup -package_name = 'cone_detection' +package_name = 'cone_detection_python' setup( name=package_name, @@ -20,7 +20,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'cone_detection = cone_detection.detection_final:main', + 'cone_detection = cone_detection_python.detection_final:main', ], }, ) diff --git a/src/visualization/foxglove_visualizer/base_tf/test/test_copyright.py b/src/perception/aquisition/cone_detection_python/test/test_copyright.py similarity index 100% rename from src/visualization/foxglove_visualizer/base_tf/test/test_copyright.py rename to src/perception/aquisition/cone_detection_python/test/test_copyright.py diff --git a/src/visualization/foxglove_visualizer/base_tf/test/test_flake8.py b/src/perception/aquisition/cone_detection_python/test/test_flake8.py similarity index 100% rename from src/visualization/foxglove_visualizer/base_tf/test/test_flake8.py rename to src/perception/aquisition/cone_detection_python/test/test_flake8.py diff --git a/src/visualization/foxglove_visualizer/base_tf/test/test_pep257.py b/src/perception/aquisition/cone_detection_python/test/test_pep257.py similarity index 100% rename from src/visualization/foxglove_visualizer/base_tf/test/test_pep257.py rename to src/perception/aquisition/cone_detection_python/test/test_pep257.py diff --git a/src/perception/aquisition/zed_launch/CMakeLists.txt b/src/perception/aquisition/zed_launch/CMakeLists.txt new file mode 100644 index 00000000..8052b12e --- /dev/null +++ b/src/perception/aquisition/zed_launch/CMakeLists.txt @@ -0,0 +1,143 @@ +cmake_minimum_required(VERSION 3.5) +project(zed_launch) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +option(LINK_SHARED_ZED "Link with the ZED SDK shared executable" ON) + +if (NOT LINK_SHARED_ZED AND MSVC) + message(FATAL_ERROR "LINK_SHARED_ZED OFF : ZED SDK static libraries not available on Windows") +endif() + +SET(SPECIAL_OS_LIBS "") + +option(ENABLE_INT8_CALIBRATOR "Enable int8 calibrator class for quantification calibration, requires opencv dnn" OFF) + +IF (ENABLE_INT8_CALIBRATOR) + ADD_DEFINITIONS(-DENABLE_INT8_CALIBRATOR) +ENDIF() + +############################################# +# Dependencies +find_package(ZED 4 REQUIRED) +find_package(OpenCV REQUIRED) +find_package(GLUT REQUIRED) +find_package(GLEW REQUIRED) +find_package(OpenGL REQUIRED) +find_package(CUDA REQUIRED) +find_package(cv_bridge REQUIRED) + +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(moa_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +############################################################################### +# INCLUDES and LIBS +include_directories(${OpenCV_INCLUDE_DIRS}) +include_directories(${ZED_INCLUDE_DIRS}) +include_directories(${GLEW_INCLUDE_DIRS}) +include_directories(${GLUT_INCLUDE_DIR}) +include_directories(${CUDA_INCLUDE_DIRS}) +include_directories(${cv_bridge_INCLUDE_DIRS}) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) + +link_directories(${ZED_LIBRARY_DIR}) +link_directories(${GLEW_LIBRARY_DIRS}) +link_directories(${GLUT_LIBRARY_DIRS}) +link_directories(${OpenGL_LIBRARY_DIRS}) +link_directories(${CUDA_LIBRARY_DIRS}) +link_directories(${OpenCV_LIBRARY_DIRS}) + +set(ZED_LIBS + ${ZED_LIBRARIES} + ${CUDA_LIBRARIES} + ${OpenCV_LIBRARIES} + ${GLEW_LIBRARIES} + ${GLUT_LIBRARIES} + ${OpenGL_LIBRARIES} +) +# cuda +include_directories(/usr/local/cuda/include) +link_directories(/usr/local/cuda/lib64) +# tensorrt +include_directories(/usr/include/x86_64-linux-gnu/) +link_directories(/usr/lib/x86_64-linux-gnu/) + +FILE(GLOB_RECURSE SRC_FILES cone_detection/cone_detection.cpp cone_detection/yolo.cpp zed_launch/zed_launch.cpp localisation/localisation.cpp velocity/velocity.cpp) +FILE(GLOB_RECURSE HDR_FILES cone_detection/*.h* zed_launch/*.h*) + +cuda_add_executable(zed_launch_node ${HDR_FILES} ${SRC_FILES}) +add_definitions(-O3 -D_MWAITXINTRIN_H_INCLUDED -Wno-deprecated-declarations) + +if (LINK_SHARED_ZED) + SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) +else() + SET(ZED_LIBS ${ZED_STATIC_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_LIBRARY}) +endif() + +SET(TRT_LIBS nvinfer nvonnxparser) + +target_link_libraries(zed_launch_node + ${TRT_LIBS} + ${SPECIAL_OS_LIBS} + ${ZED_LIBS} + ${OPENGL_LIBRARIES} + ${GLUT_LIBRARIES} + ${GLEW_LIBRARIES} + ${OpenCV_LIBRARIES}) + +if(INSTALL_SAMPLES) + LIST(APPEND SAMPLE_LIST zed_launch_node) + SET(SAMPLE_LIST "${SAMPLE_LIST}" PARENT_SCOPE) +endif() + +add_executable(cone_subscriber cone_detection/cone_subscriber.cpp) +ament_target_dependencies(cone_subscriber + rclcpp + std_msgs + moa_msgs + geometry_msgs +) + +add_executable(localisation_subscriber localisation/localisation_subscriber.cpp) +ament_target_dependencies(localisation_subscriber + rclcpp + std_msgs + moa_msgs + geometry_msgs +) + +ament_target_dependencies(zed_launch_node + rclcpp + cv_bridge + OpenCV + std_msgs + moa_msgs + geometry_msgs + visualization_msgs + sensor_msgs +) + +install(TARGETS + zed_launch_node + cone_subscriber + localisation_subscriber + DESTINATION lib/zed_launch +) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp b/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp new file mode 100644 index 00000000..0ae218a4 --- /dev/null +++ b/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp @@ -0,0 +1,199 @@ +#include +#include +#include +#include +#include +#include +#include "cuda_utils.h" +#include "logging.h" +#include "utils.h" + +#include "yolo.hpp" +#include "sl/Camera.hpp" +#include "../zed_launch/zed_launch.hpp" + +#include + +using namespace nvinfer1; +#define CONF_THRESH 0.8 +#define NMS_THRESH 0.4 + +#include + +#include "std_msgs/msg/string.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "moa_msgs/msg/detections.hpp" +#include "sensor_msgs/msg/image.hpp" + +std::mutex mtx; + +static void draw_objects(cv::Mat const& image, + cv::Mat &res, + sl::Objects const& objs, + std::vector> const& colors) +{ + res = image.clone(); + cv::Mat mask{image.clone()}; + for (sl::ObjectData const& obj : objs.object_list) { + size_t const idx_color{obj.id % colors.size()}; + cv::Scalar const color{cv::Scalar(colors[idx_color][0U], colors[idx_color][1U], colors[idx_color][2U])}; + + cv::Rect const rect{static_cast(obj.bounding_box_2d[0U].x), + static_cast(obj.bounding_box_2d[0U].y), + static_cast(obj.bounding_box_2d[1U].x - obj.bounding_box_2d[0U].x), + static_cast(obj.bounding_box_2d[2U].y - obj.bounding_box_2d[0U].y)}; + cv::rectangle(res, rect, color, 2); + + char text[256U]; + sprintf(text, "Class %d - %.1f%%", obj.raw_label, obj.confidence); + if (obj.mask.isInit() && obj.mask.getWidth() > 0U && obj.mask.getHeight() > 0U) { + const cv::Mat obj_mask = slMat2cvMat(obj.mask); + mask(rect).setTo(color, obj_mask); + } + + int baseLine{0}; + cv::Size const label_size{cv::getTextSize(text, cv::FONT_HERSHEY_SIMPLEX, 0.4, 1, &baseLine)}; + + int const x{rect.x}; + int const y{std::min(rect.y + 1, res.rows)}; + + cv::rectangle(res, cv::Rect(x, y, label_size.width, label_size.height + baseLine), {0, 0, 255}, -1); + cv::putText(res, text, cv::Point(x, y + label_size.height), cv::FONT_HERSHEY_SIMPLEX, 0.4, {255, 255, 255}, 1); + } + cv::addWeighted(res, 0.5, mask, 0.8, 1, res); +} + +std::vector cvt(const BBox &bbox_in) { + std::vector bbox_out(4); + bbox_out[0] = sl::uint2(bbox_in.x1, bbox_in.y1); + bbox_out[1] = sl::uint2(bbox_in.x2, bbox_in.y1); + bbox_out[2] = sl::uint2(bbox_in.x2, bbox_in.y2); + bbox_out[3] = sl::uint2(bbox_in.x1, bbox_in.y2); + return bbox_out; +} + +cv::Rect get_rect(BBox box) { + return cv::Rect(round(box.x1), round(box.y1), round(box.x2 - box.x1), round(box.y2 - box.y1)); +} + +void ZedLaunchNode::cone_detection_loop() + { + auto camera_config = zed.getCameraInformation().camera_configuration; + sl::Resolution pc_resolution(std::min((int) camera_config.resolution.width, 720), std::min((int) camera_config.resolution.height, 404)); + auto camera_info = zed.getCameraInformation(pc_resolution).camera_configuration; + + // Creating the inference engine class + std::string engine_name = model_name; + Yolo detector; + if (detector.init(engine_name)) { + std::cerr << "Detector init failed!" << std::endl; + return; + } + + auto display_resolution = zed.getCameraInformation().camera_configuration.resolution; + sl::Mat left_sl; + sl::ObjectDetectionRuntimeParameters objectTracker_parameters_rt; + sl::Objects objects; + + while (zed.grab() == sl::ERROR_CODE::SUCCESS) { + // Get image for inference + zed.retrieveImage(left_sl, sl::VIEW::LEFT); + + // Running inference + auto detections = detector.run(left_sl, display_resolution.height, display_resolution.width, CONF_THRESH); + + // Preparing for ZED SDK ingesting + std::vector objects_in; + for (auto &it : detections) { + sl::CustomBoxObjectData tmp; + // Fill the detections into the correct format + tmp.unique_object_id = sl::generate_unique_id(); + tmp.probability = it.prob; + tmp.label = (int) it.label; + tmp.bounding_box_2d = cvt(it.box); + tmp.is_grounded = ((int) it.label == 0); // Only the first class (person) is grounded, that is moving on the floor plane + // others are tracked in full 3D space + objects_in.push_back(tmp); + } + + // Send the custom detected boxes to the ZED + zed.ingestCustomBoxObjects(objects_in); + + // Retrieve the tracked objects, with 2D and 3D attributes + zed.retrieveObjects(objects, objectTracker_parameters_rt); + + if (visualisation) { + // publish image + cv::Mat left_cv; + left_cv = slMat2cvMat(left_sl); + draw_objects(left_cv, left_cv, objects, CLASS_COLORS); + sensor_msgs::msg::Image::SharedPtr imageMsg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgra8", left_cv).toImageMsg(); + image_publisher->publish(*imageMsg); + } + + // Publish the detected objects + moa_msgs::msg::Detections detectionsMsg; + + int count = 0; + for (sl::ObjectData& obj : objects.object_list) { + geometry_msgs::msg::Point p; + switch (obj.raw_label) { + case 0: + p.x = obj.position[0] / 1000.0; + p.y = obj.position[1] / 1000.0; + + if (sqrt(p.x * p.x + p.y * p.y) < 6.0) { + detectionsMsg.blue.push_back(p); + } + + break; + case 4: + p.x = obj.position[0] / 1000.0; + p.y = obj.position[1] / 1000.0; + + if (sqrt(p.x * p.x + p.y * p.y) < 6.0) { + detectionsMsg.yellow.push_back(p); + } + + break; + default: + p.x = obj.position[0] / 1000.0; + p.y = obj.position[1] / 1000.0; + + if (sqrt(p.x * p.x + p.y * p.y) < 6.0) { + detectionsMsg.big_orange.push_back(p); + } + + break; + } + } + + mtx.lock(); + + zed.getPosition(cam_w_pose, sl::REFERENCE_FRAME::WORLD); + detectionsMsg.car_pose.position.x = cam_w_pose.getTranslation().tx / 1000.0; + detectionsMsg.car_pose.position.y = cam_w_pose.getTranslation().ty / 1000.0; + + float ox = cam_w_pose.getOrientation().ox; + float oy = cam_w_pose.getOrientation().oy; + float oz = cam_w_pose.getOrientation().oz; + float ow = cam_w_pose.getOrientation().ow; + + float yaw = atan2(2.0f * (ow * oz + ox * oy), 1.0f - 2.0f * (oy * oy + oz * oz)); + + detectionsMsg.car_pose.orientation.w = yaw; + + if (detectionsMsg.yellow.size() > 0 && detectionsMsg.blue.size() > 0) { + cone_detection_publisher->publish(detectionsMsg); + } + + mtx.unlock(); + } + + mtx.lock(); + + camera_running = false; + + mtx.unlock(); +} diff --git a/src/perception/aquisition/zed_launch/cone_detection/cone_subscriber.cpp b/src/perception/aquisition/zed_launch/cone_detection/cone_subscriber.cpp new file mode 100644 index 00000000..78a8258d --- /dev/null +++ b/src/perception/aquisition/zed_launch/cone_detection/cone_subscriber.cpp @@ -0,0 +1,53 @@ +// Copyright 2016 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. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "moa_msgs/msg/detections.hpp" + +using std::placeholders::_1; + +class ConeSubscriber : public rclcpp::Node +{ +public: + ConeSubscriber() + : Node("cone_subscriber") + { + subscription_ = this->create_subscription( + "cone_detection", 10, std::bind(&ConeSubscriber::cone_detection_callback, this, _1)); + } + +private: + void cone_detection_callback(const moa_msgs::msg::Detections & msg) const + { + for (auto i = 0u; i < msg.blue.size(); i++) + { + float distance = sqrt(pow(msg.blue[i].x, 2) + pow(msg.blue[i].y, 2)); + std::cout << "blue: " << i << " " << msg.blue[i].x << " " << msg.blue[i].y << " " << distance << std::endl; + } + } + rclcpp::Subscription::SharedPtr subscription_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/perception/aquisition/zed_launch/cone_detection/cuda_utils.h b/src/perception/aquisition/zed_launch/cone_detection/cuda_utils.h new file mode 100755 index 00000000..8fbd3199 --- /dev/null +++ b/src/perception/aquisition/zed_launch/cone_detection/cuda_utils.h @@ -0,0 +1,18 @@ +#ifndef TRTX_CUDA_UTILS_H_ +#define TRTX_CUDA_UTILS_H_ + +#include + +#ifndef CUDA_CHECK +#define CUDA_CHECK(callstr)\ + {\ + cudaError_t error_code = callstr;\ + if (error_code != cudaSuccess) {\ + std::cerr << "CUDA error " << error_code << " at " << __FILE__ << ":" << __LINE__;\ + assert(0);\ + }\ + } +#endif // CUDA_CHECK + +#endif // TRTX_CUDA_UTILS_H_ + diff --git a/src/perception/aquisition/zed_launch/cone_detection/logging.h b/src/perception/aquisition/zed_launch/cone_detection/logging.h new file mode 100755 index 00000000..1339ee2f --- /dev/null +++ b/src/perception/aquisition/zed_launch/cone_detection/logging.h @@ -0,0 +1,509 @@ +/* + * Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved. + * + * 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. + */ + +#ifndef TENSORRT_LOGGING_H +#define TENSORRT_LOGGING_H + +#include "NvInferRuntimeCommon.h" +#include +#include +#include +#include +#include +#include +#include + +#if NV_TENSORRT_MAJOR >= 8 +#define TRT_NOEXCEPT noexcept +#else +#define TRT_NOEXCEPT +#endif + +using Severity = nvinfer1::ILogger::Severity; + +class LogStreamConsumerBuffer : public std::stringbuf +{ +public: + LogStreamConsumerBuffer(std::ostream& stream, const std::string& prefix, bool shouldLog) + : mOutput(stream) + , mPrefix(prefix) + , mShouldLog(shouldLog) + { + } + + LogStreamConsumerBuffer(LogStreamConsumerBuffer&& other) + : mOutput(other.mOutput) + { + } + + ~LogStreamConsumerBuffer() + { + // std::streambuf::pbase() gives a pointer to the beginning of the buffered part of the output sequence + // std::streambuf::pptr() gives a pointer to the current position of the output sequence + // if the pointer to the beginning is not equal to the pointer to the current position, + // call putOutput() to log the output to the stream + if (pbase() != pptr()) + { + putOutput(); + } + } + + // synchronizes the stream buffer and returns 0 on success + // synchronizing the stream buffer consists of inserting the buffer contents into the stream, + // resetting the buffer and flushing the stream + virtual int sync() + { + putOutput(); + return 0; + } + + void putOutput() + { + if (mShouldLog) + { + // prepend timestamp + std::time_t timestamp = std::time(nullptr); + tm* tm_local = std::localtime(×tamp); + std::cout << "["; + std::cout << std::setw(2) << std::setfill('0') << 1 + tm_local->tm_mon << "/"; + std::cout << std::setw(2) << std::setfill('0') << tm_local->tm_mday << "/"; + std::cout << std::setw(4) << std::setfill('0') << 1900 + tm_local->tm_year << "-"; + std::cout << std::setw(2) << std::setfill('0') << tm_local->tm_hour << ":"; + std::cout << std::setw(2) << std::setfill('0') << tm_local->tm_min << ":"; + std::cout << std::setw(2) << std::setfill('0') << tm_local->tm_sec << "] "; + // std::stringbuf::str() gets the string contents of the buffer + // insert the buffer contents pre-appended by the appropriate prefix into the stream + mOutput << mPrefix << str(); + // set the buffer to empty + str(""); + // flush the stream + mOutput.flush(); + } + } + + void setShouldLog(bool shouldLog) + { + mShouldLog = shouldLog; + } + +private: + std::ostream& mOutput; + std::string mPrefix; + bool mShouldLog; +}; + +//! +//! \class LogStreamConsumerBase +//! \brief Convenience object used to initialize LogStreamConsumerBuffer before std::ostream in LogStreamConsumer +//! +class LogStreamConsumerBase +{ +public: + LogStreamConsumerBase(std::ostream& stream, const std::string& prefix, bool shouldLog) + : mBuffer(stream, prefix, shouldLog) + { + } + +protected: + LogStreamConsumerBuffer mBuffer; +}; + +//! +//! \class LogStreamConsumer +//! \brief Convenience object used to facilitate use of C++ stream syntax when logging messages. +//! Order of base classes is LogStreamConsumerBase and then std::ostream. +//! This is because the LogStreamConsumerBase class is used to initialize the LogStreamConsumerBuffer member field +//! in LogStreamConsumer and then the address of the buffer is passed to std::ostream. +//! This is necessary to prevent the address of an uninitialized buffer from being passed to std::ostream. +//! Please do not change the order of the parent classes. +//! +class LogStreamConsumer : protected LogStreamConsumerBase, public std::ostream +{ +public: + //! \brief Creates a LogStreamConsumer which logs messages with level severity. + //! Reportable severity determines if the messages are severe enough to be logged. + LogStreamConsumer(Severity reportableSeverity, Severity severity) + : LogStreamConsumerBase(severityOstream(severity), severityPrefix(severity), severity <= reportableSeverity) + , std::ostream(&mBuffer) // links the stream buffer with the stream + , mShouldLog(severity <= reportableSeverity) + , mSeverity(severity) + { + } + + LogStreamConsumer(LogStreamConsumer&& other) + : LogStreamConsumerBase(severityOstream(other.mSeverity), severityPrefix(other.mSeverity), other.mShouldLog) + , std::ostream(&mBuffer) // links the stream buffer with the stream + , mShouldLog(other.mShouldLog) + , mSeverity(other.mSeverity) + { + } + + void setReportableSeverity(Severity reportableSeverity) + { + mShouldLog = mSeverity <= reportableSeverity; + mBuffer.setShouldLog(mShouldLog); + } + +private: + static std::ostream& severityOstream(Severity severity) + { + return severity >= Severity::kINFO ? std::cout : std::cerr; + } + + static std::string severityPrefix(Severity severity) + { + switch (severity) + { + case Severity::kINTERNAL_ERROR: return "[F] "; + case Severity::kERROR: return "[E] "; + case Severity::kWARNING: return "[W] "; + case Severity::kINFO: return "[I] "; + case Severity::kVERBOSE: return "[V] "; + default: assert(0); return ""; + } + } + + bool mShouldLog; + Severity mSeverity; +}; + +//! \class Logger +//! +//! \brief Class which manages logging of TensorRT tools and samples +//! +//! \details This class provides a common interface for TensorRT tools and samples to log information to the console, +//! and supports logging two types of messages: +//! +//! - Debugging messages with an associated severity (info, warning, error, or internal error/fatal) +//! - Test pass/fail messages +//! +//! The advantage of having all samples use this class for logging as opposed to emitting directly to stdout/stderr is +//! that the logic for controlling the verbosity and formatting of sample output is centralized in one location. +//! +//! In the future, this class could be extended to support dumping test results to a file in some standard format +//! (for example, JUnit XML), and providing additional metadata (e.g. timing the duration of a test run). +//! +//! TODO: For backwards compatibility with existing samples, this class inherits directly from the nvinfer1::ILogger +//! interface, which is problematic since there isn't a clean separation between messages coming from the TensorRT +//! library and messages coming from the sample. +//! +//! In the future (once all samples are updated to use Logger::getTRTLogger() to access the ILogger) we can refactor the +//! class to eliminate the inheritance and instead make the nvinfer1::ILogger implementation a member of the Logger +//! object. + +class Logger : public nvinfer1::ILogger +{ +public: + Logger(Severity severity = Severity::kWARNING) + : mReportableSeverity(severity) + { + } + + //! + //! \enum TestResult + //! \brief Represents the state of a given test + //! + enum class TestResult + { + kRUNNING, //!< The test is running + kPASSED, //!< The test passed + kFAILED, //!< The test failed + kWAIVED //!< The test was waived + }; + + //! + //! \brief Forward-compatible method for retrieving the nvinfer::ILogger associated with this Logger + //! \return The nvinfer1::ILogger associated with this Logger + //! + //! TODO Once all samples are updated to use this method to register the logger with TensorRT, + //! we can eliminate the inheritance of Logger from ILogger + //! + nvinfer1::ILogger& getTRTLogger() + { + return *this; + } + + //! + //! \brief Implementation of the nvinfer1::ILogger::log() virtual method + //! + //! Note samples should not be calling this function directly; it will eventually go away once we eliminate the + //! inheritance from nvinfer1::ILogger + //! + void log(Severity severity, const char* msg) TRT_NOEXCEPT override + { + LogStreamConsumer(mReportableSeverity, severity) << "[TRT] " << std::string(msg) << std::endl; + } + + //! + //! \brief Method for controlling the verbosity of logging output + //! + //! \param severity The logger will only emit messages that have severity of this level or higher. + //! + void setReportableSeverity(Severity severity) + { + mReportableSeverity = severity; + } + + //! + //! \brief Opaque handle that holds logging information for a particular test + //! + //! This object is an opaque handle to information used by the Logger to print test results. + //! The sample must call Logger::defineTest() in order to obtain a TestAtom that can be used + //! with Logger::reportTest{Start,End}(). + //! + class TestAtom + { + public: + TestAtom(TestAtom&&) = default; + + private: + friend class Logger; + + TestAtom(bool started, const std::string& name, const std::string& cmdline) + : mStarted(started) + , mName(name) + , mCmdline(cmdline) + { + } + + bool mStarted; + std::string mName; + std::string mCmdline; + }; + + //! + //! \brief Define a test for logging + //! + //! \param[in] name The name of the test. This should be a string starting with + //! "TensorRT" and containing dot-separated strings containing + //! the characters [A-Za-z0-9_]. + //! For example, "TensorRT.sample_googlenet" + //! \param[in] cmdline The command line used to reproduce the test + // + //! \return a TestAtom that can be used in Logger::reportTest{Start,End}(). + //! + static TestAtom defineTest(const std::string& name, const std::string& cmdline) + { + return TestAtom(false, name, cmdline); + } + + //! + //! \brief A convenience overloaded version of defineTest() that accepts an array of command-line arguments + //! as input + //! + //! \param[in] name The name of the test + //! \param[in] argc The number of command-line arguments + //! \param[in] argv The array of command-line arguments (given as C strings) + //! + //! \return a TestAtom that can be used in Logger::reportTest{Start,End}(). + static TestAtom defineTest(const std::string& name, int argc, char const* const* argv) + { + auto cmdline = genCmdlineString(argc, argv); + return defineTest(name, cmdline); + } + + //! + //! \brief Report that a test has started. + //! + //! \pre reportTestStart() has not been called yet for the given testAtom + //! + //! \param[in] testAtom The handle to the test that has started + //! + static void reportTestStart(TestAtom& testAtom) + { + reportTestResult(testAtom, TestResult::kRUNNING); + assert(!testAtom.mStarted); + testAtom.mStarted = true; + } + + //! + //! \brief Report that a test has ended. + //! + //! \pre reportTestStart() has been called for the given testAtom + //! + //! \param[in] testAtom The handle to the test that has ended + //! \param[in] result The result of the test. Should be one of TestResult::kPASSED, + //! TestResult::kFAILED, TestResult::kWAIVED + //! + static void reportTestEnd(const TestAtom& testAtom, TestResult result) + { + assert(result != TestResult::kRUNNING); + assert(testAtom.mStarted); + reportTestResult(testAtom, result); + } + + static int reportPass(const TestAtom& testAtom) + { + reportTestEnd(testAtom, TestResult::kPASSED); + return EXIT_SUCCESS; + } + + static int reportFail(const TestAtom& testAtom) + { + reportTestEnd(testAtom, TestResult::kFAILED); + return EXIT_FAILURE; + } + + static int reportWaive(const TestAtom& testAtom) + { + reportTestEnd(testAtom, TestResult::kWAIVED); + return EXIT_SUCCESS; + } + + static int reportTest(const TestAtom& testAtom, bool pass) + { + return pass ? reportPass(testAtom) : reportFail(testAtom); + } + + Severity getReportableSeverity() const + { + return mReportableSeverity; + } + +private: + //! + //! \brief returns an appropriate string for prefixing a log message with the given severity + //! + static const char* severityPrefix(Severity severity) + { + switch (severity) + { + case Severity::kINTERNAL_ERROR: return "[F] "; + case Severity::kERROR: return "[E] "; + case Severity::kWARNING: return "[W] "; + case Severity::kINFO: return "[I] "; + case Severity::kVERBOSE: return "[V] "; + default: assert(0); return ""; + } + } + + //! + //! \brief returns an appropriate string for prefixing a test result message with the given result + //! + static const char* testResultString(TestResult result) + { + switch (result) + { + case TestResult::kRUNNING: return "RUNNING"; + case TestResult::kPASSED: return "PASSED"; + case TestResult::kFAILED: return "FAILED"; + case TestResult::kWAIVED: return "WAIVED"; + default: assert(0); return ""; + } + } + + //! + //! \brief returns an appropriate output stream (cout or cerr) to use with the given severity + //! + static std::ostream& severityOstream(Severity severity) + { + return severity >= Severity::kINFO ? std::cout : std::cerr; + } + + //! + //! \brief method that implements logging test results + //! + static void reportTestResult(const TestAtom& testAtom, TestResult result) + { + severityOstream(Severity::kINFO) << "&&&& " << testResultString(result) << " " << testAtom.mName << " # " + << testAtom.mCmdline << std::endl; + } + + //! + //! \brief generate a command line string from the given (argc, argv) values + //! + static std::string genCmdlineString(int argc, char const* const* argv) + { + std::stringstream ss; + for (int i = 0; i < argc; i++) + { + if (i > 0) + ss << " "; + ss << argv[i]; + } + return ss.str(); + } + + Severity mReportableSeverity; +}; + +namespace +{ + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kVERBOSE +//! +//! Example usage: +//! +//! LOG_VERBOSE(logger) << "hello world" << std::endl; +//! +inline LogStreamConsumer LOG_VERBOSE(const Logger& logger) +{ + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kVERBOSE); +} + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kINFO +//! +//! Example usage: +//! +//! LOG_INFO(logger) << "hello world" << std::endl; +//! +inline LogStreamConsumer LOG_INFO(const Logger& logger) +{ + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINFO); +} + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kWARNING +//! +//! Example usage: +//! +//! LOG_WARN(logger) << "hello world" << std::endl; +//! +inline LogStreamConsumer LOG_WARN(const Logger& logger) +{ + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kWARNING); +} + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kERROR +//! +//! Example usage: +//! +//! LOG_ERROR(logger) << "hello world" << std::endl; +//! +inline LogStreamConsumer LOG_ERROR(const Logger& logger) +{ + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kERROR); +} + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kINTERNAL_ERROR +// ("fatal" severity) +//! +//! Example usage: +//! +//! LOG_FATAL(logger) << "hello world" << std::endl; +//! +inline LogStreamConsumer LOG_FATAL(const Logger& logger) +{ + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINTERNAL_ERROR); +} + +} // anonymous namespace + +#endif // TENSORRT_LOGGING_H diff --git a/src/perception/aquisition/zed_launch/cone_detection/utils.h b/src/perception/aquisition/zed_launch/cone_detection/utils.h new file mode 100755 index 00000000..c7bb2779 --- /dev/null +++ b/src/perception/aquisition/zed_launch/cone_detection/utils.h @@ -0,0 +1,148 @@ +#ifndef TRTX_YOLOV5_UTILS_H_ +#define TRTX_YOLOV5_UTILS_H_ + +#include +#include +#include +#include "yolo.hpp" + + +static inline cv::Mat preprocess_img(cv::Mat& img, int input_w, int input_h) { + int w, h, x, y; + float r_w = input_w / (img.cols * 1.0); + float r_h = input_h / (img.rows * 1.0); + if (r_h > r_w) { + w = input_w; + h = r_w * img.rows; + x = 0; + y = (input_h - h) / 2; + } else { + w = r_h * img.cols; + h = input_h; + x = (input_w - w) / 2; + y = 0; + } + cv::Mat re(h, w, CV_8UC3); + cv::resize(img, re, re.size(), 0, 0, cv::INTER_LINEAR); + cv::Mat out(input_h, input_w, CV_8UC3, cv::Scalar(128, 128, 128)); + re.copyTo(out(cv::Rect(x, y, re.cols, re.rows))); + return out; +} + +std::vector> const CLASS_COLORS = { + {0, 114, 189}, {217, 83, 25}, {237, 177, 32}, {126, 47, 142}, {119, 172, 48}, {77, 190, 238}, + {162, 20, 47}, {76, 76, 76}, {153, 153, 153}, {255, 0, 0}, {255, 128, 0}, {191, 191, 0}, + {0, 255, 0}, {0, 0, 255}, {170, 0, 255}, {85, 85, 0}, {85, 170, 0}, {85, 255, 0}, + {170, 85, 0}, {170, 170, 0}, {170, 255, 0}, {255, 85, 0}, {255, 170, 0}, {255, 255, 0}, + {0, 85, 128}, {0, 170, 128}, {0, 255, 128}, {85, 0, 128}, {85, 85, 128}, {85, 170, 128}, + {85, 255, 128}, {170, 0, 128}, {170, 85, 128}, {170, 170, 128}, {170, 255, 128}, {255, 0, 128}, + {255, 85, 128}, {255, 170, 128}, {255, 255, 128}, {0, 85, 255}, {0, 170, 255}, {0, 255, 255}, + {85, 0, 255}, {85, 85, 255}, {85, 170, 255}, {85, 255, 255}, {170, 0, 255}, {170, 85, 255}, + {170, 170, 255}, {170, 255, 255}, {255, 0, 255}, {255, 85, 255}, {255, 170, 255}, {85, 0, 0}, + {128, 0, 0}, {170, 0, 0}, {212, 0, 0}, {255, 0, 0}, {0, 43, 0}, {0, 85, 0}, + {0, 128, 0}, {0, 170, 0}, {0, 212, 0}, {0, 255, 0}, {0, 0, 43}, {0, 0, 85}, + {0, 0, 128}, {0, 0, 170}, {0, 0, 212}, {0, 0, 255}, {0, 0, 0}, {36, 36, 36}, + {73, 73, 73}, {109, 109, 109}, {146, 146, 146}, {182, 182, 182}, {219, 219, 219}, {0, 114, 189}, + {80, 183, 189}, {128, 128, 0}}; + +inline cv::Scalar generateColorID_u(int idx) { + if (idx < 0) return cv::Scalar(236, 184, 36, 255); + int color_idx = idx % CLASS_COLORS.size(); + return cv::Scalar(CLASS_COLORS[color_idx][0], CLASS_COLORS[color_idx][1], CLASS_COLORS[color_idx][2], 255); +} + +inline sl::float4 generateColorID_f(int idx) { + auto clr_u = generateColorID_u(idx); + return sl::float4(static_cast (clr_u.val[0]) / 255.f, static_cast (clr_u.val[1]) / 255.f, static_cast (clr_u.val[2]) / 255.f, 1.f); +} + +inline bool renderObject(const sl::ObjectData& i, const bool isTrackingON) { + if (isTrackingON) + return (i.tracking_state == sl::OBJECT_TRACKING_STATE::OK); + else + return (i.tracking_state == sl::OBJECT_TRACKING_STATE::OK || i.tracking_state == sl::OBJECT_TRACKING_STATE::OFF); +} + +float const class_colors[6][3] = { + { 44.0f, 117.0f, 255.0f}, // PEOPLE + { 255.0f, 0.0f, 255.0f}, // VEHICLE + { 0.0f, 0.0f, 255.0f}, + { 0.0f, 255.0f, 255.0f}, + { 0.0f, 255.0f, 0.0f}, + { 255.0f, 255.0f, 255.0f} +}; + +inline sl::float4 getColorClass(int idx) { + idx = std::min(5, idx); + sl::float4 clr(class_colors[idx][0], class_colors[idx][1], class_colors[idx][2], 1.f); + return clr / 255.f; +} + +template +inline uchar _applyFading(T val, float current_alpha, double current_clr) { + return static_cast (current_alpha * current_clr + (1.0 - current_alpha) * val); +} + +inline cv::Vec4b applyFading(cv::Scalar val, float current_alpha, cv::Scalar current_clr) { + cv::Vec4b out; + out[0] = _applyFading(val.val[0], current_alpha, current_clr.val[0]); + out[1] = _applyFading(val.val[1], current_alpha, current_clr.val[1]); + out[2] = _applyFading(val.val[2], current_alpha, current_clr.val[2]); + out[3] = 255; + return out; +} + +inline void drawVerticalLine( + cv::Mat &left_display, + cv::Point2i start_pt, + cv::Point2i end_pt, + cv::Scalar clr, + int thickness) { + int n_steps = 7; + cv::Point2i pt1, pt4; + pt1.x = ((n_steps - 1) * start_pt.x + end_pt.x) / n_steps; + pt1.y = ((n_steps - 1) * start_pt.y + end_pt.y) / n_steps; + + pt4.x = (start_pt.x + (n_steps - 1) * end_pt.x) / n_steps; + pt4.y = (start_pt.y + (n_steps - 1) * end_pt.y) / n_steps; + + cv::line(left_display, start_pt, pt1, clr, thickness); + cv::line(left_display, pt4, end_pt, clr, thickness); +} + +inline cv::Mat slMat2cvMat(sl::Mat const& input) { + // Mapping between MAT_TYPE and CV_TYPE + int cv_type = -1; + switch (input.getDataType()) { + case sl::MAT_TYPE::F32_C1: cv_type = CV_32FC1; + break; + case sl::MAT_TYPE::F32_C2: cv_type = CV_32FC2; + break; + case sl::MAT_TYPE::F32_C3: cv_type = CV_32FC3; + break; + case sl::MAT_TYPE::F32_C4: cv_type = CV_32FC4; + break; + case sl::MAT_TYPE::U8_C1: cv_type = CV_8UC1; + break; + case sl::MAT_TYPE::U8_C2: cv_type = CV_8UC2; + break; + case sl::MAT_TYPE::U8_C3: cv_type = CV_8UC3; + break; + case sl::MAT_TYPE::U8_C4: cv_type = CV_8UC4; + break; + default: break; + } + + return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr(sl::MEM::CPU)); +} + +inline bool readFile(std::string filename, std::vector &file_content) { + // open the file: + std::ifstream instream(filename, std::ios::in | std::ios::binary); + if (!instream.is_open()) return true; + file_content = std::vector((std::istreambuf_iterator(instream)), std::istreambuf_iterator()); + return false; +} + +#endif // TRTX_YOLOV5_UTILS_H_ + diff --git a/src/perception/aquisition/zed_launch/cone_detection/yolo.cpp b/src/perception/aquisition/zed_launch/cone_detection/yolo.cpp new file mode 100755 index 00000000..1a3cc5eb --- /dev/null +++ b/src/perception/aquisition/zed_launch/cone_detection/yolo.cpp @@ -0,0 +1,496 @@ +#include "yolo.hpp" +#include "NvOnnxParser.h" + +using namespace nvinfer1; + +static Logger gLogger; + +inline int clamp(int val, int min, int max) { + if (val <= min) return min; + if (val >= max) return max; + return val; +} + + +#define WEIGHTED_NMS + +std::vector nonMaximumSuppression(const float nmsThresh, std::vector binfo) { + auto overlap1D = [](float x1min, float x1max, float x2min, float x2max) -> float { + if (x1min > x2min) { + std::swap(x1min, x2min); + std::swap(x1max, x2max); + } + return x1max < x2min ? 0 : std::min(x1max, x2max) - x2min; + }; + + auto computeIoU = [&overlap1D](BBox& bbox1, BBox & bbox2) -> float { + float overlapX = overlap1D(bbox1.x1, bbox1.x2, bbox2.x1, bbox2.x2); + float overlapY = overlap1D(bbox1.y1, bbox1.y2, bbox2.y1, bbox2.y2); + float area1 = (bbox1.x2 - bbox1.x1) * (bbox1.y2 - bbox1.y1); + float area2 = (bbox2.x2 - bbox2.x1) * (bbox2.y2 - bbox2.y1); + float overlap2D = overlapX * overlapY; + float u = area1 + area2 - overlap2D; + return u == 0 ? 0 : overlap2D / u; + }; + + std::stable_sort(binfo.begin(), binfo.end(), [](const BBoxInfo& b1, const BBoxInfo & b2) { + return b1.prob > b2.prob; }); + + std::vector out; + +#if defined(WEIGHTED_NMS) + std::vector > weigthed_nms_candidates; +#endif + for (auto& i : binfo) { + bool keep = true; + +#if defined(WEIGHTED_NMS) + int j_index = 0; +#endif + + for (auto& j : out) { + if (keep) { + float overlap = computeIoU(i.box, j.box); + keep = overlap <= nmsThresh; +#if defined(WEIGHTED_NMS) + if (!keep && fabs(j.prob - i.prob) < 0.52f) // add label similarity check + weigthed_nms_candidates[j_index].push_back(i); +#endif + } else + break; + +#if defined(WEIGHTED_NMS) + j_index++; +#endif + + } + if (keep) { + out.push_back(i); +#if defined(WEIGHTED_NMS) + weigthed_nms_candidates.emplace_back(); + weigthed_nms_candidates.back().clear(); +#endif + } + } + +#if defined(WEIGHTED_NMS) + + for (int i = 0; i < out.size(); i++) { + // the best confidence + BBoxInfo& best = out[i]; + float sum_tl_x = best.box.x1 * best.prob; + float sum_tl_y = best.box.y1 * best.prob; + float sum_br_x = best.box.x2 * best.prob; + float sum_br_y = best.box.y2 * best.prob; + + float weight = best.prob; + for (auto& it : weigthed_nms_candidates[i]) { + sum_tl_x += it.box.x1 * it.prob; + sum_tl_y += it.box.y1 * it.prob; + sum_br_x += it.box.x2 * it.prob; + sum_br_y += it.box.y2 * it.prob; + weight += it.prob; + } + + weight = 1.f / weight; + best.box.x1 = sum_tl_x * weight; + best.box.y1 = sum_tl_y * weight; + best.box.x2 = sum_br_x * weight; + best.box.y2 = sum_br_y * weight; + } + +#endif + + return out; +} + +Yolo::Yolo() { +} + +Yolo::~Yolo() { + if (is_init) { + + // Release stream and buffers + cudaStreamDestroy(stream); + CUDA_CHECK(cudaFree(d_input)); + CUDA_CHECK(cudaFree(d_output)); + // Destroy the engine + context->destroy(); + engine->destroy(); + runtime->destroy(); + + delete[] h_input; + delete[] h_output; + } + is_init = false; +} + +int Yolo::build_engine(std::string onnx_path, std::string engine_path, OptimDim dyn_dim_profile) { + + + std::vector onnx_file_content; + if (readFile(onnx_path, onnx_file_content)) return 1; + + if ((!onnx_file_content.empty())) { + + ICudaEngine * engine; + // Create engine (onnx) + std::cout << "Creating engine from onnx model" << std::endl; + + gLogger.setReportableSeverity(Severity::kINFO); + auto builder = nvinfer1::createInferBuilder(gLogger); + if (!builder) { + std::cerr << "createInferBuilder failed" << std::endl; + return 1; + } + + auto explicitBatch = 1U << static_cast (nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + auto network = builder->createNetworkV2(explicitBatch); + + if (!network) { + std::cerr << "createNetwork failed" << std::endl; + return 1; + } + + auto config = builder->createBuilderConfig(); + if (!config) { + std::cerr << "createBuilderConfig failed" << std::endl; + return 1; + } + + ////////// Dynamic dimensions handling : support only 1 size at a time + if (!dyn_dim_profile.tensor_name.empty()) { + + IOptimizationProfile* profile = builder->createOptimizationProfile(); + + profile->setDimensions(dyn_dim_profile.tensor_name.c_str(), OptProfileSelector::kMIN, dyn_dim_profile.size); + profile->setDimensions(dyn_dim_profile.tensor_name.c_str(), OptProfileSelector::kOPT, dyn_dim_profile.size); + profile->setDimensions(dyn_dim_profile.tensor_name.c_str(), OptProfileSelector::kMAX, dyn_dim_profile.size); + + config->addOptimizationProfile(profile); + builder->setMaxBatchSize(1); + } + + auto parser = nvonnxparser::createParser(*network, gLogger); + if (!parser) { + std::cerr << "nvonnxparser::createParser failed" << std::endl; + return 1; + } + + bool parsed = false; + unsigned char *onnx_model_buffer = onnx_file_content.data(); + size_t onnx_model_buffer_size = onnx_file_content.size() * sizeof (char); + parsed = parser->parse(onnx_model_buffer, onnx_model_buffer_size); + + if (!parsed) { + std::cerr << "onnx file parsing failed" << std::endl; + return 1; + } + + if (builder->platformHasFastFp16()) { + std::cout << "FP16 enabled!" << std::endl; + config->setFlag(BuilderFlag::kFP16); + } + + //////////////// Actual engine building + + engine = builder->buildEngineWithConfig(*network, *config); + + onnx_file_content.clear(); + + // write plan file if it is specified + if (engine == nullptr) return 1; + IHostMemory* ptr = engine->serialize(); + assert(ptr); + if (ptr == nullptr) return 1; + + FILE *fp = fopen(engine_path.c_str(), "wb"); + fwrite(reinterpret_cast (ptr->data()), ptr->size() * sizeof (char), 1, fp); + fclose(fp); + + parser->destroy(); + network->destroy(); + config->destroy(); + builder->destroy(); + + engine->destroy(); + + return 0; + } else return 1; + + +} + +int Yolo::init(std::string engine_name) { + + + // deserialize the .engine and run inference + std::ifstream file(engine_name, std::ios::binary); + if (!file.good()) { + std::cerr << "read " << engine_name << " error!" << std::endl; + return -1; + } + char *trtModelStream = nullptr; + size_t size = 0; + file.seekg(0, file.end); + size = file.tellg(); + file.seekg(0, file.beg); + trtModelStream = new char[size]; + if (!trtModelStream) return 1; + file.read(trtModelStream, size); + file.close(); + + // prepare input data --------------------------- + runtime = createInferRuntime(gLogger); + if (runtime == nullptr) return 1; + engine = runtime->deserializeCudaEngine(trtModelStream, size); + if (engine == nullptr) return 1; + context = engine->createExecutionContext(); + if (context == nullptr) return 1; + + delete[] trtModelStream; + if (engine->getNbBindings() != 2) return 1; + + + const int bindings = engine->getNbBindings(); + for (int i = 0; i < bindings; i++) { + if (engine->bindingIsInput(i)) { + input_binding_name = engine->getBindingName(i); + Dims bind_dim = engine->getBindingDimensions(i); + input_width = bind_dim.d[3]; + input_height = bind_dim.d[2]; + inputIndex = i; + std::cout << "Inference size : " << input_height << "x" << input_width << std::endl; + }//if (engine->getTensorIOMode(engine->getBindingName(i)) == TensorIOMode::kOUTPUT) + else { + output_name = engine->getBindingName(i); + // fill size, allocation must be done externally + outputIndex = i; + Dims bind_dim = engine->getBindingDimensions(i); + size_t batch = bind_dim.d[0]; + if (batch > batch_size) { + std::cout << "batch > 1 not supported" << std::endl; + return 1; + } + size_t dim1 = bind_dim.d[1]; + size_t dim2 = bind_dim.d[2]; + + if (dim1 > dim2) { + // Yolov6 1x8400x85 // 85=5+80=cxcy+cwch+obj_conf+cls_conf + out_dim = dim1; + out_box_struct_number = 5; + out_class_number = dim2 - out_box_struct_number; + yolo_model_version = YOLO_MODEL_VERSION_OUTPUT_STYLE::YOLOV6; + std::cout << "YOLOV6 format" << std::endl; + } else { + // Yolov8 1x84x8400 + out_dim = dim2; + out_box_struct_number = 4; + out_class_number = dim1 - out_box_struct_number; + yolo_model_version = YOLO_MODEL_VERSION_OUTPUT_STYLE::YOLOV8_V5; + std::cout << "YOLOV8/YOLOV5 format" << std::endl; + } + } + } + output_size = out_dim * (out_class_number + out_box_struct_number); + h_input = new float[batch_size * 3 * input_height * input_width]; + h_output = new float[batch_size * output_size]; + // In order to bind the buffers, we need to know the names of the input and output tensors. + // Note that indices are guaranteed to be less than IEngine::getNbBindings() + assert(inputIndex == 0); + assert(outputIndex == 1); + // Create GPU buffers on device + CUDA_CHECK(cudaMalloc(&d_input, batch_size * 3 * input_height * input_width * sizeof (float))); + CUDA_CHECK(cudaMalloc(&d_output, batch_size * output_size * sizeof (float))); + // Create stream + CUDA_CHECK(cudaStreamCreate(&stream)); + + if (batch_size != 1) return 1; // This sample only support batch 1 for now + + is_init = true; + return 0; +} + +std::vector Yolo::run(sl::Mat left_sl, int orig_image_h, int orig_image_w, float thres) { + std::vector binfo; + + size_t frame_s = input_height * input_width; + + /////// Preparing inference + cv::Mat left_cv_rgba = slMat2cvMat(left_sl); + cv::cvtColor(left_cv_rgba, left_cv_rgb, cv::COLOR_BGRA2BGR); + if (left_cv_rgb.empty()) return binfo; + cv::Mat pr_img = preprocess_img(left_cv_rgb, input_width, input_height); // letterbox BGR to RGB + int i = 0; + int batch = 0; + for (int row = 0; row < input_height; ++row) { + uchar* uc_pixel = pr_img.data + row * pr_img.step; + for (int col = 0; col < input_width; ++col) { + h_input[batch * 3 * frame_s + i] = (float) uc_pixel[2] / 255.0; + h_input[batch * 3 * frame_s + i + frame_s] = (float) uc_pixel[1] / 255.0; + h_input[batch * 3 * frame_s + i + 2 * frame_s] = (float) uc_pixel[0] / 255.0; + uc_pixel += 3; + ++i; + } + } + + /////// INFERENCE + // DMA input batch data to device, infer on the batch asynchronously, and DMA output back to host + CUDA_CHECK(cudaMemcpyAsync(d_input, h_input, batch_size * 3 * frame_s * sizeof (float), cudaMemcpyHostToDevice, stream)); + + std::vector d_buffers_nvinfer(2); + d_buffers_nvinfer[inputIndex] = d_input; + d_buffers_nvinfer[outputIndex] = d_output; + context->enqueueV2(&d_buffers_nvinfer[0], stream, nullptr); + + CUDA_CHECK(cudaMemcpyAsync(h_output, d_output, batch_size * output_size * sizeof (float), cudaMemcpyDeviceToHost, stream)); + cudaStreamSynchronize(stream); + + + /////// Extraction + + float scalingFactor = std::min(static_cast (input_width) / orig_image_w, static_cast (input_height) / orig_image_h); + float xOffset = (input_width - scalingFactor * orig_image_w) * 0.5f; + float yOffset = (input_height - scalingFactor * orig_image_h) * 0.5f; + scalingFactor = 1.f / scalingFactor; + float scalingFactor_x = scalingFactor; + float scalingFactor_y = scalingFactor; + + + switch (yolo_model_version) { + default: + case YOLO_MODEL_VERSION_OUTPUT_STYLE::YOLOV8_V5: + { + // https://github.com/triple-Mu/YOLOv8-TensorRT/blob/df11cec3abaab7fefb28fb760f1cebbddd5ec826/csrc/detect/normal/include/yolov8.hpp#L343 + auto num_channels = out_class_number + out_box_struct_number; + auto num_anchors = out_dim; + auto num_labels = out_class_number; + + auto& dw = xOffset; + auto& dh = yOffset; + + auto& width = orig_image_w; + auto& height = orig_image_h; + + cv::Mat output = cv::Mat( + num_channels, + num_anchors, + CV_32F, + static_cast (h_output) + ); + output = output.t(); + for (int i = 0; i < num_anchors; i++) { + auto row_ptr = output.row(i).ptr(); + auto bboxes_ptr = row_ptr; + auto scores_ptr = row_ptr + out_box_struct_number; + auto max_s_ptr = std::max_element(scores_ptr, scores_ptr + num_labels); + float score = *max_s_ptr; + if (score > thres) { + int label = max_s_ptr - scores_ptr; + + BBoxInfo bbi; + + float x = *bboxes_ptr++ - dw; + float y = *bboxes_ptr++ - dh; + float w = *bboxes_ptr++; + float h = *bboxes_ptr; + + float x0 = clamp((x - 0.5f * w) * scalingFactor_x, 0.f, width); + float y0 = clamp((y - 0.5f * h) * scalingFactor_y, 0.f, height); + float x1 = clamp((x + 0.5f * w) * scalingFactor_x, 0.f, width); + float y1 = clamp((y + 0.5f * h) * scalingFactor_y, 0.f, height); + + cv::Rect_ bbox; + bbox.x = x0; + bbox.y = y0; + bbox.width = x1 - x0; + bbox.height = y1 - y0; + + bbi.box.x1 = x0; + bbi.box.y1 = y0; + bbi.box.x2 = x1; + bbi.box.y2 = y1; + + if ((bbi.box.x1 > bbi.box.x2) || (bbi.box.y1 > bbi.box.y2)) break; + + bbi.label = label; + bbi.prob = score; + + binfo.push_back(bbi); + } + } + break; + } + case YOLO_MODEL_VERSION_OUTPUT_STYLE::YOLOV6: + { + // https://github.com/DefTruth/lite.ai.toolkit/blob/1267584d5dae6269978e17ffd5ec29da496e503e/lite/ort/cv/yolov6.cpp#L97 + + auto& dw_ = xOffset; + auto& dh_ = yOffset; + + auto& width = orig_image_w; + auto& height = orig_image_h; + + const unsigned int num_anchors = out_dim; // n = ? + const unsigned int num_classes = out_class_number; // - out_box_struct_number; // 80 + + for (unsigned int i = 0; i < num_anchors; ++i) { + const float *offset_obj_cls_ptr = h_output + (i * (num_classes + 5)); // row ptr + float obj_conf = offset_obj_cls_ptr[4]; /*always == 1 for some reasons*/ + float cls_conf = offset_obj_cls_ptr[5]; + + // The confidence is remapped because it output basically garbage with conf < ~0.1 and we don't want to clamp it either + const float conf_offset = 0.1; + const float input_start = 0; + const float output_start = input_start; + const float output_end = 1; + const float input_end = output_end - conf_offset; + + float conf = (obj_conf * cls_conf) - conf_offset; + if (conf < 0) conf = 0; + conf = (conf - input_start) / (input_end - input_start) * (output_end - output_start) + output_start; + + if (conf > thres) { + + unsigned int label = 0; + for (unsigned int j = 0; j < num_classes; ++j) { + float tmp_conf = offset_obj_cls_ptr[j + 5]; + if (tmp_conf > cls_conf) { + cls_conf = tmp_conf; + label = j; + } + } // argmax + + BBoxInfo bbi; + + float cx = offset_obj_cls_ptr[0]; + float cy = offset_obj_cls_ptr[1]; + float w = offset_obj_cls_ptr[2]; + float h = offset_obj_cls_ptr[3]; + float x1 = ((cx - w / 2.f) - (float) dw_) * scalingFactor_x; + float y1 = ((cy - h / 2.f) - (float) dh_) * scalingFactor_y; + float x2 = ((cx + w / 2.f) - (float) dw_) * scalingFactor_x; + float y2 = ((cy + h / 2.f) - (float) dh_) * scalingFactor_y; + + bbi.box.x1 = std::max(0.f, x1); + bbi.box.y1 = std::max(0.f, y1); + bbi.box.x2 = std::min(x2, (float) width - 1.f); + bbi.box.y2 = std::min(y2, (float) height - 1.f); + + if ((bbi.box.x1 > bbi.box.x2) || (bbi.box.y1 > bbi.box.y2)) break; + + bbi.label = label; + bbi.prob = conf; + + binfo.push_back(bbi); + } + } + break; + } + }; + + /// NMS + binfo = nonMaximumSuppression(nms, binfo); + + return binfo; +} diff --git a/src/perception/aquisition/zed_launch/cone_detection/yolo.hpp b/src/perception/aquisition/zed_launch/cone_detection/yolo.hpp new file mode 100755 index 00000000..9d1e1f16 --- /dev/null +++ b/src/perception/aquisition/zed_launch/cone_detection/yolo.hpp @@ -0,0 +1,125 @@ +#ifndef YOLO_HPP +#define YOLO_HPP + +#include +#include +#include + +#include "cuda_utils.h" +#include "logging.h" +#include "utils.h" + +enum class YOLO_MODEL_VERSION_OUTPUT_STYLE { + YOLOV6, + YOLOV8_V5 +}; + +struct BBox { + float x1, y1, x2, y2; +}; + +struct BBoxInfo { + BBox box; + int label; + float prob; +}; + +inline std::vector split_str(std::string s, std::string delimiter) { + size_t pos_start = 0, pos_end, delim_len = delimiter.length(); + std::string token; + std::vector res; + + while ((pos_end = s.find(delimiter, pos_start)) != std::string::npos) { + token = s.substr(pos_start, pos_end - pos_start); + pos_start = pos_end + delim_len; + res.push_back(token); + } + + res.push_back(s.substr(pos_start)); + return res; +} + + +struct OptimDim { + nvinfer1::Dims4 size; + std::string tensor_name; + + bool setFromString(std::string &arg) { + // "images:1x3x512x512" + std::vector v_ = split_str(arg, ":"); + if (v_.size() != 2) return true; + + std::string dims_str = v_.back(); + std::vector v = split_str(dims_str, "x"); + + size.nbDims = 4; + // assuming batch is 1 and channel is 3 + size.d[0] = 1; + size.d[1] = 3; + + if (v.size() == 2) { + size.d[2] = stoi(v[0]); + size.d[3] = stoi(v[1]); + } else if (v.size() == 3) { + size.d[2] = stoi(v[1]); + size.d[3] = stoi(v[2]); + } else if (v.size() == 4) { + size.d[2] = stoi(v[2]); + size.d[3] = stoi(v[3]); + } else return true; + + if (size.d[2] != size.d[3]) std::cerr << "Warning only squared input are currently supported" << std::endl; + + tensor_name = v_.front(); + return false; + } +}; + +class Yolo { +public: + Yolo(); + ~Yolo(); + + static int build_engine(std::string onnx_path, std::string engine_path, OptimDim dyn_dim_profile); + + int init(std::string engine_path); + std::vector run(sl::Mat left_sl, int orig_image_h, int orig_image_w, float thres); + + sl::Resolution getInferenceSize() { + return sl::Resolution(input_width, input_height); + } + +private: + + cv::Mat left_cv_rgb; + + float nms = 0.4; + + // Get input dimension size + std::string input_binding_name = "images"; // input + std::string output_name = "classes"; + int inputIndex, outputIndex; + size_t input_width = 0, input_height = 0, batch_size = 1; + // Yolov6 1x8400x85 // 85=5+80=cxcy+cwch+obj_conf+cls_conf //https://github.com/DefTruth/lite.ai.toolkit/blob/1267584d5dae6269978e17ffd5ec29da496e503e/lite/ort/cv/yolov6.cpp#L97 + // Yolov8/yolov5 1x84x8400 + size_t out_dim = 8400, out_class_number = 80 /*for COCO*/, out_box_struct_number = 4; // https://github.com/ultralytics/yolov3/issues/750#issuecomment-569783354 + size_t output_size = 0; + + YOLO_MODEL_VERSION_OUTPUT_STYLE yolo_model_version; + + + float *h_input, *h_output; + float *d_input, *d_output; + + nvinfer1::IRuntime* runtime; + nvinfer1::ICudaEngine* engine; + nvinfer1::IExecutionContext* context; + cudaStream_t stream; + + bool is_init = false; + + +}; + +#endif /* YOLO_HPP */ + diff --git a/src/perception/aquisition/zed_launch/launch/zed_launch.launch.py b/src/perception/aquisition/zed_launch/launch/zed_launch.launch.py new file mode 100644 index 00000000..f2629de8 --- /dev/null +++ b/src/perception/aquisition/zed_launch/launch/zed_launch.launch.py @@ -0,0 +1,38 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + # Declare the launch argument for recording name + recording_name_arg = DeclareLaunchArgument( + 'recording_name', + default_value='default_recording', + description=""" + This launch file is used to start the ZED camera node with or + without recording functionality. + Usage: + - Without recording: ros2 launch zed_launch.launch.py + - With recording: ros2 launch zed_launch.launch.py recording_name:= + + The 'recording_name' argument is optional. If provided, the camera node will + initiate a recording session using the specified file name. + """ + ) + + # Access the recording name configured by the user + recording_name = LaunchConfiguration('recording_name') + + # Define the node, including the recording name as a command-line argument + camera_node = Node( + package='zed_launch', + executable='zed_launch_node', + name='zed_camera', + output='screen', + arguments=[recording_name] + ) + + return LaunchDescription([ + recording_name_arg, + camera_node + ]) \ No newline at end of file diff --git a/src/perception/aquisition/zed_launch/localisation/localisation.cpp b/src/perception/aquisition/zed_launch/localisation/localisation.cpp new file mode 100644 index 00000000..86d9046c --- /dev/null +++ b/src/perception/aquisition/zed_launch/localisation/localisation.cpp @@ -0,0 +1,36 @@ +#include +#include + +#include "sl/Camera.hpp" +#include "../zed_launch/zed_launch.hpp" + +#include "geometry_msgs/msg/pose.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" + +void ZedLaunchNode::car_position() { + + geometry_msgs::msg::Pose car_pose; + visualization_msgs::msg::MarkerArray car_marker_array; + geometry_msgs::msg::TransformStamped map; + + int id = 0; + while (camera_running) { + car_pose.position.x = cam_w_pose.getTranslation().tx / 1000.0; + car_pose.position.y = cam_w_pose.getTranslation().ty / 1000.0; + + float ox = cam_w_pose.getOrientation().ox; + float oy = cam_w_pose.getOrientation().oy; + float oz = cam_w_pose.getOrientation().oz; + float ow = cam_w_pose.getOrientation().ow; + + float yaw = atan2(2.0f * (ow * oz + ox * oy), 1.0f - 2.0f * (oy * oy + oz * oz)); + + car_pose.orientation.w = yaw; + + car_position_publisher->publish(car_pose); + + std::this_thread::sleep_for(100ms); + } +} \ No newline at end of file diff --git a/src/perception/aquisition/zed_launch/localisation/localisation_subscriber.cpp b/src/perception/aquisition/zed_launch/localisation/localisation_subscriber.cpp new file mode 100644 index 00000000..4309fd94 --- /dev/null +++ b/src/perception/aquisition/zed_launch/localisation/localisation_subscriber.cpp @@ -0,0 +1,49 @@ +// Copyright 2016 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. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose.hpp" + +using std::placeholders::_1; + +class LocalisationSubscriber : public rclcpp::Node +{ +public: + LocalisationSubscriber() + : Node("localisation_subscriber") + { + subscription_ = this->create_subscription( + "car_position", 10, std::bind(&LocalisationSubscriber::car_position_callback, this, _1)); + } + +private: + void car_position_callback(const geometry_msgs::msg::Pose & msg) const + { + std::cout << "car_position : " << msg.position.x << " " << msg.position.y << " " << msg.position.z << " " << std::endl; + } + rclcpp::Subscription::SharedPtr subscription_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/perception/aquisition/zed_launch/package.xml b/src/perception/aquisition/zed_launch/package.xml new file mode 100644 index 00000000..58be6bd6 --- /dev/null +++ b/src/perception/aquisition/zed_launch/package.xml @@ -0,0 +1,30 @@ + + + + zed_launch +4.1.0 + zed launch node + adrian rosioru + Apache License 2.0 + + ament_cmake + ament_cmake_auto + rclcpp + std_msgs + moa_msgs + geometry_msgs + sensor_msgs + cv_bridge + + rclcpp + std_msgs + moa_msgs + geometry_msgs + sensor_msgs + cv_bridge + ros2launch + + + ament_cmake + + diff --git a/src/perception/aquisition/zed_launch/velocity/velocity.cpp b/src/perception/aquisition/zed_launch/velocity/velocity.cpp new file mode 100644 index 00000000..aa196c38 --- /dev/null +++ b/src/perception/aquisition/zed_launch/velocity/velocity.cpp @@ -0,0 +1,33 @@ +#include +#include + +#include "sl/Camera.hpp" +#include "../zed_launch/zed_launch.hpp" + +#include "geometry_msgs/msg/vector3.hpp" + +void ZedLaunchNode::car_velocity() { + + geometry_msgs::msg::Vector3 car_velocity; + sl::SensorsData sensor_data; + sl::SensorsData::IMUData imu_data; + + car_velocity.x = 0; + car_velocity.y = 0; + car_velocity.z = 0; + + while (camera_running) { + std::this_thread::sleep_for(10ms); + + zed.getSensorsData(sensor_data, sl::TIME_REFERENCE::IMAGE); + imu_data = sensor_data.imu; + + car_velocity.x += imu_data.linear_acceleration.x * 0.01; + car_velocity.y += imu_data.linear_acceleration.y * 0.01; + + car_velocity_publisher->publish(car_velocity); + + } +} + +// ### THIS DOES NOT OUTPUT ACCURATE VELOCITY DATA ### \ No newline at end of file diff --git a/src/perception/aquisition/zed_launch/zed_launch/zed_launch.cpp b/src/perception/aquisition/zed_launch/zed_launch/zed_launch.cpp new file mode 100644 index 00000000..26b964b1 --- /dev/null +++ b/src/perception/aquisition/zed_launch/zed_launch/zed_launch.cpp @@ -0,0 +1,83 @@ +// Copyright 2022 Stereolabs +// +// 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. + +#include + +#include + +#include "sl/Camera.hpp" + +#include "zed_launch.hpp" + +int main(int argc, char * argv[]) +{ + // Force flush of the stdout buffer. + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + // Initialize any global resources needed by the middleware and the client library. + // This will also parse command line arguments one day (as of Beta 1 they are not used). + // You must call this before using any other part of the ROS system. + // This should be called once per process. + rclcpp::init(argc, argv); + + /// Opening the ZED camera before the model deserialization to avoid cuda context issue + sl::Camera zed; + sl::InitParameters init_parameters; + init_parameters.sdk_verbose = true; + init_parameters.depth_mode = sl::DEPTH_MODE::ULTRA; + init_parameters.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP; + + + if (argc > 1) { + std::string zed_opt = argv[1]; + if (zed_opt.find(".svo") != std::string::npos) + init_parameters.input.setFromSVOFile(zed_opt.c_str()); + } + + // Open the camera + auto returned_state = zed.open(init_parameters); + if (returned_state != sl::ERROR_CODE::SUCCESS) { + std::cerr << "Camera Open " << returned_state << ", exit program." << std::endl; + return EXIT_FAILURE; + } + + + // zed.setCameraSettings(sl::VIDEO_SETTINGS::EXPOSURE, sl::VIDEO_SETTINGS_VALUE_AUTO); + // zed.setCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS, 5); + // zed.setCameraSettings(sl::VIDEO_SETTINGS::CONTRAST, 7); + // zed.setCameraSettings(sl::VIDEO_SETTINGS::SATURATION, 7); + + // Enable positional tracking + zed.enablePositionalTracking(); + + // Custom OD + sl::ObjectDetectionParameters detection_parameters; + detection_parameters.enable_tracking = true; + detection_parameters.enable_segmentation = false; // designed to give person pixel mask with internal OD + detection_parameters.detection_model = sl::OBJECT_DETECTION_MODEL::CUSTOM_BOX_OBJECTS; + returned_state = zed.enableObjectDetection(detection_parameters); + if (returned_state != sl::ERROR_CODE::SUCCESS) { + std::cerr << "enableObjectDetection " << returned_state << ", exit program." << std::endl; + zed.close(); + return EXIT_FAILURE; + } + + // Create a node. + rclcpp::spin(std::make_shared(zed)); + + rclcpp::shutdown(); + + return 0; +} + diff --git a/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp b/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp new file mode 100644 index 00000000..e563d7ff --- /dev/null +++ b/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp @@ -0,0 +1,60 @@ +#include +#include +#include +#include + +#include "sl/Camera.hpp" + +#include + +#include "std_msgs/msg/string.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "moa_msgs/msg/detections.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "geometry_msgs/msg/vector3.hpp" + +using namespace std::chrono_literals; + +class ZedLaunchNode : public rclcpp::Node +{ +public: + ZedLaunchNode(sl::Camera& _zed) + : Node("cone_detection_node"), zed(_zed) + { + // Initialize the camera pose + cam_w_pose.pose_data.setIdentity(); + + // Create the publishers + cone_detection_publisher = this->create_publisher("cone_detection", 10); + car_position_publisher = this->create_publisher("car_position", 10); + car_velocity_publisher = this->create_publisher("car_velocity", 10); + image_publisher = this->create_publisher("image", 10); + + // Start the threads + std::thread t1(&ZedLaunchNode::cone_detection_loop, this); + + std::thread t2(&ZedLaunchNode::car_position, this); + + std::thread t3(&ZedLaunchNode::car_velocity, this); + + t1.join(); + } + +private: + // ZED camera object + sl::Camera& zed; + sl::Pose cam_w_pose; + bool camera_running = true; + bool visualisation = true; + std::string model_name = "cone_detection_model.engine"; + + // Publishers + rclcpp::Publisher::SharedPtr cone_detection_publisher; + rclcpp::Publisher::SharedPtr car_position_publisher; + rclcpp::Publisher::SharedPtr image_publisher; + rclcpp::Publisher::SharedPtr car_velocity_publisher; + + void cone_detection_loop(); + void car_position(); + void car_velocity(); +}; \ No newline at end of file diff --git a/src/perception/aquisition/zed_visualise/README.md b/src/perception/aquisition/zed_visualise/README.md new file mode 100755 index 00000000..d5cd6a83 --- /dev/null +++ b/src/perception/aquisition/zed_visualise/README.md @@ -0,0 +1,38 @@ +# ZED SDK - Object Detection + +This sample shows how to detect custom objects using the official Pytorch implementation of YOLOv8 from a ZED camera and ingest them into the ZED SDK to extract 3D informations and tracking for each objects. + +## Getting Started + + - Get the latest [ZED SDK](https://www.stereolabs.com/developers/release/) and [pyZED Package](https://www.stereolabs.com/docs/app-development/python/install/) + - Check the [Documentation](https://www.stereolabs.com/docs/object-detection/custom-od/) + +## Setting up + + - Install yolov8 using pip + +```sh +pip install ultralytics +``` + +## Run the program + +*NOTE: The ZED v1 is not compatible with this module* + +``` +python detector.py --weights yolov8m.pt # [--img_size 512 --conf_thres 0.1 --svo path/to/file.svo] +``` + +### Features + + - The camera point cloud is displayed in a 3D OpenGL view + - 3D bounding boxes around detected objects are drawn + - Objects classes and confidences can be changed + +## Training your own model + +This sample can use any model trained with YOLOv8, including custom trained one. For a getting started on how to trained a model on a custom dataset with YOLOv5, see here https://docs.ultralytics.com/tutorials/train-custom-datasets/ + +## Support + +If you need assistance go to our Community site at https://community.stereolabs.com/ \ No newline at end of file diff --git a/src/perception/aquisition/zed_visualise/cone-1.jpg b/src/perception/aquisition/zed_visualise/cone-1.jpg new file mode 100644 index 00000000..547322bf Binary files /dev/null and b/src/perception/aquisition/zed_visualise/cone-1.jpg differ diff --git a/src/perception/aquisition/zed_visualise/cone-2.jpg b/src/perception/aquisition/zed_visualise/cone-2.jpg new file mode 100644 index 00000000..e0c182e9 Binary files /dev/null and b/src/perception/aquisition/zed_visualise/cone-2.jpg differ diff --git a/src/perception/aquisition/zed_visualise/cone-3.jpg b/src/perception/aquisition/zed_visualise/cone-3.jpg new file mode 100644 index 00000000..244445d2 Binary files /dev/null and b/src/perception/aquisition/zed_visualise/cone-3.jpg differ diff --git a/src/perception/aquisition/zed_visualise/cones.png b/src/perception/aquisition/zed_visualise/cones.png new file mode 100644 index 00000000..8d320ef5 Binary files /dev/null and b/src/perception/aquisition/zed_visualise/cones.png differ diff --git a/src/perception/aquisition/zed_visualise/cv_viewer/tracking_viewer.py b/src/perception/aquisition/zed_visualise/cv_viewer/tracking_viewer.py new file mode 100755 index 00000000..cfc4d4bd --- /dev/null +++ b/src/perception/aquisition/zed_visualise/cv_viewer/tracking_viewer.py @@ -0,0 +1,247 @@ +import cv2 +import numpy as np + +from cv_viewer.utils import * +import pyzed.sl as sl +import math +from collections import deque + + +# ---------------------------------------------------------------------- +# 2D LEFT VIEW +# ---------------------------------------------------------------------- + + +def cvt(pt, scale): + """ + Function that scales point coordinates + """ + out = [pt[0] * scale[0], pt[1] * scale[1]] + return out + + +def get_image_position(bounding_box_image, img_scale): + out_position = np.zeros(2) + out_position[0] = (bounding_box_image[0][0] + (bounding_box_image[2][0] - bounding_box_image[0][0]) * 0.5) * \ + img_scale[0] + out_position[1] = (bounding_box_image[0][1] + (bounding_box_image[2][1] - bounding_box_image[0][1]) * 0.5) * \ + img_scale[1] + return out_position + + +def render_2D(left_display, img_scale, objects, is_tracking_on): + overlay = left_display.copy() + + line_thickness = 2 + for obj in objects.object_list: + if render_object(obj, is_tracking_on): + base_color = generate_color_id_u(obj.id) + # Display image scaled 2D bounding box + top_left_corner = cvt(obj.bounding_box_2d[0], img_scale) + top_right_corner = cvt(obj.bounding_box_2d[1], img_scale) + bottom_right_corner = cvt(obj.bounding_box_2d[2], img_scale) + bottom_left_corner = cvt(obj.bounding_box_2d[3], img_scale) + + # Creation of the 2 horizontal lines + cv2.line(left_display, (int(top_left_corner[0]), int(top_left_corner[1])), + (int(top_right_corner[0]), int(top_right_corner[1])), base_color, line_thickness) + cv2.line(left_display, (int(bottom_left_corner[0]), int(bottom_left_corner[1])), + (int(bottom_right_corner[0]), int(bottom_right_corner[1])), base_color, line_thickness) + # Creation of 2 vertical lines + draw_vertical_line(left_display, bottom_left_corner, top_left_corner, base_color, line_thickness) + draw_vertical_line(left_display, bottom_right_corner, top_right_corner, base_color, line_thickness) + + # Scaled ROI + roi_height = int(top_right_corner[0] - top_left_corner[0]) + roi_width = int(bottom_left_corner[1] - top_left_corner[1]) + overlay_roi = overlay[int(top_left_corner[1]):int(top_left_corner[1] + roi_width) + , int(top_left_corner[0]):int(top_left_corner[0] + roi_height)] + + overlay_roi[:, :, :] = base_color + + # Display Object label as text + position_image = get_image_position(obj.bounding_box_2d, img_scale) + text_position = (int(position_image[0] - 20), int(position_image[1] - 12)) + text = "class " + str(obj.raw_label) + text_color = (255, 255, 255, 255) + cv2.putText(left_display, text, text_position, cv2.FONT_HERSHEY_COMPLEX_SMALL, 0.5, text_color, 1) + + # Diplay Object distance to camera as text + if np.isfinite(obj.position[2]): + text = str(round(abs(obj.position[2]), 1)) + "M" + text_position = (int(position_image[0] - 20), int(position_image[1])) + cv2.putText(left_display, text, text_position, cv2.FONT_HERSHEY_COMPLEX_SMALL, 0.5, text_color, 1) + + # Here, overlay is as the left image, but with opaque masks on each detected objects + cv2.addWeighted(left_display, 0.7, overlay, 0.3, 0.0, left_display) + + +# ---------------------------------------------------------------------- +# 2D TRACKING VIEW +# ---------------------------------------------------------------------- + +class TrackingViewer: + def __init__(self, res, fps, D_max): + # Window size + self.window_width = res.width + self.window_height = res.height + + # Visualisation settings + self.has_background_ready = False + self.background = np.full((self.window_height, self.window_width, 4), [245, 239, 239, 255], np.uint8) + + # Invert Z due to Y axis of ocv window + # Show objects between [z_min, 0] (z_min < 0) + self.z_min = -D_max + # Show objects between [x_min, x_max] + self.x_min = self.z_min + self.x_max = -self.x_min + + # Conversion from world position to pixel coordinates + self.x_step = (self.x_max - self.x_min) / self.window_width + self.z_step = abs(self.z_min) / self.window_height + + self.camera_calibration = sl.CalibrationParameters() + + # List of alive tracks + self.tracklets = [] + + def set_camera_calibration(self, calib): + self.camera_calibration = calib + self.has_background_ready = False + + def generate_view(self, objects, current_camera_pose, tracking_view, tracking_enabled): + # To get position in WORLD reference + for obj in objects.object_list: + pos = obj.position + tmp_pos = sl.Translation() + tmp_pos.init_vector(pos[0], pos[1], pos[2]) + new_pos = ( + tmp_pos * current_camera_pose.get_orientation()).get() + current_camera_pose.get_translation().get() + obj.position = np.array([new_pos[0], new_pos[1], new_pos[2]]) + + # Initialize visualisation + if not self.has_background_ready: + self.generate_background() + + np.copyto(tracking_view, self.background, 'no') + + if tracking_enabled: + # First add new points and remove the ones that are too old + current_timestamp = objects.timestamp.get_seconds() + self.add_to_tracklets(objects, current_timestamp) + self.prune_old_points(current_timestamp) + + # Draw all tracklets + self.draw_tracklets(tracking_view, current_camera_pose) + else: + self.draw_points(objects.object_list, tracking_view, current_camera_pose) + + def add_to_tracklets(self, objects, current_timestamp): + for obj in objects.object_list: + if (obj.tracking_state != sl.OBJECT_TRACKING_STATE.OK) or (not np.isfinite(obj.position[0])) or ( + obj.id < 0): + continue + + new_object = True + for i in range(len(self.tracklets)): + if self.tracklets[i].id == obj.id: + new_object = False + self.tracklets[i].add_point(obj, current_timestamp) + + # In case this object does not belong to existing tracks + if (new_object): + self.tracklets.append(Tracklet(obj, obj.label, current_timestamp)) + + def prune_old_points(self, ts): + track_to_delete = [] + for it in self.tracklets: + if ((ts - it.last_timestamp) > (3)): + track_to_delete.append(it) + + for it in track_to_delete: + self.tracklets.remove(it) + + # ---------------------------------------------------------------------- + # Drawing functions + # ---------------------------------------------------------------------- + + def draw_points(self, objects, tracking_view, current_camera_pose): + for obj in objects: + if (not np.isfinite(obj.position[0])): + continue + clr = generate_color_id_u(obj.id) + pt = TrackPoint(obj.position) + cv_start_point = self.to_cv_point(pt.get_xyz(), current_camera_pose) + cv2.circle(tracking_view, (int(cv_start_point[0]), int(cv_start_point[1])), 6, clr, 2) + + def draw_tracklets(self, tracking_view, current_camera_pose): + for track in self.tracklets: + clr = generate_color_id_u(track.id) + cv_start_point = self.to_cv_point(track.positions[0].get_xyz(), current_camera_pose) + for point_index in range(1, len(track.positions)): + cv_end_point = self.to_cv_point(track.positions[point_index].get_xyz(), current_camera_pose) + cv2.line(tracking_view, (int(cv_start_point[0]), int(cv_start_point[1])), + (int(cv_end_point[0]), int(cv_end_point[1])), clr, 3) + cv_start_point = cv_end_point + cv2.circle(tracking_view, (int(cv_start_point[0]), int(cv_start_point[1])), 6, clr, -1) + + def generate_background(self): + camera_color = [255, 230, 204, 255] + + # Get FOV intersection with window borders + fov = 2.0 * math.atan( + self.camera_calibration.left_cam.image_size.width / (2.0 * self.camera_calibration.left_cam.fx)) + + z_at_x_max = self.x_max / math.tan(fov / 2.0) + left_intersection_pt = self.to_cv_point(self.x_min, -z_at_x_max) + right_intersection_pt = self.to_cv_point(self.x_max, -z_at_x_max) + + # Drawing camera + camera_pts = np.array([left_intersection_pt + , right_intersection_pt + , [int(self.window_width / 2), self.window_height]] + , dtype=np.int32) + cv2.fillConvexPoly(self.background, camera_pts, camera_color) + + def to_cv_point(self, x, z): + out = [] + if isinstance(x, float) and isinstance(z, float): + out = [int((x - self.x_min) / self.x_step), int((z - self.z_min) / self.z_step)] + elif isinstance(x, list) and isinstance(z, sl.Pose): + # Go to camera current pose + rotation = z.get_rotation_matrix() + rotation.inverse() + tmp = x - (z.get_translation() * rotation.get_orientation()).get() + new_position = sl.Translation() + new_position.init_vector(tmp[0], tmp[1], tmp[2]) + out = [int(((new_position.get()[0] - self.x_min) / self.x_step) + 0.5), + int(((new_position.get()[2] - self.z_min) / self.z_step) + 0.5)] + elif isinstance(x, TrackPoint) and isinstance(z, sl.Pose): + pos = x.get_xyz() + out = self.to_cv_point(pos, z) + else: + print("Unhandled argument type") + return out + + +class TrackPoint: + def __init__(self, pos_): + self.x = pos_[0] + self.y = pos_[1] + self.z = pos_[2] + + def get_xyz(self): + return [self.x, self.y, self.z] + + +class Tracklet: + def __init__(self, obj_, type_, timestamp_): + self.id = obj_.id + self.object_type = type_ + self.positions = deque() + self.add_point(obj_, timestamp_) + + def add_point(self, obj_, timestamp_): + self.positions.append(TrackPoint(obj_.position)) + self.last_timestamp = timestamp_ diff --git a/src/perception/aquisition/zed_visualise/cv_viewer/utils.py b/src/perception/aquisition/zed_visualise/cv_viewer/utils.py new file mode 100755 index 00000000..825bdbbd --- /dev/null +++ b/src/perception/aquisition/zed_visualise/cv_viewer/utils.py @@ -0,0 +1,38 @@ +import cv2 +import numpy as np +import pyzed.sl as sl + +id_colors = [(232, 176, 59), + (175, 208, 25), + (102, 205, 105), + (185, 0, 255), + (99, 107, 252)] + + +def render_object(object_data, is_tracking_on): + if is_tracking_on: + return object_data.tracking_state == sl.OBJECT_TRACKING_STATE.OK + else: + return (object_data.tracking_state == sl.OBJECT_TRACKING_STATE.OK) or ( + object_data.tracking_state == sl.OBJECT_TRACKING_STATE.OFF) + + +def generate_color_id_u(idx): + arr = [] + if idx < 0: + arr = [236, 184, 36, 255] + else: + color_idx = idx % 5 + arr = [id_colors[color_idx][0], id_colors[color_idx][1], id_colors[color_idx][2], 255] + return arr + + +def draw_vertical_line(left_display, start_pt, end_pt, clr, thickness): + n_steps = 7 + pt1 = [((n_steps - 1) * start_pt[0] + end_pt[0]) / n_steps + , ((n_steps - 1) * start_pt[1] + end_pt[1]) / n_steps] + pt4 = [(start_pt[0] + (n_steps - 1) * end_pt[0]) / n_steps + , (start_pt[1] + (n_steps - 1) * end_pt[1]) / n_steps] + + cv2.line(left_display, (int(start_pt[0]), int(start_pt[1])), (int(pt1[0]), int(pt1[1])), clr, thickness) + cv2.line(left_display, (int(pt4[0]), int(pt4[1])), (int(end_pt[0]), int(end_pt[1])), clr, thickness) diff --git a/src/perception/aquisition/zed_visualise/detector.py b/src/perception/aquisition/zed_visualise/detector.py new file mode 100755 index 00000000..f1c3dc44 --- /dev/null +++ b/src/perception/aquisition/zed_visualise/detector.py @@ -0,0 +1,213 @@ +#!/usr/bin/env python3 + +import sys +import numpy as np + +import argparse +import torch +import cv2 +import pyzed.sl as sl +from ultralytics import YOLO + +from threading import Lock, Thread +from time import sleep + +import ogl_viewer.viewer as gl +import cv_viewer.tracking_viewer as cv_viewer + +lock = Lock() +run_signal = False +exit_signal = False + + +def xywh2abcd(xywh, im_shape): + output = np.zeros((4, 2)) + + # Center / Width / Height -> BBox corners coordinates + x_min = (xywh[0] - 0.5*xywh[2]) #* im_shape[1] + x_max = (xywh[0] + 0.5*xywh[2]) #* im_shape[1] + y_min = (xywh[1] - 0.5*xywh[3]) #* im_shape[0] + y_max = (xywh[1] + 0.5*xywh[3]) #* im_shape[0] + + # A ------ B + # | Object | + # D ------ C + + output[0][0] = x_min + output[0][1] = y_min + + output[1][0] = x_max + output[1][1] = y_min + + output[2][0] = x_max + output[2][1] = y_max + + output[3][0] = x_min + output[3][1] = y_max + return output + +def detections_to_custom_box(detections, im0): + output = [] + for i, det in enumerate(detections): + xywh = det.xywh[0] + + # Creating ingestable objects for the ZED SDK + obj = sl.CustomBoxObjectData() + obj.bounding_box_2d = xywh2abcd(xywh, im0.shape) + obj.label = det.cls + obj.probability = det.conf + obj.is_grounded = False + output.append(obj) + return output + + +def torch_thread(weights, img_size, conf_thres=0.2, iou_thres=0.45): + global image_net, exit_signal, run_signal, detections + + print("Intializing Network...") + + model = YOLO(weights) + + while not exit_signal: + if run_signal: + lock.acquire() + + img = cv2.cvtColor(image_net, cv2.COLOR_BGRA2BGR) + # https://docs.ultralytics.com/modes/predict/#video-suffixes + det = model.predict(img, save=False, imgsz=img_size, conf=conf_thres, iou=iou_thres)[0].cpu().numpy().boxes + + # ZED CustomBox format (with inverse letterboxing tf applied) + detections = detections_to_custom_box(det, image_net) + lock.release() + run_signal = False + sleep(0.01) + + +def main(): + global image_net, exit_signal, run_signal, detections + + capture_thread = Thread(target=torch_thread, kwargs={'weights': opt.weights, 'img_size': opt.img_size, "conf_thres": opt.conf_thres}) + capture_thread.start() + + print("Initializing Camera...") + + zed = sl.Camera() + + input_type = sl.InputType() + if opt.svo is not None: + input_type.set_from_svo_file(opt.svo) + + # Create a InitParameters object and set configuration parameters + init_params = sl.InitParameters(input_t=input_type, svo_real_time_mode=True) + init_params.coordinate_units = sl.UNIT.METER + init_params.depth_mode = sl.DEPTH_MODE.ULTRA # QUALITY + init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP + init_params.depth_maximum_distance = 50 + + runtime_params = sl.RuntimeParameters() + status = zed.open(init_params) + + if status != sl.ERROR_CODE.SUCCESS: + print(repr(status)) + exit() + + image_left_tmp = sl.Mat() + + print("Initialized Camera") + + positional_tracking_parameters = sl.PositionalTrackingParameters() + # If the camera is static, uncomment the following line to have better performances and boxes sticked to the ground. + # positional_tracking_parameters.set_as_static = True + zed.enable_positional_tracking(positional_tracking_parameters) + + obj_param = sl.ObjectDetectionParameters() + obj_param.detection_model = sl.OBJECT_DETECTION_MODEL.CUSTOM_BOX_OBJECTS + obj_param.enable_tracking = True + obj_param.enable_segmentation = False # designed to give person pixel mask with internal OD + zed.enable_object_detection(obj_param) + + objects = sl.Objects() + obj_runtime_param = sl.ObjectDetectionRuntimeParameters() + + # Display + camera_infos = zed.get_camera_information() + camera_res = camera_infos.camera_configuration.resolution + # Create OpenGL viewer + viewer = gl.GLViewer() + point_cloud_res = sl.Resolution(min(camera_res.width, 720), min(camera_res.height, 404)) + point_cloud_render = sl.Mat() + viewer.init(camera_infos.camera_model, point_cloud_res, obj_param.enable_tracking) + point_cloud = sl.Mat(point_cloud_res.width, point_cloud_res.height, sl.MAT_TYPE.F32_C4, sl.MEM.CPU) + image_left = sl.Mat() + # Utilities for 2D display + display_resolution = sl.Resolution(min(camera_res.width, 1280), min(camera_res.height, 720)) + image_scale = [display_resolution.width / camera_res.width, display_resolution.height / camera_res.height] + image_left_ocv = np.full((display_resolution.height, display_resolution.width, 4), [245, 239, 239, 255], np.uint8) + + # Utilities for tracks view + camera_config = camera_infos.camera_configuration + tracks_resolution = sl.Resolution(400, display_resolution.height) + track_view_generator = cv_viewer.TrackingViewer(tracks_resolution, camera_config.fps, init_params.depth_maximum_distance) + track_view_generator.set_camera_calibration(camera_config.calibration_parameters) + image_track_ocv = np.zeros((tracks_resolution.height, tracks_resolution.width, 4), np.uint8) + # Camera pose + cam_w_pose = sl.Pose() + + while viewer.is_available() and not exit_signal: + if zed.grab(runtime_params) == sl.ERROR_CODE.SUCCESS: + # -- Get the image + lock.acquire() + zed.retrieve_image(image_left_tmp, sl.VIEW.LEFT) + image_net = image_left_tmp.get_data() + lock.release() + run_signal = True + + # -- Detection running on the other thread + while run_signal: + sleep(0.001) + + # Wait for detections + lock.acquire() + # -- Ingest detections + zed.ingest_custom_box_objects(detections) + lock.release() + zed.retrieve_objects(objects, obj_runtime_param) + + # -- Display + # Retrieve display data + zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA, sl.MEM.CPU, point_cloud_res) + point_cloud.copy_to(point_cloud_render) + zed.retrieve_image(image_left, sl.VIEW.LEFT, sl.MEM.CPU, display_resolution) + zed.get_position(cam_w_pose, sl.REFERENCE_FRAME.WORLD) + + # 3D rendering + viewer.updateData(point_cloud_render, objects) + # 2D rendering + np.copyto(image_left_ocv, image_left.get_data()) + cv_viewer.render_2D(image_left_ocv, image_scale, objects, obj_param.enable_tracking) + global_image = cv2.hconcat([image_left_ocv, image_track_ocv]) + # Tracking view + track_view_generator.generate_view(objects, cam_w_pose, image_track_ocv, objects.is_tracked) + + cv2.imshow("ZED | 2D View and Birds View", global_image) + key = cv2.waitKey(10) + if key == 27 or key == ord('q') or key == ord('Q'): + exit_signal = True + else: + exit_signal = True + + viewer.exit() + exit_signal = True + zed.close() + + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('--weights', type=str, default='yolov8m.pt', help='model.pt path(s)') + parser.add_argument('--svo', type=str, default=None, help='optional svo file, if not passed, use the plugged camera instead') + parser.add_argument('--img_size', type=int, default=416, help='inference size (pixels)') + parser.add_argument('--conf_thres', type=float, default=0.4, help='object confidence threshold') + opt = parser.parse_args() + + with torch.no_grad(): + main() \ No newline at end of file diff --git a/src/perception/aquisition/zed_visualise/example.ipynb b/src/perception/aquisition/zed_visualise/example.ipynb new file mode 100644 index 00000000..25518585 --- /dev/null +++ b/src/perception/aquisition/zed_visualise/example.ipynb @@ -0,0 +1,65 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "image 1/1 /home/yiyang/FSAE/autonomous/src/perception/cone-detection/cone_detection/cone-detection-roboflow/Zed_example/cone-3.jpg: 480x640 2 Yellow_Cones, 678.4ms\n", + "Speed: 3.0ms preprocess, 678.4ms inference, 0.9ms postprocess per image at shape (1, 3, 480, 640)\n" + ] + }, + { + "ename": "AttributeError", + "evalue": "'Results' object has no attribute 'cls'. See valid attributes below.\n\n A class for storing and manipulating inference results.\n\n Attributes:\n orig_img (numpy.ndarray): Original image as a numpy array.\n orig_shape (tuple): Original image shape in (height, width) format.\n boxes (Boxes, optional): Object containing detection bounding boxes.\n masks (Masks, optional): Object containing detection masks.\n probs (Probs, optional): Object containing class probabilities for classification tasks.\n keypoints (Keypoints, optional): Object containing detected keypoints for each object.\n speed (dict): Dictionary of preprocess, inference, and postprocess speeds (ms/image).\n names (dict): Dictionary of class names.\n path (str): Path to the image file.\n\n Methods:\n update(boxes=None, masks=None, probs=None, obb=None): Updates object attributes with new detection results.\n cpu(): Returns a copy of the Results object with all tensors on CPU memory.\n numpy(): Returns a copy of the Results object with all tensors as numpy arrays.\n cuda(): Returns a copy of the Results object with all tensors on GPU memory.\n to(*args, **kwargs): Returns a copy of the Results object with tensors on a specified device and dtype.\n new(): Returns a new Results object with the same image, path, and names.\n plot(...): Plots detection results on an input image, returning an annotated image.\n show(): Show annotated results to screen.\n save(filename): Save annotated results to file.\n verbose(): Returns a log string for each task, detailing detections and classifications.\n save_txt(txt_file, save_conf=False): Saves detection results to a text file.\n save_crop(save_dir, file_name=Path(\"im.jpg\")): Saves cropped detection images.\n tojson(normalize=False): Converts detection results to JSON format.\n ", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mAttributeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[6], line 9\u001b[0m\n\u001b[1;32m 7\u001b[0m result \u001b[39m=\u001b[39m model(\u001b[39m\"\u001b[39m\u001b[39mcone-3.jpg\u001b[39m\u001b[39m\"\u001b[39m)\n\u001b[1;32m 8\u001b[0m \u001b[39mfor\u001b[39;00m r \u001b[39min\u001b[39;00m result: \n\u001b[0;32m----> 9\u001b[0m \u001b[39mprint\u001b[39m(r\u001b[39m.\u001b[39;49mcls) \u001b[39m# print the Boxes object containing the detection bounding boxes\u001b[39;00m\n", + "File \u001b[0;32m~/.local/lib/python3.10/site-packages/ultralytics/utils/__init__.py:162\u001b[0m, in \u001b[0;36mSimpleClass.__getattr__\u001b[0;34m(self, attr)\u001b[0m\n\u001b[1;32m 160\u001b[0m \u001b[39m\u001b[39m\u001b[39m\"\"\"Custom attribute access error message with helpful information.\"\"\"\u001b[39;00m\n\u001b[1;32m 161\u001b[0m name \u001b[39m=\u001b[39m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39m\u001b[39m__class__\u001b[39m\u001b[39m.\u001b[39m\u001b[39m__name__\u001b[39m\n\u001b[0;32m--> 162\u001b[0m \u001b[39mraise\u001b[39;00m \u001b[39mAttributeError\u001b[39;00m(\u001b[39mf\u001b[39m\u001b[39m\"\u001b[39m\u001b[39m'\u001b[39m\u001b[39m{\u001b[39;00mname\u001b[39m}\u001b[39;00m\u001b[39m'\u001b[39m\u001b[39m object has no attribute \u001b[39m\u001b[39m'\u001b[39m\u001b[39m{\u001b[39;00mattr\u001b[39m}\u001b[39;00m\u001b[39m'\u001b[39m\u001b[39m. See valid attributes below.\u001b[39m\u001b[39m\\n\u001b[39;00m\u001b[39m{\u001b[39;00m\u001b[39mself\u001b[39m\u001b[39m.\u001b[39m\u001b[39m__doc__\u001b[39m\u001b[39m}\u001b[39;00m\u001b[39m\"\u001b[39m)\n", + "\u001b[0;31mAttributeError\u001b[0m: 'Results' object has no attribute 'cls'. See valid attributes below.\n\n A class for storing and manipulating inference results.\n\n Attributes:\n orig_img (numpy.ndarray): Original image as a numpy array.\n orig_shape (tuple): Original image shape in (height, width) format.\n boxes (Boxes, optional): Object containing detection bounding boxes.\n masks (Masks, optional): Object containing detection masks.\n probs (Probs, optional): Object containing class probabilities for classification tasks.\n keypoints (Keypoints, optional): Object containing detected keypoints for each object.\n speed (dict): Dictionary of preprocess, inference, and postprocess speeds (ms/image).\n names (dict): Dictionary of class names.\n path (str): Path to the image file.\n\n Methods:\n update(boxes=None, masks=None, probs=None, obb=None): Updates object attributes with new detection results.\n cpu(): Returns a copy of the Results object with all tensors on CPU memory.\n numpy(): Returns a copy of the Results object with all tensors as numpy arrays.\n cuda(): Returns a copy of the Results object with all tensors on GPU memory.\n to(*args, **kwargs): Returns a copy of the Results object with tensors on a specified device and dtype.\n new(): Returns a new Results object with the same image, path, and names.\n plot(...): Plots detection results on an input image, returning an annotated image.\n show(): Show annotated results to screen.\n save(filename): Save annotated results to file.\n verbose(): Returns a log string for each task, detailing detections and classifications.\n save_txt(txt_file, save_conf=False): Saves detection results to a text file.\n save_crop(save_dir, file_name=Path(\"im.jpg\")): Saves cropped detection images.\n tojson(normalize=False): Converts detection results to JSON format.\n " + ] + } + ], + "source": [ + "from ultralytics import YOLO\n", + "\n", + "# Load a model\n", + "model = YOLO(\"cone_detection_model.pt\") # pretrained YOLOv8n model\n", + "\n", + "# Run batched inference on a list of images\n", + "result = model.predict(\"cone-3.jpg\", save=False, imgsz=img_size, conf=conf_thres, iou=iou_thres)[0].cpu().numpy().boxes\n", + "for r in result: \n", + " print(r.cls) # print the Boxes object containing the detection bounding boxes\n" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.10.12" + }, + "orig_nbformat": 4 + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/src/perception/aquisition/zed_visualise/ogl_viewer/viewer.py b/src/perception/aquisition/zed_visualise/ogl_viewer/viewer.py new file mode 100755 index 00000000..102a4d15 --- /dev/null +++ b/src/perception/aquisition/zed_visualise/ogl_viewer/viewer.py @@ -0,0 +1,769 @@ +from OpenGL.GL import * +from OpenGL.GLUT import * +from OpenGL.GLU import * + +import ctypes +import sys +import math +from threading import Lock +import numpy as np +import array +from enum import IntEnum + +from cv_viewer.utils import * +import ogl_viewer.zed_model as zm +import pyzed.sl as sl + +VERTEX_SHADER = """ +# version 330 core +layout(location = 0) in vec3 in_Vertex; +layout(location = 1) in vec4 in_Color; +uniform mat4 u_mvpMatrix; +out vec4 b_color; +void main() { + b_color = in_Color; + gl_Position = u_mvpMatrix * vec4(in_Vertex, 1); +} +""" + +FRAGMENT_SHADER = """ +# version 330 core +in vec4 b_color; +layout(location = 0) out vec4 out_Color; +void main() { + out_Color = b_color; +} +""" + +POINTCLOUD_VERTEX_SHADER = """ +#version 330 core +layout(location = 0) in vec4 in_VertexRGBA; +uniform mat4 u_mvpMatrix; +out vec4 b_color; +void main() { + uint vertexColor = floatBitsToUint(in_VertexRGBA.w); + vec3 clr_int = vec3((vertexColor & uint(0x000000FF)), (vertexColor & uint(0x0000FF00)) >> 8, (vertexColor & uint(0x00FF0000)) >> 16); + b_color = vec4(clr_int.r / 255.0f, clr_int.g / 255.0f, clr_int.b / 255.0f, 1.f); + gl_Position = u_mvpMatrix * vec4(in_VertexRGBA.xyz, 1); +} +""" + +POINTCLOUD_FRAGMENT_SHADER = """ +#version 330 core +in vec4 b_color; +layout(location = 0) out vec4 out_Color; +void main() { + out_Color = b_color; +} +""" + +M_PI = 3.1415926 + +GRID_SIZE = 9.0 + + +def generate_color_id(_idx): + clr = np.divide(generate_color_id_u(_idx), 255.0) + clr[0], clr[2] = clr[2], clr[0] + return clr + + +class Shader: + def __init__(self, _vs, _fs): + + self.program_id = glCreateProgram() + vertex_id = self.compile(GL_VERTEX_SHADER, _vs) + fragment_id = self.compile(GL_FRAGMENT_SHADER, _fs) + + glAttachShader(self.program_id, vertex_id) + glAttachShader(self.program_id, fragment_id) + glBindAttribLocation(self.program_id, 0, "in_vertex") + glBindAttribLocation(self.program_id, 1, "in_texCoord") + glLinkProgram(self.program_id) + + if glGetProgramiv(self.program_id, GL_LINK_STATUS) != GL_TRUE: + info = glGetProgramInfoLog(self.program_id) + glDeleteProgram(self.program_id) + glDeleteShader(vertex_id) + glDeleteShader(fragment_id) + raise RuntimeError('Error linking program: %s' % (info)) + glDeleteShader(vertex_id) + glDeleteShader(fragment_id) + + def compile(self, _type, _src): + try: + shader_id = glCreateShader(_type) + if shader_id == 0: + print("ERROR: shader type {0} does not exist".format(_type)) + exit() + + glShaderSource(shader_id, _src) + glCompileShader(shader_id) + if glGetShaderiv(shader_id, GL_COMPILE_STATUS) != GL_TRUE: + info = glGetShaderInfoLog(shader_id) + glDeleteShader(shader_id) + raise RuntimeError('Shader compilation failed: %s' % (info)) + return shader_id + except: + glDeleteShader(shader_id) + raise + + def get_program_id(self): + return self.program_id + + +class Simple3DObject: + def __init__(self, _is_static, pts_size=3, clr_size=3): + self.is_init = False + self.drawing_type = GL_TRIANGLES + self.is_static = _is_static + self.clear() + self.pt_type = pts_size + self.clr_type = clr_size + + def add_pt(self, _pts): # _pts [x,y,z] + for pt in _pts: + self.vertices.append(pt) + + def add_clr(self, _clrs): # _clr [r,g,b] + for clr in _clrs: + self.colors.append(clr) + + def add_point_clr(self, _pt, _clr): + self.add_pt(_pt) + self.add_clr(_clr) + self.indices.append(len(self.indices)) + + def add_line(self, _p1, _p2, _clr): + self.add_point_clr(_p1, _clr) + self.add_point_clr(_p2, _clr) + + def addFace(self, p1, p2, p3, clr): + self.add_point_clr(p1, clr) + self.add_point_clr(p2, clr) + self.add_point_clr(p3, clr) + + def add_full_edges(self, _pts, _clr): + start_id = int(len(self.vertices) / 3) + + for i in range(len(_pts)): + self.add_pt(_pts[i]) + self.add_clr(_clr) + + box_links_top = np.array([0, 1, 1, 2, 2, 3, 3, 0]) + i = 0 + while i < box_links_top.size: + self.indices.append(start_id + box_links_top[i]) + self.indices.append(start_id + box_links_top[i + 1]) + i = i + 2 + + box_links_bottom = np.array([4, 5, 5, 6, 6, 7, 7, 4]) + i = 0 + while i < box_links_bottom.size: + self.indices.append(start_id + box_links_bottom[i]) + self.indices.append(start_id + box_links_bottom[i + 1]) + i = i + 2 + + def __add_single_vertical_line(self, _top_pt, _bottom_pt, _clr): + current_pts = np.array( + [_top_pt, + ((GRID_SIZE - 1) * np.array(_top_pt) + np.array(_bottom_pt)) / GRID_SIZE, + ((GRID_SIZE - 2) * np.array(_top_pt) + np.array(_bottom_pt) * 2) / GRID_SIZE, + (2 * np.array(_top_pt) + np.array(_bottom_pt) * (GRID_SIZE - 2)) / GRID_SIZE, + (np.array(_top_pt) + np.array(_bottom_pt) * (GRID_SIZE - 1)) / GRID_SIZE, + _bottom_pt + ], np.float32) + start_id = int(len(self.vertices) / 3) + for i in range(len(current_pts)): + self.add_pt(current_pts[i]) + if (i == 2 or i == 3): + _clr[3] = 0 + else: + _clr[3] = 0.75 + self.add_clr(_clr) + + box_links = np.array([0, 1, 1, 2, 2, 3, 3, 4, 4, 5]) + i = 0 + while i < box_links.size: + self.indices.append(start_id + box_links[i]) + self.indices.append(start_id + box_links[i + 1]) + i = i + 2 + + def add_vertical_edges(self, _pts, _clr): + self.__add_single_vertical_line(_pts[0], _pts[4], _clr) + self.__add_single_vertical_line(_pts[1], _pts[5], _clr) + self.__add_single_vertical_line(_pts[2], _pts[6], _clr) + self.__add_single_vertical_line(_pts[3], _pts[7], _clr) + + def add_top_face(self, _pts, _clr): + _clr[3] = 0.5 + for pt in _pts: + self.add_point_clr(pt, _clr) + + def __add_quad(self, _quad_pts, _alpha1, _alpha2, _clr): + for i in range(len(_quad_pts)): + self.add_pt(_quad_pts[i]) + if i < 2: + _clr[3] = _alpha1 + else: + _clr[3] = _alpha2 + self.add_clr(_clr) + + self.indices.append(len(self.indices)) + self.indices.append(len(self.indices)) + self.indices.append(len(self.indices)) + self.indices.append(len(self.indices)) + + def add_vertical_faces(self, _pts, _clr): + # For each face, we need to add 4 quads (the first 2 indexes are always the top points of the quad) + quads = [[0, 3, 7, 4] # Front face + , [3, 2, 6, 7] # Right face + , [2, 1, 5, 6] # Back face + , [1, 0, 4, 5]] # Left face + + alpha = .5 + + # Create gradually fading quads + for quad in quads: + quad_pts_1 = [ + _pts[quad[0]], + _pts[quad[1]], + ((GRID_SIZE - 0.5) * np.array(_pts[quad[1]]) + 0.5 * np.array(_pts[quad[2]])) / GRID_SIZE, + ((GRID_SIZE - 0.5) * np.array(_pts[quad[0]]) + 0.5 * np.array(_pts[quad[3]])) / GRID_SIZE + ] + self.__add_quad(quad_pts_1, alpha, alpha, _clr) + + quad_pts_2 = [ + ((GRID_SIZE - 0.5) * np.array(_pts[quad[0]]) + 0.5 * np.array(_pts[quad[3]])) / GRID_SIZE, + ((GRID_SIZE - 0.5) * np.array(_pts[quad[1]]) + 0.5 * np.array(_pts[quad[2]])) / GRID_SIZE, + ((GRID_SIZE - 1.0) * np.array(_pts[quad[1]]) + np.array(_pts[quad[2]])) / GRID_SIZE, + ((GRID_SIZE - 1.0) * np.array(_pts[quad[0]]) + np.array(_pts[quad[3]])) / GRID_SIZE + ] + self.__add_quad(quad_pts_2, alpha, 2 * alpha / 3, _clr) + + quad_pts_3 = [ + ((GRID_SIZE - 1.0) * np.array(_pts[quad[0]]) + np.array(_pts[quad[3]])) / GRID_SIZE, + ((GRID_SIZE - 1.0) * np.array(_pts[quad[1]]) + np.array(_pts[quad[2]])) / GRID_SIZE, + ((GRID_SIZE - 1.5) * np.array(_pts[quad[1]]) + 1.5 * np.array(_pts[quad[2]])) / GRID_SIZE, + ((GRID_SIZE - 1.5) * np.array(_pts[quad[0]]) + 1.5 * np.array(_pts[quad[3]])) / GRID_SIZE + ] + self.__add_quad(quad_pts_3, 2 * alpha / 3, alpha / 3, _clr) + + quad_pts_4 = [ + ((GRID_SIZE - 1.5) * np.array(_pts[quad[0]]) + 1.5 * np.array(_pts[quad[3]])) / GRID_SIZE, + ((GRID_SIZE - 1.5) * np.array(_pts[quad[1]]) + 1.5 * np.array(_pts[quad[2]])) / GRID_SIZE, + ((GRID_SIZE - 2.0) * np.array(_pts[quad[1]]) + 2.0 * np.array(_pts[quad[2]])) / GRID_SIZE, + ((GRID_SIZE - 2.0) * np.array(_pts[quad[0]]) + 2.0 * np.array(_pts[quad[3]])) / GRID_SIZE + ] + self.__add_quad(quad_pts_4, alpha / 3, 0.0, _clr) + + quad_pts_5 = [ + (np.array(_pts[quad[1]]) * 2.0 + (GRID_SIZE - 2.0) * np.array(_pts[quad[2]])) / GRID_SIZE, + (np.array(_pts[quad[0]]) * 2.0 + (GRID_SIZE - 2.0) * np.array(_pts[quad[3]])) / GRID_SIZE, + (np.array(_pts[quad[0]]) * 1.5 + (GRID_SIZE - 1.5) * np.array(_pts[quad[3]])) / GRID_SIZE, + (np.array(_pts[quad[1]]) * 1.5 + (GRID_SIZE - 1.5) * np.array(_pts[quad[2]])) / GRID_SIZE + ] + self.__add_quad(quad_pts_5, 0.0, alpha / 3, _clr) + + quad_pts_6 = [ + (np.array(_pts[quad[1]]) * 1.5 + (GRID_SIZE - 1.5) * np.array(_pts[quad[2]])) / GRID_SIZE, + (np.array(_pts[quad[0]]) * 1.5 + (GRID_SIZE - 1.5) * np.array(_pts[quad[3]])) / GRID_SIZE, + (np.array(_pts[quad[0]]) + (GRID_SIZE - 1.0) * np.array(_pts[quad[3]])) / GRID_SIZE, + (np.array(_pts[quad[1]]) + (GRID_SIZE - 1.0) * np.array(_pts[quad[2]])) / GRID_SIZE + ] + self.__add_quad(quad_pts_6, alpha / 3, 2 * alpha / 3, _clr) + + quad_pts_7 = [ + (np.array(_pts[quad[1]]) + (GRID_SIZE - 1.0) * np.array(_pts[quad[2]])) / GRID_SIZE, + (np.array(_pts[quad[0]]) + (GRID_SIZE - 1.0) * np.array(_pts[quad[3]])) / GRID_SIZE, + (np.array(_pts[quad[0]]) * 0.5 + (GRID_SIZE - 0.5) * np.array(_pts[quad[3]])) / GRID_SIZE, + (np.array(_pts[quad[1]]) * 0.5 + (GRID_SIZE - 0.5) * np.array(_pts[quad[2]])) / GRID_SIZE + ] + self.__add_quad(quad_pts_7, 2 * alpha / 3, alpha, _clr) + + quad_pts_8 = [ + (np.array(_pts[quad[0]]) * 0.5 + (GRID_SIZE - 0.5) * np.array(_pts[quad[3]])) / GRID_SIZE, + (np.array(_pts[quad[1]]) * 0.5 + (GRID_SIZE - 0.5) * np.array(_pts[quad[2]])) / GRID_SIZE, + np.array(_pts[quad[2]]), np.array(_pts[quad[3]]) + ] + self.__add_quad(quad_pts_8, alpha, alpha, _clr) + + def push_to_GPU(self): + if (self.is_init == False): + self.vboID = glGenBuffers(3) + self.is_init = True + + if (self.is_static): + type_draw = GL_STATIC_DRAW + else: + type_draw = GL_DYNAMIC_DRAW + + if len(self.vertices): + glBindBuffer(GL_ARRAY_BUFFER, self.vboID[0]) + glBufferData(GL_ARRAY_BUFFER, len(self.vertices) * self.vertices.itemsize, + (GLfloat * len(self.vertices))(*self.vertices), type_draw) + + if len(self.colors): + glBindBuffer(GL_ARRAY_BUFFER, self.vboID[1]) + glBufferData(GL_ARRAY_BUFFER, len(self.colors) * self.colors.itemsize, + (GLfloat * len(self.colors))(*self.colors), type_draw) + + if len(self.indices): + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, self.vboID[2]) + glBufferData(GL_ELEMENT_ARRAY_BUFFER, len(self.indices) * self.indices.itemsize, + (GLuint * len(self.indices))(*self.indices), type_draw) + + self.elementbufferSize = len(self.indices) + + def init(self, res): + if (self.is_init == False): + self.vboID = glGenBuffers(3) + self.is_init = True + + if (self.is_static): + type_draw = GL_STATIC_DRAW + else: + type_draw = GL_DYNAMIC_DRAW + + self.elementbufferSize = res.width * res.height + + glBindBuffer(GL_ARRAY_BUFFER, self.vboID[0]) + glBufferData(GL_ARRAY_BUFFER, self.elementbufferSize * self.pt_type * self.vertices.itemsize, None, type_draw) + + if (self.clr_type): + glBindBuffer(GL_ARRAY_BUFFER, self.vboID[1]) + glBufferData(GL_ARRAY_BUFFER, self.elementbufferSize * self.clr_type * self.colors.itemsize, None, + type_draw) + + for i in range(0, self.elementbufferSize): + self.indices.append(i + 1) + + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, self.vboID[2]) + glBufferData(GL_ELEMENT_ARRAY_BUFFER, len(self.indices) * self.indices.itemsize, + (GLuint * len(self.indices))(*self.indices), type_draw) + + def setPoints(self, pc): + glBindBuffer(GL_ARRAY_BUFFER, self.vboID[0]) + glBufferSubData(GL_ARRAY_BUFFER, 0, self.elementbufferSize * self.pt_type * self.vertices.itemsize, + ctypes.c_void_p(pc.get_pointer())) + glBindBuffer(GL_ARRAY_BUFFER, 0) + + def clear(self): + self.vertices = array.array('f') + self.colors = array.array('f') + self.indices = array.array('I') + self.elementbufferSize = 0 + + def set_drawing_type(self, _type): + self.drawing_type = _type + + def draw(self): + if (self.elementbufferSize): + glEnableVertexAttribArray(0) + glBindBuffer(GL_ARRAY_BUFFER, self.vboID[0]) + glVertexAttribPointer(0, self.pt_type, GL_FLOAT, GL_FALSE, 0, None) + + if (self.clr_type): + glEnableVertexAttribArray(1) + glBindBuffer(GL_ARRAY_BUFFER, self.vboID[1]) + glVertexAttribPointer(1, self.clr_type, GL_FLOAT, GL_FALSE, 0, None) + + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, self.vboID[2]) + glDrawElements(self.drawing_type, self.elementbufferSize, GL_UNSIGNED_INT, None) + + glDisableVertexAttribArray(0) + glDisableVertexAttribArray(1) + + +class GLViewer: + def __init__(self): + self.available = False + self.mutex = Lock() + self.camera = CameraGL() + self.wheelPosition = 0. + self.mouse_button = [False, False] + self.mouseCurrentPosition = [0., 0.] + self.previousMouseMotion = [0., 0.] + self.mouseMotion = [0., 0.] + self.zedModel = Simple3DObject(True) + self.BBox_faces = Simple3DObject(False, 3, 4) + self.BBox_edges = Simple3DObject(False, 3, 4) + self.point_cloud = Simple3DObject(False, 4) + self.is_tracking_on = False # Show tracked objects only + + def init(self, camera_model, res, is_tracking_on): + glutInit(sys.argv) + wnd_w = int(glutGet(GLUT_SCREEN_WIDTH) * 0.9) + wnd_h = int(glutGet(GLUT_SCREEN_HEIGHT) * 0.9) + glutInitWindowSize(wnd_w, wnd_h) + glutInitWindowPosition(int(wnd_w * 0.05), int(wnd_h * 0.05)) + + glutInitDisplayMode(GLUT_DOUBLE | GLUT_SRGB | GLUT_DEPTH) + glutCreateWindow("ZED Object Detection") + glViewport(0, 0, wnd_w, wnd_h) + + glutSetOption(GLUT_ACTION_ON_WINDOW_CLOSE, + GLUT_ACTION_CONTINUE_EXECUTION) + + glEnable(GL_DEPTH_TEST) + + glEnable(GL_BLEND) + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) + + glEnable(GL_LINE_SMOOTH) + glHint(GL_LINE_SMOOTH_HINT, GL_NICEST) + + self.is_tracking_on = is_tracking_on + + # Compile and create the shader for 3D objects + self.shader_image = Shader(VERTEX_SHADER, FRAGMENT_SHADER) + self.shader_image_MVP = glGetUniformLocation(self.shader_image.get_program_id(), "u_mvpMatrix") + + self.shader_pc = Shader(POINTCLOUD_VERTEX_SHADER, POINTCLOUD_FRAGMENT_SHADER) + self.shader_pc_MVP = glGetUniformLocation(self.shader_pc.get_program_id(), "u_mvpMatrix") + + self.bckgrnd_clr = np.array([223 / 255., 230 / 255., 233 / 255.]) + + if camera_model == sl.MODEL.ZED: + for i in range(0, zm.NB_ALLUMINIUM_TRIANGLES * 3, 3): + for j in range(3): + index = int(zm.alluminium_triangles[i + j] - 1) + self.zedModel.add_point_clr( + [zm.vertices[index * 3], zm.vertices[index * 3 + 1], zm.vertices[index * 3 + 2]], + [zm.ALLUMINIUM_COLOR.r, zm.ALLUMINIUM_COLOR.g, zm.ALLUMINIUM_COLOR.b]) + + for i in range(0, zm.NB_DARK_TRIANGLES * 3, 3): + for j in range(3): + index = int(zm.dark_triangles[i + j] - 1) + self.zedModel.add_point_clr( + [zm.vertices[index * 3], zm.vertices[index * 3 + 1], zm.vertices[index * 3 + 2]], + [zm.DARK_COLOR.r, zm.DARK_COLOR.g, zm.DARK_COLOR.b]) + elif camera_model == sl.MODEL.ZED_M: + for i in range(0, zm.NB_AL_ZEDM_TRI * 3, 3): + for j in range(3): + index = int(zm.al_triangles_m[i + j] - 1) + self.zedModel.add_point_clr( + [zm.vertices_m[index * 3], zm.vertices_m[index * 3 + 1], zm.vertices_m[index * 3 + 2]], + [zm.ALLUMINIUM_COLOR.r, zm.ALLUMINIUM_COLOR.g, zm.ALLUMINIUM_COLOR.b]) + + for i in range(0, zm.NB_DARK_ZEDM_TRI * 3, 3): + for j in range(3): + index = int(zm.dark_triangles_m[i + j] - 1) + self.zedModel.add_point_clr( + [zm.vertices_m[index * 3], zm.vertices_m[index * 3 + 1], zm.vertices_m[index * 3 + 2]], + [zm.DARK_COLOR.r, zm.DARK_COLOR.g, zm.DARK_COLOR.b]) + + for i in range(0, zm.NB_GRAY_ZEDM_TRI * 3, 3): + for j in range(3): + index = int(zm.gray_triangles_m[i + j] - 1) + self.zedModel.add_point_clr( + [zm.vertices_m[index * 3], zm.vertices_m[index * 3 + 1], zm.vertices_m[index * 3 + 2]], + [zm.GRAY_COLOR.r, zm.GRAY_COLOR.g, zm.GRAY_COLOR.b]) + + for i in range(0, zm.NB_YELLOW_ZEDM_TRI * 3, 3): + for j in range(3): + index = int(zm.yellow_triangles_m[i + j] - 1) + self.zedModel.add_point_clr( + [zm.vertices_m[index * 3], zm.vertices_m[index * 3 + 1], zm.vertices_m[index * 3 + 2]], + [zm.YELLOW_COLOR.r, zm.YELLOW_COLOR.g, zm.YELLOW_COLOR.b]) + + elif camera_model == sl.MODEL.ZED2: + for i in range(0, zm.NB_ALLUMINIUM_TRIANGLES * 3, 3): + for j in range(3): + index = int(zm.alluminium_triangles[i + j] - 1) + self.zedModel.add_point_clr( + [zm.vertices[index * 3], zm.vertices[index * 3 + 1], zm.vertices[index * 3 + 2]], + [zm.DARK_COLOR.r, zm.DARK_COLOR.g, zm.DARK_COLOR.b]) + + for i in range(0, zm.NB_DARK_TRIANGLES * 3, 3): + for j in range(3): + index = int(zm.dark_triangles[i + j] - 1) + self.zedModel.add_point_clr( + [zm.vertices[index * 3], zm.vertices[index * 3 + 1], zm.vertices[index * 3 + 2]], + [zm.GRAY_COLOR.r, zm.GRAY_COLOR.g, zm.GRAY_COLOR.b]) + self.zedModel.set_drawing_type(GL_TRIANGLES) + self.zedModel.push_to_GPU() + + self.point_cloud.init(res) + self.point_cloud.set_drawing_type(GL_POINTS) + + self.BBox_edges.set_drawing_type(GL_LINES) + self.BBox_faces.set_drawing_type(GL_QUADS) + + # Register GLUT callback functions + glutDisplayFunc(self.draw_callback) + glutIdleFunc(self.idle) + glutKeyboardFunc(self.keyPressedCallback) + glutCloseFunc(self.close_func) + glutMouseFunc(self.on_mouse) + glutMotionFunc(self.on_mousemove) + glutReshapeFunc(self.on_resize) + + self.available = True + + def is_available(self): + if self.available: + glutMainLoopEvent() + return self.available + + def render_object(self, _object_data): # _object_data of type sl.ObjectData + if self.is_tracking_on: + return (_object_data.tracking_state == sl.OBJECT_TRACKING_STATE.OK) + else: + return ( + _object_data.tracking_state == sl.OBJECT_TRACKING_STATE.OK or _object_data.tracking_state == sl.OBJECT_TRACKING_STATE.OFF) + + def updateData(self, pc, _objs): + self.mutex.acquire() + self.point_cloud.setPoints(pc) + + # Clear frame objects + self.BBox_edges.clear() + self.BBox_faces.clear() + + for i in range(len(_objs.object_list)): + if self.render_object(_objs.object_list[i]): + bounding_box = np.array(_objs.object_list[i].bounding_box) + if bounding_box.any(): + color_id = generate_color_id(_objs.object_list[i].id) + self.create_bbox_rendering(bounding_box, color_id) + + self.mutex.release() + + def create_bbox_rendering(self, _bbox, _bbox_clr): + # First create top and bottom full edges + self.BBox_edges.add_full_edges(_bbox, _bbox_clr) + # Add faded vertical edges + self.BBox_edges.add_vertical_edges(_bbox, _bbox_clr) + # Add faces + self.BBox_faces.add_vertical_faces(_bbox, _bbox_clr) + # Add top face + self.BBox_faces.add_top_face(_bbox, _bbox_clr) + + def idle(self): + if self.available: + glutPostRedisplay() + + def exit(self): + if self.available: + self.available = False + + def close_func(self): + if self.available: + self.available = False + + def keyPressedCallback(self, key, x, y): + if ord(key) == 27: # 'Esc' key + self.close_func() + + def on_mouse(self, *args, **kwargs): + (key, Up, x, y) = args + if key == 0: + self.mouse_button[0] = (Up == 0) + elif key == 2: + self.mouse_button[1] = (Up == 0) + elif (key == 3): + self.wheelPosition = self.wheelPosition + 1 + elif (key == 4): + self.wheelPosition = self.wheelPosition - 1 + + self.mouseCurrentPosition = [x, y] + self.previousMouseMotion = [x, y] + + def on_mousemove(self, *args, **kwargs): + (x, y) = args + self.mouseMotion[0] = x - self.previousMouseMotion[0] + self.mouseMotion[1] = y - self.previousMouseMotion[1] + self.previousMouseMotion = [x, y] + glutPostRedisplay() + + def on_resize(self, Width, Height): + glViewport(0, 0, Width, Height) + self.camera.setProjection(Height / Width) + + def draw_callback(self): + if self.available: + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) + glClearColor(self.bckgrnd_clr[0], self.bckgrnd_clr[1], self.bckgrnd_clr[2], 1.) + + self.mutex.acquire() + self.update() + self.draw() + self.mutex.release() + + glutSwapBuffers() + glutPostRedisplay() + + def update(self): + if (self.mouse_button[0]): + r = sl.Rotation() + vert = self.camera.vertical_ + tmp = vert.get() + vert.init_vector(tmp[0] * 1., tmp[1] * 1., tmp[2] * 1.) + r.init_angle_translation(self.mouseMotion[0] * 0.002, vert) + self.camera.rotate(r) + + r.init_angle_translation(self.mouseMotion[1] * 0.002, self.camera.right_) + self.camera.rotate(r) + + if (self.mouse_button[1]): + t = sl.Translation() + tmp = self.camera.right_.get() + scale = self.mouseMotion[0] * -0.01 + t.init_vector(tmp[0] * scale, tmp[1] * scale, tmp[2] * scale) + self.camera.translate(t) + + tmp = self.camera.up_.get() + scale = self.mouseMotion[1] * 0.01 + t.init_vector(tmp[0] * scale, tmp[1] * scale, tmp[2] * scale) + self.camera.translate(t) + + if (self.wheelPosition != 0): + t = sl.Translation() + tmp = self.camera.forward_.get() + scale = self.wheelPosition * -0.065 + t.init_vector(tmp[0] * scale, tmp[1] * scale, tmp[2] * scale) + self.camera.translate(t) + + self.BBox_edges.push_to_GPU() + self.BBox_faces.push_to_GPU() + + self.camera.update() + + self.mouseMotion = [0., 0.] + self.wheelPosition = 0 + + def draw(self): + vpMatrix = self.camera.getViewProjectionMatrix() + + glUseProgram(self.shader_pc.get_program_id()) + glUniformMatrix4fv(self.shader_pc_MVP, 1, GL_TRUE, (GLfloat * len(vpMatrix))(*vpMatrix)) + glPointSize(1.2) + self.point_cloud.draw() + glUseProgram(0) + + glUseProgram(self.shader_image.get_program_id()) + glUniformMatrix4fv(self.shader_image_MVP, 1, GL_TRUE, (GLfloat * len(vpMatrix))(*vpMatrix)) + glLineWidth(4.) + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL) + self.zedModel.draw() + self.BBox_faces.draw() + glPolygonMode(GL_FRONT_AND_BACK, GL_LINE) + glLineWidth(2.) + self.BBox_edges.draw() + glUseProgram(0) + + +class CameraGL: + def __init__(self): + self.ORIGINAL_FORWARD = sl.Translation() + self.ORIGINAL_FORWARD.init_vector(0, 0, 1) + self.ORIGINAL_UP = sl.Translation() + self.ORIGINAL_UP.init_vector(0, 1, 0) + self.ORIGINAL_RIGHT = sl.Translation() + self.ORIGINAL_RIGHT.init_vector(1, 0, 0) + self.znear = 0.5 + self.zfar = 100. + self.horizontalFOV = 70. + self.orientation_ = sl.Orientation() + self.position_ = sl.Translation() + self.forward_ = sl.Translation() + self.up_ = sl.Translation() + self.right_ = sl.Translation() + self.vertical_ = sl.Translation() + self.vpMatrix_ = sl.Matrix4f() + self.offset_ = sl.Translation() + self.offset_.init_vector(0, 0, 5) + self.projection_ = sl.Matrix4f() + self.projection_.set_identity() + self.setProjection(1.78) + + self.position_.init_vector(0., 0., 0.) + tmp = sl.Translation() + tmp.init_vector(0, 0, -.1) + tmp2 = sl.Translation() + tmp2.init_vector(0, 1, 0) + self.setDirection(tmp, tmp2) + + def update(self): + dot_ = sl.Translation.dot_translation(self.vertical_, self.up_) + if (dot_ < 0.): + tmp = self.vertical_.get() + self.vertical_.init_vector(tmp[0] * -1., tmp[1] * -1., tmp[2] * -1.) + transformation = sl.Transform() + + tmp_position = self.position_.get() + tmp = (self.offset_ * self.orientation_).get() + new_position = sl.Translation() + new_position.init_vector(tmp_position[0] + tmp[0], tmp_position[1] + tmp[1], tmp_position[2] + tmp[2]) + transformation.init_orientation_translation(self.orientation_, new_position) + transformation.inverse() + self.vpMatrix_ = self.projection_ * transformation + + def setProjection(self, im_ratio): + fov_x = self.horizontalFOV * 3.1416 / 180. + fov_y = self.horizontalFOV * im_ratio * 3.1416 / 180. + + self.projection_[(0, 0)] = 1. / math.tan(fov_x * .5) + self.projection_[(1, 1)] = 1. / math.tan(fov_y * .5) + self.projection_[(2, 2)] = -(self.zfar + self.znear) / (self.zfar - self.znear) + self.projection_[(3, 2)] = -1. + self.projection_[(2, 3)] = -(2. * self.zfar * self.znear) / (self.zfar - self.znear) + self.projection_[(3, 3)] = 0. + + def getViewProjectionMatrix(self): + tmp = self.vpMatrix_.m + vpMat = array.array('f') + for row in tmp: + for v in row: + vpMat.append(v) + return vpMat + + def getViewProjectionMatrixRT(self, tr): + tmp = self.vpMatrix_ + tmp.transpose() + tr.transpose() + tmp = (tr * tmp).m + vpMat = array.array('f') + for row in tmp: + for v in row: + vpMat.append(v) + return vpMat + + def setDirection(self, dir, vert): + dir.normalize() + tmp = dir.get() + dir.init_vector(tmp[0] * -1., tmp[1] * -1., tmp[2] * -1.) + self.orientation_.init_translation(self.ORIGINAL_FORWARD, dir) + self.updateVectors() + self.vertical_ = vert + if (sl.Translation.dot_translation(self.vertical_, self.up_) < 0.): + tmp = sl.Rotation() + tmp.init_angle_translation(3.14, self.ORIGINAL_FORWARD) + self.rotate(tmp) + + def translate(self, t): + ref = self.position_.get() + tmp = t.get() + self.position_.init_vector(ref[0] + tmp[0], ref[1] + tmp[1], ref[2] + tmp[2]) + + def setPosition(self, p): + self.position_ = p + + def rotate(self, r): + tmp = sl.Orientation() + tmp.init_rotation(r) + self.orientation_ = tmp * self.orientation_ + self.updateVectors() + + def setRotation(self, r): + self.orientation_.init_rotation(r) + self.updateVectors() + + def updateVectors(self): + self.forward_ = self.ORIGINAL_FORWARD * self.orientation_ + self.up_ = self.ORIGINAL_UP * self.orientation_ + right = self.ORIGINAL_RIGHT + tmp = right.get() + right.init_vector(tmp[0] * -1., tmp[1] * -1., tmp[2] * -1.) + self.right_ = right * self.orientation_ diff --git a/src/perception/aquisition/zed_visualise/ogl_viewer/zed_model.py b/src/perception/aquisition/zed_visualise/ogl_viewer/zed_model.py new file mode 100755 index 00000000..f0a23ff4 --- /dev/null +++ b/src/perception/aquisition/zed_visualise/ogl_viewer/zed_model.py @@ -0,0 +1,2736 @@ +import numpy as np + + +class Color: + def __init__(self, pr, pg, pb): + self.r = pr + self.g = pg + self.b = pb + + +NB_ALLUMINIUM_TRIANGLES = 54 +NB_DARK_TRIANGLES = 54 +ALLUMINIUM_COLOR = Color(0.79, 0.82, 0.93) +DARK_COLOR = Color(0.07, 0.07, 0.07) + +vertices = np.array([ + -0.068456, -0.016299, 0.016299 + , -0.068456, 0.016299, 0.016299 + , -0.068456, 0.016299, -0.016299 + , -0.068456, -0.016299, -0.016299 + , -0.076606, 0.014115, 0.016299 + , -0.082572, 0.008150, 0.016299 + , -0.084755, -0.000000, 0.016299 + , -0.082572, -0.008150, 0.016299 + , -0.076606, -0.014115, 0.016299 + , -0.076606, -0.014115, -0.016299 + , -0.082572, -0.008150, -0.016299 + , -0.084755, -0.000000, -0.016299 + , -0.082572, 0.008150, -0.016299 + , -0.076606, 0.014115, -0.016299 + , -0.053494, -0.009779, -0.016299 + , -0.048604, -0.008469, -0.016299 + , -0.045024, -0.004890, -0.016299 + , -0.043714, 0.000000, -0.016299 + , -0.045024, 0.004890, -0.016299 + , -0.048604, 0.008469, -0.016299 + , -0.053494, 0.009779, -0.016299 + , -0.058383, 0.008469, -0.016299 + , -0.061963, 0.004890, -0.016299 + , -0.063273, 0.000000, -0.016299 + , -0.061963, -0.004890, -0.016299 + , -0.058383, -0.008469, -0.016299 + , 0.000000, -0.016299, -0.016299 + , 0.068456, -0.016299, 0.016299 + , 0.000000, 0.016299, -0.016299 + , 0.068456, 0.016299, 0.016299 + , 0.068456, 0.016299, -0.016299 + , 0.068456, -0.016299, -0.016299 + , 0.076606, 0.014115, 0.016299 + , 0.082572, 0.008150, 0.016299 + , 0.084755, -0.000000, 0.016299 + , 0.082572, -0.008150, 0.016299 + , 0.076606, -0.014115, 0.016299 + , 0.076606, -0.014115, -0.016299 + , 0.082572, -0.008150, -0.016299 + , 0.084755, -0.000000, -0.016299 + , 0.082572, 0.008150, -0.016299 + , 0.076606, 0.014115, -0.016299 + , 0.053494, -0.009779, -0.016299 + , 0.048604, -0.008469, -0.016299 + , 0.045024, -0.004890, -0.016299 + , 0.043714, 0.000000, -0.016299 + , 0.045024, 0.004890, -0.016299 + , 0.048604, 0.008469, -0.016299 + , 0.053494, 0.009779, -0.016299 + , 0.058383, 0.008469, -0.016299 + , 0.061963, 0.004890, -0.016299 + , 0.063273, 0.000000, -0.016299 + , 0.061963, -0.004890, -0.016299 + , 0.058383, -0.008469, -0.016299 + , 0.053494, 0.000000, -0.016299 + , -0.053494, 0.000000, -0.016299 +]) + +alluminium_triangles = np.array([ + 1, 10, 4 + , 6, 14, 13 + , 7, 13, 12 + , 8, 12, 11 + , 9, 11, 10 + , 5, 3, 14 + , 44, 45, 55 + , 47, 48, 55 + , 43, 44, 55 + , 46, 47, 55 + , 52, 53, 55 + , 48, 49, 55 + , 54, 43, 55 + , 50, 51, 55 + , 53, 54, 55 + , 49, 50, 55 + , 45, 46, 55 + , 51, 52, 55 + , 27, 32, 28 + , 38, 28, 32 + , 42, 34, 41 + , 41, 35, 40 + , 40, 36, 39 + , 39, 37, 38 + , 31, 33, 42 + , 27, 1, 4 + , 20, 19, 56 + , 22, 21, 56 + , 23, 22, 56 + , 24, 23, 56 + , 19, 18, 56 + , 21, 20, 56 + , 17, 16, 56 + , 26, 25, 56 + , 15, 26, 56 + , 18, 17, 56 + , 16, 15, 56 + , 25, 24, 56 + , 2, 29, 3 + , 31, 29, 30 + , 1, 9, 10 + , 6, 5, 14 + , 7, 6, 13 + , 8, 7, 12 + , 9, 8, 11 + , 5, 2, 3 + , 38, 37, 28 + , 42, 33, 34 + , 41, 34, 35 + , 40, 35, 36 + , 39, 36, 37 + , 31, 30, 33 + , 27, 28, 1 + , 2, 30, 29 +]) + +dark_triangles = np.array([ + 23, 3, 22 + , 13, 10, 11 + , 4, 14, 3 + , 11, 12, 13 + , 9, 6, 8 + , 1, 5, 9 + , 8, 6, 7 + , 1, 30, 2 + , 21, 22, 3 + , 23, 24, 3 + , 24, 25, 4 + , 3, 24, 4 + , 25, 26, 4 + , 26, 15, 4 + , 16, 17, 27 + , 17, 18, 27 + , 18, 19, 29 + , 27, 18, 29 + , 19, 20, 29 + , 20, 21, 29 + , 3, 29, 21 + , 16, 27, 15 + , 27, 4, 15 + , 51, 50, 31 + , 38, 41, 39 + , 32, 42, 38 + , 39, 41, 40 + , 34, 37, 36 + , 28, 33, 30 + , 36, 35, 34 + , 49, 31, 50 + , 51, 31, 52 + , 52, 32, 53 + , 31, 32, 52 + , 53, 32, 54 + , 54, 32, 43 + , 44, 27, 45 + , 45, 27, 46 + , 46, 29, 47 + , 27, 29, 46 + , 47, 29, 48 + , 48, 29, 49 + , 31, 49, 29 + , 44, 43, 27 + , 27, 43, 32 + , 13, 14, 10 + , 4, 10, 14 + , 9, 5, 6 + , 1, 2, 5 + , 1, 28, 30 + , 38, 42, 41 + , 32, 31, 42 + , 34, 33, 37 + , 28, 37, 33 +]) + +vertices_m = np.array([ + 0.030800, 0.013300, 0.000001 + , 0.058785, 0.013300, -0.002250 + , 0.058785, 0.013300, 0.002251 + , 0.059839, 0.013300, -0.001999 + , 0.060770, 0.013300, -0.001351 + , 0.059839, 0.013300, 0.002000 + , 0.060770, 0.013300, 0.001352 + , 0.002815, 0.013300, -0.002250 + , 0.002815, 0.013300, 0.002251 + , 0.001761, 0.013300, -0.001999 + , 0.000830, 0.013300, -0.001351 + , 0.001761, 0.013300, 0.002000 + , 0.000830, 0.013300, 0.001352 + , 0.061449, 0.013300, -0.000563 + , 0.061449, 0.013300, 0.000564 + , 0.000152, 0.013300, 0.000564 + , 0.000152, 0.013300, -0.000563 + , 0.030800, -0.013333, 0.000001 + , 0.058785, -0.013333, -0.002250 + , 0.058785, -0.013333, 0.002251 + , 0.059839, -0.013333, -0.001999 + , 0.060770, -0.013333, -0.001351 + , 0.059839, -0.013333, 0.002000 + , 0.060770, -0.013333, 0.001352 + , 0.002815, -0.013333, -0.002250 + , 0.002815, -0.013333, 0.002251 + , 0.001761, -0.013333, -0.001999 + , 0.000830, -0.013333, -0.001351 + , 0.001761, -0.013333, 0.002000 + , 0.000830, -0.013333, 0.001352 + , 0.061449, -0.013333, 0.000564 + , 0.061449, -0.013333, -0.000563 + , 0.000152, -0.013333, -0.000563 + , 0.000152, -0.013333, 0.000564 + , -0.031684, 0.009412, 0.000501 + , -0.031809, 0.008300, 0.000501 + , -0.028977, 0.012805, 0.000501 + , -0.029926, 0.012209, 0.000501 + , -0.026809, 0.013300, 0.000501 + , -0.027920, 0.013175, 0.000501 + , -0.030718, 0.011417, 0.000501 + , -0.031314, 0.010469, 0.000501 + , -0.031809, -0.008310, 0.000501 + , -0.031684, -0.009431, 0.000501 + , -0.028977, -0.012824, 0.000501 + , -0.029926, -0.012228, 0.000501 + , -0.026847, -0.013300, 0.000500 + , -0.027920, -0.013194, 0.000501 + , -0.030718, -0.011437, 0.000501 + , -0.031314, -0.010488, 0.000501 + , -0.031684, 0.009412, -0.000500 + , -0.031809, 0.008300, -0.000500 + , -0.028977, 0.012805, -0.000500 + , -0.029926, 0.012209, -0.000500 + , -0.026809, 0.013300, -0.000500 + , -0.027920, 0.013175, -0.000500 + , -0.030718, 0.011417, -0.000500 + , -0.031314, 0.010469, -0.000500 + , -0.031809, -0.008310, -0.000500 + , -0.031684, -0.009431, -0.000500 + , -0.029926, -0.012228, -0.000500 + , -0.028977, -0.012824, -0.000500 + , -0.027920, -0.013194, -0.000500 + , -0.026847, -0.013300, -0.000500 + , -0.031314, -0.010488, -0.000500 + , -0.030718, -0.011437, -0.000500 + , -0.031809, 0.006354, -0.000500 + , -0.031809, 0.006354, 0.000501 + , -0.031809, -0.006364, -0.000500 + , -0.031809, -0.006364, 0.000501 + , -0.031809, 0.005707, -0.000700 + , -0.031809, 0.005707, 0.000701 + , -0.031809, -0.005716, -0.000700 + , -0.031809, -0.005716, 0.000701 + , -0.031809, 0.005128, -0.001423 + , -0.031809, 0.005128, 0.001424 + , -0.031809, -0.005138, -0.001423 + , -0.031809, -0.005138, 0.001424 + , -0.031809, 0.004146, -0.002297 + , -0.031809, 0.004146, 0.002299 + , -0.031809, -0.004156, -0.002297 + , -0.031809, -0.004156, 0.002299 + , -0.031809, 0.003313, -0.002495 + , -0.031809, 0.003313, 0.002497 + , -0.031809, -0.003322, -0.002495 + , -0.031809, -0.003322, 0.002497 + , -0.031809, -0.000005, 0.000001 + , -0.026800, 0.013300, -0.000500 + , -0.026800, 0.013300, 0.000501 + , 0.088376, 0.013300, -0.000500 + , 0.088376, 0.013300, 0.000501 + , 0.093228, 0.009412, 0.000501 + , 0.093353, 0.008300, 0.000501 + , 0.090522, 0.012805, 0.000501 + , 0.091470, 0.012209, 0.000501 + , 0.089464, 0.013175, 0.000501 + , 0.092262, 0.011417, 0.000501 + , 0.092858, 0.010469, 0.000501 + , 0.093353, -0.008310, 0.000501 + , 0.093228, -0.009431, 0.000501 + , 0.090522, -0.012824, 0.000501 + , 0.091470, -0.012228, 0.000501 + , 0.088376, -0.013321, 0.000501 + , 0.089464, -0.013194, 0.000501 + , 0.092262, -0.011437, 0.000501 + , 0.092858, -0.010488, 0.000501 + , 0.093228, 0.009412, -0.000500 + , 0.093353, 0.008300, -0.000500 + , 0.090522, 0.012805, -0.000500 + , 0.091470, 0.012209, -0.000500 + , 0.089464, 0.013175, -0.000500 + , 0.092262, 0.011417, -0.000500 + , 0.092858, 0.010469, -0.000500 + , 0.093353, -0.008310, -0.000500 + , 0.093228, -0.009431, -0.000500 + , 0.091470, -0.012228, -0.000500 + , 0.090522, -0.012824, -0.000500 + , 0.089464, -0.013194, -0.000500 + , 0.088376, -0.013321, -0.000500 + , 0.092858, -0.010488, -0.000500 + , 0.092262, -0.011437, -0.000500 + , -0.001600, 0.000000, -0.018000 + , 0.017592, 0.000000, -0.018000 + , 0.007996, -0.009596, -0.018000 + , 0.007996, -0.009763, -0.008431 + , 0.007996, 0.009763, -0.008431 + , 0.007996, 0.009596, -0.018000 + , -0.001767, 0.000000, -0.008431 + , 0.002258, -0.007899, -0.008431 + , 0.002356, -0.007764, -0.018000 + , 0.005033, -0.009127, -0.018000 + , 0.004982, -0.009286, -0.008431 + , 0.000097, -0.005738, -0.008431 + , 0.000232, -0.005640, -0.018000 + , -0.001131, -0.002963, -0.018000 + , -0.001290, -0.003014, -0.008431 + , 0.000097, 0.005738, -0.008431 + , 0.000232, 0.005640, -0.018000 + , -0.001131, 0.002963, -0.018000 + , -0.001290, 0.003014, -0.008431 + , 0.002258, 0.007899, -0.008431 + , 0.002356, 0.007764, -0.018000 + , 0.005033, 0.009127, -0.018000 + , 0.004982, 0.009286, -0.008431 + , 0.017759, 0.000000, -0.008431 + , 0.013734, 0.007899, -0.008431 + , 0.013636, 0.007764, -0.018000 + , 0.010959, 0.009127, -0.018000 + , 0.011010, 0.009286, -0.008431 + , 0.015895, 0.005738, -0.008431 + , 0.015760, 0.005640, -0.018000 + , 0.017123, 0.002963, -0.018000 + , 0.017282, 0.003014, -0.008431 + , 0.015895, -0.005738, -0.008431 + , 0.015760, -0.005640, -0.018000 + , 0.017123, -0.002963, -0.018000 + , 0.017282, -0.003014, -0.008431 + , 0.013734, -0.007899, -0.008431 + , 0.013636, -0.007764, -0.018000 + , 0.010959, -0.009127, -0.018000 + , 0.011010, -0.009286, -0.008431 + , 0.004827, 0.009763, -0.007940 + , 0.007996, 0.010264, -0.007940 + , -0.001767, -0.003169, -0.007940 + , -0.002269, 0.000000, -0.007940 + , 0.004827, -0.009763, -0.007940 + , 0.001963, -0.008304, -0.007940 + , 0.007996, -0.010264, -0.007940 + , -0.000308, -0.006033, -0.007940 + , -0.001767, 0.003169, -0.007940 + , -0.000308, 0.006033, -0.007940 + , 0.001963, 0.008304, -0.007940 + , 0.011165, -0.009763, -0.007940 + , 0.017759, 0.003169, -0.007940 + , 0.018260, -0.000000, -0.007940 + , 0.011165, 0.009763, -0.007940 + , 0.014029, 0.008304, -0.007940 + , 0.016300, 0.006033, -0.007940 + , 0.017759, -0.003169, -0.007940 + , 0.016300, -0.006033, -0.007940 + , 0.014029, -0.008304, -0.007940 + , 0.002356, -0.007764, -0.019500 + , 0.005033, -0.009127, -0.019500 + , 0.007996, -0.009596, -0.019500 + , 0.000232, -0.005640, -0.019500 + , -0.001600, 0.000000, -0.019500 + , -0.001131, -0.002963, -0.019500 + , 0.000232, 0.005640, -0.019500 + , -0.001131, 0.002963, -0.019500 + , 0.002356, 0.007764, -0.019500 + , 0.007996, 0.009596, -0.019500 + , 0.005033, 0.009127, -0.019500 + , 0.013636, 0.007764, -0.019500 + , 0.010959, 0.009127, -0.019500 + , 0.015760, 0.005640, -0.019500 + , 0.017592, 0.000000, -0.019500 + , 0.017123, 0.002963, -0.019500 + , 0.015760, -0.005640, -0.019500 + , 0.017123, -0.002963, -0.019500 + , 0.013636, -0.007764, -0.019500 + , 0.010959, -0.009127, -0.019500 + , 0.002356, -0.007764, -0.022997 + , 0.005033, -0.009127, -0.022997 + , 0.007996, -0.009596, -0.022997 + , 0.000232, -0.005640, -0.022997 + , -0.001600, 0.000000, -0.022997 + , -0.001131, -0.002963, -0.022997 + , 0.000232, 0.005640, -0.022997 + , -0.001131, 0.002963, -0.022997 + , 0.002356, 0.007764, -0.022997 + , 0.007996, 0.009596, -0.022997 + , 0.005033, 0.009127, -0.022997 + , 0.013636, 0.007764, -0.022997 + , 0.010959, 0.009127, -0.022997 + , 0.015760, 0.005640, -0.022997 + , 0.017592, 0.000000, -0.022997 + , 0.017123, 0.002963, -0.022997 + , 0.015760, -0.005640, -0.022997 + , 0.017123, -0.002963, -0.022997 + , 0.013636, -0.007764, -0.022997 + , 0.010959, -0.009127, -0.022997 + , 0.002745, -0.007227, -0.022997 + , 0.005238, -0.008497, -0.022997 + , 0.007996, -0.008933, -0.022997 + , 0.000769, -0.005250, -0.022997 + , -0.000937, 0.000000, -0.022997 + , -0.000501, -0.002758, -0.022997 + , 0.000769, 0.005250, -0.022997 + , -0.000501, 0.002758, -0.022997 + , 0.002745, 0.007227, -0.022997 + , 0.007996, 0.008933, -0.022997 + , 0.005238, 0.008497, -0.022997 + , 0.013246, 0.007227, -0.022997 + , 0.010754, 0.008497, -0.022997 + , 0.015223, 0.005250, -0.022997 + , 0.016929, 0.000000, -0.022997 + , 0.016493, 0.002758, -0.022997 + , 0.015223, -0.005250, -0.022997 + , 0.016493, -0.002758, -0.022997 + , 0.013246, -0.007227, -0.022997 + , 0.010754, -0.008497, -0.022997 + , 0.004095, -0.005369, -0.022203 + , 0.005947, -0.006313, -0.022203 + , 0.007996, -0.006637, -0.022203 + , 0.002626, -0.003901, -0.022203 + , 0.001359, 0.000000, -0.022203 + , 0.001683, -0.002049, -0.022203 + , 0.002626, 0.003901, -0.022203 + , 0.001683, 0.002049, -0.022203 + , 0.004095, 0.005369, -0.022203 + , 0.007996, 0.006637, -0.022203 + , 0.005947, 0.006313, -0.022203 + , 0.011897, 0.005369, -0.022203 + , 0.010045, 0.006313, -0.022203 + , 0.013365, 0.003901, -0.022203 + , 0.014633, 0.000000, -0.022203 + , 0.014308, 0.002049, -0.022203 + , 0.013365, -0.003901, -0.022203 + , 0.014308, -0.002049, -0.022203 + , 0.011897, -0.005369, -0.022203 + , 0.010045, -0.006313, -0.022203 + , 0.004446, -0.004886, -0.021500 + , 0.006131, -0.005744, -0.021500 + , 0.007996, -0.006039, -0.021500 + , 0.003110, -0.003549, -0.021500 + , 0.001957, 0.000000, -0.021500 + , 0.002252, -0.001865, -0.021500 + , 0.003110, 0.003549, -0.021500 + , 0.002252, 0.001865, -0.021500 + , 0.004446, 0.004886, -0.021500 + , 0.007996, 0.006039, -0.021500 + , 0.006131, 0.005744, -0.021500 + , 0.011545, 0.004886, -0.021500 + , 0.009861, 0.005744, -0.021500 + , 0.012882, 0.003549, -0.021500 + , 0.014035, 0.000000, -0.021500 + , 0.013740, 0.001865, -0.021500 + , 0.012882, -0.003549, -0.021500 + , 0.013740, -0.001865, -0.021500 + , 0.011545, -0.004886, -0.021500 + , 0.009861, -0.005744, -0.021500 + , 0.004446, -0.004886, -0.020078 + , 0.006131, -0.005744, -0.020078 + , 0.007996, -0.006039, -0.020078 + , 0.003110, -0.003549, -0.020078 + , 0.001957, 0.000000, -0.020078 + , 0.002252, -0.001865, -0.020078 + , 0.003110, 0.003549, -0.020078 + , 0.002252, 0.001865, -0.020078 + , 0.004446, 0.004886, -0.020078 + , 0.007996, 0.006039, -0.020078 + , 0.006131, 0.005744, -0.020078 + , 0.011545, 0.004886, -0.020078 + , 0.009861, 0.005744, -0.020078 + , 0.012882, 0.003549, -0.020078 + , 0.014035, 0.000000, -0.020078 + , 0.013740, 0.001865, -0.020078 + , 0.012882, -0.003549, -0.020078 + , 0.013740, -0.001865, -0.020078 + , 0.011545, -0.004886, -0.020078 + , 0.009861, -0.005744, -0.020078 + , -0.026847, -0.013300, -0.006500 + , -0.031847, -0.008300, -0.006500 + , -0.029965, -0.012209, -0.006500 + , -0.027959, -0.013175, -0.006500 + , -0.029016, -0.012805, -0.006500 + , -0.031352, -0.010469, -0.006500 + , -0.030756, -0.011417, -0.006500 + , -0.031722, -0.009412, -0.006500 + , 0.088353, -0.013310, -0.006500 + , -0.031847, 0.008300, -0.006500 + , -0.026847, 0.013300, -0.006500 + , -0.030756, 0.011417, -0.006500 + , -0.031722, 0.009412, -0.006500 + , -0.031352, 0.010469, -0.006500 + , -0.029016, 0.012805, -0.006500 + , -0.029965, 0.012209, -0.006500 + , -0.027959, 0.013175, -0.006500 + , 0.088353, 0.013300, -0.006500 + , 0.093353, 0.008300, -0.006500 + , 0.091470, 0.012209, -0.006500 + , 0.089464, 0.013175, -0.006500 + , 0.090522, 0.012805, -0.006500 + , 0.092858, 0.010469, -0.006500 + , 0.092262, 0.011417, -0.006500 + , 0.093228, 0.009412, -0.006500 + , 0.093353, -0.008310, -0.006500 + , 0.091470, -0.012228, -0.006500 + , 0.089464, -0.013194, -0.006500 + , 0.090522, -0.012824, -0.006500 + , 0.092858, -0.010488, -0.006500 + , 0.092262, -0.011437, -0.006500 + , 0.093228, -0.009431, -0.006500 + , -0.031722, -0.009412, -0.002250 + , -0.031809, -0.004156, -0.002297 + , -0.029016, -0.012805, -0.002250 + , -0.029965, -0.012209, -0.002250 + , -0.026847, -0.013300, -0.002250 + , -0.027959, -0.013175, -0.002250 + , -0.030756, -0.011417, -0.002250 + , -0.031352, -0.010469, -0.002250 + , 0.088353, -0.013310, -0.002250 + , -0.031809, 0.004146, -0.002297 + , -0.027959, 0.013175, -0.002250 + , -0.026847, 0.013300, -0.002250 + , -0.031352, 0.010469, -0.002250 + , -0.030756, 0.011417, -0.002250 + , -0.031722, 0.009412, -0.002250 + , -0.029965, 0.012209, -0.002250 + , -0.029016, 0.012805, -0.002250 + , 0.088353, 0.013300, -0.002250 + , 0.093228, 0.009412, -0.002250 + , 0.093353, 0.008300, -0.002250 + , 0.090522, 0.012805, -0.002250 + , 0.091470, 0.012209, -0.002250 + , 0.089464, 0.013175, -0.002250 + , 0.092262, 0.011417, -0.002250 + , 0.092858, 0.010469, -0.002250 + , 0.093353, -0.008310, -0.002250 + , 0.093228, -0.009431, -0.002250 + , 0.090522, -0.012824, -0.002250 + , 0.091470, -0.012228, -0.002250 + , 0.089464, -0.013194, -0.002250 + , 0.092262, -0.011437, -0.002250 + , 0.092858, -0.010488, -0.002250 + , 0.002815, -0.013333, -0.002250 + , 0.001761, 0.013300, -0.001999 + , 0.058785, -0.013333, -0.002250 + , 0.059839, 0.013300, -0.001999 + , -0.031722, -0.009412, -0.000500 + , -0.031847, -0.006340, -0.000500 + , -0.029016, -0.012805, -0.000500 + , -0.029965, -0.012209, -0.000500 + , -0.026847, -0.013300, -0.000500 + , -0.027959, -0.013175, -0.000500 + , -0.030756, -0.011417, -0.000500 + , -0.031352, -0.010469, -0.000500 + , 0.000152, -0.013333, -0.000563 + , -0.031847, 0.006354, -0.000500 + , -0.027959, 0.013175, -0.000500 + , -0.026847, 0.013300, -0.000500 + , -0.031352, 0.010469, -0.000500 + , -0.030756, 0.011417, -0.000500 + , -0.031722, 0.009412, -0.000500 + , -0.029965, 0.012209, -0.000500 + , -0.029016, 0.012805, -0.000500 + , 0.088353, 0.013300, -0.000500 + , 0.061448, 0.013300, -0.000563 + , 0.093228, 0.009412, -0.000500 + , 0.093353, 0.008300, -0.000500 + , 0.090522, 0.012805, -0.000500 + , 0.091470, 0.012209, -0.000500 + , 0.089464, 0.013175, -0.000500 + , 0.092262, 0.011417, -0.000500 + , 0.092858, 0.010469, -0.000500 + , 0.093353, -0.008310, -0.000500 + , 0.093228, -0.009431, -0.000500 + , 0.090522, -0.012824, -0.000500 + , 0.091470, -0.012228, -0.000500 + , 0.088353, -0.013310, -0.000500 + , 0.089464, -0.013194, -0.000500 + , 0.092262, -0.011437, -0.000500 + , 0.092858, -0.010488, -0.000500 + , 0.000151, 0.013300, -0.000563 + , 0.061448, -0.013333, -0.000563 + , 0.058800, 0.013300, -0.002250 + , 0.002815, 0.013300, -0.002250 + , 0.000830, 0.013300, -0.001351 + , 0.060770, 0.013300, -0.001351 + , 0.060770, -0.013333, -0.001351 + , 0.059839, -0.013333, -0.001999 + , 0.000830, -0.013333, -0.001351 + , 0.001761, -0.013333, -0.001999 + , -0.026844, -0.011518, -0.007940 + , -0.026847, -0.011634, -0.007898 + , -0.027589, -0.011551, -0.007898 + , -0.027563, -0.011437, -0.007940 + , -0.028294, -0.011304, -0.007898 + , -0.028243, -0.011199, -0.007940 + , -0.028926, -0.010907, -0.007898 + , -0.028854, -0.010816, -0.007940 + , -0.029454, -0.010379, -0.007898 + , -0.029363, -0.010306, -0.007940 + , -0.029852, -0.009746, -0.007898 + , -0.029747, -0.009696, -0.007940 + , -0.030098, -0.009041, -0.007898 + , -0.029985, -0.009016, -0.007940 + , -0.030181, -0.008300, -0.007898 + , -0.030066, -0.008297, -0.007940 + , -0.030181, 0.008300, -0.007898 + , -0.030065, 0.008296, -0.007940 + , -0.030098, 0.009041, -0.007898 + , -0.029985, 0.009015, -0.007940 + , -0.029852, 0.009746, -0.007898 + , -0.029747, 0.009696, -0.007940 + , -0.029454, 0.010379, -0.007898 + , -0.029363, 0.010306, -0.007940 + , -0.028926, 0.010907, -0.007898 + , -0.028854, 0.010816, -0.007940 + , -0.028294, 0.011304, -0.007898 + , -0.028243, 0.011199, -0.007940 + , -0.027589, 0.011551, -0.007898 + , -0.027563, 0.011437, -0.007940 + , -0.026847, 0.011634, -0.007898 + , -0.026844, 0.011518, -0.007940 + , 0.088349, 0.011518, -0.007940 + , 0.088353, 0.011634, -0.007898 + , 0.089094, 0.011551, -0.007898 + , 0.089068, 0.011437, -0.007940 + , 0.089799, 0.011304, -0.007898 + , 0.089749, 0.011199, -0.007940 + , 0.090432, 0.010907, -0.007898 + , 0.090359, 0.010816, -0.007940 + , 0.090960, 0.010379, -0.007898 + , 0.090869, 0.010306, -0.007940 + , 0.091357, 0.009746, -0.007898 + , 0.091252, 0.009696, -0.007940 + , 0.091604, 0.009041, -0.007898 + , 0.091490, 0.009016, -0.007940 + , 0.091637, 0.008187, -0.007940 + , 0.091724, 0.008300, -0.007867 + , 0.088353, -0.011644, -0.007898 + , 0.088349, -0.011528, -0.007940 + , 0.089094, -0.011570, -0.007898 + , 0.089069, -0.011456, -0.007940 + , 0.089799, -0.011323, -0.007898 + , 0.089749, -0.011219, -0.007940 + , 0.090432, -0.010926, -0.007898 + , 0.090359, -0.010835, -0.007940 + , 0.090960, -0.010398, -0.007898 + , 0.090869, -0.010325, -0.007940 + , 0.091357, -0.009766, -0.007898 + , 0.091252, -0.009715, -0.007940 + , 0.091604, -0.009061, -0.007898 + , 0.091490, -0.009035, -0.007940 + , 0.091724, -0.008310, -0.007867 + , 0.091637, -0.008196, -0.007940 + , -0.031809, -0.003322, -0.002495 + , -0.031809, 0.003313, -0.002495 + , -0.031809, 0.005707, -0.000700 + , -0.031809, 0.005128, -0.001423 + , -0.031809, -0.005716, -0.000700 + , -0.031809, -0.005138, -0.001423 + , 0.061397, 0.000000, -0.018000 + , 0.080589, 0.000000, -0.018000 + , 0.070993, -0.009596, -0.018000 + , 0.070993, -0.009763, -0.008431 + , 0.070993, 0.009763, -0.008431 + , 0.070993, 0.009596, -0.018000 + , 0.061230, 0.000000, -0.008431 + , 0.065255, -0.007899, -0.008431 + , 0.065353, -0.007764, -0.018000 + , 0.068030, -0.009127, -0.018000 + , 0.067979, -0.009286, -0.008431 + , 0.063094, -0.005738, -0.008431 + , 0.063229, -0.005640, -0.018000 + , 0.061866, -0.002963, -0.018000 + , 0.061707, -0.003014, -0.008431 + , 0.063094, 0.005738, -0.008431 + , 0.063229, 0.005640, -0.018000 + , 0.061866, 0.002963, -0.018000 + , 0.061707, 0.003014, -0.008431 + , 0.065255, 0.007899, -0.008431 + , 0.065353, 0.007764, -0.018000 + , 0.068030, 0.009127, -0.018000 + , 0.067979, 0.009286, -0.008431 + , 0.080756, 0.000000, -0.008431 + , 0.076731, 0.007899, -0.008431 + , 0.076633, 0.007764, -0.018000 + , 0.073956, 0.009127, -0.018000 + , 0.074007, 0.009286, -0.008431 + , 0.078892, 0.005738, -0.008431 + , 0.078757, 0.005640, -0.018000 + , 0.080120, 0.002963, -0.018000 + , 0.080279, 0.003014, -0.008431 + , 0.078892, -0.005738, -0.008431 + , 0.078757, -0.005640, -0.018000 + , 0.080120, -0.002963, -0.018000 + , 0.080279, -0.003014, -0.008431 + , 0.076731, -0.007899, -0.008431 + , 0.076633, -0.007764, -0.018000 + , 0.073956, -0.009127, -0.018000 + , 0.074007, -0.009286, -0.008431 + , 0.067824, 0.009763, -0.007940 + , 0.070993, 0.010264, -0.007940 + , 0.061230, -0.003169, -0.007940 + , 0.060728, 0.000000, -0.007940 + , 0.067824, -0.009763, -0.007940 + , 0.064960, -0.008304, -0.007940 + , 0.070993, -0.010264, -0.007940 + , 0.062688, -0.006033, -0.007940 + , 0.061230, 0.003169, -0.007940 + , 0.062688, 0.006033, -0.007940 + , 0.064960, 0.008304, -0.007940 + , 0.074162, -0.009763, -0.007940 + , 0.080756, 0.003169, -0.007940 + , 0.081257, -0.000000, -0.007940 + , 0.074162, 0.009763, -0.007940 + , 0.077026, 0.008304, -0.007940 + , 0.079297, 0.006033, -0.007940 + , 0.080756, -0.003169, -0.007940 + , 0.079297, -0.006033, -0.007940 + , 0.077026, -0.008304, -0.007940 + , 0.065353, -0.007764, -0.019500 + , 0.068030, -0.009127, -0.019500 + , 0.070993, -0.009596, -0.019500 + , 0.063229, -0.005640, -0.019500 + , 0.061397, 0.000000, -0.019500 + , 0.061866, -0.002963, -0.019500 + , 0.063229, 0.005640, -0.019500 + , 0.061866, 0.002963, -0.019500 + , 0.065353, 0.007764, -0.019500 + , 0.070993, 0.009596, -0.019500 + , 0.068030, 0.009127, -0.019500 + , 0.076633, 0.007764, -0.019500 + , 0.073956, 0.009127, -0.019500 + , 0.078757, 0.005640, -0.019500 + , 0.080589, 0.000000, -0.019500 + , 0.080120, 0.002963, -0.019500 + , 0.078757, -0.005640, -0.019500 + , 0.080120, -0.002963, -0.019500 + , 0.076633, -0.007764, -0.019500 + , 0.073956, -0.009127, -0.019500 + , 0.065353, -0.007764, -0.022997 + , 0.068030, -0.009127, -0.022997 + , 0.070993, -0.009596, -0.022997 + , 0.063229, -0.005640, -0.022997 + , 0.061397, 0.000000, -0.022997 + , 0.061866, -0.002963, -0.022997 + , 0.063229, 0.005640, -0.022997 + , 0.061866, 0.002963, -0.022997 + , 0.065353, 0.007764, -0.022997 + , 0.070993, 0.009596, -0.022997 + , 0.068030, 0.009127, -0.022997 + , 0.076633, 0.007764, -0.022997 + , 0.073956, 0.009127, -0.022997 + , 0.078757, 0.005640, -0.022997 + , 0.080589, 0.000000, -0.022997 + , 0.080120, 0.002963, -0.022997 + , 0.078757, -0.005640, -0.022997 + , 0.080120, -0.002963, -0.022997 + , 0.076633, -0.007764, -0.022997 + , 0.073956, -0.009127, -0.022997 + , 0.065742, -0.007227, -0.022997 + , 0.068235, -0.008497, -0.022997 + , 0.070993, -0.008933, -0.022997 + , 0.063766, -0.005250, -0.022997 + , 0.062060, 0.000000, -0.022997 + , 0.062496, -0.002758, -0.022997 + , 0.063766, 0.005250, -0.022997 + , 0.062496, 0.002758, -0.022997 + , 0.065742, 0.007227, -0.022997 + , 0.070993, 0.008933, -0.022997 + , 0.068235, 0.008497, -0.022997 + , 0.076243, 0.007227, -0.022997 + , 0.073751, 0.008497, -0.022997 + , 0.078220, 0.005250, -0.022997 + , 0.079926, 0.000000, -0.022997 + , 0.079490, 0.002758, -0.022997 + , 0.078220, -0.005250, -0.022997 + , 0.079490, -0.002758, -0.022997 + , 0.076243, -0.007227, -0.022997 + , 0.073751, -0.008497, -0.022997 + , 0.067092, -0.005369, -0.022203 + , 0.068944, -0.006313, -0.022203 + , 0.070993, -0.006637, -0.022203 + , 0.065623, -0.003901, -0.022203 + , 0.064356, 0.000000, -0.022203 + , 0.064680, -0.002049, -0.022203 + , 0.065623, 0.003901, -0.022203 + , 0.064680, 0.002049, -0.022203 + , 0.067092, 0.005369, -0.022203 + , 0.070993, 0.006637, -0.022203 + , 0.068944, 0.006313, -0.022203 + , 0.074894, 0.005369, -0.022203 + , 0.073042, 0.006313, -0.022203 + , 0.076362, 0.003901, -0.022203 + , 0.077630, 0.000000, -0.022203 + , 0.077305, 0.002049, -0.022203 + , 0.076362, -0.003901, -0.022203 + , 0.077305, -0.002049, -0.022203 + , 0.074894, -0.005369, -0.022203 + , 0.073042, -0.006313, -0.022203 + , 0.067443, -0.004886, -0.021500 + , 0.069128, -0.005744, -0.021500 + , 0.070993, -0.006039, -0.021500 + , 0.066107, -0.003549, -0.021500 + , 0.064954, 0.000000, -0.021500 + , 0.065249, -0.001865, -0.021500 + , 0.066107, 0.003549, -0.021500 + , 0.065249, 0.001865, -0.021500 + , 0.067443, 0.004886, -0.021500 + , 0.070993, 0.006039, -0.021500 + , 0.069128, 0.005744, -0.021500 + , 0.074542, 0.004886, -0.021500 + , 0.072858, 0.005744, -0.021500 + , 0.075879, 0.003549, -0.021500 + , 0.077032, 0.000000, -0.021500 + , 0.076737, 0.001865, -0.021500 + , 0.075879, -0.003549, -0.021500 + , 0.076737, -0.001865, -0.021500 + , 0.074542, -0.004886, -0.021500 + , 0.072858, -0.005744, -0.021500 + , 0.067443, -0.004886, -0.020078 + , 0.069128, -0.005744, -0.020078 + , 0.070993, -0.006039, -0.020078 + , 0.066107, -0.003549, -0.020078 + , 0.064954, 0.000000, -0.020078 + , 0.065249, -0.001865, -0.020078 + , 0.066107, 0.003549, -0.020078 + , 0.065249, 0.001865, -0.020078 + , 0.067443, 0.004886, -0.020078 + , 0.070993, 0.006039, -0.020078 + , 0.069128, 0.005744, -0.020078 + , 0.074542, 0.004886, -0.020078 + , 0.072858, 0.005744, -0.020078 + , 0.075879, 0.003549, -0.020078 + , 0.077032, 0.000000, -0.020078 + , 0.076737, 0.001865, -0.020078 + , 0.075879, -0.003549, -0.020078 + , 0.076737, -0.001865, -0.020078 + , 0.074542, -0.004886, -0.020078 + , 0.072858, -0.005744, -0.020078 + , -0.026847, -0.013300, 0.006300 + , 0.088353, -0.013310, 0.006300 + , 0.002815, -0.013333, 0.002250 + , -0.026847, 0.013300, 0.006300 + , 0.002815, 0.013300, 0.002250 + , 0.058800, 0.013300, 0.002250 + , 0.088353, 0.013300, 0.006300 + , -0.026847, -0.013300, 0.002250 + , -0.026847, -0.013300, 0.000500 + , -0.027959, -0.013175, 0.006300 + , -0.029016, -0.012805, 0.006300 + , -0.026847, 0.013300, 0.000500 + , 0.000151, 0.013300, 0.000563 + , 0.000830, 0.013300, 0.001351 + , -0.029965, -0.012209, 0.006300 + , 0.001761, 0.013300, 0.001999 + , -0.030756, -0.011417, 0.006300 + , -0.031352, -0.010469, 0.006300 + , -0.031722, -0.009412, 0.006300 + , -0.031847, -0.008300, 0.006300 + , -0.026847, 0.013300, 0.002250 + , -0.031847, 0.008300, 0.006300 + , -0.027959, 0.013175, 0.006300 + , -0.031722, 0.009412, 0.006300 + , -0.031352, 0.010469, 0.006300 + , -0.029016, 0.012805, 0.006300 + , -0.030756, 0.011417, 0.006300 + , -0.029965, 0.012209, 0.006300 + , -0.027959, -0.013175, 0.000500 + , -0.027959, -0.013175, 0.002250 + , -0.029016, -0.012805, 0.000500 + , -0.029016, -0.012805, 0.002250 + , -0.029965, -0.012209, 0.000500 + , -0.029965, -0.012209, 0.002250 + , -0.030756, -0.011417, 0.000500 + , -0.030756, -0.011417, 0.002250 + , -0.031352, -0.010469, 0.000500 + , -0.031352, -0.010469, 0.002250 + , -0.031722, -0.009412, 0.000500 + , -0.031722, -0.009412, 0.002250 + , -0.031847, -0.006364, 0.000500 + , -0.031809, -0.004156, 0.002299 + , -0.031847, 0.006354, 0.000500 + , -0.031809, 0.004146, 0.002299 + , -0.031722, 0.009412, 0.000500 + , -0.031722, 0.009412, 0.002250 + , -0.027959, 0.013175, 0.002250 + , -0.027959, 0.013175, 0.000500 + , -0.031352, 0.010469, 0.000500 + , -0.031352, 0.010469, 0.002250 + , -0.029016, 0.012805, 0.002250 + , -0.029016, 0.012805, 0.000500 + , -0.030756, 0.011417, 0.000500 + , -0.030756, 0.011417, 0.002250 + , -0.029965, 0.012209, 0.002250 + , -0.029965, 0.012209, 0.000500 + , 0.059839, 0.013300, 0.001999 + , 0.060770, 0.013300, 0.001351 + , 0.088353, 0.013300, 0.002250 + , 0.058785, -0.013333, 0.002250 + , 0.089464, 0.013175, 0.006300 + , 0.090522, 0.012805, 0.006300 + , 0.091470, 0.012209, 0.006300 + , 0.092262, 0.011417, 0.006300 + , 0.092858, 0.010469, 0.006300 + , 0.093228, 0.009412, 0.006300 + , 0.093353, 0.008300, 0.006300 + , 0.088353, -0.013310, 0.002250 + , 0.059839, -0.013333, 0.001999 + , 0.060770, -0.013333, 0.001351 + , 0.061448, 0.013300, 0.000563 + , 0.088353, 0.013300, 0.000500 + , 0.088353, -0.013310, 0.000500 + , 0.061448, -0.013333, 0.000563 + , 0.093353, -0.008310, 0.006300 + , 0.089464, -0.013194, 0.006300 + , 0.093228, -0.009431, 0.006300 + , 0.092858, -0.010488, 0.006300 + , 0.090522, -0.012824, 0.006300 + , 0.092262, -0.011437, 0.006300 + , 0.091470, -0.012228, 0.006300 + , 0.089464, 0.013175, 0.002250 + , 0.090522, 0.012805, 0.002250 + , 0.091470, 0.012209, 0.002250 + , 0.089464, 0.013175, 0.000500 + , 0.090522, 0.012805, 0.000500 + , 0.092262, 0.011417, 0.002250 + , 0.091470, 0.012209, 0.000500 + , 0.092858, 0.010469, 0.002250 + , 0.092262, 0.011417, 0.000500 + , 0.093228, 0.009412, 0.002250 + , 0.093353, 0.008300, 0.002250 + , 0.092858, 0.010469, 0.000500 + , 0.093228, 0.009412, 0.000500 + , 0.093353, 0.008300, 0.000500 + , 0.093353, -0.008310, 0.002250 + , 0.093353, -0.008310, 0.000500 + , 0.093228, -0.009431, 0.002250 + , 0.089464, -0.013194, 0.002250 + , 0.092858, -0.010488, 0.002250 + , 0.090522, -0.012824, 0.002250 + , 0.092262, -0.011437, 0.002250 + , 0.091470, -0.012228, 0.002250 + , 0.093228, -0.009431, 0.000500 + , 0.089464, -0.013194, 0.000500 + , 0.092858, -0.010488, 0.000500 + , 0.090522, -0.012824, 0.000500 + , 0.092262, -0.011437, 0.000500 + , 0.091470, -0.012228, 0.000500 + , 0.000152, -0.013333, 0.000564 + , 0.000830, -0.013333, 0.001352 + , 0.001761, -0.013333, 0.002000 + , -0.026591, -0.013201, 0.006500 + , 0.088097, -0.013211, 0.006500 + , -0.026591, 0.013201, 0.006500 + , 0.088097, 0.013201, 0.006500 + , 0.088477, 0.011982, 0.006500 + , -0.026591, 0.011982, 0.006500 + , -0.027698, -0.013077, 0.006500 + , -0.028751, -0.012710, 0.006500 + , -0.029695, -0.012119, 0.006500 + , -0.030483, -0.011333, 0.006500 + , -0.031076, -0.010391, 0.006500 + , -0.031445, -0.009342, 0.006500 + , -0.031569, -0.008238, 0.006500 + , -0.027698, 0.013077, 0.006500 + , -0.031445, 0.009342, 0.006500 + , -0.031569, 0.008238, 0.006500 + , -0.028751, 0.012710, 0.006500 + , -0.031076, 0.010391, 0.006500 + , -0.029695, 0.012118, 0.006500 + , -0.030483, 0.011332, 0.006500 + , -0.027703, -0.011897, 0.006500 + , -0.026192, -0.011982, 0.006500 + , -0.028399, -0.011645, 0.006500 + , -0.029024, -0.011241, 0.006500 + , -0.029545, -0.010703, 0.006500 + , -0.029937, -0.010058, 0.006500 + , -0.030180, -0.009340, 0.006500 + , -0.030263, -0.008585, 0.006500 + , -0.030263, 0.008585, 0.006500 + , -0.030180, 0.009340, 0.006500 + , -0.027703, 0.011897, 0.006500 + , -0.029937, 0.010059, 0.006500 + , -0.028399, 0.011646, 0.006500 + , -0.029545, 0.010703, 0.006500 + , -0.029024, 0.011241, 0.006500 + , 0.088477, -0.011992, 0.006500 + , 0.089204, 0.013077, 0.006500 + , 0.090256, 0.012710, 0.006500 + , 0.091200, 0.012118, 0.006500 + , 0.091989, 0.011332, 0.006500 + , 0.092582, 0.010391, 0.006500 + , 0.092950, 0.009342, 0.006500 + , 0.093075, 0.008238, 0.006500 + , 0.089204, -0.013096, 0.006500 + , 0.092950, -0.009361, 0.006500 + , 0.093075, -0.008248, 0.006500 + , 0.090256, -0.012729, 0.006500 + , 0.092582, -0.010410, 0.006500 + , 0.091200, -0.012138, 0.006500 + , 0.091989, -0.011352, 0.006500 + , 0.089209, 0.011897, 0.006500 + , 0.089905, 0.011646, 0.006500 + , 0.090529, 0.011241, 0.006500 + , 0.091050, 0.010703, 0.006500 + , 0.091443, 0.010059, 0.006500 + , 0.091686, 0.009340, 0.006500 + , 0.091768, 0.008585, 0.006500 + , 0.091768, -0.008595, 0.006500 + , 0.091686, -0.009360, 0.006500 + , 0.089209, -0.011916, 0.006500 + , 0.091443, -0.010078, 0.006500 + , 0.089905, -0.011665, 0.006500 + , 0.091050, -0.010723, 0.006500 + , 0.090529, -0.011260, 0.006500 + , -0.031809, -0.003322, 0.002497 + , -0.031809, 0.003313, 0.002497 + , -0.031809, 0.005707, 0.000701 + , -0.031809, 0.005128, 0.001424 + , -0.031809, -0.005138, 0.001424 + , -0.031809, -0.005716, 0.000701 + , 0.070993, -0.000000, -0.021500 + , 0.076551, 0.004038, -0.021500 + , 0.075030, 0.005558, -0.021500 + , 0.073114, 0.006534, -0.021500 + , 0.077863, 0.000000, -0.021500 + , 0.077527, 0.002121, -0.021500 + , 0.065435, -0.004038, -0.021500 + , 0.066955, -0.005558, -0.021500 + , 0.068872, -0.006534, -0.021500 + , 0.076551, -0.004038, -0.021500 + , 0.077527, -0.002121, -0.021500 + , 0.065435, 0.004038, -0.021500 + , 0.064459, 0.002121, -0.021500 + , 0.064123, 0.000000, -0.021500 + , 0.068872, 0.006534, -0.021500 + , 0.066955, 0.005558, -0.021500 + , 0.070993, 0.006870, -0.021500 + , 0.073114, -0.006534, -0.021500 + , 0.075030, -0.005558, -0.021500 + , 0.070993, -0.006870, -0.021500 + , 0.064459, -0.002121, -0.021500 + , 0.065353, -0.007764, -0.018000 + , 0.068030, -0.009127, -0.018000 + , 0.068030, -0.009127, -0.019500 + , 0.065353, -0.007764, -0.019500 + , 0.070993, -0.009596, -0.018000 + , 0.070993, -0.009596, -0.019500 + , 0.063229, -0.005640, -0.018000 + , 0.063229, -0.005640, -0.019500 + , 0.061397, 0.000000, -0.018000 + , 0.061866, -0.002963, -0.018000 + , 0.061866, -0.002963, -0.019500 + , 0.061397, 0.000000, -0.019500 + , 0.063229, 0.005640, -0.018000 + , 0.061866, 0.002963, -0.018000 + , 0.061866, 0.002963, -0.019500 + , 0.063229, 0.005640, -0.019500 + , 0.065353, 0.007764, -0.018000 + , 0.065353, 0.007764, -0.019500 + , 0.070993, 0.009596, -0.018000 + , 0.068030, 0.009127, -0.018000 + , 0.068030, 0.009127, -0.019500 + , 0.070993, 0.009596, -0.019500 + , 0.076633, 0.007764, -0.018000 + , 0.073956, 0.009127, -0.018000 + , 0.073956, 0.009127, -0.019500 + , 0.076633, 0.007764, -0.019500 + , 0.078757, 0.005640, -0.018000 + , 0.078757, 0.005640, -0.019500 + , 0.080589, 0.000000, -0.018000 + , 0.080120, 0.002963, -0.018000 + , 0.080120, 0.002963, -0.019500 + , 0.080589, 0.000000, -0.019500 + , 0.078757, -0.005640, -0.018000 + , 0.080120, -0.002963, -0.018000 + , 0.080120, -0.002963, -0.019500 + , 0.078757, -0.005640, -0.019500 + , 0.076633, -0.007764, -0.018000 + , 0.076633, -0.007764, -0.019500 + , 0.073956, -0.009127, -0.018000 + , 0.073956, -0.009127, -0.019500 + , 0.002356, -0.007764, -0.018000 + , 0.005033, -0.009127, -0.018000 + , 0.005033, -0.009127, -0.019500 + , 0.002356, -0.007764, -0.019500 + , 0.007996, -0.009596, -0.018000 + , 0.007996, -0.009596, -0.019500 + , 0.000232, -0.005640, -0.018000 + , 0.000232, -0.005640, -0.019500 + , -0.001600, 0.000000, -0.018000 + , -0.001131, -0.002963, -0.018000 + , -0.001131, -0.002963, -0.019500 + , -0.001600, 0.000000, -0.019500 + , 0.000232, 0.005640, -0.018000 + , -0.001131, 0.002963, -0.018000 + , -0.001131, 0.002963, -0.019500 + , 0.000232, 0.005640, -0.019500 + , 0.002356, 0.007764, -0.018000 + , 0.002356, 0.007764, -0.019500 + , 0.007996, 0.009596, -0.018000 + , 0.005033, 0.009127, -0.018000 + , 0.005033, 0.009127, -0.019500 + , 0.007996, 0.009596, -0.019500 + , 0.013636, 0.007764, -0.018000 + , 0.010959, 0.009127, -0.018000 + , 0.010959, 0.009127, -0.019500 + , 0.013636, 0.007764, -0.019500 + , 0.015760, 0.005640, -0.018000 + , 0.015760, 0.005640, -0.019500 + , 0.017592, 0.000000, -0.018000 + , 0.017123, 0.002963, -0.018000 + , 0.017123, 0.002963, -0.019500 + , 0.017592, 0.000000, -0.019500 + , 0.015760, -0.005640, -0.018000 + , 0.017123, -0.002963, -0.018000 + , 0.017123, -0.002963, -0.019500 + , 0.015760, -0.005640, -0.019500 + , 0.013636, -0.007764, -0.018000 + , 0.013636, -0.007764, -0.019500 + , 0.010959, -0.009127, -0.018000 + , 0.010959, -0.009127, -0.019500 + , 0.007996, -0.000000, -0.021500 + , 0.013554, 0.004038, -0.021500 + , 0.012033, 0.005558, -0.021500 + , 0.010117, 0.006534, -0.021500 + , 0.014866, 0.000000, -0.021500 + , 0.014530, 0.002121, -0.021500 + , 0.002438, -0.004038, -0.021500 + , 0.003958, -0.005558, -0.021500 + , 0.005875, -0.006534, -0.021500 + , 0.013554, -0.004038, -0.021500 + , 0.014530, -0.002121, -0.021500 + , 0.002438, 0.004038, -0.021500 + , 0.001462, 0.002121, -0.021500 + , 0.001126, 0.000000, -0.021500 + , 0.005875, 0.006534, -0.021500 + , 0.003958, 0.005558, -0.021500 + , 0.007996, 0.006870, -0.021500 + , 0.010117, -0.006534, -0.021500 + , 0.012033, -0.005558, -0.021500 + , 0.007996, -0.006870, -0.021500 + , 0.001462, -0.002121, -0.021500 + , 0.090529, -0.008928, 0.006500 + , 0.089905, -0.009249, 0.006500 + , 0.089209, -0.009448, 0.006500 + , 0.088477, -0.009506, 0.006500 + , -0.026234, -0.009497, 0.006500 + , -0.027703, -0.009430, 0.006500 + , -0.028399, -0.009231, 0.006500 + , -0.029024, -0.008910, 0.006500 + , 0.090529, -0.009792, 0.006500 + , 0.089905, -0.010144, 0.006500 + , 0.089209, -0.010362, 0.006500 + , 0.088477, -0.010427, 0.006500 + , -0.026219, -0.010417, 0.006500 + , -0.027703, -0.010344, 0.006500 + , -0.028399, -0.010125, 0.006500 + , -0.029024, -0.009773, 0.006500 + , 0.090529, 0.008867, 0.006500 + , 0.089905, 0.009187, 0.006500 + , 0.089209, 0.009385, 0.006500 + , 0.088477, 0.009453, 0.006500 + , -0.026549, 0.009454, 0.006500 + , -0.027703, 0.009387, 0.006500 + , -0.028399, 0.009189, 0.006500 + , -0.029024, 0.008869, 0.006500 + , 0.090529, 0.009705, 0.006500 + , 0.089905, 0.010055, 0.006500 + , 0.089209, 0.010272, 0.006500 + , 0.088477, 0.010346, 0.006500 + , -0.026564, 0.010347, 0.006500 + , -0.027703, 0.010273, 0.006500 + , -0.028399, 0.010056, 0.006500 + , -0.029024, 0.009707, 0.006500 + , 0.090529, -0.010555, 0.006500 + , 0.089905, -0.010935, 0.006500 + , 0.089209, -0.011170, 0.006500 + , 0.088477, -0.011240, 0.006500 + , -0.026205, -0.011231, 0.006500 + , -0.027703, -0.011151, 0.006500 + , -0.028399, -0.010915, 0.006500 + , -0.029024, -0.010536, 0.006500 + , 0.090529, 0.010411, 0.006500 + , 0.089905, 0.010786, 0.006500 + , 0.089209, 0.011019, 0.006500 + , 0.088477, 0.011098, 0.006500 + , -0.026577, 0.011099, 0.006500 + , -0.027703, 0.011020, 0.006500 + , -0.028399, 0.010787, 0.006500 + , -0.029024, 0.010412, 0.006500 +]) + +NB_AL_ZEDM_TRI = 125 +al_triangles_m = np.array([ + 2, 1, 8 + , 1, 2, 4 + , 1, 4, 5 + , 1, 5, 14 + , 1, 14, 15 + , 1, 6, 3 + , 1, 7, 6 + , 1, 15, 7 + , 1, 10, 8 + , 1, 11, 10 + , 1, 17, 11 + , 1, 16, 17 + , 1, 9, 12 + , 1, 12, 13 + , 1, 13, 16 + , 3, 9, 1 + , 3, 9, 1 + , 18, 21, 19 + , 18, 22, 21 + , 18, 32, 22 + , 18, 31, 32 + , 18, 31, 32 + , 18, 20, 23 + , 18, 23, 24 + , 18, 24, 31 + , 19, 25, 18 + , 20, 18, 26 + , 18, 25, 27 + , 18, 27, 28 + , 18, 28, 33 + , 18, 33, 34 + , 18, 33, 34 + , 18, 29, 26 + , 18, 30, 29 + , 18, 34, 30 + , 51, 35, 52 + , 52, 35, 36 + , 53, 37, 54 + , 54, 37, 38 + , 55, 39, 56 + , 56, 39, 40 + , 56, 40, 53 + , 53, 40, 37 + , 57, 41, 58 + , 58, 41, 42 + , 54, 38, 57 + , 57, 38, 41 + , 58, 42, 51 + , 51, 42, 35 + , 59, 43, 60 + , 60, 43, 44 + , 61, 46, 62 + , 62, 46, 45 + , 48, 47, 63 + , 63, 47, 64 + , 62, 45, 63 + , 63, 45, 48 + , 65, 50, 66 + , 66, 50, 49 + , 66, 49, 61 + , 61, 49, 46 + , 60, 44, 65 + , 65, 44, 50 + , 52, 36, 67 + , 67, 36, 68 + , 43, 59, 70 + , 70, 59, 69 + , 67, 68, 71 + , 71, 68, 72 + , 70, 69, 74 + , 74, 69, 73 + , 71, 72, 87 + , 74, 73, 87 + , 71, 87, 75 + , 87, 72, 76 + , 74, 87, 78 + , 87, 73, 77 + , 75, 87, 79 + , 87, 76, 80 + , 87, 77, 81 + , 78, 87, 82 + , 79, 87, 83 + , 87, 80, 84 + , 87, 81, 85 + , 82, 87, 86 + , 83, 87, 85 + , 87, 84, 86 + , 88, 17, 89 + , 89, 17, 16 + , 90, 91, 14 + , 14, 91, 15 + , 107, 108, 92 + , 92, 108, 93 + , 109, 110, 94 + , 94, 110, 95 + , 90, 111, 91 + , 91, 111, 96 + , 111, 109, 96 + , 96, 109, 94 + , 112, 113, 97 + , 97, 113, 98 + , 110, 112, 95 + , 95, 112, 97 + , 113, 107, 98 + , 98, 107, 92 + , 108, 114, 93 + , 93, 114, 99 + , 114, 115, 99 + , 99, 115, 100 + , 116, 117, 102 + , 102, 117, 101 + , 118, 119, 104 + , 104, 119, 103 + , 117, 118, 101 + , 101, 118, 104 + , 120, 121, 106 + , 106, 121, 105 + , 121, 116, 105 + , 105, 116, 102 + , 115, 120, 100 + , 100, 120, 106 + , 64, 47, 33 + , 33, 47, 34 + , 119, 32, 103 + , 103, 32, 31 +]) + +NB_DARK_ZEDM_TRI = 1268 +dark_triangles_m = np.array([ + 126, 144, 127 + , 127, 144, 143 + , 128, 136, 122 + , 122, 136, 135 + , 129, 132, 130 + , 130, 132, 131 + , 132, 125, 131 + , 131, 125, 124 + , 133, 129, 134 + , 134, 129, 130 + , 136, 133, 135 + , 135, 133, 134 + , 137, 140, 138 + , 138, 140, 139 + , 140, 128, 139 + , 139, 128, 122 + , 141, 137, 142 + , 142, 137, 138 + , 144, 141, 143 + , 143, 141, 142 + , 124, 125, 160 + , 160, 125, 161 + , 145, 153, 123 + , 123, 153, 152 + , 146, 149, 147 + , 147, 149, 148 + , 149, 126, 148 + , 148, 126, 127 + , 150, 146, 151 + , 151, 146, 147 + , 153, 150, 152 + , 152, 150, 151 + , 154, 157, 155 + , 155, 157, 156 + , 157, 145, 156 + , 156, 145, 123 + , 158, 154, 159 + , 159, 154, 155 + , 161, 158, 160 + , 160, 158, 159 + , 126, 163, 144 + , 144, 163, 162 + , 128, 165, 136 + , 136, 165, 164 + , 129, 167, 132 + , 132, 167, 166 + , 132, 166, 125 + , 125, 166, 168 + , 133, 169, 129 + , 129, 169, 167 + , 136, 164, 133 + , 133, 164, 169 + , 137, 171, 140 + , 140, 171, 170 + , 140, 170, 128 + , 128, 170, 165 + , 141, 172, 137 + , 137, 172, 171 + , 144, 162, 141 + , 141, 162, 172 + , 125, 168, 161 + , 161, 168, 173 + , 145, 175, 153 + , 153, 175, 174 + , 146, 177, 149 + , 149, 177, 176 + , 149, 176, 126 + , 126, 176, 163 + , 150, 178, 146 + , 146, 178, 177 + , 153, 174, 150 + , 150, 174, 178 + , 154, 180, 157 + , 157, 180, 179 + , 157, 179, 145 + , 145, 179, 175 + , 158, 181, 154 + , 154, 181, 180 + , 161, 173, 158 + , 158, 173, 181 + , 182, 183, 202 + , 202, 183, 203 + , 183, 184, 203 + , 203, 184, 204 + , 185, 182, 205 + , 205, 182, 202 + , 186, 187, 206 + , 206, 187, 207 + , 187, 185, 207 + , 207, 185, 205 + , 188, 189, 208 + , 208, 189, 209 + , 189, 186, 209 + , 209, 186, 206 + , 190, 188, 210 + , 210, 188, 208 + , 191, 192, 211 + , 211, 192, 212 + , 192, 190, 212 + , 212, 190, 210 + , 193, 194, 213 + , 213, 194, 214 + , 194, 191, 214 + , 214, 191, 211 + , 195, 193, 215 + , 215, 193, 213 + , 196, 197, 216 + , 216, 197, 217 + , 197, 195, 217 + , 217, 195, 215 + , 198, 199, 218 + , 218, 199, 219 + , 199, 196, 219 + , 219, 196, 216 + , 200, 198, 220 + , 220, 198, 218 + , 184, 201, 204 + , 204, 201, 221 + , 201, 200, 221 + , 221, 200, 220 + , 202, 203, 222 + , 222, 203, 223 + , 203, 204, 223 + , 223, 204, 224 + , 205, 202, 225 + , 225, 202, 222 + , 206, 207, 226 + , 226, 207, 227 + , 207, 205, 227 + , 227, 205, 225 + , 208, 209, 228 + , 228, 209, 229 + , 209, 206, 229 + , 229, 206, 226 + , 210, 208, 230 + , 230, 208, 228 + , 211, 212, 231 + , 231, 212, 232 + , 212, 210, 232 + , 232, 210, 230 + , 213, 214, 233 + , 233, 214, 234 + , 214, 211, 234 + , 234, 211, 231 + , 215, 213, 235 + , 235, 213, 233 + , 216, 217, 236 + , 236, 217, 237 + , 217, 215, 237 + , 237, 215, 235 + , 218, 219, 238 + , 238, 219, 239 + , 219, 216, 239 + , 239, 216, 236 + , 220, 218, 240 + , 240, 218, 238 + , 204, 221, 224 + , 224, 221, 241 + , 221, 220, 241 + , 241, 220, 240 + , 223, 243, 222 + , 222, 243, 242 + , 224, 244, 223 + , 223, 244, 243 + , 222, 242, 225 + , 225, 242, 245 + , 227, 247, 226 + , 226, 247, 246 + , 225, 245, 227 + , 227, 245, 247 + , 229, 249, 228 + , 228, 249, 248 + , 226, 246, 229 + , 229, 246, 249 + , 228, 248, 230 + , 230, 248, 250 + , 232, 252, 231 + , 231, 252, 251 + , 230, 250, 232 + , 232, 250, 252 + , 234, 254, 233 + , 233, 254, 253 + , 231, 251, 234 + , 234, 251, 254 + , 233, 253, 235 + , 235, 253, 255 + , 237, 257, 236 + , 236, 257, 256 + , 235, 255, 237 + , 237, 255, 257 + , 239, 259, 238 + , 238, 259, 258 + , 236, 256, 239 + , 239, 256, 259 + , 238, 258, 240 + , 240, 258, 260 + , 241, 261, 224 + , 224, 261, 244 + , 240, 260, 241 + , 241, 260, 261 + , 263, 262, 243 + , 243, 262, 242 + , 264, 263, 244 + , 244, 263, 243 + , 242, 262, 245 + , 245, 262, 265 + , 267, 266, 247 + , 247, 266, 246 + , 265, 267, 245 + , 245, 267, 247 + , 269, 268, 249 + , 249, 268, 248 + , 266, 269, 246 + , 246, 269, 249 + , 268, 270, 248 + , 248, 270, 250 + , 272, 271, 252 + , 252, 271, 251 + , 270, 272, 250 + , 250, 272, 252 + , 274, 273, 254 + , 254, 273, 253 + , 271, 274, 251 + , 251, 274, 254 + , 273, 275, 253 + , 253, 275, 255 + , 277, 276, 257 + , 257, 276, 256 + , 275, 277, 255 + , 255, 277, 257 + , 279, 278, 259 + , 259, 278, 258 + , 276, 279, 256 + , 256, 279, 259 + , 278, 280, 258 + , 258, 280, 260 + , 281, 264, 261 + , 261, 264, 244 + , 280, 281, 260 + , 260, 281, 261 + , 262, 263, 282 + , 282, 263, 283 + , 263, 264, 283 + , 283, 264, 284 + , 265, 262, 285 + , 285, 262, 282 + , 266, 267, 286 + , 286, 267, 287 + , 267, 265, 287 + , 287, 265, 285 + , 268, 269, 288 + , 288, 269, 289 + , 269, 266, 289 + , 289, 266, 286 + , 270, 268, 290 + , 290, 268, 288 + , 271, 272, 291 + , 291, 272, 292 + , 272, 270, 292 + , 292, 270, 290 + , 273, 274, 293 + , 293, 274, 294 + , 274, 271, 294 + , 294, 271, 291 + , 275, 273, 295 + , 295, 273, 293 + , 276, 277, 296 + , 296, 277, 297 + , 277, 275, 297 + , 297, 275, 295 + , 278, 279, 298 + , 298, 279, 299 + , 279, 276, 299 + , 299, 276, 296 + , 280, 278, 300 + , 300, 278, 298 + , 264, 281, 284 + , 284, 281, 301 + , 281, 280, 301 + , 301, 280, 300 + , 320, 461, 327 + , 461, 460, 327 + , 460, 477, 327 + , 477, 476, 327 + , 309, 303, 334 + , 334, 303, 335 + , 306, 304, 336 + , 336, 304, 337 + , 302, 305, 338 + , 338, 305, 339 + , 305, 306, 339 + , 339, 306, 336 + , 308, 307, 340 + , 340, 307, 341 + , 304, 308, 337 + , 337, 308, 340 + , 307, 309, 341 + , 341, 309, 334 + , 366, 302, 338 + , 303, 478, 335 + , 318, 312, 344 + , 344, 312, 345 + , 315, 313, 346 + , 346, 313, 347 + , 314, 348, 311 + , 311, 348, 343 + , 314, 315, 348 + , 348, 315, 346 + , 317, 316, 349 + , 349, 316, 350 + , 313, 317, 347 + , 347, 317, 349 + , 316, 318, 350 + , 350, 318, 344 + , 312, 367, 345 + , 326, 320, 352 + , 352, 320, 353 + , 323, 321, 354 + , 354, 321, 355 + , 319, 322, 351 + , 351, 322, 356 + , 322, 323, 356 + , 356, 323, 354 + , 325, 324, 357 + , 357, 324, 358 + , 321, 325, 355 + , 355, 325, 357 + , 324, 326, 358 + , 358, 326, 352 + , 320, 327, 353 + , 353, 327, 359 + , 327, 333, 359 + , 359, 333, 360 + , 328, 330, 362 + , 362, 330, 361 + , 329, 310, 363 + , 363, 310, 342 + , 330, 329, 361 + , 361, 329, 363 + , 331, 332, 365 + , 365, 332, 364 + , 332, 328, 364 + , 364, 328, 362 + , 333, 331, 360 + , 360, 331, 365 + , 368, 310, 366 + , 366, 310, 302 + , 367, 312, 407 + , 310, 368, 342 + , 369, 319, 351 + , 370, 482, 371 + , 336, 337, 372 + , 372, 337, 373 + , 338, 339, 374 + , 374, 339, 375 + , 339, 336, 375 + , 375, 336, 372 + , 340, 341, 376 + , 376, 341, 377 + , 337, 340, 373 + , 373, 340, 376 + , 341, 334, 377 + , 377, 334, 370 + , 366, 338, 413 + , 413, 338, 412 + , 412, 338, 378 + , 338, 374, 378 + , 344, 345, 380 + , 380, 345, 381 + , 346, 347, 382 + , 382, 347, 383 + , 481, 348, 480 + , 480, 348, 384 + , 348, 346, 384 + , 384, 346, 382 + , 349, 350, 385 + , 385, 350, 386 + , 347, 349, 383 + , 383, 349, 385 + , 350, 344, 386 + , 386, 344, 380 + , 409, 351, 388 + , 388, 351, 387 + , 352, 353, 389 + , 389, 353, 390 + , 354, 355, 391 + , 391, 355, 392 + , 351, 356, 387 + , 387, 356, 393 + , 356, 354, 393 + , 393, 354, 391 + , 357, 358, 394 + , 394, 358, 395 + , 355, 357, 392 + , 392, 357, 394 + , 358, 352, 395 + , 395, 352, 389 + , 353, 359, 390 + , 390, 359, 396 + , 359, 360, 396 + , 396, 360, 397 + , 362, 361, 399 + , 399, 361, 398 + , 363, 342, 401 + , 401, 342, 400 + , 361, 363, 398 + , 398, 363, 401 + , 365, 364, 403 + , 403, 364, 402 + , 364, 362, 402 + , 402, 362, 399 + , 360, 365, 397 + , 397, 365, 403 + , 367, 408, 345 + , 345, 408, 381 + , 342, 368, 411 + , 406, 319, 369 + , 312, 319, 407 + , 407, 319, 406 + , 369, 351, 409 + , 381, 408, 404 + , 400, 410, 405 + , 411, 410, 342 + , 342, 410, 400 + , 414, 415, 463 + , 463, 415, 462 + , 414, 417, 415 + , 415, 417, 416 + , 416, 417, 418 + , 418, 417, 419 + , 418, 419, 420 + , 420, 419, 421 + , 420, 421, 422 + , 422, 421, 423 + , 422, 423, 424 + , 424, 423, 425 + , 424, 425, 426 + , 426, 425, 427 + , 426, 427, 428 + , 428, 427, 429 + , 428, 429, 430 + , 430, 429, 431 + , 431, 433, 430 + , 430, 433, 432 + , 432, 433, 434 + , 434, 433, 435 + , 434, 435, 436 + , 436, 435, 437 + , 436, 437, 438 + , 438, 437, 439 + , 438, 439, 440 + , 440, 439, 441 + , 440, 441, 442 + , 442, 441, 443 + , 442, 443, 444 + , 444, 443, 445 + , 444, 445, 447 + , 447, 445, 446 + , 446, 449, 447 + , 447, 449, 448 + , 448, 449, 450 + , 450, 449, 451 + , 450, 451, 452 + , 452, 451, 453 + , 452, 453, 454 + , 454, 453, 455 + , 454, 455, 456 + , 456, 455, 457 + , 456, 457, 458 + , 458, 457, 459 + , 458, 459, 461 + , 461, 459, 460 + , 463, 462, 465 + , 465, 462, 464 + , 464, 466, 465 + , 465, 466, 467 + , 467, 466, 469 + , 469, 466, 468 + , 469, 468, 471 + , 471, 468, 470 + , 471, 470, 473 + , 473, 470, 472 + , 473, 472, 475 + , 475, 472, 474 + , 474, 476, 475 + , 475, 476, 477 + , 303, 309, 428 + , 428, 309, 426 + , 304, 306, 420 + , 420, 306, 418 + , 305, 302, 416 + , 416, 302, 415 + , 306, 305, 418 + , 418, 305, 416 + , 307, 308, 424 + , 424, 308, 422 + , 308, 304, 422 + , 422, 304, 420 + , 309, 307, 426 + , 426, 307, 424 + , 428, 430, 303 + , 303, 430, 311 + , 312, 318, 444 + , 444, 318, 442 + , 313, 315, 436 + , 436, 315, 434 + , 314, 311, 432 + , 432, 311, 430 + , 315, 314, 434 + , 434, 314, 432 + , 316, 317, 440 + , 440, 317, 438 + , 317, 313, 438 + , 438, 313, 436 + , 318, 316, 442 + , 442, 316, 440 + , 320, 326, 461 + , 461, 326, 458 + , 321, 323, 452 + , 452, 323, 450 + , 322, 319, 448 + , 448, 319, 447 + , 323, 322, 450 + , 450, 322, 448 + , 324, 325, 456 + , 456, 325, 454 + , 325, 321, 454 + , 454, 321, 452 + , 326, 324, 458 + , 458, 324, 456 + , 462, 310, 464 + , 464, 310, 329 + , 466, 464, 330 + , 330, 464, 329 + , 468, 466, 328 + , 328, 466, 330 + , 470, 468, 332 + , 332, 468, 328 + , 472, 470, 331 + , 331, 470, 332 + , 474, 472, 333 + , 333, 472, 331 + , 327, 476, 333 + , 333, 476, 474 + , 427, 425, 433 + , 433, 425, 435 + , 425, 423, 435 + , 435, 423, 437 + , 423, 421, 437 + , 437, 421, 439 + , 421, 419, 439 + , 439, 419, 441 + , 419, 417, 441 + , 441, 417, 443 + , 417, 414, 443 + , 443, 414, 445 + , 463, 465, 446 + , 446, 465, 449 + , 465, 467, 449 + , 449, 467, 451 + , 467, 469, 451 + , 451, 469, 453 + , 469, 471, 453 + , 453, 471, 455 + , 471, 473, 455 + , 455, 473, 457 + , 473, 475, 457 + , 457, 475, 459 + , 475, 477, 459 + , 459, 477, 460 + , 431, 429, 433 + , 433, 429, 427 + , 415, 302, 462 + , 462, 302, 310 + , 463, 446, 414 + , 414, 446, 445 + , 312, 444, 319 + , 319, 444, 447 + , 311, 479, 303 + , 303, 479, 478 + , 479, 311, 343 + , 480, 384, 379 + , 343, 348, 481 + , 483, 482, 334 + , 334, 482, 370 + , 334, 335, 483 + , 488, 506, 489 + , 489, 506, 505 + , 490, 498, 484 + , 484, 498, 497 + , 491, 494, 492 + , 492, 494, 493 + , 494, 487, 493 + , 493, 487, 486 + , 495, 491, 496 + , 496, 491, 492 + , 498, 495, 497 + , 497, 495, 496 + , 499, 502, 500 + , 500, 502, 501 + , 502, 490, 501 + , 501, 490, 484 + , 503, 499, 504 + , 504, 499, 500 + , 506, 503, 505 + , 505, 503, 504 + , 486, 487, 522 + , 522, 487, 523 + , 507, 515, 485 + , 485, 515, 514 + , 508, 511, 509 + , 509, 511, 510 + , 511, 488, 510 + , 510, 488, 489 + , 512, 508, 513 + , 513, 508, 509 + , 515, 512, 514 + , 514, 512, 513 + , 516, 519, 517 + , 517, 519, 518 + , 519, 507, 518 + , 518, 507, 485 + , 520, 516, 521 + , 521, 516, 517 + , 523, 520, 522 + , 522, 520, 521 + , 488, 525, 506 + , 506, 525, 524 + , 490, 527, 498 + , 498, 527, 526 + , 491, 529, 494 + , 494, 529, 528 + , 494, 528, 487 + , 487, 528, 530 + , 495, 531, 491 + , 491, 531, 529 + , 498, 526, 495 + , 495, 526, 531 + , 499, 533, 502 + , 502, 533, 532 + , 502, 532, 490 + , 490, 532, 527 + , 503, 534, 499 + , 499, 534, 533 + , 506, 524, 503 + , 503, 524, 534 + , 487, 530, 523 + , 523, 530, 535 + , 507, 537, 515 + , 515, 537, 536 + , 508, 539, 511 + , 511, 539, 538 + , 511, 538, 488 + , 488, 538, 525 + , 512, 540, 508 + , 508, 540, 539 + , 515, 536, 512 + , 512, 536, 540 + , 516, 542, 519 + , 519, 542, 541 + , 519, 541, 507 + , 507, 541, 537 + , 520, 543, 516 + , 516, 543, 542 + , 523, 535, 520 + , 520, 535, 543 + , 544, 545, 564 + , 564, 545, 565 + , 545, 546, 565 + , 565, 546, 566 + , 547, 544, 567 + , 567, 544, 564 + , 548, 549, 568 + , 568, 549, 569 + , 549, 547, 569 + , 569, 547, 567 + , 550, 551, 570 + , 570, 551, 571 + , 551, 548, 571 + , 571, 548, 568 + , 552, 550, 572 + , 572, 550, 570 + , 553, 554, 573 + , 573, 554, 574 + , 554, 552, 574 + , 574, 552, 572 + , 555, 556, 575 + , 575, 556, 576 + , 556, 553, 576 + , 576, 553, 573 + , 557, 555, 577 + , 577, 555, 575 + , 558, 559, 578 + , 578, 559, 579 + , 559, 557, 579 + , 579, 557, 577 + , 560, 561, 580 + , 580, 561, 581 + , 561, 558, 581 + , 581, 558, 578 + , 562, 560, 582 + , 582, 560, 580 + , 546, 563, 566 + , 566, 563, 583 + , 563, 562, 583 + , 583, 562, 582 + , 564, 565, 584 + , 584, 565, 585 + , 565, 566, 585 + , 585, 566, 586 + , 567, 564, 587 + , 587, 564, 584 + , 568, 569, 588 + , 588, 569, 589 + , 569, 567, 589 + , 589, 567, 587 + , 570, 571, 590 + , 590, 571, 591 + , 571, 568, 591 + , 591, 568, 588 + , 572, 570, 592 + , 592, 570, 590 + , 573, 574, 593 + , 593, 574, 594 + , 574, 572, 594 + , 594, 572, 592 + , 575, 576, 595 + , 595, 576, 596 + , 576, 573, 596 + , 596, 573, 593 + , 577, 575, 597 + , 597, 575, 595 + , 578, 579, 598 + , 598, 579, 599 + , 579, 577, 599 + , 599, 577, 597 + , 580, 581, 600 + , 600, 581, 601 + , 581, 578, 601 + , 601, 578, 598 + , 582, 580, 602 + , 602, 580, 600 + , 566, 583, 586 + , 586, 583, 603 + , 583, 582, 603 + , 603, 582, 602 + , 585, 605, 584 + , 584, 605, 604 + , 586, 606, 585 + , 585, 606, 605 + , 584, 604, 587 + , 587, 604, 607 + , 589, 609, 588 + , 588, 609, 608 + , 587, 607, 589 + , 589, 607, 609 + , 591, 611, 590 + , 590, 611, 610 + , 588, 608, 591 + , 591, 608, 611 + , 590, 610, 592 + , 592, 610, 612 + , 594, 614, 593 + , 593, 614, 613 + , 592, 612, 594 + , 594, 612, 614 + , 596, 616, 595 + , 595, 616, 615 + , 593, 613, 596 + , 596, 613, 616 + , 595, 615, 597 + , 597, 615, 617 + , 599, 619, 598 + , 598, 619, 618 + , 597, 617, 599 + , 599, 617, 619 + , 601, 621, 600 + , 600, 621, 620 + , 598, 618, 601 + , 601, 618, 621 + , 600, 620, 602 + , 602, 620, 622 + , 603, 623, 586 + , 586, 623, 606 + , 602, 622, 603 + , 603, 622, 623 + , 625, 624, 605 + , 605, 624, 604 + , 626, 625, 606 + , 606, 625, 605 + , 604, 624, 607 + , 607, 624, 627 + , 629, 628, 609 + , 609, 628, 608 + , 627, 629, 607 + , 607, 629, 609 + , 631, 630, 611 + , 611, 630, 610 + , 628, 631, 608 + , 608, 631, 611 + , 630, 632, 610 + , 610, 632, 612 + , 634, 633, 614 + , 614, 633, 613 + , 632, 634, 612 + , 612, 634, 614 + , 636, 635, 616 + , 616, 635, 615 + , 633, 636, 613 + , 613, 636, 616 + , 635, 637, 615 + , 615, 637, 617 + , 639, 638, 619 + , 619, 638, 618 + , 637, 639, 617 + , 617, 639, 619 + , 641, 640, 621 + , 621, 640, 620 + , 638, 641, 618 + , 618, 641, 621 + , 640, 642, 620 + , 620, 642, 622 + , 643, 626, 623 + , 623, 626, 606 + , 642, 643, 622 + , 622, 643, 623 + , 624, 625, 644 + , 644, 625, 645 + , 625, 626, 645 + , 645, 626, 646 + , 627, 624, 647 + , 647, 624, 644 + , 628, 629, 648 + , 648, 629, 649 + , 629, 627, 649 + , 649, 627, 647 + , 630, 631, 650 + , 650, 631, 651 + , 631, 628, 651 + , 651, 628, 648 + , 632, 630, 652 + , 652, 630, 650 + , 633, 634, 653 + , 653, 634, 654 + , 634, 632, 654 + , 654, 632, 652 + , 635, 636, 655 + , 655, 636, 656 + , 636, 633, 656 + , 656, 633, 653 + , 637, 635, 657 + , 657, 635, 655 + , 638, 639, 658 + , 658, 639, 659 + , 639, 637, 659 + , 659, 637, 657 + , 640, 641, 660 + , 660, 641, 661 + , 641, 638, 661 + , 661, 638, 658 + , 642, 640, 662 + , 662, 640, 660 + , 626, 643, 646 + , 646, 643, 663 + , 643, 642, 663 + , 663, 642, 662 + , 664, 665, 666 + , 666, 665, 723 + , 665, 664, 777 + , 777, 664, 776 + , 667, 668, 670 + , 670, 668, 669 + , 778, 779, 781 + , 781, 779, 780 + , 671, 666, 775 + , 667, 670, 778 + , 778, 670, 779 + , 666, 671, 664 + , 776, 664, 782 + , 782, 664, 673 + , 782, 673, 783 + , 783, 673, 674 + , 675, 676, 677 + , 783, 674, 784 + , 784, 674, 678 + , 679, 668, 667 + , 784, 678, 785 + , 785, 678, 680 + , 785, 680, 786 + , 786, 680, 681 + , 786, 681, 787 + , 787, 681, 682 + , 682, 683, 787 + , 787, 683, 788 + , 667, 684, 679 + , 778, 789, 667 + , 667, 789, 686 + , 791, 685, 790 + , 790, 685, 687 + , 789, 792, 686 + , 686, 792, 689 + , 687, 688, 790 + , 790, 688, 793 + , 792, 794, 689 + , 689, 794, 691 + , 688, 690, 793 + , 793, 690, 795 + , 794, 795, 691 + , 691, 795, 690 + , 671, 672, 693 + , 693, 672, 692 + , 693, 692, 695 + , 695, 692, 694 + , 695, 694, 697 + , 697, 694, 696 + , 664, 671, 673 + , 673, 671, 693 + , 673, 693, 674 + , 674, 693, 695 + , 697, 696, 699 + , 699, 696, 698 + , 782, 796, 776 + , 776, 796, 797 + , 674, 695, 678 + , 678, 695, 697 + , 699, 698, 701 + , 701, 698, 700 + , 798, 796, 783 + , 783, 796, 782 + , 678, 697, 680 + , 680, 697, 699 + , 701, 700, 703 + , 703, 700, 702 + , 799, 798, 784 + , 784, 798, 783 + , 703, 844, 705 + , 680, 699, 681 + , 681, 699, 701 + , 785, 800, 784 + , 784, 800, 799 + , 681, 701, 682 + , 682, 701, 703 + , 786, 801, 785 + , 785, 801, 800 + , 682, 703, 683 + , 683, 703, 705 + , 787, 802, 786 + , 786, 802, 801 + , 675, 677, 684 + , 684, 677, 679 + , 788, 803, 787 + , 787, 803, 802 + , 685, 841, 707 + , 803, 788, 804 + , 804, 788, 791 + , 683, 685, 788 + , 788, 685, 791 + , 708, 842, 706 + , 710, 711, 684 + , 684, 711, 675 + , 709, 708, 713 + , 713, 708, 712 + , 714, 715, 710 + , 710, 715, 711 + , 713, 712, 717 + , 717, 712, 716 + , 718, 719, 714 + , 714, 719, 715 + , 717, 716, 718 + , 718, 716, 719 + , 707, 709, 685 + , 685, 709, 687 + , 686, 710, 667 + , 667, 710, 684 + , 687, 709, 688 + , 688, 709, 713 + , 689, 714, 686 + , 686, 714, 710 + , 805, 804, 790 + , 790, 804, 791 + , 688, 713, 690 + , 690, 713, 717 + , 691, 718, 689 + , 689, 718, 714 + , 778, 781, 789 + , 789, 781, 806 + , 690, 717, 691 + , 691, 717, 718 + , 807, 805, 793 + , 793, 805, 790 + , 789, 806, 792 + , 792, 806, 808 + , 809, 807, 795 + , 795, 807, 793 + , 792, 808, 794 + , 794, 808, 810 + , 810, 809, 794 + , 794, 809, 795 + , 811, 777, 797 + , 797, 777, 776 + , 669, 720, 670 + , 720, 721, 722 + , 720, 722, 670 + , 779, 670, 812 + , 812, 670, 724 + , 812, 724, 813 + , 813, 724, 725 + , 813, 725, 814 + , 814, 725, 726 + , 814, 726, 815 + , 815, 726, 727 + , 815, 727, 816 + , 816, 727, 728 + , 816, 728, 817 + , 817, 728, 729 + , 729, 730, 817 + , 817, 730, 818 + , 731, 732, 723 + , 732, 731, 733 + , 733, 731, 736 + , 721, 734, 722 + , 722, 734, 735 + , 736, 737, 733 + , 665, 731, 723 + , 777, 819, 665 + , 665, 819, 739 + , 821, 738, 820 + , 820, 738, 740 + , 819, 822, 739 + , 739, 822, 742 + , 740, 741, 820 + , 820, 741, 823 + , 822, 824, 742 + , 742, 824, 744 + , 825, 823, 743 + , 743, 823, 741 + , 824, 825, 744 + , 744, 825, 743 + , 826, 780, 812 + , 812, 780, 779 + , 827, 826, 813 + , 813, 826, 812 + , 670, 722, 724 + , 724, 722, 745 + , 828, 827, 814 + , 814, 827, 813 + , 724, 745, 725 + , 725, 745, 746 + , 815, 829, 814 + , 814, 829, 828 + , 725, 746, 726 + , 726, 746, 747 + , 722, 735, 745 + , 745, 735, 748 + , 816, 830, 815 + , 815, 830, 829 + , 745, 748, 746 + , 746, 748, 749 + , 726, 747, 727 + , 727, 747, 750 + , 817, 831, 816 + , 816, 831, 830 + , 818, 832, 817 + , 817, 832, 831 + , 746, 749, 747 + , 747, 749, 751 + , 727, 750, 728 + , 728, 750, 752 + , 747, 751, 750 + , 750, 751, 753 + , 728, 752, 729 + , 729, 752, 754 + , 729, 754, 730 + , 730, 754, 755 + , 750, 753, 752 + , 752, 753, 756 + , 752, 756, 754 + , 754, 756, 757 + , 754, 757, 755 + , 755, 757, 758 + , 818, 821, 832 + , 832, 821, 833 + , 730, 738, 818 + , 818, 738, 821 + , 730, 755, 738 + , 738, 755, 759 + , 755, 758, 759 + , 759, 758, 760 + , 821, 820, 833 + , 833, 820, 834 + , 835, 819, 811 + , 811, 819, 777 + , 820, 823, 834 + , 834, 823, 836 + , 837, 822, 835 + , 835, 822, 819 + , 823, 825, 836 + , 836, 825, 838 + , 839, 824, 837 + , 837, 824, 822 + , 825, 824, 838 + , 838, 824, 839 + , 738, 759, 740 + , 740, 759, 761 + , 739, 762, 665 + , 665, 762, 731 + , 740, 761, 741 + , 741, 761, 763 + , 742, 764, 739 + , 739, 764, 762 + , 741, 763, 743 + , 743, 763, 765 + , 744, 766, 742 + , 742, 766, 764 + , 743, 765, 744 + , 744, 765, 766 + , 759, 760, 761 + , 761, 760, 767 + , 762, 768, 731 + , 731, 768, 736 + , 761, 767, 763 + , 763, 767, 769 + , 764, 770, 762 + , 762, 770, 768 + , 763, 769, 765 + , 765, 769, 771 + , 766, 772, 764 + , 764, 772, 770 + , 765, 771, 766 + , 766, 771, 772 + , 775, 774, 671 + , 671, 774, 672 + , 672, 774, 773 + , 683, 705, 840 + , 840, 841, 683 + , 683, 841, 685 + , 843, 842, 709 + , 709, 842, 708 + , 707, 843, 709 + , 702, 845, 703 + , 703, 845, 844 + , 845, 702, 704 + , 808, 1014, 810 + , 810, 1014, 1015 + , 806, 1013, 808 + , 808, 1013, 1014 + , 781, 1012, 806 + , 806, 1012, 1013 + , 781, 780, 1012 + , 1012, 780, 1011 + , 780, 826, 1011 + , 1011, 826, 1010 + , 826, 827, 1010 + , 1010, 827, 1009 + , 827, 828, 1009 + , 1009, 828, 1008 + , 968, 976, 969 + , 969, 976, 977 + , 969, 977, 970 + , 970, 977, 978 + , 970, 978, 971 + , 971, 978, 979 + , 972, 971, 980 + , 980, 971, 979 + , 973, 972, 981 + , 981, 972, 980 + , 974, 973, 982 + , 982, 973, 981 + , 975, 974, 983 + , 983, 974, 982 + , 976, 1000, 977 + , 977, 1000, 1001 + , 977, 1001, 978 + , 978, 1001, 1002 + , 978, 1002, 979 + , 979, 1002, 1003 + , 980, 979, 1004 + , 1004, 979, 1003 + , 981, 980, 1005 + , 1005, 980, 1004 + , 982, 981, 1006 + , 1006, 981, 1005 + , 983, 982, 1007 + , 1007, 982, 1006 + , 985, 984, 969 + , 969, 984, 968 + , 986, 985, 970 + , 970, 985, 969 + , 987, 986, 971 + , 971, 986, 970 + , 988, 987, 972 + , 972, 987, 971 + , 989, 988, 973 + , 973, 988, 972 + , 990, 989, 974 + , 974, 989, 973 + , 991, 990, 975 + , 975, 990, 974 + , 993, 992, 985 + , 985, 992, 984 + , 994, 993, 986 + , 986, 993, 985 + , 995, 994, 987 + , 987, 994, 986 + , 996, 995, 988 + , 988, 995, 987 + , 996, 988, 997 + , 997, 988, 989 + , 997, 989, 998 + , 998, 989, 990 + , 998, 990, 999 + , 999, 990, 991 + , 1000, 839, 1001 + , 1001, 839, 837 + , 1001, 837, 1002 + , 1002, 837, 835 + , 1002, 835, 1003 + , 1003, 835, 811 + , 1004, 1003, 797 + , 797, 1003, 811 + , 1005, 1004, 796 + , 796, 1004, 797 + , 1006, 1005, 798 + , 798, 1005, 796 + , 1007, 1006, 799 + , 799, 1006, 798 + , 1009, 1008, 993 + , 993, 1008, 992 + , 1010, 1009, 994 + , 994, 1009, 993 + , 1011, 1010, 995 + , 995, 1010, 994 + , 1012, 1011, 996 + , 996, 1011, 995 + , 1012, 996, 1013 + , 1013, 996, 997 + , 1013, 997, 1014 + , 1014, 997, 998 + , 1014, 998, 1015 + , 1015, 998, 999 + , 831, 992, 830 + , 830, 992, 1008 + , 832, 984, 831 + , 831, 984, 992 + , 832, 833, 984 + , 984, 833, 968 + , 833, 834, 968 + , 968, 834, 976 + , 834, 836, 976 + , 976, 836, 1000 + , 802, 983, 801 + , 801, 983, 1007 + , 803, 975, 802 + , 802, 975, 983 + , 804, 991, 803 + , 803, 991, 975 + , 804, 805, 991 + , 991, 805, 999 + , 805, 807, 999 + , 999, 807, 1015 + , 801, 1007, 800 + , 800, 1007, 799 + , 807, 809, 1015 + , 1015, 809, 810 + , 830, 1008, 829 + , 829, 1008, 828 + , 839, 1000, 838 + , 838, 1000, 836 +]) + +NB_GRAY_ZEDM_TRI = 40 +GRAY_COLOR = Color(0.22, 0.22, 0.22) +gray_triangles_m = np.array([ + 849, 846, 848 + , 846, 847, 848 + , 847, 846, 851 + , 846, 850, 851 + , 852, 853, 846 + , 846, 853, 854 + , 850, 846, 856 + , 846, 855, 856 + , 859, 846, 858 + , 846, 857, 858 + , 860, 861, 846 + , 846, 861, 857 + , 860, 846, 862 + , 862, 846, 849 + , 863, 864, 846 + , 846, 864, 855 + , 854, 865, 846 + , 846, 865, 863 + , 859, 866, 846 + , 846, 866, 852 + , 950, 947, 949 + , 947, 948, 949 + , 948, 947, 952 + , 947, 951, 952 + , 953, 954, 947 + , 947, 954, 955 + , 951, 947, 957 + , 947, 956, 957 + , 960, 947, 959 + , 947, 958, 959 + , 961, 962, 947 + , 947, 962, 958 + , 961, 947, 963 + , 963, 947, 950 + , 964, 965, 947 + , 947, 965, 956 + , 955, 966, 947 + , 947, 966, 964 + , 960, 967, 947 + , 947, 967, 953 +]) + +NB_YELLOW_ZEDM_TRI = 80 +YELLOW_COLOR = Color(1., 1., 0.) +yellow_triangles_m = np.array([ + 867, 868, 870 + , 870, 868, 869 + , 868, 871, 869 + , 869, 871, 872 + , 873, 867, 874 + , 874, 867, 870 + , 875, 876, 878 + , 878, 876, 877 + , 876, 873, 877 + , 877, 873, 874 + , 879, 880, 882 + , 882, 880, 881 + , 880, 875, 881 + , 881, 875, 878 + , 883, 879, 884 + , 884, 879, 882 + , 885, 886, 888 + , 888, 886, 887 + , 886, 883, 887 + , 887, 883, 884 + , 889, 890, 892 + , 892, 890, 891 + , 890, 885, 891 + , 891, 885, 888 + , 893, 889, 894 + , 894, 889, 892 + , 895, 896, 898 + , 898, 896, 897 + , 896, 893, 897 + , 897, 893, 894 + , 899, 900, 902 + , 902, 900, 901 + , 900, 895, 901 + , 901, 895, 898 + , 903, 899, 904 + , 904, 899, 902 + , 871, 905, 872 + , 872, 905, 906 + , 905, 903, 906 + , 906, 903, 904 + , 907, 908, 910 + , 910, 908, 909 + , 908, 911, 909 + , 909, 911, 912 + , 913, 907, 914 + , 914, 907, 910 + , 915, 916, 918 + , 918, 916, 917 + , 916, 913, 917 + , 917, 913, 914 + , 919, 920, 922 + , 922, 920, 921 + , 920, 915, 921 + , 921, 915, 918 + , 923, 919, 924 + , 924, 919, 922 + , 925, 926, 928 + , 928, 926, 927 + , 926, 923, 927 + , 927, 923, 924 + , 929, 930, 932 + , 932, 930, 931 + , 930, 925, 931 + , 931, 925, 928 + , 933, 929, 934 + , 934, 929, 932 + , 935, 936, 938 + , 938, 936, 937 + , 936, 933, 937 + , 937, 933, 934 + , 939, 940, 942 + , 942, 940, 941 + , 940, 935, 941 + , 941, 935, 938 + , 943, 939, 944 + , 944, 939, 942 + , 911, 945, 912 + , 912, 945, 946 + , 945, 943, 946 + , 946, 943, 944 +]) diff --git a/src/perception/cone-detection/cone_detection/detector-clean-worked.py b/src/perception/cone-detection/cone_detection/detector-clean-worked.py deleted file mode 100644 index 538dec24..00000000 --- a/src/perception/cone-detection/cone_detection/detector-clean-worked.py +++ /dev/null @@ -1,228 +0,0 @@ -#!/usr/bin/env python3 -# This is a working version -import sys -import numpy as np -from OpenGL.GLUT import * -import argparse -import torch -import cv2 -import pyzed.sl as sl -import torch.backends.cudnn as cudnn - -sys.path.insert(0, './yolov7') -from models.experimental import attempt_load -from utils.general import check_img_size, non_max_suppression, scale_coords, xyxy2xywh -from utils.torch_utils import select_device -from utils.datasets import letterbox - -from threading import Lock, Thread -from time import sleep - -lock = Lock() -run_signal = False -exit_signal = False - - -def img_preprocess(img, device, half, net_size): - net_image, ratio, pad = letterbox(img[:, :, :3], net_size, auto=False) - net_image = net_image.transpose((2, 0, 1))[::-1] # HWC to CHW, BGR to RGB - net_image = np.ascontiguousarray(net_image) - - img = torch.from_numpy(net_image).to(device) - img = img.half() if half else img.float() # uint8 to fp16/32 - img /= 255.0 # 0 - 255 to 0.0 - 1.0 - - if img.ndimension() == 3: - img = img.unsqueeze(0) - return img, ratio, pad - - -def xywh2abcd(xywh, im_shape): - output = np.zeros((4, 2)) - - # Center / Width / Height -> BBox corners coordinates - x_min = (xywh[0] - 0.5 * xywh[2]) * im_shape[1] - x_max = (xywh[0] + 0.5 * xywh[2]) * im_shape[1] - y_min = (xywh[1] - 0.5 * xywh[3]) * im_shape[0] - y_max = (xywh[1] + 0.5 * xywh[3]) * im_shape[0] - - # A ------ B - # | Object | - # D ------ C - - output[0][0] = x_min - output[0][1] = y_min - - output[1][0] = x_max - output[1][1] = y_min - - output[2][0] = x_min - output[2][1] = y_max - - output[3][0] = x_max - output[3][1] = y_max - return output - - -def detections_to_custom_box(detections, im, im0): - output = [] - for i, det in enumerate(detections): - if len(det): - det[:, :4] = scale_coords(im.shape[2:], det[:, :4], im0.shape).round() - gn = torch.tensor(im0.shape)[[1, 0, 1, 0]] # normalization gain whwh - - for *xyxy, conf, cls in reversed(det): - xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) / gn).view(-1).tolist() # normalized xywh - - # Creating ingestable objects for the ZED SDK - obj = sl.CustomBoxObjectData() - obj.bounding_box_2d = xywh2abcd(xywh, im0.shape) - obj.label = cls - obj.probability = conf - obj.is_grounded = False - output.append(obj) - return output - - -def torch_thread(weights, img_size, conf_thres=0.2, iou_thres=0.45): - global image_net, exit_signal, run_signal, detections - - print("Intializing Network...") - - device = select_device() - half = device.type != 'cpu' # half precision only supported on CUDA - imgsz = img_size - - # Load model - model = attempt_load(weights, map_location=device) # load FP32 - stride = int(model.stride.max()) # model stride - imgsz = check_img_size(imgsz, s=stride) # check img_size - if half: - model.half() # to FP16 - cudnn.benchmark = True - - # Run inference - if device.type != 'cpu': - model(torch.zeros(1, 3, imgsz, imgsz).to(device).type_as(next(model.parameters()))) # run once - - while not exit_signal: - if run_signal: - lock.acquire() - img, ratio, pad = img_preprocess(image_net, device, half, imgsz) - - pred = model(img)[0] - det = non_max_suppression(pred, conf_thres, iou_thres) - - # ZED CustomBox format (with inverse letterboxing tf applied) - detections = detections_to_custom_box(det, img, image_net) - lock.release() - run_signal = False - sleep(0.01) - - -def main(): - global image_net, exit_signal, run_signal, detections - - capture_thread = Thread(target=torch_thread, - kwargs={'weights': opt.weights, 'img_size': opt.img_size, "conf_thres": opt.conf_thres}) - capture_thread.start() - - print("Initializing Camera...") - - zed = sl.Camera() - - input_type = sl.InputType() - if opt.svo is not None: - input_type.set_from_svo_file(opt.svo) - - # Create a InitParameters object and set configuration parameters - init_params = sl.InitParameters(input_t=input_type, svo_real_time_mode=True) - init_params.camera_resolution = sl.RESOLUTION.HD720 - init_params.coordinate_units = sl.UNIT.METER - init_params.depth_mode = sl.DEPTH_MODE.ULTRA # QUALITY - init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP - init_params.depth_maximum_distance = 50 - - runtime_params = sl.RuntimeParameters() - status = zed.open(init_params) - - if status != sl.ERROR_CODE.SUCCESS: - print(repr(status)) - exit() - - image_left_tmp = sl.Mat() - - print("Initialized Camera") - - py_transform = sl.Transform() - - positional_tracking_parameters = sl.PositionalTrackingParameters(_init_pos=py_transform) - # If the camera is static, uncomment the following line to have better performances and boxes sticked to the ground. - # positional_tracking_parameters.set_as_static = True - zed.enable_positional_tracking(positional_tracking_parameters) - - obj_param = sl.ObjectDetectionParameters() - obj_param.detection_model = sl.OBJECT_DETECTION_MODEL.CUSTOM_BOX_OBJECTS - obj_param.enable_tracking = True - zed.enable_object_detection(obj_param) - - objects = sl.Objects() - obj_runtime_param = sl.ObjectDetectionRuntimeParameters() - - zed_pose = sl.Pose() - zed_sensors = sl.SensorsData() - zed_info = zed.get_camera_information() - py_translation = sl.Translation() - - while not exit_signal: - if zed.grab(runtime_params) == sl.ERROR_CODE.SUCCESS: - # -- Get the image - lock.acquire() - zed.retrieve_image(image_left_tmp, sl.VIEW.LEFT) - image_net = image_left_tmp.get_data() - lock.release() - run_signal = True - - # -- Detection running on the other thread - while run_signal: - sleep(0.001) - - # Wait for detections - lock.acquire() - # -- Ingest detections - zed.ingest_custom_box_objects(detections) - lock.release() - zed.retrieve_objects(objects, obj_runtime_param) - string_output = ""; - for object in objects.object_list: - string_output += "q" - #print("{} {}".format(object.id, object.position)) - print(string_output); - - zed.get_position(zed_pose, sl.REFERENCE_FRAME.WORLD) - rotation = zed_pose.get_rotation_vector() - translation = zed_pose.get_translation(py_translation) - text_rotation = str((round(rotation[0] * 180 / np.pi, 2), round(rotation[1] * 180 / np.pi, 2), round(rotation[2] * 180 / np.pi, 2))) - text_translation = str( - (round(translation.get()[0], 2), round(translation.get()[1], 2), round(translation.get()[2], 2))) - #print(text_rotation) - #print(text_translation) - - else: - exit_signal = True - - - exit_signal = True - zed.close() - -# Arguments available when call the script -if __name__ == '__main__': - parser = argparse.ArgumentParser() - parser.add_argument('--weights', nargs='+', type=str, default='yolov7m.pt', help='model.pt path(s)') - parser.add_argument('--svo', type=str, default=None, help='optional svo file') - parser.add_argument('--img_size', type=int, default=416, help='inference size (pixels)') - parser.add_argument('--conf_thres', type=float, default=0.4, help='object confidence threshold') - opt = parser.parse_args() - - with torch.no_grad(): - main() diff --git a/src/perception/cone-detection/cone_detection/yolov7 b/src/perception/cone-detection/cone_detection/yolov7 deleted file mode 160000 index a207844b..00000000 --- a/src/perception/cone-detection/cone_detection/yolov7 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit a207844b1ce82d204ab36d87d496728d3d2348e7 diff --git a/src/perception/cone-detection/setup.cfg b/src/perception/cone-detection/setup.cfg deleted file mode 100644 index 5b738628..00000000 --- a/src/perception/cone-detection/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/cone_detection -[install] -install_scripts=$base/lib/cone_detection diff --git a/src/visualization/pure_pursuit_visualizer/pure_pursuit_visualizer/__init__.py b/src/perception/sythesis/cone_mapping/cone_mapping/__init__.py similarity index 100% rename from src/visualization/pure_pursuit_visualizer/pure_pursuit_visualizer/__init__.py rename to src/perception/sythesis/cone_mapping/cone_mapping/__init__.py diff --git a/src/perception/sythesis/cone_mapping/cone_mapping/cone_map_collection.py b/src/perception/sythesis/cone_mapping/cone_mapping/cone_map_collection.py new file mode 100644 index 00000000..140a94ab --- /dev/null +++ b/src/perception/sythesis/cone_mapping/cone_mapping/cone_map_collection.py @@ -0,0 +1,50 @@ +from moa_msgs.msg import Cone +from geometry_msgs.msg import Point +from geometry_msgs.msg import Quaternion +from geometry_msgs.msg import Pose + +import math + +class ConeMapList(list): + def __init__(self): + super().__init__(self) + + def insert_cone(self, car_pose, cone_coord): + car_coord = car_pose.position.x, car_pose.position.y + # Insert if first cone detected + if len(self) == 0: + self.append(cone_coord) + return + + # + if len(self) == 1: + coord0 = self[0].x, self[0].y + if self.distance(car_coord, cone_coord) < self.distance(car_coord, coord0): + self.insert(0, cone_coord) + return + else: + self.append(cone_coord) + return + + for i in range(len(self) - 1): + coordi = self[i].position.x, self[i].position.y + coordi1 = self[i+1].position.x, self[i+1].position.y + if self.distance(coordi, cone_coord) < self.distance(coordi, coordi1): + self.insert(i, cone_coord) + return + + self.append(cone_coord) + + def distance(self, coord1, coord2): + return math.sqrt((coord1[0].x - coord2[0].x)**2 + (coord1[1].y - coord2[1].y)**2) + + + +class ConeWithId(Cone): + def __init__(self, id): + super().__init__(self) + + + + +cone = ConeWithId(cone, id) \ No newline at end of file diff --git a/src/perception/sythesis/cone_mapping/cone_mapping/db_scan.py b/src/perception/sythesis/cone_mapping/cone_mapping/db_scan.py new file mode 100644 index 00000000..b7cb2c01 --- /dev/null +++ b/src/perception/sythesis/cone_mapping/cone_mapping/db_scan.py @@ -0,0 +1,410 @@ +import rclpy +import random +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy + +from moa_msgs.msg import ConeMap +from moa_msgs.msg import Cones +from moa_msgs.msg import Cone +from geometry_msgs.msg import Point +from geometry_msgs.msg import Quaternion +from geometry_msgs.msg import PoseWithCovariance; +from geometry_msgs.msg import Pose; + +#Plotting and mathematic related +import math +import numpy as np +#import matplotlib.pyplot as plt +import time + +import threading +from std_msgs.msg import Float32 + +class Cone_Mapper(Node): + +# Initializer + def __init__(self): + super().__init__('cone_mapper') + qos_profile = QoSProfile(reliability=QoSReliabilityPolicy.BEST_EFFORT, history=QoSHistoryPolicy.KEEP_LAST, depth=10) + self.subscription = self.create_subscription(Cones, 'cone_detection', self.cones_callback, qos_profile) + self.subscription = self.create_subscription(Pose, 'car_position', self.car_position_callback, 10) + self.publisher = self.create_publisher(ConeMap, 'cone_map', 5) + + # Clustering related tuning parameter + self.default_standard_deviation = 0.5 # Also minimal standard deviation + self.default_variance = self.default_standard_deviation ** 2 + self.default_covariance = [self.default_variance, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, self.default_variance, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; + self.new_cone_if_n_sigma_exceed_this = 4 + + # Clustering method initialization + self.most_updated_cone_map = ConeMap() + self.number_of_measurements_for_cones = [] + self.car_positions = [] + self.cone_id = 1 + + # Cone deletion tune + self.count_above_this_are_safe = 7 + self.count_rate_above_this_are_safe = 3 + self.period_for_cone_deletion = 2 + + # Cone deletion initialization + #self.cone_deletion_timer = self.create_timer(self.period_for_cone_deletion, self.cone_deletion_callback) + self.number_of_measurements_increase_rate = [] + self.previous_number_of_measurements = [] + + self.get_logger().info("Cone Map Initialization Completed") + +################################################################################ (measure duration for each cone map update) + + # Create update duration publisher + self.duration_publisher = self.create_publisher(Float32, 'duration', 10) + + # Record the total update duration + self.total_time = 0 + + # Record the number of updates + self.counter = 0 + + # Lock for thread safety + self.lock = threading.Lock() + + # Create a timer to calculate the average duration for each cone map update + self.timer = self.create_timer(10.0, self.average_time_callback) + + def average_time_callback(self): + """This function calculates and publishes the average cone map update duration every 10s to the /duration topic and write to a file + """ + # Calculate the average update duration for the most recent 10s + with self.lock: + try: + average_duration = self.total_time / self.counter + except ZeroDivisionError: + average_duration = 0.0 + self.total_time = 0 + self.counter = 0 + + # Write the average update duration to a file + with open("/home/fsae/Documents/pang/cone_mapping_data/dbscan.txt", "a") as file: + file.write(f"{average_duration}\n") + + # Publishes the average update duration to the /duration topic + duration_msg = Float32() + duration_msg.data = average_duration + self.duration_publisher.publish(duration_msg) + + + def listener_callback(self, msg): + # Update the existing cone map with new measurements + before = self.get_clock().now() + msg_in_local_coordinate = msg + msg_in_global_coordinate = self.transform_raw_input_to_global_coordinate(msg_in_local_coordinate) + self.most_updated_cone_map = self.clustering_update(msg_in_global_coordinate) + after = self.get_clock().now() + + # Calculate the update duration in micro seconds + duration = (after - before).nanoseconds / 1000 + with self.lock: + self.total_time += duration + self.counter += 1 + + self.publisher.publish(self.most_updated_cone_map) + #self.get_logger().info("Cone Map Published") + +# Conversion to Global Coordinate +## Main + def transform_raw_input_to_global_coordinate(self, cones: Cones, car_position: Pose): + """Extract measurement state from the Cone Map message subscription + + Args: + msg: Input ConeMap message from Cone detection + + Returns: + None + + Raises: + None + """ + # Convert Cone Map message into position (x and y), orientation (theta) and list of cones + x, y, theta, list_of_cones_in_local_coordinates = self.convert_message_to_data(cones, car_position) + # Use list of cones and states (x, y and theta) to get the position vector and rotation matrix + position_vector, rotation_matrix_local_to_global = self.get_coordinate_conversion_matrices(x, y, theta) + # Conversion from local reference frame to global reference frame + list_of_cones_in_global_coordinates = self.get_list_of_cones_in_global_coordinate(position_vector, rotation_matrix_local_to_global, list_of_cones_in_local_coordinates) + # Get unsorted Cone Map that contains all measured cone map at moment + output_in_ConeMap_type = self.produce_cone_map_message(x, y, theta, list_of_cones_in_global_coordinates) + + return output_in_ConeMap_type + +## Utility Functions for Conversion + def get_coordinate_conversion_matrices(self, x: float, y: float, theta: float) -> (np.array, np.array, np.array): + '''Convert state and list_of_cones input into position vector, rotation matrix (DCM) and the matrix of list of cones + + Args: + x: x position specified in float + y: y position specified in float + theta: theta orientation specified in float + list_of_cones: np.array (numpy array) for matrix of cone positions in 2 by n matrix (n is number of cones recorded in input)) + + Returns: + position_vector: np.array of position vector of cart + rotation_matrix: np.array DCM matrix to convert reading from local frame into global frame + list_of_cones: np.array 2 x n matrix of the list of cones measured in global frame + + Raises: + None + ''' + position_vector = np.array([[x], [y], [0]]) + rotation_matrix_local_to_global = np.array( + [[math.cos(theta), -math.sin(theta), 0], [math.sin(theta), math.cos(theta), 0], [0, 0, 1]]) # Inverse DCM + + return position_vector, rotation_matrix_local_to_global + + def get_list_of_cones_in_global_coordinate(self, position_vector : np.array, rotation_matrix : np.array, list_of_cones : np.array) -> np.array: + list_of_cones_unrotated = np.matmul(rotation_matrix, list_of_cones) + list_of_cones_output = list_of_cones_unrotated + position_vector + return list_of_cones_output + +# Clustering Method +## Main + def clustering_update(self, msg: ConeMap): + output = ConeMap() + cone_for_car_coordinate = msg.cones[0] + output.cones.append(cone_for_car_coordinate) + + measured_cones_in_list_type = list(msg.cones[1:]) + recorded_cones = self.most_updated_cone_map.cones[1:] + + # Sort the measurement to the existing cones + index = 0 + for individual_recorded_cone in recorded_cones: + if len(measured_cones_in_list_type) > 0: + closest_measurement, distance = self.sort_recorded_cone_and_find_closest_measurement(individual_recorded_cone, measured_cones_in_list_type) + if self.measurement_is_not_new_cone(distance, individual_recorded_cone) and self.is_same_color(individual_recorded_cone, closest_measurement): + self.number_of_measurements_for_cones[index] += 1 + updated_recorded_cone = self.update_mean_covariance_count(individual_recorded_cone, closest_measurement, index) + measured_cones_in_list_type.pop(0) + else: + updated_recorded_cone = individual_recorded_cone + else: + updated_recorded_cone = individual_recorded_cone + output.cones.append(updated_recorded_cone) + index += 1 + + # If measurement_cones_in_list_type are not empty, add rest of cones as new measurements + for cone_measurement_that_is_new in measured_cones_in_list_type: + x, y, theta, _, color, _ = self.extract_data_from_cone(cone_measurement_that_is_new) + new_cone = self.pack_cone_message(x, y, theta, self.cone_id, self.default_covariance, color) + self.number_of_measurements_for_cones.append(1) + np.append(self.number_of_measurements_increase_rate, 1) + self.previous_number_of_measurements.append(0) + output.cones.append(new_cone) + self.cone_id += 1 + + return output + + def update_mean_covariance_count(self, recorded_cone, measured_cone, index): + x_record, y_record, theta_record, covariance_record, color_record, cone_id = self.extract_data_from_cone(recorded_cone) + x_measure, y_measure, theta_measure, covariance_measure, color_measure, _ = self.extract_data_from_cone(measured_cone) + variance_record = covariance_record[0] + number_of_meaasurements = self.number_of_measurements_for_cones[index] + # Update mean + x_new_rec = self.get_new_mean(x_record, x_measure, number_of_meaasurements) + y_new_rec = self.get_new_mean(y_record, y_measure, number_of_meaasurements) + # Update covariance + distance = self.distance_between_two_cones(recorded_cone, measured_cone) + new_variance = self.get_new_variance(variance_record, distance, number_of_meaasurements) + new_covariance = [new_variance, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, new_variance, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; + output_cone = self.pack_cone_message(x_new_rec, y_new_rec, theta_record, cone_id, new_covariance, color_record) + return output_cone + + def get_new_mean(self, x_old_record, x_measurement, count): + return x_old_record + (x_measurement - x_old_record) / count + + def get_new_variance(self, old_variance, distance, count): + return (count - 2) * old_variance / (count - 1) + distance ** 2 / count + + def sort_recorded_cone_and_find_closest_measurement(self, recorded_cone: Cone, measured_cones_in_list_type): + measured_cones_in_list_type.sort(key=lambda x: self.distance_between_two_cones(x, recorded_cone)) + closest_measured_cone_to_given_existing_cone = measured_cones_in_list_type[0] + cone_output = closest_measured_cone_to_given_existing_cone + distance_output = self.distance_between_two_cones(closest_measured_cone_to_given_existing_cone, recorded_cone) + return cone_output, distance_output + + def measurement_is_not_new_cone(self, distance, recorded_cone: Cone): + x, y, theta, covariance, color, _ = self.extract_data_from_cone(recorded_cone) + standard_deviation = np.sqrt(covariance[0]) + criterion_standard_deviation = max([standard_deviation, self.default_standard_deviation]) + return distance <= self.new_cone_if_n_sigma_exceed_this * criterion_standard_deviation + +## Utility Functions for Clustering + def convert_message_to_data(self, cones: Cones, car_position: Pose) -> (float, float, float, np.array): + """Convert cone map message into useable data + + Args: + data: Input Cone Map message from Cone detection + + Returns: + Tuple consists of: + x: float for x position + y: float for y position + theta: float for orientation + list_of_cones: 2 x n numpy list (np.array) that contains the position of the cones. + + Raises: + + """ + # Convert from Cone Map to data that is processed in the node + list_of_cones = cones.cones + + # Extract the first cone in cone map as the cart localization data + cart_info = car_position + + # Convert cart info to readable datas for cart + x, y, theta, covariance, color, _ = self.extract_data_from_cone(cart_info) + + list_of_local_cones_x = []; + list_of_local_cones_y = []; + list_of_local_cones_color = []; + + # Convert message of cones to 2 by n matrix where n is the number of cones in the measurement message + for index in range(0, len(list_of_cones), 1): + individual_x, individual_y, individual_theta, individual_covariance, individual_color, _ = self.extract_data_from_cone( + list_of_cones[index]) + list_of_local_cones_x.append(individual_x); + list_of_local_cones_y.append(individual_y); + list_of_local_cones_color.append(individual_color); + + list_of_cones = np.array([list_of_local_cones_x, list_of_local_cones_y, list_of_local_cones_color]) + + return x, y, theta, list_of_cones + + def extract_data_from_cone(self, cone_input: Cone): + # For later process: We need to use quaternion orientation + x = cone_input.pose.pose.position.x + y = cone_input.pose.pose.position.y + theta = cone_input.pose.pose.orientation.w + covariance = cone_input.pose.covariance + color = cone_input.colour + cone_id = cone_input.id + return x, y, theta, covariance, color, cone_id + + def extract_data_from_car(self, car_input: Pose): + x = car_input.position.x + y = car_input.position.y + theta = car_input.orientation.w + return x, y, theta + + + +# Delete non-existing cones + def cone_deletion_callback(self): + # Count > 100 are cones, count rate increasing > 5 are cones + self.update_increase_rate() + initial_indexes = [i for i in range(0, len(self.number_of_measurements_for_cones), 1)] + low_count_index = self.get_index_that_has_low_count(initial_indexes) + false_cone_index = self.get_index_that_has_low_increase(low_count_index) + self.delete_selected_cones(false_cone_index) + self.previous_number_of_measurements = self.number_of_measurements_for_cones.copy() + + def delete_selected_cones(self, index_input): + reversed_index_input = sorted(index_input, reverse=True) + for index in reversed_index_input: + self.most_updated_cone_map.cones.pop(index + 1) + self.number_of_measurements_for_cones.pop(index) + self.previous_number_of_measurements.pop(index) + self.number_of_measurements_increase_rate.pop(index) + + def update_increase_rate(self): + if len(self.number_of_measurements_for_cones) > len(self.previous_number_of_measurements): + differences = len(self.number_of_measurements_for_cones) - len(self.previous_number_of_measurements) + self.previous_number_of_measurements = self.previous_number_of_measurements + [0] * differences + elif len(self.number_of_measurements_for_cones) < len(self.previous_number_of_measurements): + differences = len(self.previous_number_of_measurements) - len(self.number_of_measurements_for_cones) + self.previous_number_of_measurements = self.previous_number_of_measurements + [0] * differences + + self.number_of_measurements_increase_rate = [] + for i in range(len(self.number_of_measurements_for_cones)): + differences = self.number_of_measurements_for_cones[i] - self.previous_number_of_measurements[i] + self.number_of_measurements_increase_rate.append(differences / self.period_for_cone_deletion) + + def get_index_that_has_low_count(self, index_input): + output_list = [] + list_to_study = [self.number_of_measurements_for_cones[i] for i in index_input] + index = 0 + for individual_count in list_to_study: + if individual_count < self.count_above_this_are_safe: + output_list.append(index_input[index]) + index += 1 + return output_list + + def get_index_that_has_low_increase(self, index_input): + output_list = [] + list_to_study = [self.number_of_measurements_increase_rate[i] for i in index_input] + index = 0 + for individual_count_rate in list_to_study: + if individual_count_rate < self.count_rate_above_this_are_safe: + output_list.append(index_input[index]) + index += 1 + return output_list + +# Debug only: Get all datas + +# Other Utility Functions + def produce_cone_map_message(self, x : float, y : float, theta : float, list_of_cones : np.array) -> ConeMap: #Produce message from array input + output_map = ConeMap() + cart_input = self.pack_cone_message(x, y, theta, 0, np.zeros(36), 0) + output_map.cones.append(cart_input) + list_of_cones_x = list_of_cones[0] + list_of_cones_y = list_of_cones[1] + list_of_cones_color = list_of_cones[2] + length = len(list_of_cones_x) + for index in range(length): + cone_out = self.pack_cone_message(list_of_cones_x[index], list_of_cones_y[index], 0.0, index + 1, self.default_covariance, int(list_of_cones_color[index])); + output_map.cones.append(cone_out) + return output_map + + def pack_cone_message(self, x : float, y : float, theta : float, cone_id : int, covariance_vector, color : int) -> Cone: + output_cone = Pose() + position = Point() + orientation = Quaternion() + pose_with_covariance = PoseWithCovariance() + pose = Pose() + position.x = x + position.y = y + orientation.w = theta + pose.position = position + pose.orientation = orientation + pose_with_covariance.pose = pose + pose_with_covariance.covariance = covariance_vector + output_cone_pose = pose_with_covariance.pose + handedness = 1 if color == 1 else 0 + return (output_cone_pose, handedness) + + def distance_between_two_cones(self, cone_1, cone_2): + x1, y1, _, _, _, _ = self.extract_data_from_cone(cone_1) + x2, y2, _, _, _, _ = self.extract_data_from_cone(cone_2) + delta_x = x1 - x2 + delta_y = y1 - y2 + return (delta_x ** 2) ** (1/2) + (delta_y ** 2) ** (1/2) + + def is_same_color(self, cone_1, cone_2): + _, _, _, _, color_1, _ = self.extract_data_from_cone(cone_1) + _, _, _, _, color_2, _ = self.extract_data_from_cone(cone_2) + return color_1 == color_2 + +def main(args=None): + rclpy.init(args=args) + + cone_mapper = Cone_Mapper() + + rclpy.spin(cone_mapper) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + cone_mapper.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter copy.py b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter copy.py new file mode 100644 index 00000000..8476729d --- /dev/null +++ b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter copy.py @@ -0,0 +1,389 @@ +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy + +from std_msgs.msg import Float32MultiArray +from moa_msgs.msg import Detections, Track +from geometry_msgs.msg import Point, Quaternion, Pose, PoseWithCovariance + +import math +import numpy as np +import time +import threading + +import kdtree + +class Item(object): + def __init__(self, x, y, data): + self.coords = (x, y) + self.data = data + + def __len__(self): + return len(self.coords) + + def __getitem__(self, i): + return self.coords[i] + + def __repr__(self): + return 'Item({}, {}, {})'.format(self.coords[0], self.coords[1], self.data) + + +class Cone_Mapper(Node): + def __init__(self): + super().__init__('cone_mapper') + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10 + ) + + # Create cone detection subscriber + self.cones_subscription = self.create_subscription( + Detections, + 'cone_detection', + self.cones_callback, + qos_profile) + + # Create left track publisher + self.left_track_publisher = self.create_publisher(Track, 'left_track', 10) + + # Create right track publisher + self.right_track_publisher = self.create_publisher(Track, 'right_track', 10) + + # times_modified publisher + self.times_modified_publisher = self.create_publisher(Float32MultiArray, 'times_modified', 10) + + # Existing cone map + self.left_track = Track() + self.right_track = Track() + + self.left_track.cones = [] + self.right_track.cones = [] + + # KDTrees for searching + self.left_tree = None + self.right_tree = None + + self.times_modified_counter = 0 + + # recording sleep time + time.sleep(12) + +################################################################################ (parameters to tune) + + # Initial error in the estimate + self.default_error_in_estimate = 4.0 + + # Error in measurement + self.error_in_measurement = 2.5 + + # Cone match radius + self.match_radius = 1.0 + + # Counter to remove points + self.remove_point_counter = 200 + + # value for the times_modified + self.times_modified_limit = 300000 + + # Modify rate + self.modify_rate = 1.1 + +################################################################################ (parameters to tune) + + def cones_callback(self, msg: Detections) -> None: + """This function updates the existing cone map with newly detected cones and + publishes left-track cones to the /left_track topic and right-track cones to the /right_track topic. + + Args: + msg (Detections): car's position and input cones from the /cone_detection topic + """ + # Update the existing cone map with new measurements + self.update_existing_cone_map(msg) + + # Publishes left-track cones to the /left_track topic and right-track cones to the /right_track topic + # print("Left track length: ", len(self.left_track.cones)) + # print("Right track length: ", len(self.right_track.cones)) + self.times_modified_counter = self.times_modified_counter + 1 + if self.times_modified_counter > self.remove_point_counter: + self.times_modified_counter = 0 + self.remove_points() + + self.left_track_publisher.publish(self.left_track) + self.right_track_publisher.publish(self.right_track) + + + def update_existing_cone_map(self, msg: Detections) -> None: + """This function updates the existing cone map using new cone measurementsf + + Args: + msg (Detections): Contains the car's position and lists of detected cones + """ + # Get all cones' global positions + car_position = msg.car_pose + global_blue_cone_positions = self.cones_local_to_global(msg.blue, car_position) + global_yellow_cone_positions = self.cones_local_to_global(msg.yellow, car_position) + + # Update the left track and the right track + self.update_left_track(global_blue_cone_positions) + self.update_right_track(global_yellow_cone_positions) + + + def update_left_track(self, points: list) -> None: + """This function updates the left track of the cone map + + Args: + points (list): A list of blue cones' global (x, y) coordinates + """ + if self.left_track.cones == []: + # If the left track is empty, pack all cone measurements and update the left track + self.left_track = self.produce_track_message(points) + # self.left_tree = kdtree.create([Item(coord[0], coord[1], (index, self.default_error_in_estimate)) for index, coord in enumerate(points)]) + times_modified = 1 + self.left_tree = kdtree.create( + [Item(coord[0], coord[1], [self.left_track.cones[index], self.default_error_in_estimate, times_modified]) for index, coord in enumerate(points)], dimensions=2) # For 2D points + else: + # If the left track is not empty, find the closest cone for each newly measured cone and update its coordinates or add it to the track + for coord in points: + point, distance = self.left_tree.search_nn(coord) + # print(self.left_tree.search_nn(coord)) + # kdtree.visualize(self.left_tree) + # If the newly measured cone is in the match radius, this cone already exists in the left track, update its coordinates + if distance <= self.match_radius: + cone = point.data.data[0] + error_in_estimate = point.data.data[1] + times_modified = point.data.data[2] * self.modify_rate + new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate) + point.data.coords = (new_estimate_x, new_estimate_y) + point.data.data = (cone, new_error_in_estimate, times_modified) + else: # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map + cone = Point() + cone.x = coord[0] + cone.y = coord[1] + self.left_track.cones.append(cone) + times_modified = 1 + self.left_tree.add(Item(coord[0], coord[1], (cone, self.default_error_in_estimate, times_modified))) + + + + def update_right_track(self, points: list) -> None: + """This function updates the right track of the cone map + + Args: + points (list): A list of yellow cones' (x, y) coordinates + """ + if self.right_track.cones == []: + # If the right track is empty, pack all cone measurements and update the right track + self.right_track = self.produce_track_message(points) + times_modified = 1 + self.right_tree = kdtree.create([Item(coord[0], coord[1], [self.right_track.cones[index], self.default_error_in_estimate, times_modified]) for index, coord in enumerate(points)], dimensions=2) + else: + # If the right track is not empty, find the closest cone for each newly measured cone and update its coordinates or add it to the track + for coord in points: + point, distance = self.right_tree.search_nn(coord) + # print(self.right_tree.search_nn(coord)) + # If the newly measured cone is in the match radius, this cone already exists in the right track, update its coordinates + # check if distance is nan + # kdtree.visualize(self.right_tree) + if distance <= self.match_radius: + cone = point.data.data[0] + error_in_estimate = point.data.data[1] + times_modified = point.data.data[2] * self.modify_rate + new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate) + point.data.coords = (new_estimate_x, new_estimate_y) + point.data.data = (cone, new_error_in_estimate, times_modified) + else: + # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map + cone = Point() + cone.x = coord[0] + cone.y = coord[1] + self.right_track.cones.append(cone) + times_modified = 1 + self.right_tree.add(Item(coord[0], coord[1], (cone, self.default_error_in_estimate, times_modified))) + + + def kalman_filtering(self, cone: Point, coord: tuple, error_in_estimate: float) -> tuple: + """This function update the x and y coordinate of of the input cone using kalman filtering + + Args: + cone (Point): input cone's location + coord (tuple): new measurement of (x, y) of the input cone + error_in_estimate (float): error in estimate of the cone's actual position + + Returns: + new_estimate_x (float): cone's new estimated x coordinate + new_estimate_y (float): cone's new estimated y coordinate + new_error_in_estimate (float): cone's new error in estimate + """ + # Get the previous error_in_estimate, estimated x and estimated y coordinate + prev_error_in_estimate = error_in_estimate + prev_estimate_x = cone.x + prev_estimate_y = cone.y + + # Calculate the kalman gain + kalman_gain = prev_error_in_estimate / (prev_error_in_estimate + self.error_in_measurement) + + # Calculate the new error_in_estimate, estimated x and estimated y coordinate + new_estimate_x = prev_estimate_x + kalman_gain * (coord[0] - prev_estimate_x) + new_estimate_y = prev_estimate_y + kalman_gain * (coord[1] - prev_estimate_y) + new_error_in_estimate = (1 - kalman_gain) * (prev_error_in_estimate) + + # update x and y coordinate + cone.x = new_estimate_x + cone.y = new_estimate_y + + return new_estimate_x, new_estimate_y, new_error_in_estimate + + + def cones_local_to_global(self, cones: list, car_position: Pose) -> list: + """This function converts all cones from the local frame to the global frame + + Args: + cones ([Point]): contains all the cones' positions in local frame + car_position (Pose): car's current position + + Returns: + global_cone_positions (list): a list contains each cone's global (x, y) + """ + # If no cones are detected, return an empty np.array + if len(cones) == 0: + return [] + + # Extract car's x coordinate, y coordinate and rotation angle + x, y, theta = self.extract_data_from_car(car_position) + + # Convert cones positions into a np.array contains each cone's local x coordinate, y coordinate + list_of_cones = self.convert_cones_to_data(cones) + + # Use cart's (x, y and theta) to get the cart's position vector and rotation matrix + position_vector, rotation_matrix = self.get_coordinate_conversion_matrices(x, y, theta) + + # Convert cone's coordinates from local frame to global frame + global_cone_columns = self.local_to_global(position_vector, rotation_matrix, list_of_cones) + + # Convert global_cone_columns to a list of cones' (x,y) position + global_cone_positions = [tuple(global_cone_columns[:,i]) for i in range(len(cones))] + + return global_cone_positions + + + def extract_data_from_car(self, car_position: Pose) -> tuple: + """This function extracts car's x coordinate, y coordinate and rotation angle from car_position + + Args: + car_position (Pose): car's current position + + Returns: + x (float): car's x coordinate + y (float): car's y coordinate + theta (float): car's rotation angle + """ + x = car_position.position.x + y = car_position.position.y + theta = car_position.orientation.w + return x, y, theta + + + def convert_cones_to_data(self, cones: list) -> np.array: + """Convert a list of cones' positions into a 2*n np.array contains each cone's local x coordinate and y coordinate + + Args: + cones ([Point]): A list of cones, each cone is represented as a Point + + Returns: + list_of_cones (np.array): 2*n np.array contains each cone's local x coordinate and y coordinate + """ + # Extract x and y coordinate from all cones and store them in a 2*n np.arra + list_of_local_cones_x = [cone.x for cone in cones] + list_of_local_cones_y = [cone.y for cone in cones] + list_of_cones = np.array([list_of_local_cones_x, list_of_local_cones_y]) + return list_of_cones + + + def get_coordinate_conversion_matrices(self, x: float, y: float, theta: float) -> tuple: + """This function gets car's gloabl position vector and local coordinate rotation matrix + + Args: + x (float): car's x coordinate + y (float): car's y coordinate + theta (float): car's rotation angle + + Returns: + position_vector (np.array): car's global position vector + rotation_matrix (np.array): rotation matrix for cone's local coordinates + """ + position_vector = np.array([[x],[y]]) + rotation_matrix = np.array([[math.cos(theta), -math.sin(theta)],[math.sin(theta), math.cos(theta)]]) + + return position_vector, rotation_matrix + + + def local_to_global(self, position_vector: np.array, rotation_matrix: np.array, list_of_cones: np.array) -> np.array: + """This function converts all cones' (x, y) from the local frame to the global frame + + Args: + position_vector (np.array): car's global position vector + rotation_matrix (np.array): rotation matrix for cone's local coordinates + list_of_cones (np.array): 2*n np.array contains each cone's local x coordinate and y coordinate + + Returns: + list_of_cones_output (np.array): 2*n np.array contains each cone's global x coordinate and y coordinate + """ + list_of_cones_unrotated = np.matmul(rotation_matrix, list_of_cones) + list_of_cones_output = list_of_cones_unrotated + position_vector + return list_of_cones_output + + + def produce_track_message(self, points: list) -> Track: + """This function packs all cones' (x, y) coordinates into a cone map message + + Args: + points (list): A list of cones' (x, y) coordinates + + Returns: + output_track (Track): Track message + """ + output_track = Track() + for p in points: + point = Point() + point.x = p[0] + point.y = p[1] + output_track.cones.append(point) + return output_track + + def remove_points(self): + if self.left_track is None or self.right_track is None: + return + + times_modified_list = Float32MultiArray() + + left_tree = self.left_tree + right_tree = self.right_tree + for point in kdtree.level_order(self.left_tree): + # print(point) + times_modified_list.data.append(point.data.data[2]) + if point.data.data[2] < self.times_modified_limit: + self.left_track.cones.remove(point.data.data[0]) + self.left_tree.remove(point.data) + for point in kdtree.level_order(self.right_tree): + times_modified_list.data.append(point.data.data[2]) + if point.data.data[2] < self.times_modified_limit: + self.right_track.cones.remove(point.data.data[0]) + self.right_tree.remove(point.data) + + self.times_modified_publisher.publish(times_modified_list) + + # kdtree.visualize(self.left_tree) + # kdtree.visualize(self.right_tree) + + +def main(args=None): + rclpy.init(args=args) + cone_mapper = Cone_Mapper() + rclpy.spin(cone_mapper) + cone_mapper.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py new file mode 100644 index 00000000..1bc80143 --- /dev/null +++ b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py @@ -0,0 +1,423 @@ +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy + +from std_msgs.msg import Float32MultiArray +from moa_msgs.msg import Detections, Track +from geometry_msgs.msg import Point, Quaternion, Pose, PoseWithCovariance + +import math +import numpy as np +import time +import threading + +import kdtree + +class Item(object): + def __init__(self, x, y, data): + self.coords = (x, y) + self.data = data + + def __len__(self): + return len(self.coords) + + def __getitem__(self, i): + return self.coords[i] + + def __repr__(self): + return 'Item({}, {}, {})'.format(self.coords[0], self.coords[1], self.data) + + +class Cone_Mapper(Node): + def __init__(self, *vargs, **kwargs): + super().__init__('cone_mapper', *vargs, **kwargs) + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10 + ) + + # Create cone detection subscriber + self.cones_subscription = self.create_subscription( + Detections, + 'cone_detection', + self.cones_callback, + qos_profile) + + # Create left track publisher + self.left_track_publisher = self.create_publisher(Track, 'left_track', 10) + + # Create right track publisher + self.right_track_publisher = self.create_publisher(Track, 'right_track', 10) + + # times_modified publisher + self.times_modified_publisher = self.create_publisher(Float32MultiArray, 'times_modified', 10) + + # Existing cone map + self.left_track = Track() + self.left_track.cones = [] + self.right_track = Track() + self.right_track.cones = [] + + # KDTrees for searching + self.left_tree = kdtree.create(None, dimensions=2) + self.right_tree = kdtree.create(None, dimensions=2) + + self.times_updated_counter = 0 + +################################################################################ (parameters to tune) + + # Initial error in the estimate + self.default_error_in_estimate = 4.0 + + # Error in measurement + self.error_in_measurement = 2.5 + + # Cone match radius + self.match_radius = 1.0 + + # Counter to remove points + self.remove_point_counter = 200 + + # value for the times_modified + self.times_modified_limit = 100 + + # Modify rate + self.modify_rate = 1.08 + + # Detection reset counter + self.reset_counter = 0 + self.reset_count = 60 + + self.declare_parameters( + namespace='', + parameters=[ + ('invert_cones', False), + ] + ) + +################################################################################ +#(parameters to tune) + + def reset_map(self): + self.left_track = Track() + self.left_track.cones = [] + self.right_track = Track() + self.right_track.cones = [] + + # KDTrees for searching + self.left_tree = kdtree.create(None, dimensions=2) + self.right_tree = kdtree.create(None, dimensions=2) + + def cones_callback(self, msg: Detections) -> None: + """This function updates the existing cone map with newly detected cones and + publishes left-track cones to the /left_track topic and right-track cones to the /right_track topic. + + Args: + msg (Detections): car's position and input cones from the /cone_detection topic + """ + # Update the existing cone map with new measurements + self.update_existing_cone_map(msg) + + self.times_updated_counter += 1 + if self.times_updated_counter > self.remove_point_counter: + self.times_updated_counter = 0 + self.remove_cones() + + self.reset_counter += 1 + if self.reset_counter > self.reset_count: + self.reset_counter = 0 + self.reset_map() + + self.left_track_publisher.publish(self.left_track) + self.right_track_publisher.publish(self.right_track) + + + def update_existing_cone_map(self, msg: Detections) -> None: + """This function updates the existing cone map using new cone measurements + + Args: + msg (Detections): Contains the car's position and lists of detected cones + """ + # Get all cones' global positions + car_position = msg.car_pose + global_blue_cone_positions = self.cones_local_to_global(msg.blue, car_position) + global_yellow_cone_positions = self.cones_local_to_global(msg.yellow, car_position) + + # Update the left track and the right track if cones got detected + if not self.get_parameter('invert_cones').get_parameter_value().bool_value: + if len(global_blue_cone_positions) != 0: + self.update_left_track(global_blue_cone_positions) + if len(global_yellow_cone_positions) != 0: + self.update_right_track(global_yellow_cone_positions) + else: + if len(global_yellow_cone_positions) != 0: + self.update_left_track(global_yellow_cone_positions) + if len(global_blue_cone_positions) != 0: + self.update_right_track(global_blue_cone_positions) + + + def update_left_track(self, points: list) -> None: + """This function updates the left track of the cone map + + Args: + points (list): A list of blue cones' global (x, y) coordinates + """ + if len(self.left_track.cones) == 0: + # If the left track is empty, pack all cone measurements and update the left track + self.add_cones_to_left_track(points) + self.left_tree = kdtree.create([Item(coord[0], coord[1], [self.left_track.cones[index], self.default_error_in_estimate, 1]) for index, coord in enumerate(points)]) # !!!!!! + else: + # If the left track is not empty, find the closest cone for each newly measured cone and update its coordinates or add it to the track + for coord in points: + point, distance = self.left_tree.search_nn(coord) + # If the newly measured cone is in the match radius, this cone already exists in the left track, update its coordinates + if distance <= self.match_radius: + cone = point.data.data[0] + error_in_estimate = point.data.data[1] + times_modified = point.data.data[2] * self.modify_rate + new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate) + point.data.coords = (new_estimate_x, new_estimate_y) + point.data.data[1] = new_error_in_estimate + point.data.data[2] = times_modified + # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map + else: + cone = Point() + cone.x = coord[0] + cone.y = coord[1] + self.left_track.cones.append(cone) + self.left_tree.add(Item(coord[0], coord[1], [cone, self.default_error_in_estimate, 1])) + + + + def update_right_track(self, points: list) -> None: + """This function updates the right track of the cone map + + Args: + points (list): A list of blue cones' global (x, y) coordinates + """ + if len(self.right_track.cones) == 0: + # If the right track is empty, pack all cone measurements and update the right track + self.add_cones_to_right_track(points) + self.right_tree = kdtree.create([Item(coord[0], coord[1], [self.right_track.cones[index], self.default_error_in_estimate, 1]) for index, coord in enumerate(points)]) # !!!!!! + else: + # If the right track is not empty, find the closest cone for each newly measured cone and update its coordinates or add it to the track + for coord in points: + point, distance = self.right_tree.search_nn(coord) + # If the newly measured cone is in the match radius, this cone already exists in the right track, update its coordinates + if distance <= self.match_radius: + cone = point.data.data[0] + error_in_estimate = point.data.data[1] + times_modified = point.data.data[2] * self.modify_rate + new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate) + point.data.coords = (new_estimate_x, new_estimate_y) + point.data.data[1] = new_error_in_estimate + point.data.data[2] = times_modified + # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map + else: + cone = Point() + cone.x = coord[0] + cone.y = coord[1] + self.right_track.cones.append(cone) + self.right_tree.add(Item(coord[0], coord[1], [cone, self.default_error_in_estimate, 1])) + + + def kalman_filtering(self, cone: Point, coord: tuple, error_in_estimate: float) -> tuple: + """This function update the x and y coordinate of of the input cone using kalman filtering + + Args: + cone (Point): input cone's location + coord (tuple): new measurement of (x, y) of the input cone + error_in_estimate (float): error in estimate of the cone's actual position + + Returns: + new_estimate_x (float): cone's new estimated x coordinate + new_estimate_y (float): cone's new estimated y coordinate + new_error_in_estimate (float): cone's new error in estimate + """ + # Get the previous error_in_estimate, estimated x and estimated y coordinate + prev_error_in_estimate = error_in_estimate + prev_estimate_x = cone.x + prev_estimate_y = cone.y + + # Calculate the kalman gain + kalman_gain = prev_error_in_estimate / (prev_error_in_estimate + self.error_in_measurement) + + # Calculate the new error_in_estimate, estimated x and estimated y coordinate + new_estimate_x = prev_estimate_x + kalman_gain * (coord[0] - prev_estimate_x) + new_estimate_y = prev_estimate_y + kalman_gain * (coord[1] - prev_estimate_y) + new_error_in_estimate = (1 - kalman_gain) * (prev_error_in_estimate) + + # update x and y coordinate + cone.x = new_estimate_x + cone.y = new_estimate_y + + return new_estimate_x, new_estimate_y, new_error_in_estimate + + + def cones_local_to_global(self, cones: list, car_position: Pose) -> list: + """This function converts all cones from the local frame to the global frame + + Args: + cones ([Point]): contains all the cones' positions in local frame + car_position (Pose): car's current position + + Returns: + global_cone_positions (list): a list contains each cone's global (x, y) + """ + # If no cones are detected, return an empty np.array + if len(cones) == 0: + return [] + + # Extract car's x coordinate, y coordinate and rotation angle + x, y, theta = self.extract_data_from_car(car_position) + + # Convert cones positions into a np.array contains each cone's local x coordinate, y coordinate + list_of_cones = self.convert_cones_to_data(cones) + + # Use cart's (x, y and theta) to get the cart's position vector and rotation matrix + position_vector, rotation_matrix = self.get_coordinate_conversion_matrices(x, y, theta) + + # Convert cone's coordinates from local frame to global frame + global_cone_columns = self.local_to_global(position_vector, rotation_matrix, list_of_cones) + + # Convert global_cone_columns to a list of cones' (x,y) position + global_cone_positions = [tuple(global_cone_columns[:,i]) for i in range(len(cones))] + + return global_cone_positions + + + def extract_data_from_car(self, car_position: Pose) -> tuple: + """This function extracts car's x coordinate, y coordinate and rotation angle from car_position + + Args: + car_position (Pose): car's current position + + Returns: + x (float): car's x coordinate + y (float): car's y coordinate + theta (float): car's rotation angle + """ + x = car_position.position.x + y = car_position.position.y + theta = car_position.orientation.w + return x, y, theta + + + def convert_cones_to_data(self, cones: list) -> np.array: + """Convert a list of cones' positions into a 2*n np.array contains each cone's local x coordinate and y coordinate + + Args: + cones ([Point]): A list of cones, each cone is represented as a Point + + Returns: + list_of_cones (np.array): 2*n np.array contains each cone's local x coordinate and y coordinate + """ + # Extract x and y coordinate from all cones and store them in a 2*n np.arra + list_of_local_cones_x = [cone.x for cone in cones] + list_of_local_cones_y = [cone.y for cone in cones] + list_of_cones = np.array([list_of_local_cones_x, list_of_local_cones_y]) + return list_of_cones + + + def get_coordinate_conversion_matrices(self, x: float, y: float, theta: float) -> tuple: + """This function gets car's gloabl position vector and local coordinate rotation matrix + + Args: + x (float): car's x coordinate + y (float): car's y coordinate + theta (float): car's rotation angle + + Returns: + position_vector (np.array): car's global position vector + rotation_matrix (np.array): rotation matrix for cone's local coordinates + """ + position_vector = np.array([[x],[y]]) + rotation_matrix = np.array([[math.cos(theta), -math.sin(theta)],[math.sin(theta), math.cos(theta)]]) + + return position_vector, rotation_matrix + + + def local_to_global(self, position_vector: np.array, rotation_matrix: np.array, list_of_cones: np.array) -> np.array: + """This function converts all cones' (x, y) from the local frame to the global frame + + Args: + position_vector (np.array): car's global position vector + rotation_matrix (np.array): rotation matrix for cone's local coordinates + list_of_cones (np.array): 2*n np.array contains each cone's local x coordinate and y coordinate + + Returns: + list_of_cones_output (np.array): 2*n np.array contains each cone's global x coordinate and y coordinate + """ + list_of_cones_unrotated = np.matmul(rotation_matrix, list_of_cones) + list_of_cones_output = list_of_cones_unrotated + position_vector + return list_of_cones_output + + + def add_cones_to_left_track(self, points: list) -> None: + """This function packs all cones' (x, y) coordinates and add them to the left track + + Args: + points (list): A list of cones' (x, y) coordinates + """ + for p in points: + point = Point() + point.x = p[0] + point.y = p[1] + self.left_track.cones.append(point) + + def add_cones_to_right_track(self, points: list) -> None: + """This function packs all cones' (x, y) coordinates and add them to the right track + + Args: + points (list): A list of cones' (x, y) coordinates + """ + for p in points: + point = Point() + point.x = p[0] + point.y = p[1] + self.right_track.cones.append(point) + + def remove_cones(self): + if len(self.left_track.cones) == 0 or len(self.right_track.cones) == 0: + return + + times_modified_list = Float32MultiArray() + + for point in kdtree.level_order(self.left_tree): + if point.data.data[2] < self.times_modified_limit: + times_modified_list.data.append(point.data.data[2]) + self.left_track.cones.remove(point.data.data[0]) + self.left_tree = self.left_tree.remove(point.data) + + for point in kdtree.level_order(self.right_tree): + if point.data.data[2] < self.times_modified_limit: + times_modified_list.data.append(point.data.data[2]) + self.right_track.cones.remove(point.data.data[0]) + self.right_tree = self.right_tree.remove(point.data) + + self.times_modified_publisher.publish(times_modified_list) + + +from rclpy.context import Context +from rclpy.executors import SingleThreadedExecutor + +def main(args=None): + ctx = Context() + rclpy.init(args=args, context=ctx) + executor = SingleThreadedExecutor(context=ctx) + cone_mapper = Cone_Mapper(context=ctx) + executor.add_node(cone_mapper) + + try: + executor.spin() # shuts down internally on SIGINT signal + except KeyboardInterrupt: + cone_mapper.get_logger().info("keyboard interrupt signal intercepted") + finally: + cone_mapper.get_logger().info("shutting down") + + cone_mapper.destroy_node() + + +if __name__ == '__main__': + main() diff --git a/src/perception/sythesis/cone_mapping/cone_mapping/old_cone_mapping.py b/src/perception/sythesis/cone_mapping/cone_mapping/old_cone_mapping.py new file mode 100644 index 00000000..185df738 --- /dev/null +++ b/src/perception/sythesis/cone_mapping/cone_mapping/old_cone_mapping.py @@ -0,0 +1,374 @@ +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy + +from moa_msgs.msg import ConeMap, Cones, Cone +from geometry_msgs.msg import Point, Quaternion, Pose, PoseWithCovariance + +import math +import numpy as np + +import kdtree + +class Item(object): + def __init__(self, x, y, data): + self.coords = (x, y) + self.data = data + + def __len__(self): + return len(self.coords) + + def __getitem__(self, i): + return self.coords[i] + + def __repr__(self): + return 'Item({}, {}, {})'.format(self.coords[0], self.coords[1], self.data) + + +class Cone_Mapper(Node): + def __init__(self): + super().__init__('cone_mapper') + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10 + ) + + # Create cone detection subscriber + self.cones_subscription = self.create_subscription( + Cones, + 'cone_detection', + self.cones_callback, + qos_profile) + + # Create car position subscriber + self.car_subscription = self.create_subscription( + Pose, + 'car_position', + self.car_position_callback, + qos_profile) + + # Store car positions + self.car_positions = [] + + # Create cone map publisher + self.publisher = self.create_publisher(ConeMap, 'cone_map', 10) + + # Cone types + self.blue = 0 + self.yellow = 2 + + # Existing cone map + self.Cone_map = None + + # KDTrees for searching + self.left_tree = None + self.right_tree = None + +################################################################################ (parameters to tune) + + # Initial error in the estimate + self.default_error_in_estimate = 5.0 + + # Error in measurement + self.error_in_measurement = 2.5 + + # Cone match radius + self.match_radius = 0.1 + +################################################################################ (parameters to tune) + + def car_position_callback(self, msg: Pose) -> None: + """This function receives the car's current position + + Args: + msg (Pose): input car position from the /car_position topic + """ + self.car_positions.append(msg) + + + def cones_callback(self, msg: Cones) -> None: + """This function updates the existing cone map and publish it to the /cone_map topic + + Args: + msg (Cones): input cones from the /cone_detection topic + """ + # Get the car's current position + while len(self.car_positions) == 0: + pass + car_position = self.car_positions.pop(0) + + # Update the existing cone map with new measurements + self.update_existing_cone_map(msg, car_position) + + # Publishes the existing cone map to the /cone_map topic + self.publisher.publish(self.Cone_map) + + + def update_existing_cone_map(self, msg: Cones, car_position: Pose) -> None: + """This function updates the existing cone map using new cone measurements + + Args: + msg (Cones): A list of detected cones + car_position (Pose): car's current position + """ + # Get global measurements + global_cone_columns = self.cones_local_to_global(msg, car_position) + + # Collect all cone's (x, y) + list_of_cones_x, list_of_cones_y, list_of_cones_type = global_cone_columns + left_points = [(list_of_cones_x[i], list_of_cones_y[i]) for i in range(len(list_of_cones_x)) if list_of_cones_type[i] == self.blue] + right_points = [(list_of_cones_x[i], list_of_cones_y[i]) for i in range(len(list_of_cones_x)) if list_of_cones_type[i] == self.yellow] + + # Update the existing cone map + if self.Cone_map == None: + # If the existing cone map is empty, pack all cone measurements and update the existing cone map + self.Cone_map = self.produce_cone_map_message(left_points, right_points) + self.left_tree = kdtree.create([Item(coord[0], coord[1], (index, self.default_error_in_estimate)) for index, coord in enumerate(left_points)]) + self.right_tree = kdtree.create([Item(coord[0], coord[1], (index, self.default_error_in_estimate)) for index, coord in enumerate(right_points)]) + else: + # If the existing cone map is not empty, find the closest cone for each newly measured cone and update its coordinates or add it to the map + self.update_left_track(left_points) + self.update_right_track(right_points) + + + def update_left_track(self, points: list) -> None: + """This function updates the left track of the cone map + + Args: + points (list): A list of blue cones' (x, y) coordinates + """ + for coord in points: + point, distance = self.left_tree.search_nn(coord) + # If the newly measured cone is in the match radius, this cone already exists in the existing cone map, update its coordinates + if distance <= self.match_radius: + index = point.data.data[0] + cone = self.Cone_map.left_cones[index] + error_in_estimate = point.data.data[1] + new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate) + point.data.coords = (new_estimate_x, new_estimate_y) + point.data.data[1] = new_error_in_estimate + else: + # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map + cone = Point() + cone.x = coord[0] + cone.y = coord[1] + self.Cone_map.left_cones.append(cone) + self.left_tree.add(Item(coord[0], coord[1], (len(self.Cone_map.left_cones) - 1, self.default_error_in_estimate))) + + + def update_right_track(self, points: list) -> None: + """This function updates the right track of the cone map + + Args: + points (list): A list of yellow cones' (x, y) coordinates + """ + for coord in points: + point, distance = self.right_tree.search_nn(coord) + # If the newly measured cone is in the match radius, this cone already exists in the existing cone map, update its coordinates + if distance <= self.match_radius: + index = point.data.data[0] + cone = self.Cone_map.right_cones[index] + error_in_estimate = point.data.data[1] + new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate) + point.data.coords = (new_estimate_x, new_estimate_y) + point.data.data[1] = new_error_in_estimate + else: + # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map + cone = Point() + cone.x = coord[0] + cone.y = coord[1] + self.Cone_map.right_cones.append(cone) + self.right_tree.add(Item(coord[0], coord[1], (len(self.Cone_map.left_cones) - 1, self.default_error_in_estimate))) + + + def kalman_filtering(self, cone: Point, coord: tuple, error_in_estimate: float) -> tuple: + """This function update the x and y coordinate of of the input cone using kalman filtering + + Args: + cone (Point): input cone's location + coord (tuple): new measurement of (x, y) of the input cone + error_in_estimate (float): error in estimate of the cone's actual position + + Returns: + new_estimate_x (float): cone's new estimated x coordinate + new_estimate_y (float): cone's new estimated y coordinate + new_error_in_estimate (float): cone's new error in estimate + """ + # Get the previous error_in_estimate, estimated x and estimated y coordinate + prev_error_in_estimate = error_in_estimate + prev_estimate_x = cone.x + prev_estimate_y = cone.y + + # Calculate the kalman gain + kalman_gain = prev_error_in_estimate / (prev_error_in_estimate + self.error_in_measurement) + + # Calculate the new error_in_estimate, estimated x and estimated y coordinate + new_estimate_x = prev_estimate_x + kalman_gain * (coord[0] - prev_estimate_x) + new_estimate_y = prev_estimate_y + kalman_gain * (coord[1] - prev_estimate_y) + new_error_in_estimate = (1 - kalman_gain) * (prev_error_in_estimate) + + # update x and y coordinate + cone.x = new_estimate_x + cone.y = new_estimate_y + + return new_estimate_x, new_estimate_y, new_error_in_estimate + + + def cones_local_to_global(self, msg: Cones, car_position: Pose) -> np.array: + """This function converts all cones from the local frame to the global frame + + Args: + msg (Cones): contains all the measured cones in local frame + car_position (Pose): car's current position + + Returns: + global_cone_columns (np.array): 3*n np.array contains each cone's global x coordinate, y coordinate and type + """ + # Extract car's x coordinate, y coordinate and rotation angle + x, y, theta = self.extract_data_from_car(car_position) + + # Convert cones message into a np.array contains each cone's local x coordinate, y coordinate and type + list_of_cones = self.convert_cones_to_data(msg) + + # Use cart's (x, y and theta) to get the cart's position vector and rotation matrix + position_vector, rotation_matrix = self.get_coordinate_conversion_matrices(x, y, theta) + + # Convert cone's coordinates from local frame to global frame + global_cone_columns = self.local_to_global(position_vector, rotation_matrix, list_of_cones) + + return global_cone_columns + + + def extract_data_from_car(self, car_position: Pose) -> tuple: + """This function extracts car's x coordinate, y coordinate and rotation angle from car_position + + Args: + car_position (Pose): car's current position + + Returns: + x (float): car's x coordinate + y (float): car's y coordinate + theta (float): car's rotation angle + """ + x = car_position.position.x + y = car_position.position.y + theta = car_position.orientation.w + return x, y, theta + + + def convert_cones_to_data(self, msg: Cones) -> np.array: + """Convert cones message into a 3*n np.array contains each cone's local x coordinate, y coordinate and type + + Args: + msg (Cones): cones message + + Returns: + list_of_cones (np.array): 3*n np.array contains each cone's local x coordinate, y coordinate and type + """ + list_of_cones = msg.cones + + # Store cones' data + list_of_local_cones_x = [] + list_of_local_cones_y = [] + list_of_local_cones_type = [] + + # Extract data from all cone messages and store them in a 3*n np.array + for cone in list_of_cones: + individual_x, individual_y, individual_type = self.extract_data_from_cone(cone) + list_of_local_cones_x.append(individual_x) + list_of_local_cones_y.append(individual_y) + list_of_local_cones_type.append(individual_type) + + list_of_cones = np.array([list_of_local_cones_x, list_of_local_cones_y, list_of_local_cones_type]) + + return list_of_cones + + + def extract_data_from_cone(self, cone_input: Cone) -> tuple: + """This function extracts data from the cone message + + Args: + cone_input (Cone): cone message + + Returns: + x (float): cone's x coordinate + y (float): cone's y coordinate + type (int): cone's type + """ + x = cone_input.position.x + y = cone_input.position.y + cone_type = cone_input.type + return x, y, cone_type + + + def get_coordinate_conversion_matrices(self, x: float, y: float, theta: float) -> tuple: + """This function gets car's gloabl position vector and local coordinate rotation matrix + + Args: + x (float): car's x coordinate + y (float): car's y coordinate + theta (float): car's rotation angle + + Returns: + position_vector (np.array): car's global position vector + rotation_matrix (np.array): rotation matrix for cone's local coordinates + """ + position_vector = np.array([[x],[y],[0]]) + rotation_matrix = np.array([[math.cos(theta), -math.sin(theta), 0],[math.sin(theta), math.cos(theta), 0], [0, 0, 1]]) + + return position_vector, rotation_matrix + + + def local_to_global(self, position_vector: np.array, rotation_matrix: np.array, list_of_cones: np.array) -> np.array: + """This function converts all cones data (x, y, type) from the local frame to the global frame + + Args: + position_vector (np.array): car's global position vector + rotation_matrix (np.array): rotation matrix for cone's local coordinates + list_of_cones (np.array): 3*n np.array contains each cone's local x coordinate, y coordinate and type + + Returns: + list_of_cones_output (np.array): 3*n np.array contains each cone's global x coordinate, y coordinate and type + """ + list_of_cones_unrotated = np.matmul(rotation_matrix, list_of_cones) + list_of_cones_output = list_of_cones_unrotated + position_vector + return list_of_cones_output + + + def produce_cone_map_message(self, left_points: list, right_points: list) -> ConeMap: + """This function packs all cones' (x, y) coordinates into a cone map message + + Args: + left_points (list): A list of blue cones' (x, y) coordinates + right_points (list): A list of yellow cones' (x, y) coordinates + + Returns: + output_map (ConeMap): cone map message + """ + output_map = ConeMap() + for lp in left_points: + point = Point() + point.x = lp[0] + point.y = lp[1] + output_map.left_cones.append(point) + + for rp in right_points: + point = Point() + point.x = rp[0] + point.y = rp[1] + output_map.right_cones.append(point) + + return output_map + + +def main(args=None): + rclpy.init(args=args) + cone_mapper = Cone_Mapper() + rclpy.spin(cone_mapper) + cone_mapper.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/perception/cone_mapping/cone_mapping/cone_mapping/subscriber_member_function.py b/src/perception/sythesis/cone_mapping/cone_mapping/old_kalman_filter.py similarity index 89% rename from src/perception/cone_mapping/cone_mapping/cone_mapping/subscriber_member_function.py rename to src/perception/sythesis/cone_mapping/cone_mapping/old_kalman_filter.py index 548c301a..00234723 100644 --- a/src/perception/cone_mapping/cone_mapping/cone_mapping/subscriber_member_function.py +++ b/src/perception/sythesis/cone_mapping/cone_mapping/old_kalman_filter.py @@ -20,8 +20,7 @@ from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy from std_msgs.msg import String -from moa_msgs.msg import ConeMap -from moa_msgs.msg import Cone +from moa_msgs.msg import ConeMap, Cones, Cone from geometry_msgs.msg import Point from geometry_msgs.msg import Quaternion from geometry_msgs.msg import PoseWithCovariance; @@ -33,6 +32,9 @@ #import matplotlib.pyplot as plt import time +import threading +from std_msgs.msg import Float32 + class Cone_Mapper(Node): def __init__(self): @@ -44,9 +46,14 @@ def __init__(self): depth=10 ) self.subscription = self.create_subscription( - ConeMap, + Cones, 'cone_detection', - self.listener_callback, + self.cones_callback, + qos_profile) + self.subscription = self.create_subscription( + Pose, + 'car_position', + self.car_position_callback, qos_profile) self.subscription # prevent unused variable warning @@ -94,18 +101,73 @@ def __init__(self): #self.Kalman_gain = 1; self.counter = 0 + def car_position_callback(self, pose: Pose) -> None: + self.pose.x + +################################################################################ (measure duration for each cone map update) + + # Create update duration publisher + self.duration_publisher = self.create_publisher(Float32, 'duration', 10) + + # Record the total update duration + self.total_time = 0 + + # Record the number of updates + self.counter = 0 + + # Lock for thread safety + self.lock = threading.Lock() + + # Create a timer to calculate the average duration for each cone map update + self.timer = self.create_timer(10.0, self.average_time_callback) + + def average_time_callback(self): + """This function calculates and publishes the average cone map update duration every 10s to the /duration topic and write to a file + """ + # Calculate the average update duration for the most recent 10s + with self.lock: + try: + average_duration = self.total_time / self.counter + except ZeroDivisionError: + average_duration = 0.0 + self.total_time = 0 + self.counter = 0 + + # Write the average update duration to a file + with open("/home/fsae/Documents/pang/cone_mapping_data/kf.txt", "a") as file: + file.write(f"{average_duration}\n") + + # Publishes the average update duration to the /duration topic + duration_msg = Float32() + duration_msg.data = average_duration + self.duration_publisher.publish(duration_msg) def listener_callback(self, msg): + # Update the existing cone map with new measurements + before = self.get_clock().now() + self.kalman_filter_update(msg) + after = self.get_clock().now() + + # Calculate the update duration in micro seconds + duration = (after - before).nanoseconds / 1000 + with self.lock: + self.total_time += duration + self.counter += 1 + + self.publisher.publish(self.Cone_map) + # self.get_logger().info('Mapped result: "%s"' % msg.cones) #print("Listened") #self.Transformation_test(msg); #self.publisher.publish(msg) # for debug - self.kalman_filter_update(msg) - self.always_trust_position() - self.publisher.publish(self.Cone_map) + # self.Add_All_Measurement_Test(msg); + #self.kalman_filter_update(msg) + + #self.always_trust_position() + # self.publisher.publish(self.Cone_map) - self.get_logger().info("Cone Map Published") + # self.get_logger().info("Cone Map Published") # print("######################New Message##########################") # is_first = True @@ -127,7 +189,8 @@ def listener_callback(self, msg): # time.sleep(1) ####Temporal test functions############################################################################################## - def Transformation_test(self, msg : ConeMap): + + def Add_All_Measurement_Test(self, msg: ConeMap): """Extract measurement state from the Cone Map message subscription Args: @@ -142,7 +205,6 @@ def Transformation_test(self, msg : ConeMap): # Convert Cone Map message into position (x and y), orientation (theta) and list of cones x, y, theta, list_of_cones = self.convert_message_to_data(msg) # Use list of cones and states (x, y and theta) to get the position vector and rotation matrix - # position_vector, rotation_matrix, list_of_cones = self.convert_to_input_matrix(x, y, theta - np.pi / 2, list_of_cones); position_vector, rotation_matrix, list_of_cones = self.convert_to_input_matrix(x, y, theta, list_of_cones); # Conversion from local reference frame to global reference frame new_cone_columns = self.create_cone_map(position_vector, rotation_matrix, list_of_cones) @@ -151,8 +213,12 @@ def Transformation_test(self, msg : ConeMap): # Get unsorted Cone Map that contains all measured cone map at moment cone_map_measurement_unsorted = self.produce_cone_map_message(x, y, theta, self.cone_map_array_measured) # Produce map message - self.publisher.publish(cone_map_measurement_unsorted); + # Sort cones that is measured into the cones that are logged into the map. If the cone is new, add new logged cone. + self.Cone_map.cones = self.Cone_map.cones + cone_map_measurement_unsorted.cones[1:] + + # Reset orientation whenever prediction to measurement differences of orientation has 2 pi differencnes\ + # self.periodic_orientation(); ####SLAM fucntion below################################################################################################################################ def sort_and_add_cones(self, cone_map_measurement_input : ConeMap) -> ConeMap: @@ -447,16 +513,16 @@ def extract_data_from_cone(self, cone_input : Cone): covaraince = cone_input.pose.covariance; color = cone_input.colour; return x, y, theta, covaraince, color - + def convert_to_input_matrix(self, x: float, y: float, theta: float, list_of_cones: np.array) -> (np.array, np.array, np.array): '''Convert state and list_of_cones input into position vector, rotation matrix (DCM) and the matrix of list of cones - + Args: x: x position specified in float y: y position specified in float theta: theta orientation specified in float list_of_cones: np.array (numpy array) for matrix of cone positions in 2 by n matrix (n is number of cones recorded in input)) - + Returns: position_vector: np.array of position vector of cart rotation_matrix: np.array DCM matrix to convert reading from local frame into global frame @@ -468,7 +534,7 @@ def convert_to_input_matrix(self, x: float, y: float, theta: float, list_of_cone position_vector = np.array([[x],[y],[0]]); rotation_matrix = np.array([[math.cos(theta), -math.sin(theta), 0],[math.sin(theta), math.cos(theta), 0], [0, 0, 1]]) #Inverse DCM - return position_vector, rotation_matrix, list_of_cones; + return position_vector, rotation_matrix, list_of_cones def create_cone_map(self, position_vector : np.array, rotation_matrix : np.array, list_of_cones : np.array) -> np.array: list_of_cones_unrotated = np.matmul(rotation_matrix, list_of_cones) @@ -517,7 +583,7 @@ def produce_cone_map_message(self, x : float, y : float, theta : float, list_of_ for index in range(length): cone_input = self.pack_cone_message(list_of_cones_x[index], list_of_cones_y[index], 0.0, index + 1, self.default_cone_covariance, int(list_of_cones_color[index])); output_map.cones.append(cone_input); - return output_map; + return output_map def is_repeating(self, cone_x : float, cone_y : float, target_cone_x_list, target_cone_y_list, tolerance : float) -> bool: number_of_cones = len(target_cone_x_list); diff --git a/src/perception/sythesis/cone_mapping/cone_mapping/old_kalman_filter_improved.py b/src/perception/sythesis/cone_mapping/cone_mapping/old_kalman_filter_improved.py new file mode 100644 index 00000000..842a2458 --- /dev/null +++ b/src/perception/sythesis/cone_mapping/cone_mapping/old_kalman_filter_improved.py @@ -0,0 +1,394 @@ +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy + +from std_msgs.msg import Float32 +from moa_msgs.msg import ConeMap +from moa_msgs.msg import Cone +from geometry_msgs.msg import Point +from geometry_msgs.msg import Quaternion +from geometry_msgs.msg import PoseWithCovariance +from geometry_msgs.msg import Pose + +import math +import numpy as np +import threading + +import kdtree + +class Item(object): + def __init__(self, x, y, index): + self.coords = (x, y) + self.index = index + + def __len__(self): + return len(self.coords) + + def __getitem__(self, i): + return self.coords[i] + + def __repr__(self): + return 'Item({}, {}, {})'.format(self.coords[0], self.coords[1], self.index) + + +class Cone_Mapper(Node): + + def __init__(self): + super().__init__('cone_mapper') + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10 + ) + + # Create cone detection subscriber + self.subscription = self.create_subscription( + ConeMap, + 'cone_detection', + self.listener_callback, + qos_profile) + self.subscription # prevent unused variable warning + + # Create cone map publisher + self.publisher = self.create_publisher(ConeMap, 'cone_map', 10) + + # Existing cone map + self.Cone_map = None + + # KDTree + self.tree = None + + # Current cone id + self.current_cone_id = 1 + +################################################################################ (parameters to tune) + + # Initial error in the estimate + self.default_cone_covariance = [5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + # Error in measurement + self.error_in_measurement = 2.5 + + # Cone match radius + self.match_radius = 0.1 + +################################################################################ (measure duration for each cone map update) + + # Create update duration publisher + self.duration_publisher = self.create_publisher(Float32, 'duration', 10) + + # Record the total update duration + self.total_time = 0 + + # Record the number of updates + self.counter = 0 + + # Lock for thread safety + self.lock = threading.Lock() + + # Create a timer to calculate the average duration for each cone map update + self.timer = self.create_timer(10.0, self.average_time_callback) + + def average_time_callback(self): + """This function calculates and publishes the average cone map update duration every 10s to the /duration topic and write to a file + """ + # Calculate the average update duration for the most recent 10s + with self.lock: + try: + average_duration = self.total_time / self.counter + except ZeroDivisionError: + average_duration = 0.0 + self.total_time = 0 + self.counter = 0 + + # Write the average update duration to a file + with open("/home/fsae/Documents/pang/cone_mapping_data/ikf.txt", "a") as file: + file.write(f"{average_duration}\n") + + # Publishes the average update duration to the /duration topic + duration_msg = Float32() + duration_msg.data = average_duration + self.duration_publisher.publish(duration_msg) + +################################################################################ (cone_mapping functions) + + def listener_callback(self, msg: ConeMap) -> None: + """This function updates the existing cone map and publish it to the /cone_map topic + + Args: + msg (ConeMap): input cone map from the /cone_detection topic + """ + # Update the existing cone map with new measurements + before = self.get_clock().now() + self.update_existing_cone_map(msg) + after = self.get_clock().now() + + # Calculate the update duration in micro seconds + duration = (after - before).nanoseconds / 1000 + with self.lock: + self.total_time += duration + self.counter += 1 + + # Publishes the existing cone map to the /cone_map topic + self.publisher.publish(self.Cone_map) + + + def update_existing_cone_map(self, msg: ConeMap) -> None: + """This function updates the existing cone map using new measurements + + Args: + msg (ConeMap): input cone map message + """ + # Get global measurements + x, y, theta, global_cone_columns = self.cone_map_local_to_global(msg) + + # Collect all cone's (x, y) + list_of_cones_x, list_of_cones_y, list_of_cones_color = global_cone_columns + measured_points = [(list_of_cones_x[i], list_of_cones_y[i]) for i in range(len(list_of_cones_x))] + + # Update the existing cone map + if self.Cone_map == None: + # If the existing cone map is empty, pack all cone measurements and update the existing cone map + self.Cone_map = self.produce_cone_map_message(x, y, theta, global_cone_columns) + self.tree = kdtree.create([Item(coord[0], coord[1], index + 1) for index, coord in enumerate(measured_points)]) + self.current_cone_id = len(measured_points) + 1 + else: + # Update the cart position in the existing cone map + cart = self.Cone_map.cones[0] + cart.pose.pose.position.x = x + cart.pose.pose.position.y = y + cart.pose.pose.orientation.w = theta + + # If the existing cone map is not empty, find the closest cone for each newly measured cone + for i, coord in enumerate(measured_points): + point, distance = self.tree.search_nn(coord) + # If the newly measured cone is in the match radius, this cone already exists in the existing cone map, update its coordinates + if distance <= self.match_radius: + index = point.data.index + cone = self.Cone_map.cones[index] + point.data.coords = self.kalman_filtering(cone, coord) + else: + # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map + cone = self.pack_cone_message(coord[0], coord[1], 0.0, self.default_cone_covariance, list_of_cones_color[i], self.current_cone_id) + self.Cone_map.cones.append(cone) + self.tree.add(Item(coord[0], coord[1], self.current_cone_id)) + self.current_cone_id += 1 + + + def kalman_filtering(self, cone: Cone, coord: tuple) -> tuple: + """This function update the x and y coordinate of of the input cone using kalman filtering + + Args: + cone (Cone): input cone message + coord (tuple): new measurement of (x, y) of the input cone + + Returns: + new_estimate_x (float): cart's new estimated x coordinate + new_estimate_y (float): cart's new estimated y coordinate + """ + # Get the previous error_in_estimate, estimated x and estimated y coordinate + prev_error_in_estimate = cone.pose.covariance[0] + prev_estimate_x = cone.pose.pose.position.x + prev_estimate_y = cone.pose.pose.position.y + + # Calculate the kalman gain + kalman_gain = prev_error_in_estimate / (prev_error_in_estimate + self.error_in_measurement) + + # Calculate the new error_in_estimate, estimated x and estimated y coordinate + new_estimate_x = prev_estimate_x + kalman_gain * (coord[0] - prev_estimate_x) + new_estimate_y = prev_estimate_y + kalman_gain * (coord[1] - prev_estimate_y) + new_error_in_estimate = (1 - kalman_gain) * (prev_error_in_estimate) + + # update x and y coordinate and error_in_estimate + cone.pose.pose.position.x = new_estimate_x + cone.pose.pose.position.y = new_estimate_y + cone.pose.covariance[0] = new_error_in_estimate + + return new_estimate_x, new_estimate_y + + + def cone_map_local_to_global(self, msg: ConeMap) -> tuple: + """This function converts all cones in a cone map from the local frame to the global frame + + Args: + msg (ConeMap): cone map that contains all the measured cones in local frame + + Returns: + x (float): cart's x coordinate + y (float): cart's y coordinate + theta (float): cart's rotation angle + global_cone_columns (np.array): 3*n np.array contains each cone's global x coordinate, y coordinate and color + """ + # Convert cone map message into cart's (x, y and theta) and list of cones (cones' x,y and color) + x, y, theta, list_of_cones = self.convert_cone_map_to_data(msg) + + # Use cart's (x, y and theta) to get the cart's position vector and rotation matrix + position_vector, rotation_matrix = self.get_coordinate_conversion_matrices(x, y, theta) + + # Convert cone's coordinates from local frame to global frame + global_cone_columns = self.local_to_global(position_vector, rotation_matrix, list_of_cones) + + return x, y, theta, global_cone_columns + + + def convert_cone_map_to_data(self, msg: ConeMap) -> tuple: + """This function extracts data from the cone map message + + Args: + msg (ConeMap): cone map message + + Returns: + x (float): cart's x coordinate + y (float): cart's y coordinate + theta (float): cart's rotation angle + list_of_cones (np.array): 3*n np.array contains each cone's local x coordinate, y coordinate and color + """ + # Get the first cone (The first cone in the cone map contains the cart's pose data) + cart = msg.cones[0] + + # Extract the cart's pose data + x, y, theta, *other = self.extract_data_from_cone(cart) + + # Get all the cones in the cone map + list_of_cones = msg.cones[1:] + + # Store cones' data + list_of_local_cones_x = [] + list_of_local_cones_y = [] + list_of_local_cones_color = [] + + # Extract data from all cone messages and store them in a 3*n np.array + length = len(list_of_cones) + for index in range(length): + individual_x, individual_y, individual_theta, individual_covariance, individual_color, individual_cone_id = self.extract_data_from_cone(list_of_cones[index]) + list_of_local_cones_x.append(individual_x) + list_of_local_cones_y.append(individual_y) + list_of_local_cones_color.append(individual_color) + + list_of_cones = np.array([list_of_local_cones_x, list_of_local_cones_y, list_of_local_cones_color]) + + return x, y, theta, list_of_cones + + + def extract_data_from_cone(self, cone_input: Cone) -> tuple: + """This function extracts data from the cone message + + Args: + cone_input (Cone): cone message + + Returns: + x (float): cone's x coordinate + y (float): cone's y coordinate + theta (float): cone's rotation angle (only applicable when the cone is the cart) + covariance (list[float]): cone's covariance vector + color (int): cone's color + cone_id (int): cone's id + """ + x = cone_input.pose.pose.position.x + y = cone_input.pose.pose.position.y + theta = cone_input.pose.pose.orientation.w + covariance = cone_input.pose.covariance + color = cone_input.colour + cone_id = cone_input.id + return x, y, theta, covariance, color, cone_id + + + def get_coordinate_conversion_matrices(self, x: float, y: float, theta: float) -> tuple: + """This function gets cart's gloabl position vector and local coordinate rotation matrix + + Args: + x (float): cart's x coordinate + y (float): cart's y coordinate + theta (float): cart's rotation angle + + Returns: + position_vector (np.array): cart's global position vector + rotation_matrix (np.array): rotation matrix for cone's local coordinates + """ + position_vector = np.array([[x],[y],[0]]) + rotation_matrix = np.array([[math.cos(theta), -math.sin(theta), 0],[math.sin(theta), math.cos(theta), 0], [0, 0, 1]]) + + return position_vector, rotation_matrix + + + def local_to_global(self, position_vector: np.array, rotation_matrix: np.array, list_of_cones: np.array) -> np.array: + """This function converts all cones data (x, y, color) from the local frame to the global frame + + Args: + position_vector (np.array): cart's global position vector + rotation_matrix (np.array): rotation matrix for cone's local coordinates + list_of_cones (np.array): 3*n np.array contains each cone's local x coordinate, y coordinate and color + + Returns: + list_of_cones_output (np.array): 3*n np.array contains each cone's global x coordinate, y coordinate and color + """ + list_of_cones_unrotated = np.matmul(rotation_matrix, list_of_cones) + list_of_cones_output = list_of_cones_unrotated + position_vector + return list_of_cones_output + + + def pack_cone_message(self, x: float, y: float, theta: float, covariance_vector: list[float], color: int, cone_id: int) -> Cone: + """This function packs a cone's data into a cone message + + Args: + x (float): cone's x coordinate + y (float): cone's y coordinate + theta (float): cart's rotation angle (not applicable) + covariance_vector (list[float]): cone's covariance_vector + color (int): cone's color + cone_id (int): cone's id + + Returns: + Cone: cone message + """ + output_cone = Cone() + position = Point() + orientation = Quaternion() + pose_with_covariance = PoseWithCovariance() + pose = Pose() + position.x = x + position.y = y + orientation.w = theta + pose.position = position + pose.orientation = orientation + pose_with_covariance.pose = pose + pose_with_covariance.covariance = covariance_vector + output_cone.pose = pose_with_covariance + output_cone.id = cone_id + output_cone.colour = int(color) + return output_cone + + + def produce_cone_map_message(self, x: float, y: float, theta: float, list_of_cones: np.array) -> ConeMap: + """This function packs cart's data and all cones' data into a cone map message + + Args: + x (float): cart's x coordinate + y (float): cart's y coordinate + theta (float): cart's rotation angle + list_of_cones (np.array): 3*n np.array contains each cone's x coordinate, y coordinate and color + + Returns: + output_map (ConeMap): cone map message + """ + output_map = ConeMap() + cart = self.pack_cone_message(x, y, theta, np.zeros(36), 0, 0) + output_map.cones.append(cart) + list_of_cones_x, list_of_cones_y, list_of_cones_color = list_of_cones + length = len(list_of_cones_x) + for index in range(length): + cone = self.pack_cone_message(list_of_cones_x[index], list_of_cones_y[index], 0.0, self.default_cone_covariance, int(list_of_cones_color[index]), index + 1) + output_map.cones.append(cone) + return output_map + + +def main(args=None): + rclpy.init(args=args) + cone_mapper = Cone_Mapper() + rclpy.spin(cone_mapper) + cone_mapper.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/perception/cone_mapping/cone_mapping/cone_mapping/test.py b/src/perception/sythesis/cone_mapping/cone_mapping/test.py similarity index 100% rename from src/perception/cone_mapping/cone_mapping/cone_mapping/test.py rename to src/perception/sythesis/cone_mapping/cone_mapping/test.py diff --git a/src/perception/sythesis/cone_mapping/cone_mapping/tests/test_mapper_kalman_filter.py b/src/perception/sythesis/cone_mapping/cone_mapping/tests/test_mapper_kalman_filter.py new file mode 100644 index 00000000..ee3f4c5c --- /dev/null +++ b/src/perception/sythesis/cone_mapping/cone_mapping/tests/test_mapper_kalman_filter.py @@ -0,0 +1,28 @@ +from perception.sythesis.cone_mapping.cone_mapping.kalman_filter import Cone_Mapper +from moa_msgs.msg import ConeMap + +def Transformation_test(mapper: Cone_Mapper, msg : ConeMap): + """Extract measurement state from the Cone Map message subscription + + Args: + msg: Input ConeMap message from Cone detection + + Returns: + None + + Raises: + None + """ + # Convert Cone Map message into position (x and y), orientation (theta) and list of cones + x, y, theta, list_of_cones = mapper.convert_message_to_data(msg) + # Use list of cones and states (x, y and theta) to get the position vector and rotation matrix + # position_vector, rotation_matrix, list_of_cones = mapper.convert_to_input_matrix(x, y, theta - np.pi / 2, list_of_cones); + position_vector, rotation_matrix, list_of_cones = mapper.convert_to_input_matrix(x, y, theta, list_of_cones); + # Conversion from local reference frame to global reference frame + new_cone_columns = mapper.create_cone_map(position_vector, rotation_matrix, list_of_cones) + mapper.cone_map_array_measured = new_cone_columns; # Produce latest measurement + + # Get unsorted Cone Map that contains all measured cone map at moment + cone_map_measurement_unsorted = mapper.produce_cone_map_message(x, y, theta, + mapper.cone_map_array_measured) # Produce map message + mapper.publisher.publish(cone_map_measurement_unsorted); diff --git a/src/perception/sythesis/cone_mapping/launch/cone_mapping.launch.py b/src/perception/sythesis/cone_mapping/launch/cone_mapping.launch.py new file mode 100644 index 00000000..14cd284a --- /dev/null +++ b/src/perception/sythesis/cone_mapping/launch/cone_mapping.launch.py @@ -0,0 +1,44 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ament_index_python import get_package_prefix +from launch.actions import OpaqueFunction +import os + +def generate_launch_description(): + package_name = 'cone_mapping' + package_dir = os.path.join(get_package_prefix(package_name), 'lib', package_name) # directory of installed node names + mappers = os.listdir(package_dir) # retrieves installed node names + + launch_descriptions = [ + # launch arguments + DeclareLaunchArgument( + 'node_name', + default_value='kalman_filter', + description='which node from cone_mapping package to launch' + ) + ] + + node = OpaqueFunction(function=get_node, args=[package_name, mappers]) # get a list of actions + launch_descriptions.append(node) + + return LaunchDescription(launch_descriptions) + +def get_node(context, package_name, mappers): + NODE = None + node_2_run = LaunchConfiguration('node_name').perform(context) # get runtime value of argument + + for node_name in mappers: + if node_name == node_2_run: + node = Node( + package=package_name, + executable=node_name, + name=node_name, + ) + NODE = node + break + else: + raise Exception(f"selected node {node_2_run} does not exist!") + + return NODE diff --git a/src/perception/cone_mapping/cone_mapping/package.xml b/src/perception/sythesis/cone_mapping/package.xml similarity index 80% rename from src/perception/cone_mapping/cone_mapping/package.xml rename to src/perception/sythesis/cone_mapping/package.xml index af561814..dc5979be 100644 --- a/src/perception/cone_mapping/cone_mapping/package.xml +++ b/src/perception/sythesis/cone_mapping/package.xml @@ -7,6 +7,13 @@ dyu056 TODO: License declaration + ackermann_msgs + geometry_msgs + std_msgs + moa_msgs + + python3-numpy + ament_copyright ament_flake8 ament_pep257 diff --git a/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/resource/cone_map_foxglove_visualizer b/src/perception/sythesis/cone_mapping/resource/cone_mapping similarity index 100% rename from src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/resource/cone_map_foxglove_visualizer rename to src/perception/sythesis/cone_mapping/resource/cone_mapping diff --git a/src/perception/cone_mapping/cone_mapping/setup.cfg b/src/perception/sythesis/cone_mapping/setup.cfg similarity index 100% rename from src/perception/cone_mapping/cone_mapping/setup.cfg rename to src/perception/sythesis/cone_mapping/setup.cfg diff --git a/src/perception/cone_mapping/cone_mapping/setup.py b/src/perception/sythesis/cone_mapping/setup.py similarity index 82% rename from src/perception/cone_mapping/cone_mapping/setup.py rename to src/perception/sythesis/cone_mapping/setup.py index ee5c7434..fcf2e5c9 100644 --- a/src/perception/cone_mapping/cone_mapping/setup.py +++ b/src/perception/sythesis/cone_mapping/setup.py @@ -20,7 +20,8 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'listener = cone_mapping.subscriber_member_function:main', + 'kalman_filter = cone_mapping.kalman_filter:main', + 'db_scan = cone_mapping.db_scan:main', ], }, ) diff --git a/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/test/test_copyright.py b/src/perception/sythesis/cone_mapping/test/test_copyright.py similarity index 100% rename from src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/test/test_copyright.py rename to src/perception/sythesis/cone_mapping/test/test_copyright.py diff --git a/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/test/test_flake8.py b/src/perception/sythesis/cone_mapping/test/test_flake8.py similarity index 100% rename from src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/test/test_flake8.py rename to src/perception/sythesis/cone_mapping/test/test_flake8.py diff --git a/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/test/test_pep257.py b/src/perception/sythesis/cone_mapping/test/test_pep257.py similarity index 100% rename from src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/test/test_pep257.py rename to src/perception/sythesis/cone_mapping/test/test_pep257.py diff --git a/src/perception/sythesis/localization/LICENSE b/src/perception/sythesis/localization/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/src/perception/sythesis/localization/LICENSE @@ -0,0 +1,202 @@ + + 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/src/perception/sythesis/localization/launch/localization_launch.py b/src/perception/sythesis/localization/launch/localization_launch.py new file mode 100644 index 00000000..38e55b57 --- /dev/null +++ b/src/perception/sythesis/localization/launch/localization_launch.py @@ -0,0 +1,17 @@ +#!/usr/bin/env python3 + +# no parameters, run ros2 launch localization localization_launch.py + + +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='localization', + executable='localization', + name='localization', + output='screen' + ), + ]) diff --git a/src/visualization/path_planning_visualization/resource/path_planning_visualization b/src/perception/sythesis/localization/localization/__init__.py similarity index 100% rename from src/visualization/path_planning_visualization/resource/path_planning_visualization rename to src/perception/sythesis/localization/localization/__init__.py diff --git a/src/perception/localization/localization/localization.py b/src/perception/sythesis/localization/localization/localization.py similarity index 60% rename from src/perception/localization/localization/localization.py rename to src/perception/sythesis/localization/localization/localization.py index 2841f878..5f70f644 100644 --- a/src/perception/localization/localization/localization.py +++ b/src/perception/sythesis/localization/localization/localization.py @@ -5,7 +5,7 @@ from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy from std_msgs.msg import String from geometry_msgs.msg import Pose, PoseArray -from moa_msgs.msg import ConeMap, Cone +from moa_msgs.msg import Track class Localization(Node): @@ -17,19 +17,34 @@ def __init__(self): history=QoSHistoryPolicy.KEEP_LAST, depth=10 ) + self.left_cones = [] + self.right_cones = [] self.localization_publisher = self.create_publisher(Pose, 'car_position', 5) - self.cone_detection_subscription = self.create_subscription( - ConeMap, - 'cone_detection', - self.listener_callback, + self.cone_detection_subscription_left_track = self.create_subscription( + Track, + 'left_track', + self.left_cone_map_callback, qos_profile) + + self.subscription_right_cone_map = self.create_subscription( + Track, + 'right_track', + self.right_cone_map_callback, + qos_profile) + self.get_logger().info('Localization node started') - def listener_callback(self, msg: ConeMap): - car_cone = msg.cones[0] - car_pose = car_cone.pose.pose - self.localization_publisher.publish(car_pose) - self.get_logger().info("Car localization data published") + # def listener_callback(self, msg: Track): + # car_cone = msg.cones[0] + # car_pose = car_cone.pose.pose + # self.localization_publisher.publish(car_pose) + # self.get_logger().info("Car localization data published") + + def left_cone_map_callback(self, msg:Track) -> None: + self.left_cones = msg.cones + + def right_cone_map_callback(self, msg:Track) -> None: + self.right_cones = msg.cones def main(args=None): diff --git a/src/perception/localization/package.xml b/src/perception/sythesis/localization/package.xml similarity index 83% rename from src/perception/localization/package.xml rename to src/perception/sythesis/localization/package.xml index 7f00590a..1f4ee05d 100644 --- a/src/perception/localization/package.xml +++ b/src/perception/sythesis/localization/package.xml @@ -7,6 +7,11 @@ dyu056 Apache-2.0 + ackermann_msgs + geometry_msgs + std_msgs + moa_msgs + ament_copyright ament_flake8 ament_pep257 diff --git a/src/visualization/pure_pursuit_visualizer/resource/pure_pursuit_visualizer b/src/perception/sythesis/localization/resource/localization similarity index 100% rename from src/visualization/pure_pursuit_visualizer/resource/pure_pursuit_visualizer rename to src/perception/sythesis/localization/resource/localization diff --git a/src/perception/localization/setup.cfg b/src/perception/sythesis/localization/setup.cfg similarity index 100% rename from src/perception/localization/setup.cfg rename to src/perception/sythesis/localization/setup.cfg diff --git a/src/perception/localization/setup.py b/src/perception/sythesis/localization/setup.py similarity index 82% rename from src/perception/localization/setup.py rename to src/perception/sythesis/localization/setup.py index 2c31dc26..7710a43b 100644 --- a/src/perception/localization/setup.py +++ b/src/perception/sythesis/localization/setup.py @@ -1,4 +1,6 @@ from setuptools import find_packages, setup +import os +from glob import glob package_name = 'localization' @@ -10,6 +12,8 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + # include the launch directory + (os.path.join('share', package_name, 'localization'), glob('launch/*.py')), ], install_requires=['setuptools'], zip_safe=True, diff --git a/src/visualization/path_planning_visualization/test/test_copyright.py b/src/perception/sythesis/localization/test/test_copyright.py similarity index 100% rename from src/visualization/path_planning_visualization/test/test_copyright.py rename to src/perception/sythesis/localization/test/test_copyright.py diff --git a/src/visualization/path_planning_visualization/test/test_flake8.py b/src/perception/sythesis/localization/test/test_flake8.py similarity index 100% rename from src/visualization/path_planning_visualization/test/test_flake8.py rename to src/perception/sythesis/localization/test/test_flake8.py diff --git a/src/visualization/path_planning_visualization/test/test_pep257.py b/src/perception/sythesis/localization/test/test_pep257.py similarity index 100% rename from src/visualization/path_planning_visualization/test/test_pep257.py rename to src/perception/sythesis/localization/test/test_pep257.py diff --git a/src/perception/virtual_sensor/launch/virtual_sensor.launch.py b/src/perception/virtual_sensor/launch/virtual_sensor.launch.py new file mode 100644 index 00000000..c4c29f22 --- /dev/null +++ b/src/perception/virtual_sensor/launch/virtual_sensor.launch.py @@ -0,0 +1,22 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + talker = Node( + package='virtual_sensor', + executable='talker', + name='virtual_sensor_talker_node', + output='screen' + ) + + listener = Node( + package='virtual_sensor', + executable='listener', + name='virtual_sensor_listener_node', + output='screen' + ) + + return LaunchDescription([ + talker, + listener + ]) diff --git a/src/perception/virtual_sensor/test_data.txt b/src/perception/virtual_sensor/test_data.txt new file mode 100644 index 00000000..39d79213 --- /dev/null +++ b/src/perception/virtual_sensor/test_data.txt @@ -0,0 +1,1302 @@ +0,5,0,0,(-4.0306,3.3508) +0.01,5.1,0.0,0.0,(-4.0306,3.2508) +0.02,5.2,0.0,0.0,(-4.0306,3.1508) +0.03,5.3,0.0,0.0,(-4.0306,3.0508) +0.04,5.4,0.0,0.0,(-4.0306,2.9508) +0.05,5.5,0.0,0.0,(-4.0306,2.8508) +0.06,5.6,0.0,0.0,(-4.0306,2.7508) +0.07,5.7,0.0,0.0,(-4.0306,2.6508) +0.08,5.8,0.0,0.0,(-4.0306,2.5508) +0.09,5.9,0.0,0.0,(-4.0306,2.4508) +0.1,6.0,0.0,0.0,(-4.0306,2.3508) +0.11,6.1,0.0,0.0,(-4.0306,2.2508) +0.12,6.2,0.0,0.0,(-4.0306,2.1508) +0.13,6.3,0.0,0.0,(-4.0306,2.0508) +0.14,6.4,0.0,0.0,(-4.0306,1.9508) +0.15,6.5,0.0,0.0,(-4.0306,1.8508) +0.16,6.6,0.0,0.0,(-4.0306,1.7508) +0.17,6.7,0.0,0.0,(-4.0306,1.6508) +0.18,6.8,0.0,0.0,(-4.0306,1.5508) +0.19,6.9,0.0,0.0,(-4.0306,1.4508) +0.2,7.0,0.0,0.0,(-4.0306,1.3508) +0.21,7.1,0.0,0.0,(-4.0306,1.2508) +0.22,7.2,0.0,0.0,(-4.0306,1.1508) +0.23,7.3,0.0,0.0,(-4.0306,1.0508) +0.24,7.4,0.0,0.0,(-4.0306,0.9508) +0.25,7.5,0.0,0.0,(-4.0306,0.8508) +0.26,7.6,0.0,0.0,(-4.0306,0.7508) +0.27,7.7,0.0,0.0,(-4.0306,0.6508) +0.28,7.8,0.0,0.0,(-4.0306,0.5508) +0.29,7.9,0.0,0.0,(-4.0306,0.4508) +0.3,8.0,0.0,0.0,(-4.0306,0.3508) +0.31,8.1,0.0,0.0,(-4.0306,0.2508) +0.32,8.2,0.0,0.0,(-4.0306,0.1508) +0.33,8.3,0.0,0.0,(-4.0306,0.0508) +0.34,8.4,0.0,0.0, +0.35,8.5,0.0,0.0, +0.36,8.6,0.0,0.0, +0.37,8.7,0.0,0.0, +0.38,8.8,0.0,0.0, +0.39,8.9,0.0,0.0, +0.4,9.0,0.0,0.0, +0.41,9.1,0.0,0.0, +0.42,9.2,0.0,0.0, +0.43,9.3,0.0,0.0, +0.44,9.4,0.0,0.0, +0.45,9.5,0.0,0.0, +0.46,9.6,0.0,0.0, +0.47,9.7,0.0,0.0, +0.48,9.8,0.0,0.0, +0.49,9.9,0.0,0.0, +0.5,10.0,0.0,0.0, +0.51,10.1,0.0,0.0, +0.52,10.2,0.0,0.0, +0.53,10.3,0.0,0.0, +0.54,10.4,0.0,0.0, +0.55,10.5,0.0,0.0, +0.56,10.6,0.0,0.0, +0.57,10.7,0.0,0.0, +0.58,10.8,0.0,0.0, +0.59,10.9,0.0,0.0, +0.6,11.0,0.0,0.0, +0.61,11.1,0.0,0.0, +0.62,11.2,0.0,0.0, +0.63,11.3,0.0,0.0, +0.64,11.4,0.0,0.0, +0.65,11.5,0.0,0.0, +0.66,11.6,0.0,0.0, +0.67,11.7,0.0,0.0, +0.68,11.8,0.0,0.0, +0.69,11.9,0.0,0.0, +0.7,12.0,0.0,0.0, +0.71,12.1,0.0,0.0, +0.72,12.2,0.0,0.0, +0.73,12.3,0.0,0.0, +0.74,12.4,0.0,0.0, +0.75,12.5,0.0,0.0, +0.76,12.6,0.0,0.0, +0.77,12.7,0.0,0.0, +0.78,12.8,0.0,0.0, +0.79,12.9,0.0,0.0, +0.8,13.0,0.0,0.0, +0.81,13.1,0.0,0.0, +0.82,13.2,0.0,0.0, +0.83,13.3,0.0,0.0, +0.84,13.4,0.0,0.0, +0.85,13.5,0.0,0.0, +0.86,13.6,0.0,0.0, +0.87,13.7,0.0,0.0, +0.88,13.8,0.0,0.0, +0.89,13.9,0.0,0.0, +0.9,14.0,0.0,0.0, +0.91,14.1,0.0,0.0, +0.92,14.2,0.0,0.0, +0.93,14.3,0.0,0.0, +0.94,14.4,0.0,0.0, +0.95,14.5,0.0,0.0, +0.96,14.6,0.0,0.0, +0.97,14.7,0.0,0.0, +0.98,14.8,0.0,0.0, +0.99,14.9,0.0,0.0, +1.0,15.0,0.0,0.0, +1.01,15.0,0.0,0.0079, +1.02,15.0,0.0,0.0157, +1.03,15.0,0.0,0.0236, +1.04,15.0,0.0,0.0314, +1.05,15.0,0.0,0.0393, +1.06,15.0,0.0,0.0471, +1.07,15.0,0.0,0.055, +1.08,15.0,0.0,0.0628, +1.09,15.0,0.0,0.0707, +1.1,15.0,0.0,0.0785, +1.11,15.0,0.0,0.0864, +1.12,15.0,0.0,0.0942, +1.13,15.0,0.0,0.1021, +1.14,15.0,0.0,0.11, +1.15,15.0,0.0,0.1178, +1.16,15.0,0.0,0.1257, +1.17,15.0,0.0,0.1335, +1.18,15.0,0.0,0.1414, +1.19,15.0,0.0,0.1492, +1.2,15.0,0.0,0.1571, +1.21,15.0,0.0,0.1649, +1.22,15.0,0.0,0.1728, +1.23,15.0,0.0,0.1806, +1.24,15.0,0.0,0.1885, +1.25,15.0,0.0,0.1963, +1.26,15.0,0.0,0.2042, +1.27,15.0,0.0,0.2121, +1.28,15.0,0.0,0.2199, +1.29,15.0,0.0,0.2278, +1.3,15.0,0.0,0.2356, +1.31,15.0,0.0,0.2435, +1.32,15.0,0.0,0.2513, +1.33,15.0,0.0,0.2592, +1.34,15.0,0.0,0.267, +1.35,15.0,0.0,0.2749, +1.36,15.0,0.0,0.2827, +1.37,15.0,0.0,0.2906, +1.38,15.0,0.0,0.2985, +1.39,15.0,0.0,0.3063, +1.4,15.0,0.0,0.3142, +1.41,15.0,0.0,0.322, +1.42,15.0,0.0,0.3299, +1.43,15.0,0.0,0.3377, +1.44,15.0,0.0,0.3456, +1.45,15.0,0.0,0.3534, +1.46,15.0,0.0,0.3613, +1.47,15.0,0.0,0.3691, +1.48,15.0,0.0,0.377, +1.49,15.0,0.0,0.3848, +1.5,15.0,0.0,0.3927, +1.51,15.0,0.0,0.4006, +1.52,15.0,0.0,0.4084, +1.53,15.0,0.0,0.4163, +1.54,15.0,0.0,0.4241, +1.55,15.0,0.0,0.432, +1.56,15.0,0.0,0.4398, +1.57,15.0,0.0,0.4477, +1.58,15.0,0.0,0.4555, +1.59,15.0,0.0,0.4634, +1.6,15.0,0.0,0.4712, +1.61,15.0,0.0,0.4791, +1.62,15.0,0.0,0.4869, +1.63,15.0,0.0,0.4948, +1.64,15.0,0.0,0.5027, +1.65,15.0,0.0,0.5105, +1.66,15.0,0.0,0.5184, +1.67,15.0,0.0,0.5262, +1.68,15.0,0.0,0.5341, +1.69,15.0,0.0,0.5419, +1.7,15.0,0.0,0.5498, +1.71,15.0,0.0,0.5576, +1.72,15.0,0.0,0.5655, +1.73,15.0,0.0,0.5733, +1.74,15.0,0.0,0.5812, +1.75,15.0,0.0,0.589, +1.76,15.0,0.0,0.5969, +1.77,15.0,0.0,0.6048, +1.78,15.0,0.0,0.6126, +1.79,15.0,0.0,0.6205, +1.8,15.0,0.0,0.6283, +1.81,15.0,0.0,0.6362, +1.82,15.0,0.0,0.644, +1.83,15.0,0.0,0.6519, +1.84,15.0,0.0,0.6597, +1.85,15.0,0.0,0.6676, +1.86,15.0,0.0,0.6754, +1.87,15.0,0.0,0.6833,(-21.1363,0.1328) +1.88,15.0,0.0,0.6912,(-21.1346,0.2988) +1.89,15.0,0.0,0.699,(-21.1316,0.4648) +1.9,15.0,0.0,0.7069,(-21.1273,0.6307) +1.91,15.0,0.0,0.7147,(-21.1217,0.7966) +1.92,15.0,0.0,0.7226,(-21.1148,0.9625) +1.93,15.0,0.0,0.7304,(-21.1066,1.1283) +1.94,15.0,0.0,0.7383,(-21.0971,1.294) +1.95,15.0,0.0,0.7461,(-21.0862,1.4597) +1.96,15.0,0.0,0.754,(-21.0741,1.6253) +1.97,15.0,0.0,0.7618,(-21.0607,1.7907) +1.98,15.0,0.0,0.7697,(-21.046,1.9561) +1.99,15.0,0.0,0.7775,(-21.03,2.1213) +2.0,15.0,0.0,0.7854,(-21.0127,2.2864) +2.01,15.0,0.0,0.7933,(-20.9941,2.4514) +2.02,15.0,0.0,0.8011,(-20.9742,2.6162) +2.03,15.0,0.0,0.809,(-20.953,2.7808) +2.04,15.0,0.0,0.8168,(-20.9305,2.9453) +2.05,15.0,0.0,0.8247,(-20.9067,3.1096) +2.06,15.0,0.0,0.8325,(-20.8817,3.2737) +2.07,15.0,0.0,0.8404,(-20.8553,3.4376) +2.08,15.0,0.0,0.8482,(-17.4873,0.1032),(-20.8277,3.6013) +2.09,15.0,0.0,0.8561,(-17.4859,0.2405),(-20.7987,3.7648) +2.1,15.0,0.0,0.8639,(-17.4835,0.3779),(-20.7685,3.928) +2.11,15.0,0.0,0.8718,(-17.48,0.5152),(-20.737,4.091) +2.12,15.0,0.0,0.8796,(-17.4754,0.6524),(-20.7043,4.2537) +2.13,15.0,0.0,0.8875,(-17.4697,0.7897),(-20.6702,4.4162) +2.14,15.0,0.0,0.8954,(-17.463,0.9268),(-20.6349,4.5784) +2.15,15.0,0.0,0.9032,(-17.4552,1.064),(-20.5983,4.7403) +2.16,15.0,0.0,0.9111,(-17.4463,1.201),(-20.5604,4.902) +2.17,15.0,0.0,0.9189,(-17.4363,1.338),(-20.5213,5.0633) +2.18,15.0,0.0,0.9268,(-17.4253,1.4749),(-20.4809,5.2243) +2.19,15.0,0.0,0.9346,(-17.4131,1.6117),(-20.4392,5.385) +2.2,15.0,0.0,0.9425,(-17.3999,1.7484),(-20.3963,5.5454) +2.21,15.0,0.0,0.9503,(-17.3857,1.885),(-20.3521,5.7054) +2.22,15.0,0.0,0.9582,(-17.3703,2.0215),(-20.3067,5.865) +2.23,15.0,0.0,0.966,(-17.3539,2.1579),(-20.26,6.0244) +2.24,15.0,0.0,0.9739,(-17.3364,2.2941),(-20.2121,6.1833) +2.25,15.0,0.0,0.9817,(-17.3179,2.4302),(-20.1629,6.3418) +2.26,15.0,0.0,0.9896,(-17.2983,2.5661),(-20.1124,6.5) +2.27,15.0,0.0,0.9975,(-17.2776,2.7019),(-20.0608,6.6578) +2.28,15.0,0.0,1.0053,(-17.2558,2.8375),(-20.0079,6.8151) +2.29,15.0,0.0,1.0132,(-17.233,2.973),(-19.9537,6.972) +2.3,15.0,0.0,1.021,(-17.2091,3.1082),(-19.8983,7.1285) +2.31,15.0,0.0,1.0289,(-7.7754,0.0235),(-17.1842,3.2433),(-19.8417,7.2846) +2.32,15.0,0.0,1.0367,(-7.775,0.0846),(-17.1582,3.3781),(-19.7839,7.4402) +2.33,15.0,0.0,1.0446,(-7.7741,0.1457),(-17.1311,3.5128),(-19.7249,7.5954) +2.34,15.0,0.0,1.0524,(-7.7727,0.2067),(-17.103,3.6472),(-19.6646,7.75) +2.35,15.0,0.0,1.0603,(-7.7708,0.2678),(-17.0738,3.7815),(-19.6031,7.9043) +2.36,15.0,0.0,1.0681,(-7.7685,0.3288),(-17.0436,3.9154),(-19.5405,8.058) +2.37,15.0,0.0,1.076,(-7.7656,0.3898),(-17.0123,4.0492),(-19.4766,8.2112) +2.38,15.0,0.0,1.0838,(-7.7623,0.4508),(-16.98,4.1827),(-19.4115,8.3639) +2.39,15.0,0.0,1.0917,(-7.7586,0.5117),(-16.9466,4.3159),(-19.3452,8.5161) +2.4,15.0,0.0,1.0996,(-7.7543,0.5726),(-16.9122,4.4489),(-19.2777,8.6678) +2.41,15.0,0.0,1.1074,(-7.7496,0.6335),(-16.8767,4.5815),(-19.209,8.8189) +2.42,15.0,0.0,1.1153,(-7.7444,0.6944),(-16.8402,4.7139),(-19.1392,8.9695) +2.43,15.0,0.0,1.1231,(-7.7387,0.7552),(-16.8027,4.8461),(-19.0681,9.1195) +2.44,15.0,0.0,1.131,(-7.7325,0.8159),(-16.7641,4.9779),(-18.9959,9.269) +2.45,15.0,0.0,1.1388,(-7.7258,0.8766),(-16.7245,5.1094),(-18.9226,9.4179) +2.46,15.0,0.0,1.1467,(-7.7187,0.9373),(-16.6839,5.2406),(-18.848,9.5663) +2.47,15.0,0.0,1.1545,(-7.7111,0.9979),(-16.6422,5.3715),(-18.7723,9.714) +2.48,15.0,0.0,1.1624,(-7.703,1.0584),(-16.5995,5.502),(-18.6954,9.8611) +2.49,15.0,0.0,1.1702,(-7.6945,1.1189),(-16.5558,5.6322),(-18.6174,10.0077) +2.5,15.0,0.0,1.1781,(-7.6855,1.1793),(-16.511,5.7621),(-18.5382,10.1536) +2.51,15.0,0.0,1.186,(-7.676,1.2396),(-16.4653,5.8916),(-18.4579,10.2988) +2.52,15.0,0.0,1.1938,(-7.666,1.2998),(-16.4185,6.0207),(-18.3764,10.4435) +2.53,15.0,0.0,1.2017,(-7.6556,1.36),(-16.3707,6.1494),(-18.2939,10.5875) +2.54,15.0,0.0,1.2095,(-7.6446,1.4201),(-16.3219,6.2778),(-18.2101,10.7309) +2.55,15.0,0.0,1.2174,(-7.6332,1.4801),(-16.2721,6.4058),(-18.1253,10.8735) +2.56,15.0,0.0,1.2252,(-7.6214,1.54),(-16.2213,6.5334),(-18.0393,11.0156) +2.57,15.0,0.0,1.2331,(-7.6091,1.5998),(-16.1694,6.6606),(-17.9523,11.1569) +2.58,15.0,0.0,1.2409,(-7.5963,1.6595),(-16.1166,6.7874),(-17.8641,11.2976) +2.59,15.0,0.0,1.2488,(-7.583,1.7191),(-16.0628,6.9138),(-17.7748,11.4375) +2.6,15.0,0.0,1.2566,(-7.5693,1.7786),(-16.008,7.0397),(-17.6844,11.5768) +2.61,15.0,0.0,1.2645,(-7.5551,1.838),(-15.9523,7.1652),(-17.593,11.7153) +2.62,15.0,0.0,1.2723,(-7.5404,1.8973),(-15.8955,7.2903),(-17.5004,11.8531) +2.63,15.0,0.0,1.2802,(-7.5252,1.9565),(-15.8377,7.4149),(-17.4068,11.9902) +2.64,15.0,0.0,1.2881,(-7.5097,2.0155),(-15.779,7.5391),(-17.3121,12.1265) +2.65,15.0,0.0,1.2959,(-7.4936,2.0744),(-15.7193,7.6628),(-17.2163,12.2621) +2.66,15.0,0.0,1.3038,(-7.4771,2.1332),(-15.6587,7.786),(-17.1195,12.397) +2.67,15.0,0.0,1.3116,(-7.4601,2.1919),(-15.597,7.9087),(-17.0216,12.531) +2.68,15.0,0.0,1.3195,(-7.4426,2.2504),(-15.5344,8.031),(-16.9226,12.6643) +2.69,15.0,0.0,1.3273,(-7.4247,2.3088),(-15.4709,8.1528),(-16.8226,12.7968) +2.7,15.0,0.0,1.3352,(-7.4064,2.367),(-15.4064,8.274),(-16.7216,12.9286) +2.71,15.0,0.0,1.343,(-7.3876,2.4251),(-15.3409,8.3948),(-16.6196,13.0595) +2.72,15.0,0.0,1.3509,(-7.3683,2.4831),(-15.2745,8.515),(-16.5165,13.1896) +2.73,15.0,0.0,1.3587,(-7.3485,2.5409),(-15.2072,8.6347),(-16.4124,13.3189) +2.74,15.0,0.0,1.3666,(-7.3284,2.5985),(-15.1389,8.7538),(-16.3073,13.4474) +2.75,15.0,0.0,1.3744,(-7.3077,2.656),(-15.0696,8.8725),(-16.2011,13.5751) +2.76,15.0,0.0,1.3823,(-7.2866,2.7133),(-14.9995,8.9906),(-16.094,13.7019) +2.77,15.0,0.0,1.3902,(-7.2651,2.7704),(-14.9284,9.1081),(-15.9859,13.8279) +2.78,15.0,0.0,1.398,(-7.2431,2.8274),(-14.8564,9.2251),(-15.8768,13.953) +2.79,15.0,0.0,1.4059,(-7.2207,2.8842),(-14.7835,9.3414),(-15.7668,14.0773) +2.8,15.0,0.0,1.4137,(-7.1978,2.9408),(-14.7097,9.4573),(-15.6557,14.2007) +2.81,15.0,0.0,1.4216,(-7.1745,2.9973),(-14.635,9.5725),(-15.5437,14.3232) +2.82,15.0,0.0,1.4294,(-7.1507,3.0535),(-14.5593,9.6872),(-15.4307,14.4448) +2.83,15.0,0.0,1.4373,(-7.1265,3.1096),(-14.4828,9.8012),(-15.3168,14.5656) +2.84,15.0,0.0,1.4451,(-7.1019,3.1655),(-14.4054,9.9146),(-15.2019,14.6854) +2.85,15.0,0.0,1.453,(-7.0768,3.2211),(-14.3271,10.0275),(-15.0861,14.8044) +2.86,15.0,0.0,1.4608,(-7.0513,3.2766),(-14.2479,10.1397),(-14.9694,14.9224) +2.87,15.0,0.0,1.4687,(-7.0254,3.3319),(-14.1678,10.2513),(-14.8517,15.0395) +2.88,15.0,0.0,1.4765,(-6.999,3.387),(-14.0868,10.3622),(-14.7331,15.1557) +2.89,15.0,0.0,1.4844,(-6.9721,3.4418),(-14.005,10.4726),(-14.6137,15.2709) +2.9,15.0,0.0,1.4923,(-6.9449,3.4965),(-13.9223,10.5822),(-14.4933,15.3852) +2.91,15.0,0.0,1.5001,(-6.9172,3.5509),(-13.8388,10.6912),(-14.372,15.4986) +2.92,15.0,0.0,1.508,(-6.8891,3.6051),(-13.7544,10.7996),(-14.2498,15.611) +2.93,15.0,0.0,1.5158,(-6.8606,3.6591),(-13.6692,10.9073),(-14.1268,15.7224) +2.94,15.0,0.0,1.5237,(-6.8316,3.7129),(-13.5831,11.0143),(-14.0029,15.8329) +2.95,15.0,0.0,1.5315,(-6.8023,3.7665),(-13.4962,11.1207),(-13.8781,15.9424) +2.96,15.0,0.0,1.5394,(-6.7725,3.8198),(-13.4084,11.2263),(-13.7524,16.0509) +2.97,15.0,0.0,1.5472,(-6.7423,3.8728),(-13.3198,11.3313),(-13.6259,16.1584) +2.98,15.0,0.0,1.5551,(-6.7117,3.9257),(-13.2304,11.4355),(-13.4986,16.2649) +2.99,15.0,0.0,1.5629,(-6.6806,3.9783),(-13.1402,11.5391),(-13.3705,16.3704) +3.0,15.0,0.0,1.5708,(-6.6492,4.0306),(-13.0492,11.6419),(-13.2415,16.4749) +3.01,15.0,0.1,1.5708,(-6.6492,3.9306),(-13.0492,11.5419),(-13.2415,16.3749) +3.02,15.0,0.2,1.5708,(-6.6492,3.8306),(-13.0492,11.4419),(-13.2415,16.2749) +3.03,15.0,0.3,1.5708,(-6.6492,3.7306),(-13.0492,11.3419),(-13.2415,16.1749) +3.04,15.0,0.4,1.5708,(-6.6492,3.6306),(-13.0492,11.2419),(-13.2415,16.0749) +3.05,15.0,0.5,1.5708,(-6.6492,3.5306),(-13.0492,11.1419),(-13.2415,15.9749) +3.06,15.0,0.6,1.5708,(-6.6492,3.4306),(-13.0492,11.0419),(-13.2415,15.8749) +3.07,15.0,0.7,1.5708,(-6.6492,3.3306),(-13.0492,10.9419),(-13.2415,15.7749) +3.08,15.0,0.8,1.5708,(-6.6492,3.2306),(-13.0492,10.8419),(-13.2415,15.6749) +3.09,15.0,0.9,1.5708,(-6.6492,3.1306),(-13.0492,10.7419),(-13.2415,15.5749) +3.1,15.0,1.0,1.5708,(-6.6492,3.0306),(-13.0492,10.6419),(-13.2415,15.4749) +3.11,15.0,1.1,1.5708,(-6.6492,2.9306),(-13.0492,10.5419),(-13.2415,15.3749) +3.12,15.0,1.2,1.5708,(-6.6492,2.8306),(-13.0492,10.4419),(-13.2415,15.2749) +3.13,15.0,1.3,1.5708,(-6.6492,2.7306),(-13.0492,10.3419),(-13.2415,15.1749) +3.14,15.0,1.4,1.5708,(-6.6492,2.6306),(-13.0492,10.2419),(-13.2415,15.0749) +3.15,15.0,1.5,1.5708,(-6.6492,2.5306),(-13.0492,10.1419),(-13.2415,14.9749) +3.16,15.0,1.6,1.5708,(-6.6492,2.4306),(-13.0492,10.0419),(-13.2415,14.8749) +3.17,15.0,1.7,1.5708,(-6.6492,2.3306),(-13.0492,9.9419),(-10.2927,22.7232),(-13.2415,14.7749) +3.18,15.0,1.8,1.5708,(-6.6492,2.2306),(-13.0492,9.8419),(-10.2927,22.6232),(-13.2415,14.6749) +3.19,15.0,1.9,1.5708,(-6.6492,2.1306),(-13.0492,9.7419),(-10.2927,22.5232),(-13.2415,14.5749) +3.2,15.0,2.0,1.5708,(-6.6492,2.0306),(-13.0492,9.6419),(-10.2927,22.4232),(-13.2415,14.4749) +3.21,15.0,2.1,1.5708,(-6.6492,1.9306),(-13.0492,9.5419),(-10.2927,22.3232),(-13.2415,14.3749) +3.22,15.0,2.2,1.5708,(-6.6492,1.8306),(-13.0492,9.4419),(-10.2927,22.2232),(-13.2415,14.2749) +3.23,15.0,2.3,1.5708,(-6.6492,1.7306),(-13.0492,9.3419),(-10.2927,22.1232),(-13.2415,14.1749) +3.24,15.0,2.4,1.5708,(-6.6492,1.6306),(-13.0492,9.2419),(-10.2927,22.0232),(-13.2415,14.0749) +3.25,15.0,2.5,1.5708,(-6.6492,1.5306),(-13.0492,9.1419),(-10.2927,21.9232),(-13.2415,13.9749) +3.26,15.0,2.6,1.5708,(-6.6492,1.4306),(-13.0492,9.0419),(-10.2927,21.8232),(-13.2415,13.8749) +3.27,15.0,2.7,1.5708,(-6.6492,1.3306),(-13.0492,8.9419),(-10.2927,21.7232),(-13.2415,13.7749) +3.28,15.0,2.8,1.5708,(-6.6492,1.2306),(-13.0492,8.8419),(-10.2927,21.6232),(-13.2415,13.6749) +3.29,15.0,2.9,1.5708,(-6.6492,1.1306),(-13.0492,8.7419),(-10.2927,21.5232),(-13.2415,13.5749) +3.3,15.0,3.0,1.5708,(-6.6492,1.0306),(-13.0492,8.6419),(-10.2927,21.4232),(-13.2415,13.4749) +3.31,15.0,3.1,1.5708,(-6.6492,0.9306),(-13.0492,8.5419),(-10.2927,21.3232),(-13.2415,13.3749) +3.32,15.0,3.2,1.5708,(-6.6492,0.8306),(-13.0492,8.4419),(-10.2927,21.2232),(-13.2415,13.2749) +3.33,15.0,3.3,1.5708,(-6.6492,0.7306),(-13.0492,8.3419),(-10.2927,21.1232),(-13.2415,13.1749) +3.34,15.0,3.4,1.5708,(-6.6492,0.6306),(-13.0492,8.2419),(-10.2927,21.0232),(-13.2415,13.0749) +3.35,15.0,3.5,1.5708,(-6.6492,0.5306),(-13.0492,8.1419),(-10.2927,20.9232),(-13.2415,12.9749) +3.36,15.0,3.6,1.5708,(-6.6492,0.4306),(-13.0492,8.0419),(-10.2927,20.8232),(-13.2415,12.8749) +3.37,15.0,3.7,1.5708,(-6.6492,0.3306),(-13.0492,7.9419),(-10.2927,20.7232),(-13.2415,12.7749) +3.38,15.0,3.8,1.5708,(-6.6492,0.2306),(-13.0492,7.8419),(-10.2927,20.6232),(-13.2415,12.6749) +3.39,15.0,3.9,1.5708,(-6.6492,0.1306),(-13.0492,7.7419),(-10.2927,20.5232),(-13.2415,12.5749) +3.4,15.0,4.0,1.5708,(-6.6492,0.0306),(-13.0492,7.6419),(-10.2927,20.4232),(-13.2415,12.4749) +3.41,15.0,4.1,1.5708,(-13.0492,7.5419),(-10.2927,20.3232),(-13.2415,12.3749) +3.42,15.0,4.2,1.5708,(-13.0492,7.4419),(-10.2927,20.2232),(-13.2415,12.2749) +3.43,15.0,4.3,1.5708,(-13.0492,7.3419),(-10.2927,20.1232),(-13.2415,12.1749) +3.44,15.0,4.4,1.5708,(-13.0492,7.2419),(-10.2927,20.0232),(-13.2415,12.0749) +3.45,15.0,4.5,1.5708,(-13.0492,7.1419),(-10.2927,19.9232),(-13.2415,11.9749) +3.46,15.0,4.6,1.5708,(-13.0492,7.0419),(-10.2927,19.8232),(-13.2415,11.8749) +3.47,15.0,4.7,1.5708,(-13.0492,6.9419),(-10.2927,19.7232),(-13.2415,11.7749) +3.48,15.0,4.8,1.5708,(-13.0492,6.8419),(-10.2927,19.6232),(-13.2415,11.6749) +3.49,15.0,4.9,1.5708,(-13.0492,6.7419),(-10.2927,19.5232),(-13.2415,11.5749) +3.5,15.0,5.0,1.5708,(-13.0492,6.6419),(-10.2927,19.4232),(-13.2415,11.4749) +3.51,15.0,5.1,1.5708,(-13.0492,6.5419),(-10.2927,19.3232),(-13.2415,11.3749) +3.52,15.0,5.2,1.5708,(-13.0492,6.4419),(-10.2927,19.2232),(-13.2415,11.2749) +3.53,15.0,5.3,1.5708,(-13.0492,6.3419),(-10.2927,19.1232),(-13.2415,11.1749) +3.54,15.0,5.4,1.5708,(-13.0492,6.2419),(-10.2927,19.0232),(-13.2415,11.0749) +3.55,15.0,5.5,1.5708,(-13.0492,6.1419),(-10.2927,18.9232),(-13.2415,10.9749) +3.56,15.0,5.6,1.5708,(-13.0492,6.0419),(-10.2927,18.8232),(-13.2415,10.8749) +3.57,15.0,5.7,1.5708,(-13.0492,5.9419),(-10.2927,18.7232),(-13.2415,10.7749) +3.58,15.0,5.8,1.5708,(-13.0492,5.8419),(-10.2927,18.6232),(-13.2415,10.6749) +3.59,15.0,5.9,1.5708,(-13.0492,5.7419),(-10.2927,18.5232),(-13.2415,10.5749) +3.6,15.0,6.0,1.5708,(-13.0492,5.6419),(-10.2927,18.4232),(-13.2415,10.4749) +3.61,15.0,6.1,1.5708,(-13.0492,5.5419),(-10.2927,18.3232),(-13.2415,10.3749) +3.62,15.0,6.2,1.5708,(-13.0492,5.4419),(-10.2927,18.2232),(-13.2415,10.2749) +3.63,15.0,6.3,1.5708,(-13.0492,5.3419),(-10.2927,18.1232),(-13.2415,10.1749) +3.64,15.0,6.4,1.5708,(-13.0492,5.2419),(-10.2927,18.0232),(-13.2415,10.0749) +3.65,15.0,6.5,1.5708,(-13.0492,5.1419),(-10.2927,17.9232),(-13.2415,9.9749) +3.66,15.0,6.6,1.5708,(-13.0492,5.0419),(-10.2927,17.8232),(-13.2415,9.8749) +3.67,15.0,6.7,1.5708,(-13.0492,4.9419),(-10.2927,17.7232),(-13.2415,9.7749) +3.68,15.0,6.8,1.5708,(-13.0492,4.8419),(-10.2927,17.6232),(-13.2415,9.6749) +3.69,15.0,6.9,1.5708,(-13.0492,4.7419),(-10.2927,17.5232),(-13.2415,9.5749) +3.7,15.0,7.0,1.5708,(-13.0492,4.6419),(-10.2927,17.4232),(-13.2415,9.4749) +3.71,15.0,7.1,1.5708,(-13.0492,4.5419),(-10.2927,17.3232),(-13.2415,9.3749) +3.72,15.0,7.2,1.5708,(-13.0492,4.4419),(-10.2927,17.2232),(-13.2415,9.2749) +3.73,15.0,7.3,1.5708,(-13.0492,4.3419),(-10.2927,17.1232),(-13.2415,9.1749) +3.74,15.0,7.4,1.5708,(-13.0492,4.2419),(-10.2927,17.0232),(-13.2415,9.0749) +3.75,15.0,7.5,1.5708,(-13.0492,4.1419),(-10.2927,16.9232),(-13.2415,8.9749) +3.76,15.0,7.6,1.5708,(-13.0492,4.0419),(-10.2927,16.8232),(-13.2415,8.8749) +3.77,15.0,7.7,1.5708,(-13.0492,3.9419),(-10.2927,16.7232),(-13.2415,8.7749) +3.78,15.0,7.8,1.5708,(-13.0492,3.8419),(-10.2927,16.6232),(-13.2415,8.6749) +3.79,15.0,7.9,1.5708,(-13.0492,3.7419),(-10.2927,16.5232),(-13.2415,8.5749) +3.8,15.0,8.0,1.5708,(-13.0492,3.6419),(-10.2927,16.4232),(-13.2415,8.4749) +3.81,15.0,8.1,1.5708,(-13.0492,3.5419),(-10.2927,16.3232),(-13.2415,8.3749) +3.82,15.0,8.2,1.5708,(-13.0492,3.4419),(-10.2927,16.2232),(-13.2415,8.2749) +3.83,15.0,8.3,1.5708,(-13.0492,3.3419),(-10.2927,16.1232),(-13.2415,8.1749) +3.84,15.0,8.4,1.5708,(-13.0492,3.2419),(-10.2927,16.0232),(-13.2415,8.0749) +3.85,15.0,8.5,1.5708,(-13.0492,3.1419),(-10.2927,15.9232),(-13.2415,7.9749) +3.86,15.0,8.6,1.5708,(-13.0492,3.0419),(-10.2927,15.8232),(-13.2415,7.8749) +3.87,15.0,8.7,1.5708,(-13.0492,2.9419),(-10.2927,15.7232),(-13.2415,7.7749) +3.88,15.0,8.8,1.5708,(-13.0492,2.8419),(-10.2927,15.6232),(-13.2415,7.6749) +3.89,15.0,8.9,1.5708,(-13.0492,2.7419),(-10.2927,15.5232),(-13.2415,7.5749) +3.9,15.0,9.0,1.5708,(-13.0492,2.6419),(-10.2927,15.4232),(-13.2415,7.4749) +3.91,15.0,9.1,1.5708,(-13.0492,2.5419),(-10.2927,15.3232),(-13.2415,7.3749) +3.92,15.0,9.2,1.5708,(-13.0492,2.4419),(-10.2927,15.2232),(-13.2415,7.2749) +3.93,15.0,9.3,1.5708,(-13.0492,2.3419),(-10.2927,15.1232),(-13.2415,7.1749) +3.94,15.0,9.4,1.5708,(-13.0492,2.2419),(-10.2927,15.0232),(-13.2415,7.0749) +3.95,15.0,9.5,1.5708,(-13.0492,2.1419),(-10.2927,14.9232),(-13.2415,6.9749) +3.96,15.0,9.6,1.5708,(-13.0492,2.0419),(-10.2927,14.8232),(-13.2415,6.8749) +3.97,15.0,9.7,1.5708,(-13.0492,1.9419),(-10.2927,14.7232),(-13.2415,6.7749) +3.98,15.0,9.8,1.5708,(-13.0492,1.8419),(-10.2927,14.6232),(-13.2415,6.6749) +3.99,15.0,9.9,1.5708,(-13.0492,1.7419),(-10.2927,14.5232),(-13.2415,6.5749) +4.0,15.0,10.0,1.5708,(-13.0492,1.6419),(-10.2927,14.4232),(-13.2415,6.4749) +4.01,15.0,10.1,1.5708,(-13.0492,1.5419),(-10.2927,14.3232),(-13.2415,6.3749) +4.02,15.0,10.2,1.5708,(-13.0492,1.4419),(-10.2927,14.2232),(-13.2415,6.2749) +4.03,15.0,10.3,1.5708,(-13.0492,1.3419),(-10.2927,14.1232),(-13.2415,6.1749) +4.04,15.0,10.4,1.5708,(-13.0492,1.2419),(-10.2927,14.0232),(-13.2415,6.0749) +4.05,15.0,10.5,1.5708,(-13.0492,1.1419),(-10.2927,13.9232),(-13.2415,5.9749) +4.06,15.0,10.6,1.5708,(-13.0492,1.0419),(-10.2927,13.8232),(-13.2415,5.8749) +4.07,15.0,10.7,1.5708,(-13.0492,0.9419),(-10.2927,13.7232),(-13.2415,5.7749) +4.08,15.0,10.8,1.5708,(-13.0492,0.8419),(-10.2927,13.6232),(-13.2415,5.6749) +4.09,15.0,10.9,1.5708,(-13.0492,0.7419),(-10.2927,13.5232),(-13.2415,5.5749) +4.1,15.0,11.0,1.5708,(-13.0492,0.6419),(-10.2927,13.4232),(-13.2415,5.4749) +4.11,15.0,11.1,1.5708,(-13.0492,0.5419),(-10.2927,13.3232),(-13.2415,5.3749) +4.12,15.0,11.2,1.5708,(-13.0492,0.4419),(-10.2927,13.2232),(-13.2415,5.2749) +4.13,15.0,11.3,1.5708,(-13.0492,0.3419),(-10.2927,13.1232),(-13.2415,5.1749) +4.14,15.0,11.4,1.5708,(-13.0492,0.2419),(-10.2927,13.0232),(-13.2415,5.0749) +4.15,15.0,11.5,1.5708,(-13.0492,0.1419),(-10.2927,12.9232),(-13.2415,4.9749) +4.16,15.0,11.6,1.5708,(-13.0492,0.0419),(-10.2927,12.8232),(-13.2415,4.8749) +4.17,15.0,11.7,1.5708,(-10.2927,12.7232),(-13.2415,4.7749) +4.18,15.0,11.8,1.5708,(-10.2927,12.6232),(-13.2415,4.6749) +4.19,15.0,11.9,1.5708,(-10.2927,12.5232),(-13.2415,4.5749) +4.2,15.0,12.0,1.5708,(-10.2927,12.4232),(-13.2415,4.4749) +4.21,15.0,12.1,1.5708,(-10.2927,12.3232),(-13.2415,4.3749) +4.22,15.0,12.2,1.5708,(-10.2927,12.2232),(-13.2415,4.2749) +4.23,15.0,12.3,1.5708,(-10.2927,12.1232),(-13.2415,4.1749) +4.24,15.0,12.4,1.5708,(-10.2927,12.0232),(-13.2415,4.0749) +4.25,15.0,12.5,1.5708,(-10.2927,11.9232),(-13.2415,3.9749) +4.26,15.0,12.6,1.5708,(-10.2927,11.8232),(-13.2415,3.8749) +4.27,15.0,12.7,1.5708,(-10.2927,11.7232),(-13.2415,3.7749) +4.28,15.0,12.8,1.5708,(-10.2927,11.6232),(-13.2415,3.6749) +4.29,15.0,12.9,1.5708,(-10.2927,11.5232),(-13.2415,3.5749) +4.3,15.0,13.0,1.5708,(-10.2927,11.4232),(-13.2415,3.4749) +4.31,15.0,13.1,1.5708,(-10.2927,11.3232),(-13.2415,3.3749) +4.32,15.0,13.2,1.5708,(-10.2927,11.2232),(-13.2415,3.2749) +4.33,15.0,13.3,1.5708,(-10.2927,11.1232),(-13.2415,3.1749) +4.34,15.0,13.4,1.5708,(-10.2927,11.0232),(-13.2415,3.0749) +4.35,15.0,13.5,1.5708,(-10.2927,10.9232),(-13.2415,2.9749) +4.36,15.0,13.6,1.5708,(-10.2927,10.8232),(-13.2415,2.8749) +4.37,15.0,13.7,1.5708,(-10.2927,10.7232),(-13.2415,2.7749) +4.38,15.0,13.8,1.5708,(-10.2927,10.6232),(-13.2415,2.6749) +4.39,15.0,13.9,1.5708,(-10.2927,10.5232),(-13.2415,2.5749) +4.4,15.0,14.0,1.5708,(-10.2927,10.4232),(-13.2415,2.4749) +4.41,15.0,14.1,1.5708,(-10.2927,10.3232),(-13.2415,2.3749) +4.42,15.0,14.2,1.5708,(-10.2927,10.2232),(-13.2415,2.2749) +4.43,15.0,14.3,1.5708,(-10.2927,10.1232),(-13.2415,2.1749) +4.44,15.0,14.4,1.5708,(-10.2927,10.0232),(-13.2415,2.0749) +4.45,15.0,14.5,1.5708,(-10.2927,9.9232),(-13.2415,1.9749),(-3.5116,24.7341) +4.46,15.0,14.6,1.5708,(-10.2927,9.8232),(-13.2415,1.8749),(-3.5116,24.6341) +4.47,15.0,14.7,1.5708,(-10.2927,9.7232),(-13.2415,1.7749),(-3.5116,24.5341) +4.48,15.0,14.8,1.5708,(-10.2927,9.6232),(-13.2415,1.6749),(-3.5116,24.4341) +4.49,15.0,14.9,1.5708,(-10.2927,9.5232),(-13.2415,1.5749),(-3.5116,24.3341) +4.5,15.0,15.0,1.5708,(-10.2927,9.4232),(-13.2415,1.4749),(-3.5116,24.2341) +4.51,15.0,15.1,1.5708,(-10.2927,9.3232),(-13.2415,1.3749),(-3.5116,24.1341) +4.52,15.0,15.2,1.5708,(-10.2927,9.2232),(-13.2415,1.2749),(-3.5116,24.0341) +4.53,15.0,15.3,1.5708,(-10.2927,9.1232),(-13.2415,1.1749),(-3.5116,23.9341) +4.54,15.0,15.4,1.5708,(-10.2927,9.0232),(-13.2415,1.0749),(-3.5116,23.8341) +4.55,15.0,15.5,1.5708,(-10.2927,8.9232),(-13.2415,0.9749),(-3.5116,23.7341) +4.56,15.0,15.6,1.5708,(-10.2927,8.8232),(-13.2415,0.8749),(-3.5116,23.6341) +4.57,15.0,15.7,1.5708,(-10.2927,8.7232),(-13.2415,0.7749),(-3.5116,23.5341) +4.58,15.0,15.8,1.5708,(-10.2927,8.6232),(-13.2415,0.6749),(-3.5116,23.4341) +4.59,15.0,15.9,1.5708,(-10.2927,8.5232),(-13.2415,0.5749),(-3.5116,23.3341) +4.6,15.0,16.0,1.5708,(-10.2927,8.4232),(-13.2415,0.4749),(-3.5116,23.2341) +4.61,15.0,16.1,1.5708,(-10.2927,8.3232),(-13.2415,0.3749),(-3.5116,23.1341) +4.62,15.0,16.2,1.5708,(-10.2927,8.2232),(-13.2415,0.2749),(-3.5116,23.0341) +4.63,15.0,16.3,1.5708,(-10.2927,8.1232),(-13.2415,0.1749),(-3.5116,22.9341) +4.64,15.0,16.4,1.5708,(-10.2927,8.0232),(-13.2415,0.0749),(-3.5116,22.8341) +4.65,15.0,16.5,1.5708,(-10.2927,7.9232),(-3.5116,22.7341) +4.66,15.0,16.6,1.5708,(-10.2927,7.8232),(-3.5116,22.6341) +4.67,15.0,16.7,1.5708,(-10.2927,7.7232),(-3.5116,22.5341) +4.68,15.0,16.8,1.5708,(-10.2927,7.6232),(-3.5116,22.4341) +4.69,15.0,16.9,1.5708,(-10.2927,7.5232),(-3.5116,22.3341) +4.7,15.0,17.0,1.5708,(-10.2927,7.4232),(-3.5116,22.2341) +4.71,15.0,17.1,1.5708,(-10.2927,7.3232),(-3.5116,22.1341) +4.72,15.0,17.2,1.5708,(-10.2927,7.2232),(-3.5116,22.0341) +4.73,15.0,17.3,1.5708,(-10.2927,7.1232),(-3.5116,21.9341) +4.74,15.0,17.4,1.5708,(-10.2927,7.0232),(-3.5116,21.8341) +4.75,15.0,17.5,1.5708,(-10.2927,6.9232),(-3.5116,21.7341) +4.76,15.0,17.6,1.5708,(-10.2927,6.8232),(-3.5116,21.6341) +4.77,15.0,17.7,1.5708,(-10.2927,6.7232),(-3.5116,21.5341) +4.78,15.0,17.8,1.5708,(-10.2927,6.6232),(-3.5116,21.4341) +4.79,15.0,17.9,1.5708,(-10.2927,6.5232),(-3.5116,21.3341) +4.8,15.0,18.0,1.5708,(-10.2927,6.4232),(-3.5116,21.2341) +4.81,15.0,18.1,1.5708,(-10.2927,6.3232),(-3.5116,21.1341) +4.82,15.0,18.2,1.5708,(-10.2927,6.2232),(-3.5116,21.0341) +4.83,15.0,18.3,1.5708,(-10.2927,6.1232),(-3.5116,20.9341) +4.84,15.0,18.4,1.5708,(-10.2927,6.0232),(-3.5116,20.8341) +4.85,15.0,18.5,1.5708,(-10.2927,5.9232),(-3.5116,20.7341) +4.86,15.0,18.6,1.5708,(-10.2927,5.8232),(-3.5116,20.6341) +4.87,15.0,18.7,1.5708,(-10.2927,5.7232),(-3.5116,20.5341) +4.88,15.0,18.8,1.5708,(-10.2927,5.6232),(-3.5116,20.4341) +4.89,15.0,18.9,1.5708,(-10.2927,5.5232),(-3.5116,20.3341) +4.9,15.0,19.0,1.5708,(-10.2927,5.4232),(-3.5116,20.2341) +4.91,15.0,19.1,1.5708,(-10.2927,5.3232),(-3.5116,20.1341) +4.92,15.0,19.2,1.5708,(-10.2927,5.2232),(-3.5116,20.0341) +4.93,15.0,19.3,1.5708,(-10.2927,5.1232),(-3.5116,19.9341) +4.94,15.0,19.4,1.5708,(-10.2927,5.0232),(-3.5116,19.8341) +4.95,15.0,19.5,1.5708,(-10.2927,4.9232),(-3.5116,19.7341) +4.96,15.0,19.6,1.5708,(-10.2927,4.8232),(-3.5116,19.6341) +4.97,15.0,19.7,1.5708,(-10.2927,4.7232),(-3.5116,19.5341) +4.98,15.0,19.8,1.5708,(-10.2927,4.6232),(-3.5116,19.4341) +4.99,15.0,19.9,1.5708,(-10.2927,4.5232),(-3.5116,19.3341) +5.0,15.0,20.0,1.5708,(-10.2927,4.4232),(-3.5116,19.2341) +5.01,15.0,20.1,1.5708,(-10.2927,4.3232),(-3.5116,19.1341),(10.4803,22.6649) +5.02,15.0,20.2,1.5708,(-10.2927,4.2232),(-3.5116,19.0341),(10.4803,22.5649) +5.03,15.0,20.3,1.5708,(-10.2927,4.1232),(-3.5116,18.9341),(10.4803,22.4649) +5.04,15.0,20.4,1.5708,(-10.2927,4.0232),(-3.5116,18.8341),(10.4803,22.3649) +5.05,15.0,20.5,1.5708,(-10.2927,3.9232),(-3.5116,18.7341),(10.4803,22.2649) +5.06,15.0,20.6,1.5708,(-10.2927,3.8232),(-3.5116,18.6341),(10.4803,22.1649) +5.07,15.0,20.7,1.5708,(-10.2927,3.7232),(-3.5116,18.5341),(10.4803,22.0649) +5.08,15.0,20.8,1.5708,(-10.2927,3.6232),(-3.5116,18.4341),(10.4803,21.9649) +5.09,15.0,20.9,1.5708,(-10.2927,3.5232),(-3.5116,18.3341),(10.4803,21.8649) +5.1,15.0,21.0,1.5708,(-10.2927,3.4232),(-3.5116,18.2341),(10.4803,21.7649) +5.11,15.0,21.1,1.5708,(-10.2927,3.3232),(-3.5116,18.1341),(10.4803,21.6649) +5.12,15.0,21.2,1.5708,(-10.2927,3.2232),(-3.5116,18.0341),(10.4803,21.5649) +5.13,15.0,21.3,1.5708,(-10.2927,3.1232),(-3.5116,17.9341),(10.4803,21.4649) +5.14,15.0,21.4,1.5708,(-10.2927,3.0232),(-3.5116,17.8341),(10.4803,21.3649) +5.15,15.0,21.5,1.5708,(-10.2927,2.9232),(-3.5116,17.7341),(10.4803,21.2649) +5.16,15.0,21.6,1.5708,(-10.2927,2.8232),(-3.5116,17.6341),(10.4803,21.1649) +5.17,15.0,21.7,1.5708,(-10.2927,2.7232),(-3.5116,17.5341),(10.4803,21.0649) +5.18,15.0,21.8,1.5708,(-10.2927,2.6232),(-3.5116,17.4341),(10.4803,20.9649) +5.19,15.0,21.9,1.5708,(-10.2927,2.5232),(-3.5116,17.3341),(10.4803,20.8649) +5.2,15.0,22.0,1.5708,(-10.2927,2.4232),(-3.5116,17.2341),(10.4803,20.7649) +5.21,15.0,22.1,1.5708,(-10.2927,2.3232),(-3.5116,17.1341),(10.4803,20.6649) +5.22,15.0,22.2,1.5708,(-10.2927,2.2232),(-3.5116,17.0341),(10.4803,20.5649) +5.23,15.0,22.3,1.5708,(-5.9227,24.2864),(-10.2927,2.1232),(-3.5116,16.9341),(10.4803,20.4649) +5.24,15.0,22.4,1.5708,(-5.9227,24.1864),(-10.2927,2.0232),(-3.5116,16.8341),(10.4803,20.3649) +5.25,15.0,22.5,1.5708,(-5.9227,24.0864),(-10.2927,1.9232),(-3.5116,16.7341),(10.4803,20.2649) +5.26,15.0,22.6,1.5708,(-5.9227,23.9864),(-10.2927,1.8232),(-3.5116,16.6341),(10.4803,20.1649) +5.27,15.0,22.7,1.5708,(-5.9227,23.8864),(-10.2927,1.7232),(-3.5116,16.5341),(10.4803,20.0649) +5.28,15.0,22.8,1.5708,(-5.9227,23.7864),(-10.2927,1.6232),(-3.5116,16.4341),(10.4803,19.9649) +5.29,15.0,22.9,1.5708,(-5.9227,23.6864),(-10.2927,1.5232),(-3.5116,16.3341),(10.4803,19.8649) +5.3,15.0,23.0,1.5708,(-5.9227,23.5864),(-10.2927,1.4232),(-3.5116,16.2341),(10.4803,19.7649) +5.31,15.0,23.1,1.5708,(-5.9227,23.4864),(-10.2927,1.3232),(-3.5116,16.1341),(10.4803,19.6649) +5.32,15.0,23.2,1.5708,(-5.9227,23.3864),(-10.2927,1.2232),(-3.5116,16.0341),(10.4803,19.5649) +5.33,15.0,23.3,1.5708,(-5.9227,23.2864),(-10.2927,1.1232),(-3.5116,15.9341),(10.4803,19.4649) +5.34,15.0,23.4,1.5708,(-5.9227,23.1864),(-10.2927,1.0232),(-3.5116,15.8341),(10.4803,19.3649) +5.35,15.0,23.5,1.5708,(-5.9227,23.0864),(-10.2927,0.9232),(-3.5116,15.7341),(10.4803,19.2649) +5.36,15.0,23.6,1.5708,(-5.9227,22.9864),(-10.2927,0.8232),(-3.5116,15.6341),(10.4803,19.1649) +5.37,15.0,23.7,1.5708,(-5.9227,22.8864),(-10.2927,0.7232),(-3.5116,15.5341),(10.4803,19.0649) +5.38,15.0,23.8,1.5708,(-5.9227,22.7864),(-10.2927,0.6232),(-3.5116,15.4341),(10.4803,18.9649) +5.39,15.0,23.9,1.5708,(-5.9227,22.6864),(-10.2927,0.5232),(-3.5116,15.3341),(10.4803,18.8649) +5.4,15.0,24.0,1.5708,(-5.9227,22.5864),(-10.2927,0.4232),(-3.5116,15.2341),(10.4803,18.7649) +5.41,15.0,24.1,1.5708,(-5.9227,22.4864),(-10.2927,0.3232),(-3.5116,15.1341),(10.4803,18.6649) +5.42,15.0,24.2,1.5708,(-5.9227,22.3864),(-10.2927,0.2232),(-3.5116,15.0341),(10.4803,18.5649) +5.43,15.0,24.3,1.5708,(-5.9227,22.2864),(-10.2927,0.1232),(-3.5116,14.9341),(10.4803,18.4649) +5.44,15.0,24.4,1.5708,(-5.9227,22.1864),(-10.2927,0.0232),(-3.5116,14.8341),(10.4803,18.3649) +5.45,15.0,24.5,1.5708,(-5.9227,22.0864),(-3.5116,14.7341),(10.4803,18.2649) +5.46,15.0,24.6,1.5708,(-5.9227,21.9864),(-3.5116,14.6341),(10.4803,18.1649) +5.47,15.0,24.7,1.5708,(-5.9227,21.8864),(-3.5116,14.5341),(10.4803,18.0649) +5.48,15.0,24.8,1.5708,(-5.9227,21.7864),(-3.5116,14.4341),(10.4803,17.9649) +5.49,15.0,24.9,1.5708,(-5.9227,21.6864),(-3.5116,14.3341),(10.4803,17.8649) +5.5,15.0,25.0,1.5708,(-5.9227,21.5864),(-3.5116,14.2341),(10.4803,17.7649) +5.51,15.0,25.1,1.5708,(-5.9227,21.4864),(-3.5116,14.1341),(10.4803,17.6649) +5.52,15.0,25.2,1.5708,(-5.9227,21.3864),(-3.5116,14.0341),(10.4803,17.5649) +5.53,15.0,25.3,1.5708,(-5.9227,21.2864),(-3.5116,13.9341),(10.4803,17.4649) +5.54,15.0,25.4,1.5708,(-5.9227,21.1864),(-3.5116,13.8341),(10.4803,17.3649) +5.55,15.0,25.5,1.5708,(-5.9227,21.0864),(-3.5116,13.7341),(10.4803,17.2649) +5.56,15.0,25.6,1.5708,(-5.9227,20.9864),(-3.5116,13.6341),(10.4803,17.1649) +5.57,15.0,25.7,1.5708,(-5.9227,20.8864),(-3.5116,13.5341),(10.4803,17.0649) +5.58,15.0,25.8,1.5708,(-5.9227,20.7864),(-3.5116,13.4341),(10.4803,16.9649) +5.59,15.0,25.9,1.5708,(-5.9227,20.6864),(-3.5116,13.3341),(10.4803,16.8649) +5.6,15.0,26.0,1.5708,(-5.9227,20.5864),(-3.5116,13.2341),(10.4803,16.7649) +5.61,15.0,26.1,1.5708,(-5.9227,20.4864),(-3.5116,13.1341),(10.4803,16.6649) +5.62,15.0,26.2,1.5708,(-5.9227,20.3864),(-3.5116,13.0341),(10.4803,16.5649) +5.63,15.0,26.3,1.5708,(-5.9227,20.2864),(-3.5116,12.9341),(10.4803,16.4649) +5.64,15.0,26.4,1.5708,(-5.9227,20.1864),(-3.5116,12.8341),(10.4803,16.3649) +5.65,15.0,26.5,1.5708,(-5.9227,20.0864),(-3.5116,12.7341),(10.4803,16.2649) +5.66,15.0,26.6,1.5708,(-5.9227,19.9864),(-2.8263,24.7741),(-3.5116,12.6341),(10.4803,16.1649) +5.67,15.0,26.7,1.5708,(-5.9227,19.8864),(-2.8263,24.6741),(-3.5116,12.5341),(10.4803,16.0649) +5.68,15.0,26.8,1.5708,(-5.9227,19.7864),(-2.8263,24.5741),(-3.5116,12.4341),(10.4803,15.9649) +5.69,15.0,26.9,1.5708,(-5.9227,19.6864),(-2.8263,24.4741),(-3.5116,12.3341),(10.4803,15.8649) +5.7,15.0,27.0,1.5708,(-5.9227,19.5864),(-2.8263,24.3741),(-3.5116,12.2341),(10.4803,15.7649) +5.71,15.0,27.1,1.5708,(-5.9227,19.4864),(-2.8263,24.2741),(-3.5116,12.1341),(10.4803,15.6649) +5.72,15.0,27.2,1.5708,(-5.9227,19.3864),(-2.8263,24.1741),(-3.5116,12.0341),(10.4803,15.5649) +5.73,15.0,27.3,1.5708,(-5.9227,19.2864),(-2.8263,24.0741),(-3.5116,11.9341),(10.4803,15.4649) +5.74,15.0,27.4,1.5708,(-5.9227,19.1864),(-2.8263,23.9741),(-3.5116,11.8341),(10.4803,15.3649) +5.75,15.0,27.5,1.5708,(-5.9227,19.0864),(-2.8263,23.8741),(-3.5116,11.7341),(10.4803,15.2649) +5.76,15.0,27.6,1.5708,(-5.9227,18.9864),(-2.8263,23.7741),(-3.5116,11.6341),(10.4803,15.1649) +5.77,15.0,27.7,1.5708,(-5.9227,18.8864),(-2.8263,23.6741),(-3.5116,11.5341),(10.4803,15.0649) +5.78,15.0,27.8,1.5708,(-5.9227,18.7864),(-2.8263,23.5741),(-3.5116,11.4341),(10.4803,14.9649) +5.79,15.0,27.9,1.5708,(-5.9227,18.6864),(-2.8263,23.4741),(-3.5116,11.3341),(10.4803,14.8649) +5.8,15.0,28.0,1.5708,(-5.9227,18.5864),(-2.8263,23.3741),(-3.5116,11.2341),(10.4803,14.7649) +5.81,15.0,28.1,1.5708,(-5.9227,18.4864),(-2.8263,23.2741),(-3.5116,11.1341),(10.4803,14.6649) +5.82,15.0,28.2,1.5708,(-5.9227,18.3864),(-2.8263,23.1741),(-3.5116,11.0341),(10.4803,14.5649) +5.83,15.0,28.3,1.5708,(-5.9227,18.2864),(-2.8263,23.0741),(-3.5116,10.9341),(10.4803,14.4649) +5.84,15.0,28.4,1.5708,(-5.9227,18.1864),(-2.8263,22.9741),(-3.5116,10.8341),(10.4803,14.3649) +5.85,15.0,28.5,1.5708,(-5.9227,18.0864),(-2.8263,22.8741),(-3.5116,10.7341),(10.4803,14.2649) +5.86,15.0,28.6,1.5708,(-5.9227,17.9864),(-2.8263,22.7741),(-3.5116,10.6341),(10.4803,14.1649) +5.87,15.0,28.7,1.5708,(-5.9227,17.8864),(-2.8263,22.6741),(-3.5116,10.5341),(10.4803,14.0649) +5.88,15.0,28.8,1.5708,(-5.9227,17.7864),(-2.8263,22.5741),(-3.5116,10.4341),(10.4803,13.9649) +5.89,15.0,28.9,1.5708,(-5.9227,17.6864),(-2.8263,22.4741),(-3.5116,10.3341),(10.4803,13.8649) +5.9,15.0,29.0,1.5708,(-5.9227,17.5864),(-2.8263,22.3741),(-3.5116,10.2341),(10.4803,13.7649) +5.91,15.0,29.1,1.5708,(-5.9227,17.4864),(-2.8263,22.2741),(-3.5116,10.1341),(10.4803,13.6649) +5.92,15.0,29.2,1.5708,(-5.9227,17.3864),(-2.8263,22.1741),(-3.5116,10.0341),(10.4803,13.5649) +5.93,15.0,29.3,1.5708,(-5.9227,17.2864),(-2.8263,22.0741),(-3.5116,9.9341),(10.4803,13.4649) +5.94,15.0,29.4,1.5708,(-5.9227,17.1864),(-2.8263,21.9741),(-3.5116,9.8341),(10.4803,13.3649) +5.95,15.0,29.5,1.5708,(-5.9227,17.0864),(-2.8263,21.8741),(-3.5116,9.7341),(10.4803,13.2649) +5.96,15.0,29.6,1.5708,(-5.9227,16.9864),(-2.8263,21.7741),(-3.5116,9.6341),(10.4803,13.1649) +5.97,15.0,29.7,1.5708,(-5.9227,16.8864),(-2.8263,21.6741),(-3.5116,9.5341),(10.4803,13.0649) +5.98,15.0,29.8,1.5708,(-5.9227,16.7864),(-2.8263,21.5741),(-3.5116,9.4341),(10.4803,12.9649) +5.99,15.0,29.9,1.5708,(-5.9227,16.6864),(-2.8263,21.4741),(-3.5116,9.3341),(10.4803,12.8649) +6.0,15.0,30.0,1.5708,(-5.9227,16.5864),(-2.8263,21.3741),(-3.5116,9.2341),(10.4803,12.7649) +6.01,15.0,30.1,1.5708,(-5.9227,16.4864),(-2.8263,21.2741),(-3.5116,9.1341),(10.4803,12.6649) +6.02,15.0,30.2,1.5708,(-5.9227,16.3864),(-2.8263,21.1741),(-3.5116,9.0341),(10.4803,12.5649) +6.03,15.0,30.3,1.5708,(-5.9227,16.2864),(-2.8263,21.0741),(-3.5116,8.9341),(10.4803,12.4649) +6.04,15.0,30.4,1.5708,(-5.9227,16.1864),(-2.8263,20.9741),(-3.5116,8.8341),(10.4803,12.3649) +6.05,15.0,30.5,1.5708,(-5.9227,16.0864),(-2.8263,20.8741),(-3.5116,8.7341),(10.4803,12.2649) +6.06,15.0,30.6,1.5708,(-5.9227,15.9864),(-2.8263,20.7741),(-3.5116,8.6341),(10.4803,12.1649) +6.07,15.0,30.7,1.5708,(-5.9227,15.8864),(-2.8263,20.6741),(-3.5116,8.5341),(10.4803,12.0649) +6.08,15.0,30.8,1.5708,(-5.9227,15.7864),(-2.8263,20.5741),(-3.5116,8.4341),(10.4803,11.9649) +6.09,15.0,30.9,1.5708,(-5.9227,15.6864),(-2.8263,20.4741),(1.9003,24.8474),(-3.5116,8.3341),(10.4803,11.8649) +6.1,15.0,31.0,1.5708,(-5.9227,15.5864),(-2.8263,20.3741),(1.9003,24.7474),(-3.5116,8.2341),(10.4803,11.7649) +6.11,15.0,31.1,1.5708,(-5.9227,15.4864),(-2.8263,20.2741),(1.9003,24.6474),(-3.5116,8.1341),(10.4803,11.6649) +6.12,15.0,31.2,1.5708,(-5.9227,15.3864),(-2.8263,20.1741),(1.9003,24.5474),(-3.5116,8.0341),(10.4803,11.5649) +6.13,15.0,31.3,1.5708,(-5.9227,15.2864),(-2.8263,20.0741),(1.9003,24.4474),(-3.5116,7.9341),(10.4803,11.4649) +6.14,15.0,31.4,1.5708,(-5.9227,15.1864),(-2.8263,19.9741),(1.9003,24.3474),(-3.5116,7.8341),(10.4803,11.3649) +6.15,15.0,31.5,1.5708,(-5.9227,15.0864),(-2.8263,19.8741),(1.9003,24.2474),(-3.5116,7.7341),(10.4803,11.2649) +6.16,15.0,31.6,1.5708,(-5.9227,14.9864),(-2.8263,19.7741),(1.9003,24.1474),(-3.5116,7.6341),(10.4803,11.1649) +6.17,15.0,31.7,1.5708,(-5.9227,14.8864),(-2.8263,19.6741),(1.9003,24.0474),(-3.5116,7.5341),(10.4803,11.0649) +6.18,15.0,31.8,1.5708,(-5.9227,14.7864),(-2.8263,19.5741),(1.9003,23.9474),(-3.5116,7.4341),(10.4803,10.9649) +6.19,15.0,31.9,1.5708,(-5.9227,14.6864),(-2.8263,19.4741),(1.9003,23.8474),(-3.5116,7.3341),(10.4803,10.8649) +6.2,15.0,32.0,1.5708,(-5.9227,14.5864),(-2.8263,19.3741),(1.9003,23.7474),(-3.5116,7.2341),(10.4803,10.7649) +6.21,15.0,32.1,1.5708,(-5.9227,14.4864),(-2.8263,19.2741),(1.9003,23.6474),(-3.5116,7.1341),(10.4803,10.6649) +6.22,15.0,32.2,1.5708,(-5.9227,14.3864),(-2.8263,19.1741),(1.9003,23.5474),(-3.5116,7.0341),(10.4803,10.5649) +6.23,15.0,32.3,1.5708,(-5.9227,14.2864),(-2.8263,19.0741),(1.9003,23.4474),(-3.5116,6.9341),(10.4803,10.4649) +6.24,15.0,32.4,1.5708,(-5.9227,14.1864),(-2.8263,18.9741),(1.9003,23.3474),(-3.5116,6.8341),(10.4803,10.3649) +6.25,15.0,32.5,1.5708,(-5.9227,14.0864),(-2.8263,18.8741),(1.9003,23.2474),(-3.5116,6.7341),(10.4803,10.2649) +6.26,15.0,32.6,1.5708,(-5.9227,13.9864),(-2.8263,18.7741),(1.9003,23.1474),(-3.5116,6.6341),(10.4803,10.1649) +6.27,15.0,32.7,1.5708,(-5.9227,13.8864),(-2.8263,18.6741),(1.9003,23.0474),(-3.5116,6.5341),(10.4803,10.0649) +6.28,15.0,32.8,1.5708,(-5.9227,13.7864),(-2.8263,18.5741),(1.9003,22.9474),(-3.5116,6.4341),(10.4803,9.9649) +6.29,15.0,32.9,1.5708,(-5.9227,13.6864),(-2.8263,18.4741),(1.9003,22.8474),(-3.5116,6.3341),(10.4803,9.8649) +6.3,15.0,33.0,1.5708,(-5.9227,13.5864),(-2.8263,18.3741),(1.9003,22.7474),(-3.5116,6.2341),(10.4803,9.7649) +6.31,15.0,33.1,1.5708,(-5.9227,13.4864),(-2.8263,18.2741),(1.9003,22.6474),(-3.5116,6.1341),(10.4803,9.6649) +6.32,15.0,33.2,1.5708,(-5.9227,13.3864),(-2.8263,18.1741),(1.9003,22.5474),(-3.5116,6.0341),(10.4803,9.5649) +6.33,15.0,33.3,1.5708,(-5.9227,13.2864),(-2.8263,18.0741),(1.9003,22.4474),(-3.5116,5.9341),(10.4803,9.4649) +6.34,15.0,33.4,1.5708,(-5.9227,13.1864),(-2.8263,17.9741),(1.9003,22.3474),(-3.5116,5.8341),(10.4803,9.3649) +6.35,15.0,33.5,1.5708,(-5.9227,13.0864),(-2.8263,17.8741),(1.9003,22.2474),(-3.5116,5.7341),(10.4803,9.2649) +6.36,15.0,33.6,1.5708,(-5.9227,12.9864),(-2.8263,17.7741),(1.9003,22.1474),(-3.5116,5.6341),(10.4803,9.1649) +6.37,15.0,33.7,1.5708,(-5.9227,12.8864),(-2.8263,17.6741),(1.9003,22.0474),(-3.5116,5.5341),(10.4803,9.0649) +6.38,15.0,33.8,1.5708,(-5.9227,12.7864),(-2.8263,17.5741),(1.9003,21.9474),(-3.5116,5.4341),(10.4803,8.9649) +6.39,15.0,33.9,1.5708,(-5.9227,12.6864),(-2.8263,17.4741),(1.9003,21.8474),(-3.5116,5.3341),(10.4803,8.8649) +6.4,15.0,34.0,1.5708,(-5.9227,12.5864),(-2.8263,17.3741),(1.9003,21.7474),(-3.5116,5.2341),(10.4803,8.7649) +6.41,15.0,34.1,1.5708,(-5.9227,12.4864),(-2.8263,17.2741),(1.9003,21.6474),(-3.5116,5.1341),(10.4803,8.6649) +6.42,15.0,34.2,1.5708,(-5.9227,12.3864),(-2.8263,17.1741),(1.9003,21.5474),(-3.5116,5.0341),(10.4803,8.5649) +6.43,15.0,34.3,1.5708,(-5.9227,12.2864),(-2.8263,17.0741),(1.9003,21.4474),(-3.5116,4.9341),(10.4803,8.4649) +6.44,15.0,34.4,1.5708,(-5.9227,12.1864),(-2.8263,16.9741),(1.9003,21.3474),(-3.5116,4.8341),(10.4803,8.3649) +6.45,15.0,34.5,1.5708,(-5.9227,12.0864),(-2.8263,16.8741),(1.9003,21.2474),(-3.5116,4.7341),(10.4803,8.2649) +6.46,15.0,34.6,1.5708,(-5.9227,11.9864),(-2.8263,16.7741),(1.9003,21.1474),(-3.5116,4.6341),(10.4803,8.1649) +6.47,15.0,34.7,1.5708,(-5.9227,11.8864),(-2.8263,16.6741),(1.9003,21.0474),(-3.5116,4.5341),(10.4803,8.0649) +6.48,15.0,34.8,1.5708,(-5.9227,11.7864),(-2.8263,16.5741),(1.9003,20.9474),(-3.5116,4.4341),(10.4803,7.9649) +6.49,15.0,34.9,1.5708,(-5.9227,11.6864),(-2.8263,16.4741),(1.9003,20.8474),(-3.5116,4.3341),(10.4803,7.8649) +6.5,15.0,35.0,1.5708,(-5.9227,11.5864),(-2.8263,16.3741),(1.9003,20.7474),(-3.5116,4.2341),(10.4803,7.7649) +6.51,15.0,35.1,1.5708,(-5.9227,11.4864),(-2.8263,16.2741),(1.9003,20.6474),(-3.5116,4.1341),(10.4803,7.6649) +6.52,15.0,35.2,1.5708,(-5.9227,11.3864),(-2.8263,16.1741),(1.9003,20.5474),(-3.5116,4.0341),(10.4803,7.5649) +6.53,15.0,35.3,1.5708,(-5.9227,11.2864),(-2.8263,16.0741),(1.9003,20.4474),(-3.5116,3.9341),(10.4803,7.4649) +6.54,15.0,35.4,1.5708,(-5.9227,11.1864),(-2.8263,15.9741),(1.9003,20.3474),(-3.5116,3.8341),(10.4803,7.3649) +6.55,15.0,35.5,1.5708,(-5.9227,11.0864),(-2.8263,15.8741),(1.9003,20.2474),(-3.5116,3.7341),(10.4803,7.2649) +6.56,15.0,35.6,1.5708,(-5.9227,10.9864),(-2.8263,15.7741),(1.9003,20.1474),(-3.5116,3.6341),(10.4803,7.1649) +6.57,15.0,35.7,1.5708,(-5.9227,10.8864),(-2.8263,15.6741),(1.9003,20.0474),(-3.5116,3.5341),(10.4803,7.0649) +6.58,15.0,35.8,1.5708,(-5.9227,10.7864),(-2.8263,15.5741),(1.9003,19.9474),(-3.5116,3.4341),(10.4803,6.9649) +6.59,15.0,35.9,1.5708,(-5.9227,10.6864),(-2.8263,15.4741),(1.9003,19.8474),(-3.5116,3.3341),(10.4803,6.8649) +6.6,15.0,36.0,1.5708,(-5.9227,10.5864),(-2.8263,15.3741),(1.9003,19.7474),(-3.5116,3.2341),(10.4803,6.7649) +6.61,15.0,36.1,1.5708,(-5.9227,10.4864),(-2.8263,15.2741),(1.9003,19.6474),(-3.5116,3.1341),(10.4803,6.6649) +6.62,15.0,36.2,1.5708,(-5.9227,10.3864),(-2.8263,15.1741),(1.9003,19.5474),(-3.5116,3.0341),(10.4803,6.5649) +6.63,15.0,36.3,1.5708,(-5.9227,10.2864),(-2.8263,15.0741),(1.9003,19.4474),(-3.5116,2.9341),(10.4803,6.4649) +6.64,15.0,36.4,1.5708,(-5.9227,10.1864),(-2.8263,14.9741),(1.9003,19.3474),(-3.5116,2.8341),(10.4803,6.3649) +6.65,15.0,36.5,1.5708,(-5.9227,10.0864),(-2.8263,14.8741),(1.9003,19.2474),(-3.5116,2.7341),(10.4803,6.2649) +6.66,15.0,36.6,1.5708,(-5.9227,9.9864),(-2.8263,14.7741),(1.9003,19.1474),(-3.5116,2.6341),(10.4803,6.1649) +6.67,15.0,36.7,1.5708,(-5.9227,9.8864),(-2.8263,14.6741),(1.9003,19.0474),(-3.5116,2.5341),(10.4803,6.0649) +6.68,15.0,36.8,1.5708,(-5.9227,9.7864),(-2.8263,14.5741),(1.9003,18.9474),(-3.5116,2.4341),(10.4803,5.9649) +6.69,15.0,36.9,1.5708,(-5.9227,9.6864),(-2.8263,14.4741),(1.9003,18.8474),(-3.5116,2.3341),(10.4803,5.8649) +6.7,15.0,37.0,1.5708,(-5.9227,9.5864),(-2.8263,14.3741),(1.9003,18.7474),(-3.5116,2.2341),(10.4803,5.7649) +6.71,15.0,37.1,1.5708,(-5.9227,9.4864),(-2.8263,14.2741),(1.9003,18.6474),(21.0385,13.5046),(-3.5116,2.1341),(10.4803,5.6649) +6.72,15.0,37.2,1.5708,(-5.9227,9.3864),(-2.8263,14.1741),(1.9003,18.5474),(21.0385,13.4046),(-3.5116,2.0341),(10.4803,5.5649) +6.73,15.0,37.3,1.5708,(-5.9227,9.2864),(-2.8263,14.0741),(1.9003,18.4474),(21.0385,13.3046),(-3.5116,1.9341),(10.4803,5.4649) +6.74,15.0,37.4,1.5708,(-5.9227,9.1864),(-2.8263,13.9741),(1.9003,18.3474),(21.0385,13.2046),(-3.5116,1.8341),(10.4803,5.3649) +6.75,15.0,37.5,1.5708,(-5.9227,9.0864),(-2.8263,13.8741),(1.9003,18.2474),(21.0385,13.1046),(-3.5116,1.7341),(10.4803,5.2649) +6.76,15.0,37.6,1.5708,(-5.9227,8.9864),(-2.8263,13.7741),(1.9003,18.1474),(21.0385,13.0046),(-3.5116,1.6341),(10.4803,5.1649) +6.77,15.0,37.7,1.5708,(-5.9227,8.8864),(-2.8263,13.6741),(1.9003,18.0474),(21.0385,12.9046),(-3.5116,1.5341),(10.4803,5.0649) +6.78,15.0,37.8,1.5708,(-5.9227,8.7864),(-2.8263,13.5741),(1.9003,17.9474),(21.0385,12.8046),(-3.5116,1.4341),(10.4803,4.9649) +6.79,15.0,37.9,1.5708,(-5.9227,8.6864),(-2.8263,13.4741),(1.9003,17.8474),(21.0385,12.7046),(-3.5116,1.3341),(10.4803,4.8649) +6.8,15.0,38.0,1.5708,(-5.9227,8.5864),(-2.8263,13.3741),(1.9003,17.7474),(21.0385,12.6046),(-3.5116,1.2341),(10.4803,4.7649) +6.81,15.0,38.1,1.5708,(-5.9227,8.4864),(-2.8263,13.2741),(1.9003,17.6474),(21.0385,12.5046),(-3.5116,1.1341),(10.4803,4.6649) +6.82,15.0,38.2,1.5708,(-5.9227,8.3864),(-2.8263,13.1741),(1.9003,17.5474),(21.0385,12.4046),(-3.5116,1.0341),(10.4803,4.5649) +6.83,15.0,38.3,1.5708,(-5.9227,8.2864),(-2.8263,13.0741),(1.9003,17.4474),(21.0385,12.3046),(-3.5116,0.9341),(10.4803,4.4649) +6.84,15.0,38.4,1.5708,(-5.9227,8.1864),(-2.8263,12.9741),(1.9003,17.3474),(21.0385,12.2046),(-3.5116,0.8341),(10.4803,4.3649) +6.85,15.0,38.5,1.5708,(-5.9227,8.0864),(-2.8263,12.8741),(1.9003,17.2474),(21.0385,12.1046),(-3.5116,0.7341),(10.4803,4.2649) +6.86,15.0,38.6,1.5708,(-5.9227,7.9864),(-2.8263,12.7741),(1.9003,17.1474),(21.0385,12.0046),(-3.5116,0.6341),(10.4803,4.1649) +6.87,15.0,38.7,1.5708,(-5.9227,7.8864),(-2.8263,12.6741),(1.9003,17.0474),(21.0385,11.9046),(-3.5116,0.5341),(10.4803,4.0649) +6.88,15.0,38.8,1.5708,(-5.9227,7.7864),(-2.8263,12.5741),(1.9003,16.9474),(21.0385,11.8046),(-3.5116,0.4341),(10.4803,3.9649) +6.89,15.0,38.9,1.5708,(-5.9227,7.6864),(-2.8263,12.4741),(1.9003,16.8474),(21.0385,11.7046),(-3.5116,0.3341),(10.4803,3.8649) +6.9,15.0,39.0,1.5708,(-5.9227,7.5864),(-2.8263,12.3741),(1.9003,16.7474),(21.0385,11.6046),(-3.5116,0.2341),(10.4803,3.7649) +6.91,15.0,39.1,1.5708,(-5.9227,7.4864),(-2.8263,12.2741),(1.9003,16.6474),(21.0385,11.5046),(-3.5116,0.1341),(10.4803,3.6649) +6.92,15.0,39.2,1.5708,(-5.9227,7.3864),(-2.8263,12.1741),(1.9003,16.5474),(21.0385,11.4046),(-3.5116,0.0341),(10.4803,3.5649) +6.93,15.0,39.3,1.5708,(-5.9227,7.2864),(-2.8263,12.0741),(1.9003,16.4474),(21.0385,11.3046),(10.4803,3.4649) +6.94,15.0,39.4,1.5708,(-5.9227,7.1864),(-2.8263,11.9741),(1.9003,16.3474),(21.0385,11.2046),(10.4803,3.3649) +6.95,15.0,39.5,1.5708,(-5.9227,7.0864),(-2.8263,11.8741),(1.9003,16.2474),(21.0385,11.1046),(10.4803,3.2649) +6.96,15.0,39.6,1.5708,(-5.9227,6.9864),(-2.8263,11.7741),(1.9003,16.1474),(21.0385,11.0046),(10.4803,3.1649) +6.97,15.0,39.7,1.5708,(-5.9227,6.8864),(16.9153,18.3599),(-2.8263,11.6741),(1.9003,16.0474),(21.0385,10.9046),(10.4803,3.0649) +6.98,15.0,39.8,1.5708,(-5.9227,6.7864),(16.9153,18.2599),(-2.8263,11.5741),(1.9003,15.9474),(21.0385,10.8046),(10.4803,2.9649) +6.99,15.0,39.9,1.5708,(-5.9227,6.6864),(16.9153,18.1599),(-2.8263,11.4741),(1.9003,15.8474),(21.0385,10.7046),(10.4803,2.8649) +7.0,15.0,40.0,1.5708,(-5.9227,6.5864),(16.9153,18.0599),(-2.8263,11.3741),(1.9003,15.7474),(21.0385,10.6046),(10.4803,2.7649) +7.01,15.0,40.1,1.5708,(-5.9227,6.4864),(16.9153,17.9599),(-2.8263,11.2741),(1.9003,15.6474),(21.0385,10.5046),(10.4803,2.6649) +7.02,15.0,40.2,1.5708,(-5.9227,6.3864),(16.9153,17.8599),(-2.8263,11.1741),(1.9003,15.5474),(21.0385,10.4046),(10.4803,2.5649) +7.03,15.0,40.3,1.5708,(-5.9227,6.2864),(16.9153,17.7599),(-2.8263,11.0741),(1.9003,15.4474),(21.0385,10.3046),(10.4803,2.4649) +7.04,15.0,40.4,1.5708,(-5.9227,6.1864),(16.9153,17.6599),(-2.8263,10.9741),(1.9003,15.3474),(21.0385,10.2046),(10.4803,2.3649) +7.05,15.0,40.5,1.5708,(-5.9227,6.0864),(16.9153,17.5599),(-2.8263,10.8741),(1.9003,15.2474),(21.0385,10.1046),(10.4803,2.2649) +7.06,15.0,40.6,1.5708,(-5.9227,5.9864),(16.9153,17.4599),(-2.8263,10.7741),(1.9003,15.1474),(21.0385,10.0046),(10.4803,2.1649) +7.07,15.0,40.7,1.5708,(-5.9227,5.8864),(16.9153,17.3599),(-2.8263,10.6741),(1.9003,15.0474),(21.0385,9.9046),(10.4803,2.0649) +7.08,15.0,40.8,1.5708,(-5.9227,5.7864),(16.9153,17.2599),(-2.8263,10.5741),(1.9003,14.9474),(21.0385,9.8046),(10.4803,1.9649) +7.09,15.0,40.9,1.5708,(-5.9227,5.6864),(16.9153,17.1599),(-2.8263,10.4741),(1.9003,14.8474),(21.0385,9.7046),(10.4803,1.8649) +7.1,15.0,41.0,1.5708,(-5.9227,5.5864),(16.9153,17.0599),(-2.8263,10.3741),(1.9003,14.7474),(21.0385,9.6046),(-2.114,24.8161),(10.4803,1.7649) +7.11,15.0,41.1,1.5708,(-5.9227,5.4864),(16.9153,16.9599),(-2.8263,10.2741),(1.9003,14.6474),(21.0385,9.5046),(-2.114,24.7161),(10.4803,1.6649) +7.12,15.0,41.2,1.5708,(-5.9227,5.3864),(16.9153,16.8599),(-2.8263,10.1741),(1.9003,14.5474),(21.0385,9.4046),(-2.114,24.6161),(10.4803,1.5649) +7.13,15.0,41.3,1.5708,(-5.9227,5.2864),(16.9153,16.7599),(-2.8263,10.0741),(1.9003,14.4474),(21.0385,9.3046),(-2.114,24.5161),(10.4803,1.4649) +7.14,15.0,41.4,1.5708,(-5.9227,5.1864),(16.9153,16.6599),(-2.8263,9.9741),(1.9003,14.3474),(21.0385,9.2046),(-2.114,24.4161),(10.4803,1.3649) +7.15,15.0,41.5,1.5708,(-5.9227,5.0864),(16.9153,16.5599),(-2.8263,9.8741),(1.9003,14.2474),(21.0385,9.1046),(-2.114,24.3161),(10.4803,1.2649) +7.16,15.0,41.6,1.5708,(-5.9227,4.9864),(16.9153,16.4599),(-2.8263,9.7741),(1.9003,14.1474),(21.0385,9.0046),(-2.114,24.2161),(10.4803,1.1649) +7.17,15.0,41.7,1.5708,(-5.9227,4.8864),(16.9153,16.3599),(-2.8263,9.6741),(1.9003,14.0474),(21.0385,8.9046),(-2.114,24.1161),(10.4803,1.0649) +7.18,15.0,41.8,1.5708,(-5.9227,4.7864),(16.9153,16.2599),(-2.8263,9.5741),(1.9003,13.9474),(21.0385,8.8046),(-2.114,24.0161),(10.4803,0.9649) +7.19,15.0,41.9,1.5708,(-5.9227,4.6864),(16.9153,16.1599),(-2.8263,9.4741),(1.9003,13.8474),(21.0385,8.7046),(-2.114,23.9161),(10.4803,0.8649) +7.2,15.0,42.0,1.5708,(-5.9227,4.5864),(16.9153,16.0599),(-2.8263,9.3741),(1.9003,13.7474),(21.0385,8.6046),(-2.114,23.8161),(10.4803,0.7649) +7.21,15.0,42.1,1.5708,(-5.9227,4.4864),(16.9153,15.9599),(-2.8263,9.2741),(1.9003,13.6474),(21.0385,8.5046),(-2.114,23.7161),(10.4803,0.6649) +7.22,15.0,42.2,1.5708,(-5.9227,4.3864),(16.9153,15.8599),(-2.8263,9.1741),(1.9003,13.5474),(21.0385,8.4046),(7.3445,23.89),(-2.114,23.6161),(10.4803,0.5649) +7.23,15.0,42.3,1.5708,(-5.9227,4.2864),(16.9153,15.7599),(-2.8263,9.0741),(1.9003,13.4474),(21.0385,8.3046),(7.3445,23.79),(-2.114,23.5161),(10.4803,0.4649) +7.24,15.0,42.4,1.5708,(-5.9227,4.1864),(16.9153,15.6599),(-2.8263,8.9741),(1.9003,13.3474),(21.0385,8.2046),(7.3445,23.69),(-2.114,23.4161),(10.4803,0.3649) +7.25,15.0,42.5,1.5708,(-5.9227,4.0864),(16.9153,15.5599),(-2.8263,8.8741),(1.9003,13.2474),(21.0385,8.1046),(7.3445,23.59),(-2.114,23.3161),(10.4803,0.2649) +7.26,15.0,42.6,1.5708,(-5.9227,3.9864),(16.9153,15.4599),(-2.8263,8.7741),(1.9003,13.1474),(21.0385,8.0046),(7.3445,23.49),(-2.114,23.2161),(10.4803,0.1649) +7.27,15.0,42.7,1.5708,(-5.9227,3.8864),(16.9153,15.3599),(-2.8263,8.6741),(1.9003,13.0474),(21.0385,7.9046),(7.3445,23.39),(-2.114,23.1161),(10.4803,0.0649) +7.28,15.0,42.8,1.5708,(-5.9227,3.7864),(16.9153,15.2599),(-2.8263,8.5741),(1.9003,12.9474),(21.0385,7.8046),(7.3445,23.29),(-2.114,23.0161) +7.29,15.0,42.9,1.5708,(-5.9227,3.6864),(16.9153,15.1599),(-2.8263,8.4741),(1.9003,12.8474),(21.0385,7.7046),(7.3445,23.19),(-2.114,22.9161) +7.3,15.0,43.0,1.5708,(-5.9227,3.5864),(16.9153,15.0599),(-2.8263,8.3741),(1.9003,12.7474),(21.0385,7.6046),(7.3445,23.09),(-2.114,22.8161) +7.31,15.0,43.1,1.5708,(-5.9227,3.4864),(16.9153,14.9599),(-2.8263,8.2741),(1.9003,12.6474),(21.0385,7.5046),(7.3445,22.99),(-2.114,22.7161) +7.32,15.0,43.2,1.5708,(-5.9227,3.3864),(16.9153,14.8599),(-2.8263,8.1741),(1.9003,12.5474),(21.0385,7.4046),(7.3445,22.89),(-2.114,22.6161) +7.33,15.0,43.3,1.5708,(-5.9227,3.2864),(16.9153,14.7599),(-2.8263,8.0741),(1.9003,12.4474),(21.0385,7.3046),(7.3445,22.79),(-2.114,22.5161) +7.34,15.0,43.4,1.5708,(-5.9227,3.1864),(16.9153,14.6599),(-2.8263,7.9741),(1.9003,12.3474),(21.0385,7.2046),(7.3445,22.69),(-2.114,22.4161) +7.35,15.0,43.5,1.5708,(-5.9227,3.0864),(16.9153,14.5599),(-2.8263,7.8741),(1.9003,12.2474),(21.0385,7.1046),(7.3445,22.59),(-2.114,22.3161) +7.36,15.0,43.6,1.5708,(-5.9227,2.9864),(16.9153,14.4599),(-2.8263,7.7741),(1.9003,12.1474),(21.0385,7.0046),(7.3445,22.49),(-2.114,22.2161) +7.37,15.0,43.7,1.5708,(-5.9227,2.8864),(16.9153,14.3599),(-2.8263,7.6741),(1.9003,12.0474),(21.0385,6.9046),(7.3445,22.39),(-2.114,22.1161) +7.38,15.0,43.8,1.5708,(-5.9227,2.7864),(16.9153,14.2599),(-2.8263,7.5741),(1.9003,11.9474),(21.0385,6.8046),(7.3445,22.29),(-2.114,22.0161) +7.39,15.0,43.9,1.5708,(-5.9227,2.6864),(16.9153,14.1599),(-2.8263,7.4741),(1.9003,11.8474),(21.0385,6.7046),(7.3445,22.19),(-2.114,21.9161) +7.4,15.0,44.0,1.5708,(-5.9227,2.5864),(16.9153,14.0599),(-2.8263,7.3741),(1.9003,11.7474),(21.0385,6.6046),(7.3445,22.09),(-2.114,21.8161) +7.41,15.0,44.1,1.5708,(-5.9227,2.4864),(16.9153,13.9599),(-2.8263,7.2741),(1.9003,11.6474),(21.0385,6.5046),(7.3445,21.99),(-2.114,21.7161) +7.42,15.0,44.2,1.5708,(-5.9227,2.3864),(16.9153,13.8599),(-2.8263,7.1741),(1.9003,11.5474),(21.0385,6.4046),(7.3445,21.89),(-2.114,21.6161) +7.43,15.0,44.3,1.5708,(-5.9227,2.2864),(16.9153,13.7599),(-2.8263,7.0741),(1.9003,11.4474),(21.0385,6.3046),(7.3445,21.79),(-2.114,21.5161) +7.44,15.0,44.4,1.5708,(-5.9227,2.1864),(16.9153,13.6599),(-2.8263,6.9741),(1.9003,11.3474),(21.0385,6.2046),(7.3445,21.69),(-2.114,21.4161) +7.45,15.0,44.5,1.5708,(-5.9227,2.0864),(16.9153,13.5599),(-2.8263,6.8741),(1.9003,11.2474),(21.0385,6.1046),(7.3445,21.59),(-2.114,21.3161) +7.46,15.0,44.6,1.5708,(-5.9227,1.9864),(16.9153,13.4599),(-2.8263,6.7741),(1.9003,11.1474),(21.0385,6.0046),(7.3445,21.49),(-2.114,21.2161) +7.47,15.0,44.7,1.5708,(-5.9227,1.8864),(16.9153,13.3599),(-2.8263,6.6741),(1.9003,11.0474),(21.0385,5.9046),(7.3445,21.39),(-2.114,21.1161) +7.48,15.0,44.8,1.5708,(-5.9227,1.7864),(16.9153,13.2599),(-2.8263,6.5741),(1.9003,10.9474),(21.0385,5.8046),(7.3445,21.29),(-2.114,21.0161) +7.49,15.0,44.9,1.5708,(-5.9227,1.6864),(16.9153,13.1599),(-2.8263,6.4741),(1.9003,10.8474),(21.0385,5.7046),(7.3445,21.19),(-2.114,20.9161) +7.5,15.0,45.0,1.5708,(-5.9227,1.5864),(16.9153,13.0599),(-2.8263,6.3741),(1.9003,10.7474),(21.0385,5.6046),(7.3445,21.09),(-2.114,20.8161) +7.51,15.0,45.1,1.5708,(-5.9227,1.4864),(16.9153,12.9599),(-2.8263,6.2741),(1.9003,10.6474),(21.0385,5.5046),(7.3445,20.99),(-2.114,20.7161) +7.52,15.0,45.2,1.5708,(-5.9227,1.3864),(16.9153,12.8599),(-2.8263,6.1741),(1.9003,10.5474),(21.0385,5.4046),(7.3445,20.89),(-2.114,20.6161) +7.53,15.0,45.3,1.5708,(-5.9227,1.2864),(16.9153,12.7599),(-2.8263,6.0741),(1.9003,10.4474),(21.0385,5.3046),(7.3445,20.79),(-2.114,20.5161) +7.54,15.0,45.4,1.5708,(-5.9227,1.1864),(16.9153,12.6599),(-2.8263,5.9741),(1.9003,10.3474),(21.0385,5.2046),(7.3445,20.69),(-2.114,20.4161) +7.55,15.0,45.5,1.5708,(-5.9227,1.0864),(16.9153,12.5599),(-2.8263,5.8741),(1.9003,10.2474),(21.0385,5.1046),(7.3445,20.59),(-2.114,20.3161) +7.56,15.0,45.6,1.5708,(-5.9227,0.9864),(16.9153,12.4599),(-2.8263,5.7741),(1.9003,10.1474),(21.0385,5.0046),(7.3445,20.49),(-2.114,20.2161) +7.57,15.0,45.7,1.5708,(-5.9227,0.8864),(16.9153,12.3599),(-2.8263,5.6741),(1.9003,10.0474),(21.0385,4.9046),(7.3445,20.39),(-2.114,20.1161) +7.58,15.0,45.8,1.5708,(-5.9227,0.7864),(16.9153,12.2599),(-2.8263,5.5741),(1.9003,9.9474),(21.0385,4.8046),(7.3445,20.29),(-2.114,20.0161) +7.59,15.0,45.9,1.5708,(-5.9227,0.6864),(16.9153,12.1599),(-2.8263,5.4741),(1.9003,9.8474),(21.0385,4.7046),(7.3445,20.19),(-2.114,19.9161) +7.6,15.0,46.0,1.5708,(-5.9227,0.5864),(16.9153,12.0599),(-2.8263,5.3741),(1.9003,9.7474),(21.0385,4.6046),(7.3445,20.09),(-2.114,19.8161) +7.61,15.0,46.1,1.5708,(-5.9227,0.4864),(16.9153,11.9599),(-2.8263,5.2741),(1.9003,9.6474),(21.0385,4.5046),(7.3445,19.99),(-2.114,19.7161) +7.62,15.0,46.2,1.5708,(-5.9227,0.3864),(16.9153,11.8599),(-2.8263,5.1741),(1.9003,9.5474),(21.0385,4.4046),(7.3445,19.89),(-2.114,19.6161) +7.63,15.0,46.3,1.5708,(-5.9227,0.2864),(16.9153,11.7599),(-2.8263,5.0741),(1.9003,9.4474),(21.0385,4.3046),(7.3445,19.79),(-2.114,19.5161) +7.64,15.0,46.4,1.5708,(-5.9227,0.1864),(16.9153,11.6599),(-2.8263,4.9741),(1.9003,9.3474),(21.0385,4.2046),(7.3445,19.69),(-2.114,19.4161) +7.65,15.0,46.5,1.5708,(-5.9227,0.0864),(16.9153,11.5599),(-2.8263,4.8741),(1.9003,9.2474),(21.0385,4.1046),(7.3445,19.59),(-2.114,19.3161) +7.66,15.0,46.6,1.5708,(16.9153,11.4599),(-2.8263,4.7741),(1.9003,9.1474),(21.0385,4.0046),(7.3445,19.49),(-2.114,19.2161) +7.67,15.0,46.7,1.5708,(16.9153,11.3599),(-2.8263,4.6741),(1.9003,9.0474),(21.0385,3.9046),(7.3445,19.39),(-2.114,19.1161) +7.68,15.0,46.8,1.5708,(16.9153,11.2599),(-2.8263,4.5741),(1.9003,8.9474),(21.0385,3.8046),(7.3445,19.29),(-2.114,19.0161) +7.69,15.0,46.9,1.5708,(16.9153,11.1599),(-2.8263,4.4741),(1.9003,8.8474),(21.0385,3.7046),(7.3445,19.19),(-2.114,18.9161) +7.7,15.0,47.0,1.5708,(16.9153,11.0599),(-2.8263,4.3741),(1.9003,8.7474),(21.0385,3.6046),(7.3445,19.09),(-2.114,18.8161) +7.71,15.0,47.1,1.5708,(16.9153,10.9599),(-2.8263,4.2741),(1.9003,8.6474),(21.0385,3.5046),(7.3445,18.99),(-2.114,18.7161) +7.72,15.0,47.2,1.5708,(16.9153,10.8599),(-2.8263,4.1741),(1.9003,8.5474),(21.0385,3.4046),(7.3445,18.89),(-2.114,18.6161) +7.73,15.0,47.3,1.5708,(16.9153,10.7599),(-2.8263,4.0741),(1.9003,8.4474),(21.0385,3.3046),(7.3445,18.79),(-2.114,18.5161) +7.74,15.0,47.4,1.5708,(16.9153,10.6599),(-2.8263,3.9741),(1.9003,8.3474),(21.0385,3.2046),(7.3445,18.69),(-2.114,18.4161) +7.75,15.0,47.5,1.5708,(16.9153,10.5599),(-2.8263,3.8741),(1.9003,8.2474),(21.0385,3.1046),(7.3445,18.59),(-2.114,18.3161) +7.76,15.0,47.6,1.5708,(16.9153,10.4599),(-2.8263,3.7741),(1.9003,8.1474),(21.0385,3.0046),(7.3445,18.49),(-2.114,18.2161) +7.77,15.0,47.7,1.5708,(16.9153,10.3599),(-2.8263,3.6741),(1.9003,8.0474),(21.0385,2.9046),(7.3445,18.39),(-2.114,18.1161) +7.78,15.0,47.8,1.5708,(16.9153,10.2599),(-2.8263,3.5741),(1.9003,7.9474),(21.0385,2.8046),(7.3445,18.29),(-2.114,18.0161) +7.79,15.0,47.9,1.5708,(16.9153,10.1599),(-2.8263,3.4741),(1.9003,7.8474),(21.0385,2.7046),(7.3445,18.19),(-2.114,17.9161) +7.8,15.0,48.0,1.5708,(16.9153,10.0599),(-2.8263,3.3741),(1.9003,7.7474),(21.0385,2.6046),(7.3445,18.09),(-2.114,17.8161) +7.81,15.0,48.1,1.5708,(16.9153,9.9599),(-2.8263,3.2741),(1.9003,7.6474),(21.0385,2.5046),(7.3445,17.99),(-2.114,17.7161) +7.82,15.0,48.2,1.5708,(16.9153,9.8599),(-2.8263,3.1741),(1.9003,7.5474),(21.0385,2.4046),(7.3445,17.89),(-2.114,17.6161) +7.83,15.0,48.3,1.5708,(16.9153,9.7599),(-2.8263,3.0741),(1.9003,7.4474),(21.0385,2.3046),(7.3445,17.79),(-2.114,17.5161) +7.84,15.0,48.4,1.5708,(16.9153,9.6599),(-2.8263,2.9741),(1.9003,7.3474),(21.0385,2.2046),(7.3445,17.69),(-2.114,17.4161) +7.85,15.0,48.5,1.5708,(16.9153,9.5599),(-2.8263,2.8741),(1.9003,7.2474),(21.0385,2.1046),(7.3445,17.59),(-2.114,17.3161) +7.86,15.0,48.6,1.5708,(16.9153,9.4599),(-2.8263,2.7741),(1.9003,7.1474),(21.0385,2.0046),(7.3445,17.49),(-2.114,17.2161) +7.87,15.0,48.7,1.5708,(16.9153,9.3599),(-2.8263,2.6741),(1.9003,7.0474),(21.0385,1.9046),(7.3445,17.39),(-2.114,17.1161) +7.88,15.0,48.8,1.5708,(16.9153,9.2599),(-2.8263,2.5741),(1.9003,6.9474),(21.0385,1.8046),(7.3445,17.29),(-2.114,17.0161) +7.89,15.0,48.9,1.5708,(16.9153,9.1599),(-2.8263,2.4741),(1.9003,6.8474),(21.0385,1.7046),(7.3445,17.19),(-2.114,16.9161) +7.9,15.0,49.0,1.5708,(16.9153,9.0599),(-2.8263,2.3741),(1.9003,6.7474),(21.0385,1.6046),(-14.0772,20.5756),(7.3445,17.09),(-2.114,16.8161) +7.91,15.0,49.1,1.5708,(16.9153,8.9599),(-2.8263,2.2741),(1.9003,6.6474),(21.0385,1.5046),(-14.0772,20.4756),(7.3445,16.99),(-2.114,16.7161) +7.92,15.0,49.2,1.5708,(16.9153,8.8599),(-2.8263,2.1741),(1.9003,6.5474),(21.0385,1.4046),(-14.0772,20.3756),(7.3445,16.89),(-2.114,16.6161) +7.93,15.0,49.3,1.5708,(16.9153,8.7599),(-2.8263,2.0741),(1.9003,6.4474),(21.0385,1.3046),(-14.0772,20.2756),(7.3445,16.79),(-2.114,16.5161) +7.94,15.0,49.4,1.5708,(16.9153,8.6599),(-2.8263,1.9741),(1.9003,6.3474),(21.0385,1.2046),(-14.0772,20.1756),(7.3445,16.69),(-2.114,16.4161) +7.95,15.0,49.5,1.5708,(16.9153,8.5599),(-2.8263,1.8741),(1.9003,6.2474),(21.0385,1.1046),(-14.0772,20.0756),(7.3445,16.59),(-2.114,16.3161) +7.96,15.0,49.6,1.5708,(16.9153,8.4599),(-2.8263,1.7741),(1.9003,6.1474),(21.0385,1.0046),(-14.0772,19.9756),(7.3445,16.49),(-2.114,16.2161) +7.97,15.0,49.7,1.5708,(16.9153,8.3599),(-2.8263,1.6741),(1.9003,6.0474),(21.0385,0.9046),(-14.0772,19.8756),(7.3445,16.39),(-2.114,16.1161) +7.98,15.0,49.8,1.5708,(16.9153,8.2599),(-2.8263,1.5741),(1.9003,5.9474),(21.0385,0.8046),(-14.0772,19.7756),(7.3445,16.29),(-2.114,16.0161) +7.99,15.0,49.9,1.5708,(16.9153,8.1599),(-2.8263,1.4741),(1.9003,5.8474),(21.0385,0.7046),(-14.0772,19.6756),(7.3445,16.19),(-2.114,15.9161) +8.0,15.0,50.0,1.5708,(16.9153,8.0599),(-2.8263,1.3741),(1.9003,5.7474),(21.0385,0.6046),(-14.0772,19.5756),(7.3445,16.09),(-2.114,15.8161) +8.01,15.0,50.1,1.5708,(16.9153,7.9599),(-2.8263,1.2741),(1.9003,5.6474),(21.0385,0.5046),(-14.0772,19.4756),(7.3445,15.99),(-2.114,15.7161) +8.02,15.0,50.2,1.5708,(16.9153,7.8599),(-2.8263,1.1741),(1.9003,5.5474),(21.0385,0.4046),(-14.0772,19.3756),(7.3445,15.89),(-2.114,15.6161) +8.03,15.0,50.3,1.5708,(16.9153,7.7599),(-2.8263,1.0741),(1.9003,5.4474),(21.0385,0.3046),(-14.0772,19.2756),(7.3445,15.79),(-2.114,15.5161) +8.04,15.0,50.4,1.5708,(16.9153,7.6599),(-2.8263,0.9741),(1.9003,5.3474),(21.0385,0.2046),(-14.0772,19.1756),(7.3445,15.69),(-2.114,15.4161) +8.05,15.0,50.5,1.5708,(16.9153,7.5599),(-2.8263,0.8741),(1.9003,5.2474),(21.0385,0.1046),(-14.0772,19.0756),(7.3445,15.59),(-2.114,15.3161) +8.06,15.0,50.6,1.5708,(16.9153,7.4599),(-2.8263,0.7741),(1.9003,5.1474),(21.0385,0.0046),(-14.0772,18.9756),(7.3445,15.49),(-2.114,15.2161) +8.07,15.0,50.7,1.5708,(16.9153,7.3599),(-2.8263,0.6741),(1.9003,5.0474),(-14.0772,18.8756),(7.3445,15.39),(-2.114,15.1161) +8.08,15.0,50.8,1.5708,(16.9153,7.2599),(-2.8263,0.5741),(1.9003,4.9474),(-14.0772,18.7756),(7.3445,15.29),(-2.114,15.0161) +8.09,15.0,50.9,1.5708,(16.9153,7.1599),(-2.8263,0.4741),(1.9003,4.8474),(-14.0772,18.6756),(7.3445,15.19),(-2.114,14.9161) +8.1,15.0,51.0,1.5708,(16.9153,7.0599),(-2.8263,0.3741),(1.9003,4.7474),(-14.0772,18.5756),(7.3445,15.09),(-2.114,14.8161) +8.11,15.0,51.1,1.5708,(16.9153,6.9599),(-2.8263,0.2741),(1.9003,4.6474),(-14.0772,18.4756),(7.3445,14.99),(-2.114,14.7161) +8.12,15.0,51.2,1.5708,(16.9153,6.8599),(-2.8263,0.1741),(1.9003,4.5474),(-14.0772,18.3756),(7.3445,14.89),(-2.114,14.6161) +8.13,15.0,51.3,1.5708,(16.9153,6.7599),(-2.8263,0.0741),(1.9003,4.4474),(-14.0772,18.2756),(7.3445,14.79),(-2.114,14.5161) +8.14,15.0,51.4,1.5708,(16.9153,6.6599),(1.9003,4.3474),(-14.0772,18.1756),(7.3445,14.69),(-2.114,14.4161) +8.15,15.0,51.5,1.5708,(16.9153,6.5599),(1.9003,4.2474),(-14.0772,18.0756),(7.3445,14.59),(-2.114,14.3161) +8.16,15.0,51.6,1.5708,(16.9153,6.4599),(1.9003,4.1474),(-14.0772,17.9756),(7.3445,14.49),(-2.114,14.2161) +8.17,15.0,51.7,1.5708,(16.9153,6.3599),(1.9003,4.0474),(-14.0772,17.8756),(7.3445,14.39),(-2.114,14.1161) +8.18,15.0,51.8,1.5708,(16.9153,6.2599),(1.9003,3.9474),(-14.0772,17.7756),(7.3445,14.29),(-2.114,14.0161) +8.19,15.0,51.9,1.5708,(16.9153,6.1599),(1.9003,3.8474),(-14.0772,17.6756),(7.3445,14.19),(-2.114,13.9161) +8.2,15.0,52.0,1.5708,(16.9153,6.0599),(1.9003,3.7474),(-14.0772,17.5756),(7.3445,14.09),(-2.114,13.8161) +8.21,15.0,52.1,1.5708,(16.9153,5.9599),(1.9003,3.6474),(-14.0772,17.4756),(7.3445,13.99),(-2.114,13.7161) +8.22,15.0,52.2,1.5708,(16.9153,5.8599),(1.9003,3.5474),(-14.0772,17.3756),(7.3445,13.89),(-2.114,13.6161) +8.23,15.0,52.3,1.5708,(16.9153,5.7599),(1.9003,3.4474),(-14.0772,17.2756),(7.3445,13.79),(-2.114,13.5161) +8.24,15.0,52.4,1.5708,(16.9153,5.6599),(1.9003,3.3474),(-14.0772,17.1756),(7.3445,13.69),(-2.114,13.4161) +8.25,15.0,52.5,1.5708,(16.9153,5.5599),(1.9003,3.2474),(-14.0772,17.0756),(7.3445,13.59),(-2.114,13.3161) +8.26,15.0,52.6,1.5708,(16.9153,5.4599),(1.9003,3.1474),(-14.0772,16.9756),(7.3445,13.49),(-2.114,13.2161) +8.27,15.0,52.7,1.5708,(16.9153,5.3599),(1.9003,3.0474),(-14.0772,16.8756),(7.3445,13.39),(-2.114,13.1161) +8.28,15.0,52.8,1.5708,(16.9153,5.2599),(1.9003,2.9474),(-14.0772,16.7756),(7.3445,13.29),(-2.114,13.0161) +8.29,15.0,52.9,1.5708,(16.9153,5.1599),(1.9003,2.8474),(-14.0772,16.6756),(7.3445,13.19),(-2.114,12.9161) +8.3,15.0,53.0,1.5708,(16.9153,5.0599),(1.9003,2.7474),(-14.0772,16.5756),(7.3445,13.09),(-2.114,12.8161) +8.31,15.0,53.1,1.5708,(16.9153,4.9599),(1.9003,2.6474),(-14.0772,16.4756),(7.3445,12.99),(-2.114,12.7161) +8.32,15.0,53.2,1.5708,(16.9153,4.8599),(1.9003,2.5474),(-14.0772,16.3756),(7.3445,12.89),(-2.114,12.6161) +8.33,15.0,53.3,1.5708,(16.9153,4.7599),(1.9003,2.4474),(-14.0772,16.2756),(7.3445,12.79),(-2.114,12.5161) +8.34,15.0,53.4,1.5708,(16.9153,4.6599),(1.9003,2.3474),(-14.0772,16.1756),(7.3445,12.69),(-2.114,12.4161) +8.35,15.0,53.5,1.5708,(16.9153,4.5599),(1.9003,2.2474),(-14.0772,16.0756),(7.3445,12.59),(-2.114,12.3161) +8.36,15.0,53.6,1.5708,(16.9153,4.4599),(1.9003,2.1474),(-14.0772,15.9756),(7.3445,12.49),(-2.114,12.2161) +8.37,15.0,53.7,1.5708,(16.9153,4.3599),(1.9003,2.0474),(-14.0772,15.8756),(7.3445,12.39),(-2.114,12.1161) +8.38,15.0,53.8,1.5708,(16.9153,4.2599),(1.9003,1.9474),(-14.0772,15.7756),(7.3445,12.29),(-2.114,12.0161) +8.39,15.0,53.9,1.5708,(16.9153,4.1599),(1.9003,1.8474),(-14.0772,15.6756),(7.3445,12.19),(-2.114,11.9161) +8.4,15.0,54.0,1.5708,(16.9153,4.0599),(-7.521,23.7735),(1.9003,1.7474),(-14.0772,15.5756),(7.3445,12.09),(-2.114,11.8161) +8.41,15.0,54.1,1.5708,(16.9153,3.9599),(-7.521,23.6735),(1.9003,1.6474),(-14.0772,15.4756),(7.3445,11.99),(-2.114,11.7161) +8.42,15.0,54.2,1.5708,(16.9153,3.8599),(-7.521,23.5735),(1.9003,1.5474),(-14.0772,15.3756),(7.3445,11.89),(-2.114,11.6161) +8.43,15.0,54.3,1.5708,(16.9153,3.7599),(-7.521,23.4735),(1.9003,1.4474),(-14.0772,15.2756),(7.3445,11.79),(-2.114,11.5161) +8.44,15.0,54.4,1.5708,(16.9153,3.6599),(-7.521,23.3735),(1.9003,1.3474),(-14.0772,15.1756),(7.3445,11.69),(-2.114,11.4161) +8.45,15.0,54.5,1.5708,(16.9153,3.5599),(-7.521,23.2735),(1.9003,1.2474),(-14.0772,15.0756),(7.3445,11.59),(-2.114,11.3161) +8.46,15.0,54.6,1.5708,(16.9153,3.4599),(-7.521,23.1735),(1.9003,1.1474),(-14.0772,14.9756),(7.3445,11.49),(-2.114,11.2161) +8.47,15.0,54.7,1.5708,(16.9153,3.3599),(-7.521,23.0735),(1.9003,1.0474),(-14.0772,14.8756),(7.3445,11.39),(-2.114,11.1161) +8.48,15.0,54.8,1.5708,(16.9153,3.2599),(-7.521,22.9735),(1.9003,0.9474),(-14.0772,14.7756),(7.3445,11.29),(-2.114,11.0161) +8.49,15.0,54.9,1.5708,(16.9153,3.1599),(-7.521,22.8735),(1.9003,0.8474),(-14.0772,14.6756),(7.3445,11.19),(-2.114,10.9161) +8.5,15.0,55.0,1.5708,(16.9153,3.0599),(-7.521,22.7735),(1.9003,0.7474),(-14.0772,14.5756),(7.3445,11.09),(-2.114,10.8161) +8.51,15.0,55.1,1.5708,(16.9153,2.9599),(-7.521,22.6735),(1.9003,0.6474),(-14.0772,14.4756),(7.3445,10.99),(-2.114,10.7161) +8.52,15.0,55.2,1.5708,(16.9153,2.8599),(-7.521,22.5735),(1.9003,0.5474),(-14.0772,14.3756),(7.3445,10.89),(-2.114,10.6161) +8.53,15.0,55.3,1.5708,(16.9153,2.7599),(-7.521,22.4735),(1.9003,0.4474),(-14.0772,14.2756),(7.3445,10.79),(-2.114,10.5161) +8.54,15.0,55.4,1.5708,(16.9153,2.6599),(-7.521,22.3735),(1.9003,0.3474),(-14.0772,14.1756),(7.3445,10.69),(-2.114,10.4161) +8.55,15.0,55.5,1.5708,(16.9153,2.5599),(-7.521,22.2735),(1.9003,0.2474),(-14.0772,14.0756),(7.3445,10.59),(-2.114,10.3161) +8.56,15.0,55.6,1.5708,(16.9153,2.4599),(-7.521,22.1735),(1.9003,0.1474),(-14.0772,13.9756),(7.3445,10.49),(-2.114,10.2161) +8.57,15.0,55.7,1.5708,(16.9153,2.3599),(-7.521,22.0735),(1.9003,0.0474),(-14.0772,13.8756),(7.3445,10.39),(-2.114,10.1161) +8.58,15.0,55.8,1.5708,(16.9153,2.2599),(-7.521,21.9735),(-14.0772,13.7756),(7.3445,10.29),(-2.114,10.0161) +8.59,15.0,55.9,1.5708,(16.9153,2.1599),(-7.521,21.8735),(-14.0772,13.6756),(7.3445,10.19),(-2.114,9.9161) +8.6,15.0,56.0,1.5708,(16.9153,2.0599),(-7.521,21.7735),(-14.0772,13.5756),(7.3445,10.09),(-2.114,9.8161) +8.61,15.0,56.1,1.5708,(16.9153,1.9599),(-7.521,21.6735),(-14.0772,13.4756),(7.3445,9.99),(-2.114,9.7161) +8.62,15.0,56.2,1.5708,(16.9153,1.8599),(-7.521,21.5735),(-14.0772,13.3756),(7.3445,9.89),(-2.114,9.6161) +8.63,15.0,56.3,1.5708,(16.9153,1.7599),(-7.521,21.4735),(-14.0772,13.2756),(7.3445,9.79),(-2.114,9.5161) +8.64,15.0,56.4,1.5708,(16.9153,1.6599),(-7.521,21.3735),(-14.0772,13.1756),(7.3445,9.69),(-2.114,9.4161) +8.65,15.0,56.5,1.5708,(16.9153,1.5599),(-7.521,21.2735),(-14.0772,13.0756),(7.3445,9.59),(-2.114,9.3161) +8.66,15.0,56.6,1.5708,(16.9153,1.4599),(-7.521,21.1735),(-14.0772,12.9756),(7.3445,9.49),(-2.114,9.2161) +8.67,15.0,56.7,1.5708,(16.9153,1.3599),(-7.521,21.0735),(-14.0772,12.8756),(7.3445,9.39),(-2.114,9.1161) +8.68,15.0,56.8,1.5708,(16.9153,1.2599),(-7.521,20.9735),(-14.0772,12.7756),(7.3445,9.29),(-2.114,9.0161) +8.69,15.0,56.9,1.5708,(16.9153,1.1599),(-7.521,20.8735),(-14.0772,12.6756),(7.3445,9.19),(-2.114,8.9161) +8.7,15.0,57.0,1.5708,(16.9153,1.0599),(-7.521,20.7735),(-14.0772,12.5756),(7.3445,9.09),(-2.114,8.8161) +8.71,15.0,57.1,1.5708,(16.9153,0.9599),(-7.521,20.6735),(-14.0772,12.4756),(7.3445,8.99),(-2.114,8.7161) +8.72,15.0,57.2,1.5708,(16.9153,0.8599),(-7.521,20.5735),(-14.0772,12.3756),(7.3445,8.89),(-2.114,8.6161) +8.73,15.0,57.3,1.5708,(16.9153,0.7599),(-7.521,20.4735),(-14.0772,12.2756),(7.3445,8.79),(-2.114,8.5161) +8.74,15.0,57.4,1.5708,(16.9153,0.6599),(-7.521,20.3735),(-14.0772,12.1756),(7.3445,8.69),(-2.114,8.4161) +8.75,15.0,57.5,1.5708,(16.9153,0.5599),(-7.521,20.2735),(-14.0772,12.0756),(7.3445,8.59),(-2.114,8.3161) +8.76,15.0,57.6,1.5708,(16.9153,0.4599),(-7.521,20.1735),(-14.0772,11.9756),(7.3445,8.49),(-2.114,8.2161) +8.77,15.0,57.7,1.5708,(16.9153,0.3599),(-7.521,20.0735),(-14.0772,11.8756),(7.3445,8.39),(-2.114,8.1161) +8.78,15.0,57.8,1.5708,(16.9153,0.2599),(-7.521,19.9735),(-14.0772,11.7756),(7.3445,8.29),(-2.114,8.0161) +8.79,15.0,57.9,1.5708,(16.9153,0.1599),(-7.521,19.8735),(-14.0772,11.6756),(7.3445,8.19),(-2.114,7.9161) +8.8,15.0,58.0,1.5708,(16.9153,0.0599),(-7.521,19.7735),(-14.0772,11.5756),(7.3445,8.09),(-2.114,7.8161) +8.81,15.0,58.1,1.5708,(-7.521,19.6735),(-14.0772,11.4756),(7.3445,7.99),(-2.114,7.7161) +8.82,15.0,58.2,1.5708,(-7.521,19.5735),(-14.0772,11.3756),(7.3445,7.89),(-2.114,7.6161) +8.83,15.0,58.3,1.5708,(-7.521,19.4735),(-14.0772,11.2756),(7.3445,7.79),(-2.114,7.5161) +8.84,15.0,58.4,1.5708,(-7.521,19.3735),(-14.0772,11.1756),(7.3445,7.69),(-2.114,7.4161) +8.85,15.0,58.5,1.5708,(-7.521,19.2735),(-14.0772,11.0756),(7.3445,7.59),(-2.114,7.3161) +8.86,15.0,58.6,1.5708,(-7.521,19.1735),(-14.0772,10.9756),(7.3445,7.49),(-2.114,7.2161) +8.87,15.0,58.7,1.5708,(-7.521,19.0735),(-14.0772,10.8756),(7.3445,7.39),(-2.114,7.1161) +8.88,15.0,58.8,1.5708,(-7.521,18.9735),(-14.0772,10.7756),(7.3445,7.29),(-2.114,7.0161) +8.89,15.0,58.9,1.5708,(-7.521,18.8735),(-14.0772,10.6756),(7.3445,7.19),(-2.114,6.9161) +8.9,15.0,59.0,1.5708,(-7.521,18.7735),(-14.0772,10.5756),(7.3445,7.09),(-2.114,6.8161) +8.91,15.0,59.1,1.5708,(-7.521,18.6735),(-14.0772,10.4756),(7.3445,6.99),(-2.114,6.7161) +8.92,15.0,59.2,1.5708,(-7.521,18.5735),(-14.0772,10.3756),(7.3445,6.89),(-2.114,6.6161) +8.93,15.0,59.3,1.5708,(-7.521,18.4735),(-14.0772,10.2756),(7.3445,6.79),(-2.114,6.5161) +8.94,15.0,59.4,1.5708,(-7.521,18.3735),(-14.0772,10.1756),(7.3445,6.69),(-2.114,6.4161) +8.95,15.0,59.5,1.5708,(-7.521,18.2735),(-14.0772,10.0756),(7.3445,6.59),(-2.114,6.3161) +8.96,15.0,59.6,1.5708,(-7.521,18.1735),(-14.0772,9.9756),(7.3445,6.49),(-2.114,6.2161) +8.97,15.0,59.7,1.5708,(-7.521,18.0735),(-14.0772,9.8756),(7.3445,6.39),(-2.114,6.1161) +8.98,15.0,59.8,1.5708,(-7.521,17.9735),(-14.0772,9.7756),(7.3445,6.29),(-2.114,6.0161) +8.99,15.0,59.9,1.5708,(-7.521,17.8735),(-14.0772,9.6756),(7.3445,6.19),(-2.114,5.9161) +9.0,15.0,60.0,1.5708,(-7.521,17.7735),(-14.0772,9.5756),(7.3445,6.09),(-2.114,5.8161) +9.01,15.0,60.1,1.5708,(-7.521,17.6735),(-14.0772,9.4756),(7.3445,5.99),(-2.114,5.7161) +9.02,15.0,60.2,1.5708,(-7.521,17.5735),(-14.0772,9.3756),(7.3445,5.89),(-2.114,5.6161) +9.03,15.0,60.3,1.5708,(-7.521,17.4735),(-14.0772,9.2756),(7.3445,5.79),(-2.114,5.5161) +9.04,15.0,60.4,1.5708,(-7.521,17.3735),(-14.0772,9.1756),(7.3445,5.69),(-2.114,5.4161) +9.05,15.0,60.5,1.5708,(-7.521,17.2735),(-14.0772,9.0756),(7.3445,5.59),(-2.114,5.3161) +9.06,15.0,60.6,1.5708,(-7.521,17.1735),(-14.0772,8.9756),(7.3445,5.49),(-2.114,5.2161) +9.07,15.0,60.7,1.5708,(-7.521,17.0735),(-14.0772,8.8756),(7.3445,5.39),(-2.114,5.1161) +9.08,15.0,60.8,1.5708,(-7.521,16.9735),(-14.0772,8.7756),(7.3445,5.29),(-2.114,5.0161) +9.09,15.0,60.9,1.5708,(-7.521,16.8735),(-14.0772,8.6756),(7.3445,5.19),(-2.114,4.9161) +9.1,15.0,61.0,1.5708,(-7.521,16.7735),(-14.0772,8.5756),(7.3445,5.09),(-2.114,4.8161) +9.11,15.0,61.1,1.5708,(-7.521,16.6735),(-14.0772,8.4756),(7.3445,4.99),(-2.114,4.7161) +9.12,15.0,61.2,1.5708,(-7.521,16.5735),(-14.0772,8.3756),(7.3445,4.89),(-2.114,4.6161) +9.13,15.0,61.3,1.5708,(-7.521,16.4735),(-14.0772,8.2756),(7.3445,4.79),(-2.114,4.5161) +9.14,15.0,61.4,1.5708,(-7.521,16.3735),(-14.0772,8.1756),(7.3445,4.69),(-2.114,4.4161) +9.15,15.0,61.5,1.5708,(-7.521,16.2735),(-14.0772,8.0756),(7.3445,4.59),(-2.114,4.3161) +9.16,15.0,61.6,1.5708,(-7.521,16.1735),(-14.0772,7.9756),(7.3445,4.49),(-2.114,4.2161) +9.17,15.0,61.7,1.5708,(-7.521,16.0735),(-14.0772,7.8756),(7.3445,4.39),(-2.114,4.1161) +9.18,15.0,61.8,1.5708,(-7.521,15.9735),(-14.0772,7.7756),(7.3445,4.29),(-2.114,4.0161) +9.19,15.0,61.9,1.5708,(-7.521,15.8735),(-14.0772,7.6756),(7.3445,4.19),(-2.114,3.9161) +9.2,15.0,62.0,1.5708,(-7.521,15.7735),(-14.0772,7.5756),(7.3445,4.09),(-2.114,3.8161) +9.21,15.0,62.1,1.5708,(-7.521,15.6735),(-14.0772,7.4756),(7.3445,3.99),(-2.114,3.7161) +9.22,15.0,62.2,1.5708,(-7.521,15.5735),(-14.0772,7.3756),(7.3445,3.89),(-2.114,3.6161) +9.23,15.0,62.3,1.5708,(-7.521,15.4735),(-14.0772,7.2756),(7.3445,3.79),(-2.114,3.5161) +9.24,15.0,62.4,1.5708,(-7.521,15.3735),(-14.0772,7.1756),(7.3445,3.69),(-2.114,3.4161) +9.25,15.0,62.5,1.5708,(-7.521,15.2735),(-14.0772,7.0756),(7.3445,3.59),(-2.114,3.3161) +9.26,15.0,62.6,1.5708,(-7.521,15.1735),(-14.0772,6.9756),(7.3445,3.49),(-2.114,3.2161) +9.27,15.0,62.7,1.5708,(-7.521,15.0735),(-14.0772,6.8756),(7.3445,3.39),(-2.114,3.1161) +9.28,15.0,62.8,1.5708,(-7.521,14.9735),(-14.0772,6.7756),(7.3445,3.29),(-2.114,3.0161) +9.29,15.0,62.9,1.5708,(-7.521,14.8735),(-14.0772,6.6756),(7.3445,3.19),(-2.114,2.9161) +9.3,15.0,63.0,1.5708,(-7.521,14.7735),(-14.0772,6.5756),(7.3445,3.09),(-2.114,2.8161) +9.31,15.0,63.1,1.5708,(-7.521,14.6735),(-14.0772,6.4756),(-0.4734,24.9514),(7.3445,2.99),(-2.114,2.7161) +9.32,15.0,63.2,1.5708,(-7.521,14.5735),(-14.0772,6.3756),(-0.4734,24.8514),(7.3445,2.89),(-2.114,2.6161) +9.33,15.0,63.3,1.5708,(-7.521,14.4735),(-14.0772,6.2756),(-0.4734,24.7514),(7.3445,2.79),(-2.114,2.5161) +9.34,15.0,63.4,1.5708,(-7.521,14.3735),(-14.0772,6.1756),(-0.4734,24.6514),(7.3445,2.69),(-2.114,2.4161) +9.35,15.0,63.5,1.5708,(-7.521,14.2735),(-14.0772,6.0756),(-0.4734,24.5514),(7.3445,2.59),(-2.114,2.3161) +9.36,15.0,63.6,1.5708,(-7.521,14.1735),(-14.0772,5.9756),(-0.4734,24.4514),(7.3445,2.49),(-2.114,2.2161) +9.37,15.0,63.7,1.5708,(-7.521,14.0735),(-14.0772,5.8756),(-0.4734,24.3514),(7.3445,2.39),(-2.114,2.1161) +9.38,15.0,63.8,1.5708,(-7.521,13.9735),(-14.0772,5.7756),(-0.4734,24.2514),(7.3445,2.29),(-2.114,2.0161) +9.39,15.0,63.9,1.5708,(-7.521,13.8735),(-14.0772,5.6756),(-0.4734,24.1514),(7.3445,2.19),(-2.114,1.9161) +9.4,15.0,64.0,1.5708,(-7.521,13.7735),(-14.0772,5.5756),(-0.4734,24.0514),(7.3445,2.09),(-2.114,1.8161) +9.41,15.0,64.1,1.5708,(-7.521,13.6735),(-14.0772,5.4756),(-0.4734,23.9514),(7.3445,1.99),(-2.114,1.7161) +9.42,15.0,64.2,1.5708,(-7.521,13.5735),(-14.0772,5.3756),(-0.4734,23.8514),(7.3445,1.89),(-2.114,1.6161) +9.43,15.0,64.3,1.5708,(-7.521,13.4735),(-14.0772,5.2756),(-0.4734,23.7514),(7.3445,1.79),(-2.114,1.5161) +9.44,15.0,64.4,1.5708,(-7.521,13.3735),(-14.0772,5.1756),(-0.4734,23.6514),(7.3445,1.69),(-2.114,1.4161) +9.45,15.0,64.5,1.5708,(-7.521,13.2735),(-14.0772,5.0756),(-0.4734,23.5514),(7.3445,1.59),(-2.114,1.3161) +9.46,15.0,64.6,1.5708,(-7.521,13.1735),(-14.0772,4.9756),(-0.4734,23.4514),(7.3445,1.49),(-2.114,1.2161) +9.47,15.0,64.7,1.5708,(-7.521,13.0735),(-14.0772,4.8756),(-0.4734,23.3514),(7.3445,1.39),(-2.114,1.1161) +9.48,15.0,64.8,1.5708,(-7.521,12.9735),(-14.0772,4.7756),(-0.4734,23.2514),(7.3445,1.29),(-2.114,1.0161) +9.49,15.0,64.9,1.5708,(-7.521,12.8735),(-3.6145,24.6402),(-14.0772,4.6756),(-0.4734,23.1514),(7.3445,1.19),(-2.114,0.9161) +9.5,15.0,65.0,1.5708,(-7.521,12.7735),(-3.6145,24.5402),(-14.0772,4.5756),(-0.4734,23.0514),(7.3445,1.09),(-2.114,0.8161) +9.51,15.0,65.1,1.5708,(-7.521,12.6735),(-3.6145,24.4402),(-14.0772,4.4756),(-0.4734,22.9514),(7.3445,0.99),(-2.114,0.7161) +9.52,15.0,65.2,1.5708,(-7.521,12.5735),(-3.6145,24.3402),(-14.0772,4.3756),(-0.4734,22.8514),(7.3445,0.89),(-2.114,0.6161) +9.53,15.0,65.3,1.5708,(-7.521,12.4735),(-3.6145,24.2402),(-14.0772,4.2756),(-0.4734,22.7514),(7.3445,0.79),(-2.114,0.5161) +9.54,15.0,65.4,1.5708,(-7.521,12.3735),(-3.6145,24.1402),(-14.0772,4.1756),(-0.4734,22.6514),(7.3445,0.69),(-2.114,0.4161) +9.55,15.0,65.5,1.5708,(-7.521,12.2735),(-3.6145,24.0402),(-14.0772,4.0756),(-0.4734,22.5514),(7.3445,0.59),(-2.114,0.3161) +9.56,15.0,65.6,1.5708,(-7.521,12.1735),(-3.6145,23.9402),(-14.0772,3.9756),(-0.4734,22.4514),(7.3445,0.49),(-2.114,0.2161) +9.57,15.0,65.7,1.5708,(-7.521,12.0735),(-3.6145,23.8402),(-14.0772,3.8756),(-0.4734,22.3514),(7.3445,0.39),(-2.114,0.1161) +9.58,15.0,65.8,1.5708,(-7.521,11.9735),(-3.6145,23.7402),(-14.0772,3.7756),(-0.4734,22.2514),(7.3445,0.29),(-2.114,0.0161) +9.59,15.0,65.9,1.5708,(-7.521,11.8735),(-3.6145,23.6402),(-14.0772,3.6756),(-0.4734,22.1514),(7.3445,0.19) +9.6,15.0,66.0,1.5708,(-7.521,11.7735),(-3.6145,23.5402),(-14.0772,3.5756),(-0.4734,22.0514),(7.3445,0.09) +9.61,15.0,66.1,1.5708,(-7.521,11.6735),(-3.6145,23.4402),(-14.0772,3.4756),(-0.4734,21.9514) +9.62,15.0,66.2,1.5708,(-7.521,11.5735),(-3.6145,23.3402),(-14.0772,3.3756),(-0.4734,21.8514) +9.63,15.0,66.3,1.5708,(-7.521,11.4735),(-3.6145,23.2402),(-14.0772,3.2756),(-0.4734,21.7514) +9.64,15.0,66.4,1.5708,(-7.521,11.3735),(-3.6145,23.1402),(-14.0772,3.1756),(-0.4734,21.6514) +9.65,15.0,66.5,1.5708,(-7.521,11.2735),(-3.6145,23.0402),(-14.0772,3.0756),(-0.4734,21.5514) +9.66,15.0,66.6,1.5708,(-7.521,11.1735),(-3.6145,22.9402),(-14.0772,2.9756),(-0.4734,21.4514) +9.67,15.0,66.7,1.5708,(-7.521,11.0735),(-3.6145,22.8402),(-14.0772,2.8756),(-0.4734,21.3514) +9.68,15.0,66.8,1.5708,(-7.521,10.9735),(-3.6145,22.7402),(-14.0772,2.7756),(-0.4734,21.2514) +9.69,15.0,66.9,1.5708,(-7.521,10.8735),(-3.6145,22.6402),(-14.0772,2.6756),(-0.4734,21.1514) +9.7,15.0,67.0,1.5708,(-7.521,10.7735),(-3.6145,22.5402),(-14.0772,2.5756),(-0.4734,21.0514) +9.71,15.0,67.1,1.5708,(-7.521,10.6735),(-3.6145,22.4402),(-14.0772,2.4756),(-0.4734,20.9514) +9.72,15.0,67.2,1.5708,(-7.521,10.5735),(-3.6145,22.3402),(-14.0772,2.3756),(-0.4734,20.8514) +9.73,15.0,67.3,1.5708,(-7.521,10.4735),(-3.6145,22.2402),(-14.0772,2.2756),(-0.4734,20.7514) +9.74,15.0,67.4,1.5708,(-7.521,10.3735),(-3.6145,22.1402),(-14.0772,2.1756),(-0.4734,20.6514) +9.75,15.0,67.5,1.5708,(-7.521,10.2735),(-3.6145,22.0402),(-14.0772,2.0756),(-0.4734,20.5514) +9.76,15.0,67.6,1.5708,(-7.521,10.1735),(-3.6145,21.9402),(-14.0772,1.9756),(-0.4734,20.4514) +9.77,15.0,67.7,1.5708,(-7.521,10.0735),(-3.6145,21.8402),(-14.0772,1.8756),(-0.4734,20.3514) +9.78,15.0,67.8,1.5708,(-7.521,9.9735),(-3.6145,21.7402),(-14.0772,1.7756),(-0.4734,20.2514) +9.79,15.0,67.9,1.5708,(-7.521,9.8735),(-3.6145,21.6402),(-14.0772,1.6756),(-0.4734,20.1514) +9.8,15.0,68.0,1.5708,(-7.521,9.7735),(-3.6145,21.5402),(-14.0772,1.5756),(-0.4734,20.0514) +9.81,15.0,68.1,1.5708,(-7.521,9.6735),(-3.6145,21.4402),(-14.0772,1.4756),(-0.4734,19.9514) +9.82,15.0,68.2,1.5708,(-7.521,9.5735),(-3.6145,21.3402),(-14.0772,1.3756),(-0.4734,19.8514) +9.83,15.0,68.3,1.5708,(-7.521,9.4735),(-3.6145,21.2402),(-14.0772,1.2756),(-0.4734,19.7514) +9.84,15.0,68.4,1.5708,(-7.521,9.3735),(-3.6145,21.1402),(-14.0772,1.1756),(-0.4734,19.6514) +9.85,15.0,68.5,1.5708,(-7.521,9.2735),(-3.6145,21.0402),(-14.0772,1.0756),(-0.4734,19.5514) +9.86,15.0,68.6,1.5708,(-7.521,9.1735),(-3.6145,20.9402),(-14.0772,0.9756),(-0.4734,19.4514) +9.87,15.0,68.7,1.5708,(-7.521,9.0735),(-3.6145,20.8402),(-14.0772,0.8756),(-0.4734,19.3514) +9.88,15.0,68.8,1.5708,(-7.521,8.9735),(-3.6145,20.7402),(-14.0772,0.7756),(-0.4734,19.2514) +9.89,15.0,68.9,1.5708,(-7.521,8.8735),(-3.6145,20.6402),(-14.0772,0.6756),(-0.4734,19.1514) +9.9,15.0,69.0,1.5708,(-7.521,8.7735),(-3.6145,20.5402),(-14.0772,0.5756),(-0.4734,19.0514) +9.91,15.0,69.1,1.5708,(-7.521,8.6735),(-3.6145,20.4402),(-14.0772,0.4756),(-0.4734,18.9514) +9.92,15.0,69.2,1.5708,(-7.521,8.5735),(-3.6145,20.3402),(-14.0772,0.3756),(-0.4734,18.8514) +9.93,15.0,69.3,1.5708,(-7.521,8.4735),(-3.6145,20.2402),(-14.0772,0.2756),(-0.4734,18.7514) +9.94,15.0,69.4,1.5708,(-7.521,8.3735),(-3.6145,20.1402),(-14.0772,0.1756),(-0.4734,18.6514) +9.95,15.0,69.5,1.5708,(-7.521,8.2735),(-3.6145,20.0402),(-14.0772,0.0756),(-0.4734,18.5514) +9.96,15.0,69.6,1.5708,(-7.521,8.1735),(-3.6145,19.9402),(-0.4734,18.4514) +9.97,15.0,69.7,1.5708,(-7.521,8.0735),(-3.6145,19.8402),(-0.4734,18.3514) +9.98,15.0,69.8,1.5708,(-7.521,7.9735),(-3.6145,19.7402),(-0.4734,18.2514) +9.99,15.0,69.9,1.5708,(-7.521,7.8735),(-3.6145,19.6402),(-0.4734,18.1514) +10.0,15.0,70.0,1.5708,(-7.521,7.7735),(-3.6145,19.5402),(-0.4734,18.0514) +10.01,15.0,70.1,1.5708,(-7.521,7.6735),(-3.6145,19.4402),(-0.4734,17.9514) +10.02,15.0,70.2,1.5708,(-7.521,7.5735),(-3.6145,19.3402),(-0.4734,17.8514) +10.03,15.0,70.3,1.5708,(-7.521,7.4735),(-3.6145,19.2402),(-0.4734,17.7514) +10.04,15.0,70.4,1.5708,(-7.521,7.3735),(-3.6145,19.1402),(-0.4734,17.6514) +10.05,15.0,70.5,1.5708,(-7.521,7.2735),(-3.6145,19.0402),(-0.4734,17.5514) +10.06,15.0,70.6,1.5708,(-7.521,7.1735),(-3.6145,18.9402),(-0.4734,17.4514) +10.07,15.0,70.7,1.5708,(-7.521,7.0735),(-3.6145,18.8402),(-0.4734,17.3514) +10.08,15.0,70.8,1.5708,(-7.521,6.9735),(-3.6145,18.7402),(-0.4734,17.2514) +10.09,15.0,70.9,1.5708,(-7.521,6.8735),(-3.6145,18.6402),(-0.4734,17.1514) +10.1,15.0,71.0,1.5708,(-7.521,6.7735),(-3.6145,18.5402),(-0.4734,17.0514) +10.11,15.0,71.1,1.5708,(-7.521,6.6735),(-3.6145,18.4402),(-0.4734,16.9514) +10.12,15.0,71.2,1.5708,(-7.521,6.5735),(-3.6145,18.3402),(-0.4734,16.8514) +10.13,15.0,71.3,1.5708,(-7.521,6.4735),(-3.6145,18.2402),(-0.4734,16.7514) +10.14,15.0,71.4,1.5708,(-7.521,6.3735),(-3.6145,18.1402),(-0.4734,16.6514) +10.15,15.0,71.5,1.5708,(-7.521,6.2735),(-3.6145,18.0402),(-0.4734,16.5514) +10.16,15.0,71.6,1.5708,(-7.521,6.1735),(-3.6145,17.9402),(-0.4734,16.4514) +10.17,15.0,71.7,1.5708,(-7.521,6.0735),(-3.6145,17.8402),(-0.4734,16.3514) +10.18,15.0,71.8,1.5708,(-7.521,5.9735),(-3.6145,17.7402),(-0.4734,16.2514) +10.19,15.0,71.9,1.5708,(-7.521,5.8735),(-3.6145,17.6402),(-0.4734,16.1514) +10.2,15.0,72.0,1.5708,(-7.521,5.7735),(-3.6145,17.5402),(-0.4734,16.0514) +10.21,15.0,72.1,1.5708,(-7.521,5.6735),(-3.6145,17.4402),(-0.4734,15.9514),(13.8512,20.7519) +10.22,15.0,72.2,1.5708,(-7.521,5.5735),(-3.6145,17.3402),(-0.4734,15.8514),(13.8512,20.6519) +10.23,15.0,72.3,1.5708,(-7.521,5.4735),(-3.6145,17.2402),(-0.4734,15.7514),(13.8512,20.5519) +10.24,15.0,72.4,1.5708,(-7.521,5.3735),(-3.6145,17.1402),(-0.4734,15.6514),(13.8512,20.4519) +10.25,15.0,72.5,1.5708,(-7.521,5.2735),(-3.6145,17.0402),(-0.4734,15.5514),(13.8512,20.3519) +10.26,15.0,72.6,1.5708,(-7.521,5.1735),(-3.6145,16.9402),(-0.4734,15.4514),(13.8512,20.2519) +10.27,15.0,72.7,1.5708,(-7.521,5.0735),(-3.6145,16.8402),(-0.4734,15.3514),(13.8512,20.1519) +10.28,15.0,72.8,1.5708,(-7.521,4.9735),(-3.6145,16.7402),(-0.4734,15.2514),(13.8512,20.0519) +10.29,15.0,72.9,1.5708,(-7.521,4.8735),(-3.6145,16.6402),(-0.4734,15.1514),(13.8512,19.9519) +10.3,15.0,73.0,1.5708,(-7.521,4.7735),(-3.6145,16.5402),(-0.4734,15.0514),(13.8512,19.8519) +10.31,15.0,73.1,1.5708,(-7.521,4.6735),(-3.6145,16.4402),(-0.4734,14.9514),(13.8512,19.7519) +10.32,15.0,73.2,1.5708,(-7.521,4.5735),(-3.6145,16.3402),(-0.4734,14.8514),(13.8512,19.6519) +10.33,15.0,73.3,1.5708,(-7.521,4.4735),(-3.6145,16.2402),(-0.4734,14.7514),(13.8512,19.5519) +10.34,15.0,73.4,1.5708,(-7.521,4.3735),(-3.6145,16.1402),(-0.4734,14.6514),(13.8512,19.4519) +10.35,15.0,73.5,1.5708,(-7.521,4.2735),(-3.6145,16.0402),(-0.4734,14.5514),(13.8512,19.3519) +10.36,15.0,73.6,1.5708,(-7.521,4.1735),(-3.6145,15.9402),(-0.4734,14.4514),(13.8512,19.2519) +10.37,15.0,73.7,1.5708,(-7.521,4.0735),(-3.6145,15.8402),(-0.4734,14.3514),(13.8512,19.1519) +10.38,15.0,73.8,1.5708,(-7.521,3.9735),(-3.6145,15.7402),(-0.4734,14.2514),(13.8512,19.0519) +10.39,15.0,73.9,1.5708,(-7.521,3.8735),(-3.6145,15.6402),(-0.4734,14.1514),(13.8512,18.9519) +10.4,15.0,74.0,1.5708,(-7.521,3.7735),(-3.6145,15.5402),(-0.4734,14.0514),(13.8512,18.8519) +10.41,15.0,74.1,1.5708,(-7.521,3.6735),(-3.6145,15.4402),(-0.4734,13.9514),(13.8512,18.7519) +10.42,15.0,74.2,1.5708,(-7.521,3.5735),(-3.6145,15.3402),(-0.4734,13.8514),(13.8512,18.6519) +10.43,15.0,74.3,1.5708,(-7.521,3.4735),(-3.6145,15.2402),(-0.4734,13.7514),(13.8512,18.5519) +10.44,15.0,74.4,1.5708,(-7.521,3.3735),(-3.6145,15.1402),(-0.4734,13.6514),(13.8512,18.4519) +10.45,15.0,74.5,1.5708,(-7.521,3.2735),(-3.6145,15.0402),(-0.4734,13.5514),(13.8512,18.3519) +10.46,15.0,74.6,1.5708,(-7.521,3.1735),(-3.6145,14.9402),(-0.4734,13.4514),(5.9061,24.2817),(13.8512,18.2519) +10.47,15.0,74.7,1.5708,(-7.521,3.0735),(-3.6145,14.8402),(-0.4734,13.3514),(5.9061,24.1817),(13.8512,18.1519) +10.48,15.0,74.8,1.5708,(-7.521,2.9735),(-3.6145,14.7402),(-0.4734,13.2514),(5.9061,24.0817),(13.8512,18.0519) +10.49,15.0,74.9,1.5708,(-7.521,2.8735),(-3.6145,14.6402),(-0.4734,13.1514),(5.9061,23.9817),(13.8512,17.9519) +10.5,15.0,75.0,1.5708,(-7.521,2.7735),(-3.6145,14.5402),(-0.4734,13.0514),(5.9061,23.8817),(13.8512,17.8519) +10.51,15.0,75.1,1.5708,(-7.521,2.6735),(-3.6145,14.4402),(-0.4734,12.9514),(5.9061,23.7817),(13.8512,17.7519) +10.52,15.0,75.2,1.5708,(-7.521,2.5735),(-3.6145,14.3402),(-0.4734,12.8514),(5.9061,23.6817),(13.8512,17.6519) +10.53,15.0,75.3,1.5708,(-7.521,2.4735),(-3.6145,14.2402),(-0.4734,12.7514),(5.9061,23.5817),(13.8512,17.5519) +10.54,15.0,75.4,1.5708,(-7.521,2.3735),(-3.6145,14.1402),(-0.4734,12.6514),(5.9061,23.4817),(13.8512,17.4519) +10.55,15.0,75.5,1.5708,(-7.521,2.2735),(-3.6145,14.0402),(-0.4734,12.5514),(5.9061,23.3817),(13.8512,17.3519) +10.56,15.0,75.6,1.5708,(-7.521,2.1735),(-3.6145,13.9402),(-0.4734,12.4514),(5.9061,23.2817),(13.8512,17.2519) +10.57,15.0,75.7,1.5708,(-7.521,2.0735),(-3.6145,13.8402),(-0.4734,12.3514),(5.9061,23.1817),(13.8512,17.1519) +10.58,15.0,75.8,1.5708,(-7.521,1.9735),(-3.6145,13.7402),(-0.4734,12.2514),(5.9061,23.0817),(13.8512,17.0519) +10.59,15.0,75.9,1.5708,(-7.521,1.8735),(-3.6145,13.6402),(-0.4734,12.1514),(5.9061,22.9817),(13.8512,16.9519) +10.6,15.0,76.0,1.5708,(-7.521,1.7735),(-3.6145,13.5402),(-0.4734,12.0514),(5.9061,22.8817),(13.8512,16.8519) +10.61,15.0,76.1,1.5708,(-7.521,1.6735),(-3.6145,13.4402),(-0.4734,11.9514),(5.9061,22.7817),(13.8512,16.7519) +10.62,15.0,76.2,1.5708,(-7.521,1.5735),(-3.6145,13.3402),(-0.4734,11.8514),(5.9061,22.6817),(13.8512,16.6519) +10.63,15.0,76.3,1.5708,(-7.521,1.4735),(-3.6145,13.2402),(-0.4734,11.7514),(5.9061,22.5817),(13.8512,16.5519) +10.64,15.0,76.4,1.5708,(-7.521,1.3735),(-3.6145,13.1402),(-0.4734,11.6514),(5.9061,22.4817),(13.8512,16.4519) +10.65,15.0,76.5,1.5708,(-7.521,1.2735),(-3.6145,13.0402),(-0.4734,11.5514),(5.9061,22.3817),(13.8512,16.3519) +10.66,15.0,76.6,1.5708,(-7.521,1.1735),(-3.6145,12.9402),(-0.4734,11.4514),(5.9061,22.2817),(13.8512,16.2519) +10.67,15.0,76.7,1.5708,(-7.521,1.0735),(-3.6145,12.8402),(-0.4734,11.3514),(5.9061,22.1817),(13.8512,16.1519) +10.68,15.0,76.8,1.5708,(-7.521,0.9735),(-3.6145,12.7402),(-0.4734,11.2514),(5.9061,22.0817),(13.8512,16.0519) +10.69,15.0,76.9,1.5708,(-7.521,0.8735),(-3.6145,12.6402),(-0.4734,11.1514),(5.9061,21.9817),(13.8512,15.9519) +10.7,15.0,77.0,1.5708,(-7.521,0.7735),(-3.6145,12.5402),(-0.4734,11.0514),(5.9061,21.8817),(13.8512,15.8519) +10.71,15.0,77.1,1.5708,(-7.521,0.6735),(-3.6145,12.4402),(-0.4734,10.9514),(5.9061,21.7817),(13.8512,15.7519) +10.72,15.0,77.2,1.5708,(-7.521,0.5735),(-3.6145,12.3402),(-0.4734,10.8514),(5.9061,21.6817),(13.8512,15.6519) +10.73,15.0,77.3,1.5708,(-7.521,0.4735),(-3.6145,12.2402),(-0.4734,10.7514),(5.9061,21.5817),(13.8512,15.5519) +10.74,15.0,77.4,1.5708,(-7.521,0.3735),(-3.6145,12.1402),(-0.4734,10.6514),(5.9061,21.4817),(13.8512,15.4519) +10.75,15.0,77.5,1.5708,(-7.521,0.2735),(-3.6145,12.0402),(-0.4734,10.5514),(5.9061,21.3817),(13.8512,15.3519) +10.76,15.0,77.6,1.5708,(-7.521,0.1735),(-3.6145,11.9402),(-0.4734,10.4514),(5.9061,21.2817),(13.8512,15.2519) +10.77,15.0,77.7,1.5708,(-7.521,0.0735),(-3.6145,11.8402),(-0.4734,10.3514),(5.9061,21.1817),(13.8512,15.1519) +10.78,15.0,77.8,1.5708,(-3.6145,11.7402),(-0.4734,10.2514),(5.9061,21.0817),(13.8512,15.0519) +10.79,15.0,77.9,1.5708,(-3.6145,11.6402),(-0.4734,10.1514),(5.9061,20.9817),(13.8512,14.9519) +10.8,15.0,78.0,1.5708,(-3.6145,11.5402),(-0.4734,10.0514),(5.9061,20.8817),(13.8512,14.8519) +10.81,15.0,78.1,1.5708,(-3.6145,11.4402),(-0.4734,9.9514),(5.9061,20.7817),(13.8512,14.7519) +10.82,15.0,78.2,1.5708,(-3.6145,11.3402),(-0.4734,9.8514),(5.9061,20.6817),(13.8512,14.6519) +10.83,15.0,78.3,1.5708,(-3.6145,11.2402),(-0.4734,9.7514),(5.9061,20.5817),(13.8512,14.5519) +10.84,15.0,78.4,1.5708,(-3.6145,11.1402),(-0.4734,9.6514),(5.9061,20.4817),(13.8512,14.4519) +10.85,15.0,78.5,1.5708,(-3.6145,11.0402),(-0.4734,9.5514),(5.9061,20.3817),(13.8512,14.3519) +10.86,15.0,78.6,1.5708,(-3.6145,10.9402),(-0.4734,9.4514),(5.9061,20.2817),(13.8512,14.2519) +10.87,15.0,78.7,1.5708,(-3.6145,10.8402),(-0.4734,9.3514),(5.9061,20.1817),(13.8512,14.1519) +10.88,15.0,78.8,1.5708,(-3.6145,10.7402),(-0.4734,9.2514),(5.9061,20.0817),(13.8512,14.0519) +10.89,15.0,78.9,1.5708,(-3.6145,10.6402),(-0.4734,9.1514),(5.9061,19.9817),(13.8512,13.9519) +10.9,15.0,79.0,1.5708,(-3.6145,10.5402),(-0.4734,9.0514),(5.9061,19.8817),(13.8512,13.8519) +10.91,15.0,79.1,1.5708,(-3.6145,10.4402),(-0.4734,8.9514),(5.9061,19.7817),(13.8512,13.7519) +10.92,15.0,79.2,1.5708,(-3.6145,10.3402),(-0.4734,8.8514),(5.9061,19.6817),(13.8512,13.6519) +10.93,15.0,79.3,1.5708,(-3.6145,10.2402),(-0.4734,8.7514),(5.9061,19.5817),(13.8512,13.5519) +10.94,15.0,79.4,1.5708,(-3.6145,10.1402),(-0.4734,8.6514),(5.9061,19.4817),(13.8512,13.4519) +10.95,15.0,79.5,1.5708,(-3.6145,10.0402),(-0.4734,8.5514),(5.9061,19.3817),(13.8512,13.3519) +10.96,15.0,79.6,1.5708,(-3.6145,9.9402),(-0.4734,8.4514),(5.9061,19.2817),(13.8512,13.2519) +10.97,15.0,79.7,1.5708,(-3.6145,9.8402),(-0.4734,8.3514),(5.9061,19.1817),(13.8512,13.1519) +10.98,15.0,79.8,1.5708,(-3.6145,9.7402),(-0.4734,8.2514),(5.9061,19.0817),(13.8512,13.0519) +10.99,15.0,79.9,1.5708,(-3.6145,9.6402),(-0.4734,8.1514),(5.9061,18.9817),(13.8512,12.9519) +11.0,15.0,80.0,1.5708,(-3.6145,9.5402),(-0.4734,8.0514),(5.9061,18.8817),(13.8512,12.8519) +11.01,15.0,80.1,1.5708,(-3.6145,9.4402),(-0.4734,7.9514),(5.9061,18.7817),(13.8512,12.7519) +11.02,15.0,80.2,1.5708,(-3.6145,9.3402),(-0.4734,7.8514),(5.9061,18.6817),(13.8512,12.6519) +11.03,15.0,80.3,1.5708,(-3.6145,9.2402),(-0.4734,7.7514),(5.9061,18.5817),(13.8512,12.5519) +11.04,15.0,80.4,1.5708,(-3.6145,9.1402),(-0.4734,7.6514),(5.9061,18.4817),(13.8512,12.4519) +11.05,15.0,80.5,1.5708,(-3.6145,9.0402),(-0.4734,7.5514),(5.9061,18.3817),(13.8512,12.3519) +11.06,15.0,80.6,1.5708,(-3.6145,8.9402),(-0.4734,7.4514),(5.9061,18.2817),(13.8512,12.2519) +11.07,15.0,80.7,1.5708,(-3.6145,8.8402),(-0.4734,7.3514),(5.9061,18.1817),(13.8512,12.1519) +11.08,15.0,80.8,1.5708,(-3.6145,8.7402),(-0.4734,7.2514),(5.9061,18.0817),(13.8512,12.0519) +11.09,15.0,80.9,1.5708,(-3.6145,8.6402),(-0.4734,7.1514),(5.9061,17.9817),(13.8512,11.9519) +11.1,15.0,81.0,1.5708,(-3.6145,8.5402),(-0.4734,7.0514),(5.9061,17.8817),(13.8512,11.8519) +11.11,15.0,81.1,1.5708,(-3.6145,8.4402),(-0.4734,6.9514),(5.9061,17.7817),(13.8512,11.7519) +11.12,15.0,81.2,1.5708,(-3.6145,8.3402),(-0.4734,6.8514),(5.9061,17.6817),(13.8512,11.6519) +11.13,15.0,81.3,1.5708,(-3.6145,8.2402),(-0.4734,6.7514),(5.9061,17.5817),(13.8512,11.5519) +11.14,15.0,81.4,1.5708,(-3.6145,8.1402),(-0.4734,6.6514),(5.9061,17.4817),(13.8512,11.4519) +11.15,15.0,81.5,1.5708,(-3.6145,8.0402),(-0.4734,6.5514),(5.9061,17.3817),(13.8512,11.3519) +11.16,15.0,81.6,1.5708,(-3.6145,7.9402),(-0.4734,6.4514),(5.9061,17.2817),(13.8512,11.2519) +11.17,15.0,81.7,1.5708,(-3.6145,7.8402),(-0.4734,6.3514),(5.9061,17.1817),(13.8512,11.1519) +11.18,15.0,81.8,1.5708,(-3.6145,7.7402),(-0.4734,6.2514),(5.9061,17.0817),(13.8512,11.0519) +11.19,15.0,81.9,1.5708,(-3.6145,7.6402),(-0.4734,6.1514),(5.9061,16.9817),(13.8512,10.9519) +11.2,15.0,82.0,1.5708,(-3.6145,7.5402),(-0.4734,6.0514),(5.9061,16.8817),(13.8512,10.8519) +11.21,15.0,82.1,1.5708,(-3.6145,7.4402),(-0.4734,5.9514),(5.9061,16.7817),(13.8512,10.7519) +11.22,15.0,82.2,1.5708,(-3.6145,7.3402),(-0.4734,5.8514),(5.9061,16.6817),(13.8512,10.6519) +11.23,15.0,82.3,1.5708,(-3.6145,7.2402),(-0.4734,5.7514),(5.9061,16.5817),(13.8512,10.5519) +11.24,15.0,82.4,1.5708,(-3.6145,7.1402),(-0.4734,5.6514),(5.9061,16.4817),(13.8512,10.4519) +11.25,15.0,82.5,1.5708,(-3.6145,7.0402),(-0.4734,5.5514),(5.9061,16.3817),(13.8512,10.3519) +11.26,15.0,82.6,1.5708,(-3.6145,6.9402),(-0.4734,5.4514),(5.9061,16.2817),(13.8512,10.2519) +11.27,15.0,82.7,1.5708,(-3.6145,6.8402),(-0.4734,5.3514),(5.9061,16.1817),(13.8512,10.1519) +11.28,15.0,82.8,1.5708,(-3.6145,6.7402),(-0.4734,5.2514),(5.9061,16.0817),(13.8512,10.0519) +11.29,15.0,82.9,1.5708,(-3.6145,6.6402),(-0.4734,5.1514),(5.9061,15.9817),(13.8512,9.9519) +11.3,15.0,83.0,1.5708,(-3.6145,6.5402),(-0.4734,5.0514),(5.9061,15.8817),(13.8512,9.8519) +11.31,15.0,83.1,1.5708,(-3.6145,6.4402),(-0.4734,4.9514),(5.9061,15.7817),(13.8512,9.7519) +11.32,15.0,83.2,1.5708,(-3.6145,6.3402),(-0.4734,4.8514),(5.9061,15.6817),(13.8512,9.6519) +11.33,15.0,83.3,1.5708,(-3.6145,6.2402),(-0.4734,4.7514),(5.9061,15.5817),(13.8512,9.5519) +11.34,15.0,83.4,1.5708,(-3.6145,6.1402),(-0.4734,4.6514),(5.9061,15.4817),(13.8512,9.4519) +11.35,15.0,83.5,1.5708,(-3.6145,6.0402),(-0.4734,4.5514),(5.9061,15.3817),(13.8512,9.3519) +11.36,15.0,83.6,1.5708,(-3.6145,5.9402),(-0.4734,4.4514),(5.9061,15.2817),(13.8512,9.2519) +11.37,15.0,83.7,1.5708,(-3.6145,5.8402),(-0.4734,4.3514),(5.9061,15.1817),(13.8512,9.1519) +11.38,15.0,83.8,1.5708,(-3.6145,5.7402),(-0.4734,4.2514),(5.9061,15.0817),(13.8512,9.0519) +11.39,15.0,83.9,1.5708,(-3.6145,5.6402),(-0.4734,4.1514),(5.9061,14.9817),(13.8512,8.9519) +11.4,15.0,84.0,1.5708,(-3.6145,5.5402),(-0.4734,4.0514),(5.9061,14.8817),(13.8512,8.8519) +11.41,15.0,84.1,1.5708,(-3.6145,5.4402),(-0.4734,3.9514),(5.9061,14.7817),(13.8512,8.7519) +11.42,15.0,84.2,1.5708,(-3.6145,5.3402),(-0.4734,3.8514),(5.9061,14.6817),(13.8512,8.6519) +11.43,15.0,84.3,1.5708,(-3.6145,5.2402),(-0.4734,3.7514),(5.9061,14.5817),(13.8512,8.5519) +11.44,15.0,84.4,1.5708,(-3.6145,5.1402),(-0.4734,3.6514),(5.9061,14.4817),(13.8512,8.4519) +11.45,15.0,84.5,1.5708,(-3.6145,5.0402),(-0.4734,3.5514),(5.9061,14.3817),(13.8512,8.3519) +11.46,15.0,84.6,1.5708,(-3.6145,4.9402),(-0.4734,3.4514),(5.9061,14.2817),(13.8512,8.2519) +11.47,15.0,84.7,1.5708,(-3.6145,4.8402),(-0.4734,3.3514),(5.9061,14.1817),(13.8512,8.1519) +11.48,15.0,84.8,1.5708,(-3.6145,4.7402),(-0.4734,3.2514),(5.9061,14.0817),(13.8512,8.0519) +11.49,15.0,84.9,1.5708,(-3.6145,4.6402),(-0.4734,3.1514),(5.9061,13.9817),(13.8512,7.9519) +11.5,15.0,85.0,1.5708,(-3.6145,4.5402),(-0.4734,3.0514),(5.9061,13.8817),(13.8512,7.8519) +11.51,15.0,85.1,1.5708,(-3.6145,4.4402),(-0.4734,2.9514),(5.9061,13.7817),(13.8512,7.7519) +11.52,15.0,85.2,1.5708,(-3.6145,4.3402),(-0.4734,2.8514),(5.9061,13.6817),(13.8512,7.6519) +11.53,15.0,85.3,1.5708,(-3.6145,4.2402),(-0.4734,2.7514),(5.9061,13.5817),(13.8512,7.5519) +11.54,15.0,85.4,1.5708,(-3.6145,4.1402),(-0.4734,2.6514),(5.9061,13.4817),(13.8512,7.4519) +11.55,15.0,85.5,1.5708,(-3.6145,4.0402),(-0.4734,2.5514),(5.9061,13.3817),(13.8512,7.3519) +11.56,15.0,85.6,1.5708,(-3.6145,3.9402),(-0.4734,2.4514),(5.9061,13.2817),(13.8512,7.2519) +11.57,15.0,85.7,1.5708,(-3.6145,3.8402),(-0.4734,2.3514),(5.9061,13.1817),(13.8512,7.1519) +11.58,15.0,85.8,1.5708,(-3.6145,3.7402),(-0.4734,2.2514),(5.9061,13.0817),(13.8512,7.0519) +11.59,15.0,85.9,1.5708,(-3.6145,3.6402),(-0.4734,2.1514),(5.9061,12.9817),(13.8512,6.9519) +11.6,15.0,86.0,1.5708,(-3.6145,3.5402),(-0.4734,2.0514),(5.9061,12.8817),(13.8512,6.8519) +11.61,15.0,86.1,1.5708,(-3.6145,3.4402),(-0.4734,1.9514),(5.9061,12.7817),(13.8512,6.7519) +11.62,15.0,86.2,1.5708,(-3.6145,3.3402),(-0.4734,1.8514),(5.9061,12.6817),(13.8512,6.6519) +11.63,15.0,86.3,1.5708,(-3.6145,3.2402),(-0.4734,1.7514),(5.9061,12.5817),(13.8512,6.5519) +11.64,15.0,86.4,1.5708,(-3.6145,3.1402),(-0.4734,1.6514),(5.9061,12.4817),(13.8512,6.4519) +11.65,15.0,86.5,1.5708,(-3.6145,3.0402),(-0.4734,1.5514),(5.9061,12.3817),(13.8512,6.3519) +11.66,15.0,86.6,1.5708,(-3.6145,2.9402),(-0.4734,1.4514),(5.9061,12.2817),(13.8512,6.2519) +11.67,15.0,86.7,1.5708,(-3.6145,2.8402),(-0.4734,1.3514),(5.9061,12.1817),(13.8512,6.1519) +11.68,15.0,86.8,1.5708,(-3.6145,2.7402),(-0.4734,1.2514),(5.9061,12.0817),(13.8512,6.0519) +11.69,15.0,86.9,1.5708,(-3.6145,2.6402),(-0.4734,1.1514),(5.9061,11.9817),(13.8512,5.9519) +11.7,15.0,87.0,1.5708,(-3.6145,2.5402),(-0.4734,1.0514),(5.9061,11.8817),(13.8512,5.8519) +11.71,15.0,87.1,1.5708,(-3.6145,2.4402),(-0.4734,0.9514),(5.9061,11.7817),(13.8512,5.7519) +11.72,15.0,87.2,1.5708,(-3.6145,2.3402),(-0.4734,0.8514),(5.9061,11.6817),(13.8512,5.6519) +11.73,15.0,87.3,1.5708,(-3.6145,2.2402),(-0.4734,0.7514),(5.9061,11.5817),(13.8512,5.5519) +11.74,15.0,87.4,1.5708,(-3.6145,2.1402),(-0.4734,0.6514),(5.9061,11.4817),(13.8512,5.4519) +11.75,15.0,87.5,1.5708,(-3.6145,2.0402),(-0.4734,0.5514),(5.9061,11.3817),(13.8512,5.3519) +11.76,15.0,87.6,1.5708,(-3.6145,1.9402),(-0.4734,0.4514),(5.9061,11.2817),(13.8512,5.2519) +11.77,15.0,87.7,1.5708,(-3.6145,1.8402),(-0.4734,0.3514),(5.9061,11.1817),(13.8512,5.1519) +11.78,15.0,87.8,1.5708,(-3.6145,1.7402),(-0.4734,0.2514),(5.9061,11.0817),(13.8512,5.0519) +11.79,15.0,87.9,1.5708,(-3.6145,1.6402),(-0.4734,0.1514),(5.9061,10.9817),(13.8512,4.9519) +11.8,15.0,88.0,1.5708,(-3.6145,1.5402),(-0.4734,0.0514),(5.9061,10.8817),(13.8512,4.8519) +11.81,15.0,88.1,1.5708,(-3.6145,1.4402),(5.9061,10.7817),(13.8512,4.7519) +11.82,15.0,88.2,1.5708,(-3.6145,1.3402),(5.9061,10.6817),(13.8512,4.6519) +11.83,15.0,88.3,1.5708,(-3.6145,1.2402),(5.9061,10.5817),(13.8512,4.5519) +11.84,15.0,88.4,1.5708,(-3.6145,1.1402),(5.9061,10.4817),(13.8512,4.4519) +11.85,15.0,88.5,1.5708,(-3.6145,1.0402),(5.9061,10.3817),(13.8512,4.3519) +11.86,15.0,88.6,1.5708,(-3.6145,0.9402),(5.9061,10.2817),(13.8512,4.2519) +11.87,15.0,88.7,1.5708,(-3.6145,0.8402),(5.9061,10.1817),(13.8512,4.1519) +11.88,15.0,88.8,1.5708,(-3.6145,0.7402),(5.9061,10.0817),(13.8512,4.0519) +11.89,15.0,88.9,1.5708,(-3.6145,0.6402),(5.9061,9.9817),(13.8512,3.9519) +11.9,15.0,89.0,1.5708,(-3.6145,0.5402),(5.9061,9.8817),(13.8512,3.8519) +11.91,15.0,89.1,1.5708,(-3.6145,0.4402),(5.9061,9.7817),(13.8512,3.7519) +11.92,15.0,89.2,1.5708,(-3.6145,0.3402),(5.9061,9.6817),(13.8512,3.6519) +11.93,15.0,89.3,1.5708,(-3.6145,0.2402),(5.9061,9.5817),(13.8512,3.5519) +11.94,15.0,89.4,1.5708,(-3.6145,0.1402),(5.9061,9.4817),(13.8512,3.4519) +11.95,15.0,89.5,1.5708,(-3.6145,0.0402),(5.9061,9.3817),(13.8512,3.3519) +11.96,15.0,89.6,1.5708,(5.9061,9.2817),(13.8512,3.2519) +11.97,15.0,89.7,1.5708,(5.9061,9.1817),(13.8512,3.1519) +11.98,15.0,89.8,1.5708,(5.9061,9.0817),(13.8512,3.0519) +11.99,15.0,89.9,1.5708,(5.9061,8.9817),(13.8512,2.9519) +12.0,15.0,90.0,1.5708,(5.9061,8.8817),(13.8512,2.8519) +12.01,15.0,90.1,1.5708,(5.9061,8.7817),(13.8512,2.7519) +12.02,15.0,90.2,1.5708,(5.9061,8.6817),(13.8512,2.6519) +12.03,15.0,90.3,1.5708,(5.9061,8.5817),(13.8512,2.5519) +12.04,15.0,90.4,1.5708,(5.9061,8.4817),(13.8512,2.4519) +12.05,15.0,90.5,1.5708,(5.9061,8.3817),(13.8512,2.3519) +12.06,15.0,90.6,1.5708,(5.9061,8.2817),(13.8512,2.2519) +12.07,15.0,90.7,1.5708,(5.9061,8.1817),(13.8512,2.1519) +12.08,15.0,90.8,1.5708,(5.9061,8.0817),(13.8512,2.0519) +12.09,15.0,90.9,1.5708,(5.9061,7.9817),(13.8512,1.9519) +12.1,15.0,91.0,1.5708,(5.9061,7.8817),(13.8512,1.8519) +12.11,15.0,91.1,1.5708,(5.9061,7.7817),(13.8512,1.7519) +12.12,15.0,91.2,1.5708,(5.9061,7.6817),(13.8512,1.6519) +12.13,15.0,91.3,1.5708,(5.9061,7.5817),(13.8512,1.5519) +12.14,15.0,91.4,1.5708,(5.9061,7.4817),(13.8512,1.4519) +12.15,15.0,91.5,1.5708,(5.9061,7.3817),(13.8512,1.3519) +12.16,15.0,91.6,1.5708,(5.9061,7.2817),(13.8512,1.2519) +12.17,15.0,91.7,1.5708,(5.9061,7.1817),(13.8512,1.1519) +12.18,15.0,91.8,1.5708,(5.9061,7.0817),(13.8512,1.0519) +12.19,15.0,91.9,1.5708,(5.9061,6.9817),(13.8512,0.9519) +12.2,15.0,92.0,1.5708,(5.9061,6.8817),(13.8512,0.8519) +12.21,15.0,92.1,1.5708,(5.9061,6.7817),(13.8512,0.7519) +12.22,15.0,92.2,1.5708,(5.9061,6.6817),(13.8512,0.6519) +12.23,15.0,92.3,1.5708,(5.9061,6.5817),(13.8512,0.5519) +12.24,15.0,92.4,1.5708,(5.9061,6.4817),(13.8512,0.4519) +12.25,15.0,92.5,1.5708,(5.9061,6.3817),(13.8512,0.3519) +12.26,15.0,92.6,1.5708,(5.9061,6.2817),(13.8512,0.2519) +12.27,15.0,92.7,1.5708,(5.9061,6.1817),(13.8512,0.1519) +12.28,15.0,92.8,1.5708,(5.9061,6.0817),(13.8512,0.0519) +12.29,15.0,92.9,1.5708,(5.9061,5.9817) +12.3,15.0,93.0,1.5708,(5.9061,5.8817) +12.31,15.0,93.1,1.5708,(5.9061,5.7817) +12.32,15.0,93.2,1.5708,(5.9061,5.6817) +12.33,15.0,93.3,1.5708,(5.9061,5.5817) +12.34,15.0,93.4,1.5708,(5.9061,5.4817) +12.35,15.0,93.5,1.5708,(5.9061,5.3817) +12.36,15.0,93.6,1.5708,(5.9061,5.2817) +12.37,15.0,93.7,1.5708,(5.9061,5.1817) +12.38,15.0,93.8,1.5708,(5.9061,5.0817) +12.39,15.0,93.9,1.5708,(5.9061,4.9817) +12.4,15.0,94.0,1.5708,(5.9061,4.8817) +12.41,15.0,94.1,1.5708,(5.9061,4.7817) +12.42,15.0,94.2,1.5708,(5.9061,4.6817) +12.43,15.0,94.3,1.5708,(5.9061,4.5817) +12.44,15.0,94.4,1.5708,(5.9061,4.4817) +12.45,15.0,94.5,1.5708,(5.9061,4.3817) +12.46,15.0,94.6,1.5708,(5.9061,4.2817) +12.47,15.0,94.7,1.5708,(5.9061,4.1817) +12.48,15.0,94.8,1.5708,(5.9061,4.0817) +12.49,15.0,94.9,1.5708,(5.9061,3.9817) +12.5,15.0,95.0,1.5708,(5.9061,3.8817) +12.51,15.0,95.1,1.5708,(5.9061,3.7817) +12.52,15.0,95.2,1.5708,(5.9061,3.6817) +12.53,15.0,95.3,1.5708,(5.9061,3.5817) +12.54,15.0,95.4,1.5708,(5.9061,3.4817) +12.55,15.0,95.5,1.5708,(5.9061,3.3817) +12.56,15.0,95.6,1.5708,(5.9061,3.2817) +12.57,15.0,95.7,1.5708,(5.9061,3.1817) +12.58,15.0,95.8,1.5708,(5.9061,3.0817) +12.59,15.0,95.9,1.5708,(5.9061,2.9817) +12.6,15.0,96.0,1.5708,(5.9061,2.8817) +12.61,15.0,96.1,1.5708,(5.9061,2.7817) +12.62,15.0,96.2,1.5708,(5.9061,2.6817) +12.63,15.0,96.3,1.5708,(5.9061,2.5817) +12.64,15.0,96.4,1.5708,(5.9061,2.4817) +12.65,15.0,96.5,1.5708,(5.9061,2.3817) +12.66,15.0,96.6,1.5708,(5.9061,2.2817) +12.67,15.0,96.7,1.5708,(5.9061,2.1817) +12.68,15.0,96.8,1.5708,(5.9061,2.0817) +12.69,15.0,96.9,1.5708,(5.9061,1.9817) +12.7,15.0,97.0,1.5708,(5.9061,1.8817) +12.71,15.0,97.1,1.5708,(5.9061,1.7817) +12.72,15.0,97.2,1.5708,(5.9061,1.6817) +12.73,15.0,97.3,1.5708,(5.9061,1.5817) +12.74,15.0,97.4,1.5708,(5.9061,1.4817) +12.75,15.0,97.5,1.5708,(5.9061,1.3817) +12.76,15.0,97.6,1.5708,(5.9061,1.2817) +12.77,15.0,97.7,1.5708,(5.9061,1.1817) +12.78,15.0,97.8,1.5708,(5.9061,1.0817) +12.79,15.0,97.9,1.5708,(5.9061,0.9817) +12.8,15.0,98.0,1.5708,(5.9061,0.8817) +12.81,15.0,98.1,1.5708,(5.9061,0.7817) +12.82,15.0,98.2,1.5708,(5.9061,0.6817) +12.83,15.0,98.3,1.5708,(5.9061,0.5817) +12.84,15.0,98.4,1.5708,(5.9061,0.4817) +12.85,15.0,98.5,1.5708,(5.9061,0.3817) +12.86,15.0,98.6,1.5708,(5.9061,0.2817) +12.87,15.0,98.7,1.5708,(5.9061,0.1817) +12.88,15.0,98.8,1.5708,(5.9061,0.0817) +12.89,15.0,98.9,1.5708, +12.9,15.0,99.0,1.5708, +12.91,15.0,99.1,1.5708, +12.92,15.0,99.2,1.5708, +12.93,15.0,99.3,1.5708, +12.94,15.0,99.4,1.5708, +12.95,15.0,99.5,1.5708, +12.96,15.0,99.6,1.5708, +12.97,15.0,99.7,1.5708, +12.98,15.0,99.8,1.5708, +12.99,15.0,99.9,1.5708, +13.0,15.0,100.0,1.5708, +end diff --git a/src/visualization/pure_pursuit_visualizer/package.xml b/src/plan_act/test_plan_act_algorithems/package.xml similarity index 83% rename from src/visualization/pure_pursuit_visualizer/package.xml rename to src/plan_act/test_plan_act_algorithems/package.xml index 0d043b89..0e0368a5 100644 --- a/src/visualization/pure_pursuit_visualizer/package.xml +++ b/src/plan_act/test_plan_act_algorithems/package.xml @@ -1,10 +1,10 @@ - pure_pursuit_visualizer + test_plan_act_algorithems 0.0.0 TODO: Package description - dyu056 + chris TODO: License declaration ament_copyright diff --git a/src/plan_act/test_plan_act_algorithems/resource/test_plan_act_algorithems b/src/plan_act/test_plan_act_algorithems/resource/test_plan_act_algorithems new file mode 100644 index 00000000..e69de29b diff --git a/src/plan_act/test_plan_act_algorithems/setup.cfg b/src/plan_act/test_plan_act_algorithems/setup.cfg new file mode 100644 index 00000000..fe75123e --- /dev/null +++ b/src/plan_act/test_plan_act_algorithems/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/test_plan_act_algorithems +[install] +install_scripts=$base/lib/test_plan_act_algorithems diff --git a/src/plan_act/test_plan_act_algorithems/setup.py b/src/plan_act/test_plan_act_algorithems/setup.py new file mode 100644 index 00000000..30d07afa --- /dev/null +++ b/src/plan_act/test_plan_act_algorithems/setup.py @@ -0,0 +1,27 @@ +from setuptools import setup + +package_name = 'test_plan_act_algorithems' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='chris', + maintainer_email='chrisgraham908@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'aruco_triangle = test_plan_act_algorithems.aruco_triangle:main', + 'aruco_middle = test_plan_act_algorithems.aruco_middle:main', + ], + }, +) diff --git a/src/visualization/pure_pursuit_visualizer/test/test_copyright.py b/src/plan_act/test_plan_act_algorithems/test/test_copyright.py similarity index 100% rename from src/visualization/pure_pursuit_visualizer/test/test_copyright.py rename to src/plan_act/test_plan_act_algorithems/test/test_copyright.py diff --git a/src/visualization/pure_pursuit_visualizer/test/test_flake8.py b/src/plan_act/test_plan_act_algorithems/test/test_flake8.py similarity index 100% rename from src/visualization/pure_pursuit_visualizer/test/test_flake8.py rename to src/plan_act/test_plan_act_algorithems/test/test_flake8.py diff --git a/src/visualization/pure_pursuit_visualizer/test/test_pep257.py b/src/plan_act/test_plan_act_algorithems/test/test_pep257.py similarity index 100% rename from src/visualization/pure_pursuit_visualizer/test/test_pep257.py rename to src/plan_act/test_plan_act_algorithems/test/test_pep257.py diff --git a/src/plan_act/test_plan_act_algorithems/test_plan_act_algorithems/__init__.py b/src/plan_act/test_plan_act_algorithems/test_plan_act_algorithems/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/plan_act/test_plan_act_algorithems/test_plan_act_algorithems/aruco_middle.py b/src/plan_act/test_plan_act_algorithems/test_plan_act_algorithems/aruco_middle.py new file mode 100644 index 00000000..bf483df4 --- /dev/null +++ b/src/plan_act/test_plan_act_algorithems/test_plan_act_algorithems/aruco_middle.py @@ -0,0 +1,357 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from std_msgs.msg import Float32 +from cv_bridge import CvBridge + +import cv2 +import cv2.aruco as aruco +import numpy as np + + +def get_middle_point(x1, x2): + return np.array([ + (x1[0] + x2[0]) / 2, + (x1[1] + x2[1]) / 2, + ]) + + +def find_middle_point_of_trig(point1, point2, point3): + x_middle = (point1[0] + point2[0] + point3[0]) / 3 + y_middle = (point1[1] + point2[1] + point3[1]) / 3 + middle_point = [x_middle, y_middle] + return middle_point + + +def create_boundary(points): + sorted_points = sorted(points, key=lambda p: p[1]) + + min_x_point = sorted_points.pop(-1) + sorted_points.sort(key=lambda p: np.linalg.norm(np.array(min_x_point) - np.array(p))) + + sorted_points.insert(0, min_x_point) + return sorted_points + + +def get_mid_trig_sets(set1, set2, debug_image=None): + if (len(set1) < 2 or len(set2 + + ) < 2) and len(set1) + len(set2) < 3: + return + + # merge lists + set_of_points = [] + for point1, point2 in zip(set1, set2): + set_of_points.append(point1) + set_of_points.append(point2) + + middle_points = [] + for idx in range(0, len(set_of_points) - 2, 2): + middle_points.append( + find_middle_point_of_trig(set_of_points[idx], + set_of_points[idx + 1], + set_of_points[idx + 2])) + middle_points.append( + find_middle_point_of_trig(set_of_points[idx], + set_of_points[idx + 1], + set_of_points[idx + 3])) + middle_points.append( + find_middle_point_of_trig(set_of_points[idx + 1], + set_of_points[idx + 2], + set_of_points[idx + 3])) + middle_points.append( + find_middle_point_of_trig(set_of_points[idx], + set_of_points[idx + 2], + set_of_points[idx + 3])) + + if debug_image is not None: + draw_trig(debug_image, set_of_points[idx], + set_of_points[idx + 1], + set_of_points[idx + 2]) + draw_trig(debug_image, set_of_points[idx], + set_of_points[idx + 1], + set_of_points[idx + 3]) + draw_trig(debug_image, set_of_points[idx + 1], + set_of_points[idx + 2], + set_of_points[idx + 3]) + draw_trig(debug_image, set_of_points[idx], + set_of_points[idx + 2], + set_of_points[idx + 3]) + + return middle_points + + +def draw_trig(image, point1, point2, point3): + cv2.line(image, + (int(point1[0]), int(point1[1])), + (int(point2[0]), int(point2[1])), + (0, 122, 122), 2) + cv2.line(image, + (int(point2[0]), int(point2[1])), + (int(point3[0]), int(point3[1])), + (0, 122, 122), 2) + cv2.line(image, + (int(point3[0]), int(point3[1])), + (int(point1[0]), int(point1[1])), + (0, 122, 122), 2) + + +class ArucoMiddleNode(Node): + def __init__(self): + super().__init__('aruco_middle_node') + self.bridge = CvBridge() + self.debug = True + self.Kp_controller = PController(2) + self.current_point = None + self.image_shape = None + + self.create_subscription( + Image, + '/zed2i/zed_node/rgb/image_rect_color', + self.image_callback, + 10) + + self.steering_pub = self.create_publisher( + Float32, + '/set_steering', + 10) + + self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) + self.parameters = aruco.DetectorParameters() + self.detector = aruco.ArucoDetector(self.aruco_dict, self.parameters) + + self.steering_percentage = 0.0 + self.current_target = [0, 0] + + def image_callback(self, msg): + + # Convert ROS image message to OpenCV image. + cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') + + self.current_point = ( + int(1 * (cv_image.shape[1] / 2)), + int(5 * (cv_image.shape[0] / 6)), + ) + + self.image_shape = cv_image.shape + + # Detect ArUco markers in image. + corners, ids, _ = self.detector.detectMarkers(cv_image) + even_markers = [] + odd_markers = [] + + # has to be a better way of doing this + if not 0 == len(corners): + even_markers = [ + np.mean(marker, axis=1)[0] + for (marker_id, marker) in zip(ids, corners) if marker_id == 2 + ] + + if not 0 == len(corners): + odd_markers = [ + np.mean(marker, axis=1)[0] + for (marker_id, marker) in zip(ids, corners) if marker_id == 1 + ] + + if 0 == len(even_markers) and 0 != len(odd_markers): + odd_boundary = create_boundary(odd_markers) + error = self.get_target_error(target_point=(int(odd_boundary[0][0]), int(odd_boundary[0][1])), + debug_img=cv_image) + if error: + error = error[0] + change_steering = -100.0 - self.Kp_controller.process(error, self.current_point[0]) + steering_msg = Float32() + steering_msg.data = change_steering + self.steering_pub.publish(steering_msg) + + prev_point = np.array([int(odd_boundary[0][0]), cv_image.shape[0]]) + for odd_boundary_point in odd_boundary: + cv2.line(cv_image, prev_point, + (int(odd_boundary_point[0]), int(odd_boundary_point[1])), + (0, 122, 255), 2) + prev_point = (int(odd_boundary_point[0]), int(odd_boundary_point[1])) + + cv2.aruco.drawDetectedMarkers(cv_image, corners, ids) + cv2.imshow("Aruco Middle ", cv_image) + cv2.waitKey(1) + + return + + elif 0 != len(even_markers) and 0 == len(odd_markers): + even_boundary = create_boundary(even_markers) + error = self.get_target_error(target_point=(int(even_boundary[0][0]), int(even_boundary[0][1])), + debug_img=cv_image) + + print(f'{error=}') + if error: + error = error[0] + change_steering = -100.0 - self.Kp_controller.process(error, self.current_point[0]) + print(change_steering) + steering_msg = Float32() + steering_msg.data = change_steering + self.steering_pub.publish(steering_msg) + + prev_point = np.array([int(even_boundary[0][0]), cv_image.shape[0]]) + for even_boundary_point in even_boundary: + cv2.line(cv_image, prev_point, + (int(even_boundary_point[0]), int(even_boundary_point[1])), + (0, 122, 255), 2) + prev_point = (int(even_boundary_point[0]), int(even_boundary_point[1])) + + cv2.aruco.drawDetectedMarkers(cv_image, corners, ids) + cv2.imshow("Aruco Middle ", cv_image) + cv2.waitKey(1) + + return + + elif 0 == len(even_markers) and 0 == len(odd_markers): + return + + even_boundary = create_boundary(even_markers) + odd_boundary = create_boundary(odd_markers) + + # middle points of every even and odd ID marker + # middle_points = [ + # get_middle_point(even_marker, odd_marker) \ + # for even_marker in even_markers \ + # for odd_marker in odd_markers + # ] + + # Middle points of the bounday lines + middle_points = [ + get_middle_point(point1, point2) \ + for (point1, point2) in zip(even_boundary, odd_boundary) + ] + + middle_trig_points = get_mid_trig_sets(even_boundary, odd_boundary, cv_image) + + # calculate the target point and then get the error to correct for + error = self.get_target_error(target_point=(int(middle_points[0][0]), int(middle_points[0][1])), + debug_img=cv_image) + + if error: + error = error[0] + change_steering = self.Kp_controller.process(error, self.current_point[0]) + steering_msg = Float32() + steering_msg.data = change_steering + self.steering_pub.publish(steering_msg) + + if not self.debug: + return + + print("------------------------------") + print(cv_image.shape) + for middle_point in middle_points: + print(middle_point) + + cv2.aruco.drawDetectedMarkers(cv_image, corners, ids) + + for middle_point in middle_points: + point = (int(middle_point[0]), int(middle_point[1])) + cv2.circle(cv_image, point, radius=5, color=(0, 0, 255), thickness=-1) + + # odd boundary line + prev_point = np.array([int(odd_boundary[0][0]), cv_image.shape[0]]) + for odd_boundary_point in odd_boundary: + cv2.line(cv_image, prev_point, + (int(odd_boundary_point[0]), int(odd_boundary_point[1])), + (0, 122, 255), 2) + prev_point = (int(odd_boundary_point[0]), int(odd_boundary_point[1])) + # + # # even boundary line + prev_point = np.array([int(even_boundary[0][0]), cv_image.shape[0]]) + for even_boundary_point in even_boundary: + cv2.line(cv_image, prev_point, + (int(even_boundary_point[0]), int(even_boundary_point[1])), + (0, 122, 255), 2) + prev_point = (int(even_boundary_point[0]), int(even_boundary_point[1])) + + # Middle track line + prev_point = np.array([cv_image.shape[1] // 2, cv_image.shape[0]]) + for middle_point in middle_points: + cv2.line(cv_image, prev_point, + (int(middle_point[0]), int(middle_point[1])), + (0, 255, 0), 2) + prev_point = (int(middle_point[0]), int(middle_point[1])) + + # Trig track line + prev_point = np.array([cv_image.shape[1] // 2, cv_image.shape[0]]) + if middle_trig_points is not None: + for middle_trig_point in middle_trig_points: + cv2.line(cv_image, prev_point, + (int(middle_trig_point[0]), int(middle_trig_point[1])), + (0, 122, 122), 2) + prev_point = (int(middle_trig_point[0]), int(middle_trig_point[1])) + + cv2.circle(cv_image, self.current_point, radius=5, color=(0, 255, 0), thickness=-1) + + # cv2.circle(cv_image, (int(cv_image.shape[1]/2), int(cv_image.shape[0]/2)), + # radius=15, color=(0, 0, 255), thickness=-1) + + cv2.imshow("Aruco Middle ", cv_image) + cv2.waitKey(1) + + def get_target_error(self, target_point, debug_img=None): + # draws a line across the screen and gets the intersect at that point + def line(p1, p2): + A = (p1[1] - p2[1]) + B = (p2[0] - p1[0]) + C = (p1[0] * p2[1] - p2[0] * p1[1]) + return A, B, -C + + def intersection(L1, L2): + D = L1[0] * L2[1] - L1[1] * L2[0] + Dx = L1[2] * L2[1] - L1[1] * L2[2] + Dy = L1[0] * L2[2] - L1[2] * L2[0] + if D != 0: + x = Dx / D + y = Dy / D + return x, y + else: + return False + + L1 = line(target_point, + (self.image_shape[1] // 2, + self.image_shape[0])) + L2 = line((0, self.current_point[1]), (self.image_shape[1], self.current_point[1])) + R = intersection(L1, L2) + + if debug_img is not None and R: + cv2.line(debug_img, + (0, self.current_point[1]), + (self.image_shape[1], self.current_point[1]), + (255, 0, 0), 1) + cv2.line( + debug_img, + (int(R[0]), int(R[1])), + self.current_point, + (0, 255, 0), 2 + ) + + print(f'{R=}') + + return R + + +class PController: + def __init__(self, Kp): + self.Kp = Kp + + def process(self, target, current): + control_effort = target - current + return control_effort * self.Kp + + +def main(args=None): + rclpy.init(args=args) + + node = ArucoMiddleNode() + + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/plan_act/test_plan_act_algorithems/test_plan_act_algorithems/aruco_triangle.py b/src/plan_act/test_plan_act_algorithems/test_plan_act_algorithems/aruco_triangle.py new file mode 100644 index 00000000..60aa423e --- /dev/null +++ b/src/plan_act/test_plan_act_algorithems/test_plan_act_algorithems/aruco_triangle.py @@ -0,0 +1,160 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from std_msgs.msg import Float32 +from cv_bridge import CvBridge + +import cv2 +import cv2.aruco as aruco +import numpy as np + + +def detect_center_of_markers(corners, ids): + """ + Function to detect the centers of ArUco markers in an image. + + Args: + corners (list): List of detected marker corners. + ids (list): List of detected marker ids. + + Returns: + tuple: Tuple of the centers of the closest pair of odd and even markers. + """ + # Lists to hold markers with odd and even ids. + odd_markers = [] + even_markers = [] + + # Loop over detected marker corners and ids. + for i, corner in zip(ids, corners): + # Check if marker id is odd or even and append to appropriate list. + if i % 2 == 0: + even_markers.append(np.mean(corner, axis=1)) + else: + odd_markers.append(np.mean(corner, axis=1)) + + # Variables to hold closest pair of odd and even markers and their distance. + min_distance = np.inf + closest_pair = None + + # Loop over pairs of odd and even markers. + for odd in odd_markers: + for even in even_markers: + # Calculate Euclidean distance between odd and even marker. + dist = np.linalg.norm(odd - even) + + # If distance is less than current minimum, update minimum distance and closest pair. + if dist < min_distance: + min_distance = dist + closest_pair = (odd, even) + + # Return closest pair of markers. + return closest_pair + + +def get_angle_from_number(num): + return (num + 1) * 45 + + +def draw_line(image, angle_degrees): + angle_radians = np.deg2rad(angle_degrees) + center_x = image.shape[1] // 2 + end_x = int(center_x + image.shape[1] / 2 * np.tan(angle_radians)) + end_y = image.shape[0] + print(center_x, image.shape[0], end_x, end_y) + cv2.line(image, (center_x, image.shape[0]), (end_x, end_y), color=(0, 0, 255), thickness=10) + + +def debug_show(image, markers): + if markers[0] is not None and markers[1] is not None: + cv2.aruco.drawDetectedMarkers(image, markers[0], markers[1]) + + closest_pair = detect_center_of_markers(markers[0], markers[1]) + # Add a cirlce to the image in the center of the two images + if closest_pair is not None: + center_x = int((closest_pair[0][0][0] + closest_pair[1][0][0]) / 2) + center_y = int((closest_pair[0][0][1] + closest_pair[1][0][1]) / 2) + cv2.circle(image, (center_x, center_y), radius=5, color=(0, 0, 255), thickness=-1) + + middle_x = image.shape[1] // 2 + cv2.line(image, (middle_x, image.shape[0]), (center_x, center_y), color=(0, 0, 255), thickness=10) + + cv2.imshow('Aruco Markers', image) + cv2.waitKey(1) + + +class ArucoTriangleNode(Node): + def __init__(self): + """ + Constructor for the ArucoTriangleNode class. + + Initializes the node, CvBridge, subscriptions, publishers and ArUco detector. + """ + super().__init__('aruco_triangle_node') + self.bridge = CvBridge() + + self.create_subscription( + Image, + '/zed2i/zed_node/rgb/image_rect_color', + self.image_callback, + 10) + + self.steering_pub = self.create_publisher( + Float32, + '/set_steering', + 10) + + self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250) + self.parameters = aruco.DetectorParameters() + self.detector = aruco.ArucoDetector(self.aruco_dict, self.parameters) + + self.steering_percentage = 0.0 + self.current_target = [0, 0] + + def image_callback(self, msg): + """ + Callback for image messages from the subscribed topic. + + Args: + msg (sensor_msgs.msg.Image): The incoming image message. + """ + # Convert ROS image message to OpenCV image. + cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') + + # Detect ArUco markers in image. + corners, ids, _ = self.detector.detectMarkers(cv_image) + + debug_show(cv_image, (corners, ids)) + + if corners is None or ids is None: + return + # Detect center of closest pair of markers. + closest_pair = detect_center_of_markers(corners, ids) + + if closest_pair is not None: + # Calculate steering command based on position of center of closest pair of markers. + self.current_target = np.mean(closest_pair, axis=0)[0] + self.steering_percentage = ((self.current_target[0] / cv_image.shape[1]) - 0.5) * -2 + print(self.steering_percentage) + + # Publish steering command. + msg = Float32() + msg.data = float(self.steering_percentage) + self.steering_pub.publish(msg) + # else: + # Log message if no ArUco markers detected. + # self.get_logger().info('No ArUco markers set detected') + + +def main(args=None): + rclpy.init(args=args) + + node = ArucoTriangleNode() + + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/planning/path_planning/launch/path_planning.launch.py b/src/planning/path_planning/launch/path_planning.launch.py new file mode 100644 index 00000000..077bf9ad --- /dev/null +++ b/src/planning/path_planning/launch/path_planning.launch.py @@ -0,0 +1,52 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ament_index_python import get_package_prefix +from launch.actions import OpaqueFunction +import os + +def generate_launch_description() -> LaunchDescription: + package_name = 'path_planning' + package_dir = os.path.join(get_package_prefix(package_name), 'lib', package_name) # directory of installed node names + planners = os.listdir(package_dir) # retrieves installed node names + + launch_descriptions = [ + # launch arguments + DeclareLaunchArgument( + 'node_name', + default_value='centerline_planner', + description='which node from this package to launch' + ), + DeclareLaunchArgument( + 'viz', + default_value='True', + description='visualise node output?' + ) + ] + + nodes = OpaqueFunction(function=get_node, args=[package_name, planners]) # get a list of actions + launch_descriptions.append(nodes) + + return LaunchDescription(launch_descriptions) + +def get_node(context, package_name, planners): + NODES = [] + node_2_run = LaunchConfiguration('node_name').perform(context) # get runtime value of argument + + for node_name in planners: + if node_name == node_2_run: + _ = Node( + package=package_name, + executable=node_name, + name=node_name, + parameters=[{'viz': LaunchConfiguration('viz')}] + ) + + NODES.append(_) + + if len(NODES) == 0: + raise Exception(f"selected node {node_2_run} does not exist!") + + return NODES + diff --git a/src/planning/path_planning/package.xml b/src/planning/path_planning/package.xml index 924eafe9..0cd208e1 100644 --- a/src/planning/path_planning/package.xml +++ b/src/planning/path_planning/package.xml @@ -11,12 +11,20 @@ geometry_msgs moa_msgs + python3-numpy + python3-matplotlib + python3-scipy + python3-shapely + ament_copyright ament_flake8 ament_pep257 python3-pytest foxglove_msgs + std_msgs + + ros2launch diff --git a/src/planning/path_planning/path_planning/centerline_planner.py b/src/planning/path_planning/path_planning/centerline_planner.py new file mode 100644 index 00000000..f0e32e64 --- /dev/null +++ b/src/planning/path_planning/path_planning/centerline_planner.py @@ -0,0 +1,222 @@ +import rclpy +from rclpy.node import Node + +from moa_msgs.msg import Track +from geometry_msgs.msg import Pose, PoseArray + +import numpy as np +import matplotlib.pyplot as plt +from scipy.interpolate import UnivariateSpline + +class centerline_planner(Node): + def __init__(self): + super().__init__("centerline_planner") + + # parameters + self._plot = True + self.look_forward = 4 + self.num_points = self.look_forward*2 + self.smoothing_factor = 1 + self.left_cones = [] + self.right_cones = [] + + # subscribers + self.subscription_left_cone_map = self.create_subscription(Track, 'left_track', self.left_cone_map_callback, 10) + self.subscription_right_cone_map = self.create_subscription(Track, 'right_track', self.right_cone_map_callback, 10) + self.create_subscription(Pose, "car_position", self.set_car_position, 10) # car pose + + # publishers + self.centerline_publisher = self.create_publisher(PoseArray, "moa/selected_trajectory", 10) + + def set_car_position(self, msg:Pose) -> None: + self.car_pose = msg + self.loop() + + def left_cone_map_callback(self, msg:Track) -> None: + self.left_cones = msg.cones + + def right_cone_map_callback(self, msg:Track) -> None: + self.right_cones = msg.cones + + def loop(self) -> None: + self.get_logger().info(f"car pose recieved = {hasattr(self,'car_pose')}") + + if not hasattr(self,'car_pose'): + return + + lb, rb = self.get_boundaries() # get boundaries (list of [x,y] points) + + if not len(lb) > 0 or not len(rb) > 0: + return + + lblocal, _ = self.get_next_points(lb,self.look_forward) # get local points (list of [x,y] points) close to car + rblocal, car_position = self.get_next_points(rb,self.look_forward) + + if not len(lblocal) > 0 or not len(rblocal) > 0: + return + + centerline = self.compute_centerline(lblocal,rblocal,car_position) # compute centerline + + if not len(centerline) > 0: + return + + centerline = self.interpolate_line(centerline,self.num_points,self.smoothing_factor) # interpolate/smooth + + # publish + msg = self.get_posearray_msg(centerline) + self.centerline_publisher.publish(msg) + + self.plot(lb,rb,centerline,self._plot) # plot + + # MAIN FUNCTIONS + def get_boundaries(self): + """Retrieves the left and right boundary from msg and returns as a [x,y] list""" + left_cones = [[P.x, P.y] for P in self.left_cones] + right_cones = [[P.x, P.y] for P in self.right_cones] + + return left_cones, right_cones + + def get_next_points(self,line,look_forward): + """Retrieves the points closest to the car + *ASSUMES THE points ARE SORTED/ORDERED + """ + car_point = np.array([self.car_pose.position.x,self.car_pose.position.y]) + car_orientation = np.array([self.car_pose.orientation.x,self.car_pose.orientation.y, + self.car_pose.orientation.z,self.car_pose.orientation.w]) + points = np.array(line) + + min_indx = np.argmin(np.linalg.norm(car_point-points, axis=1)) + # min_indx += self.is_behind_car(points[min_indx], car_point, car_orientation) + points = self.get_local_points(min_indx,points,look_forward) + + return points, car_point + + def compute_centerline(self,lb,rb,car_position): + """Computes the centerline using given boundary points based on closest distance + boundary points do not need to be sorted + Also adds car position at the beginning of the centerline + """ + centerline = np.array([car_position]) + + for P in lb: # loop through left boundary + closest_point = self.get_closest_point(P,rb) # get the point closest in the right boundary + centerpoint = (P+closest_point)/2 # midpoint + centerline = np.append(centerline,[centerpoint],axis=0) + + return centerline + + def interpolate_line(self,line,num_points=100,smoothing_factor=1): + line = self.univariate_interpolate(line,num_points,smoothing_factor) + + return line + + def get_posearray_msg(self, line): + pose_array = PoseArray() + + for P in line: + pose = Pose() + pose.position.x = P[0] # assing x,y values to position of pose + pose.position.y = P[1] + pose_array.poses.append(pose) + + return pose_array + + def plot(self,lb,rb,centerline,to_plot=False): + """Plots the boundary and centerline points""" + if to_plot: + # x, y points + lb = np.array(lb) + rb = np.array(rb) + centerline = np.array(centerline) + + lbx, lby = lb[:,0], lb[:,1] + rbx, rby = rb[:,0], rb[:,1] + centx, centy = centerline[:,0], centerline[:,1] + car_x, car_y = [self.car_pose.position.x, self.car_pose.position.y] + + # plot + plt.ion() + plt.clf() + + plt.plot(lbx,lby,'*b',label='left') + plt.plot(rbx,rby,'*y',label='right') + plt.plot(centx,centy,'-r',label='centerline') + plt.plot(car_x,car_y,'*k',label='car position') + + plt.pause(0.1) + plt.legend() + plt.show() + + # HERLPERS FUNCTIONS + def get_closest_point(self,point,points): + point = np.array(point) + points = np.array(points) + + distances = np.linalg.norm(point - points, axis=1) + + return points[np.argmin(distances)] + + def univariate_interpolate(self,line,num_points,smoothness): + if len(line) < 4: + return line + + line = np.array(line) + x,y = line[:,0], line[:,1] + t = range(len(x)) # t is for defining x and y parametrically + weights = np.ones(len(x)) # second point should be weighted less (to minimise if its behind) + weights[1] = 0.1 + + spline_x = UnivariateSpline(t,x,w=weights,s=smoothness) + spline_y = UnivariateSpline(t,y,w=weights,s=smoothness) + + t_new = np.linspace(min(t),max(t),num_points) + + x = spline_x(t_new) + y = spline_y(t_new) + + return np.array(list(zip(x,y))) + + def is_behind_car(self, closet_point, car_point, car_orientation): + from_car_point_to_closest_point = closet_point-car_point # vector from car to closest point + car_direction_vector = self.get_car_direction(car_orientation) + if (from_car_point_to_closest_point @ car_direction_vector) < 0: + return True + else: + return False + + def get_car_direction(self, orientation): + rotation_matrix = self.get_rotation_matrix(orientation[3]) + + return rotation_matrix @ np.array([1,0]) + + def get_local_points(self,min_indx,points,look_forward): + remaining_points = len(points) - (min_indx+1) # number of points forwards the car has detected + if remaining_points < look_forward: look_forward = remaining_points+1 + points = points[min_indx:min_indx+look_forward] # local points based on closest distance to car + + return points + + def get_rotation_matrix(self,theta): + rotation_matrix = np.array([[np.cos(theta), -np.sin(theta)],[np.sin(theta), np.cos(theta)]]) + + return rotation_matrix + + +def main(args=None): + rclpy.init(args=args) + + node = centerline_planner() + + rclpy.spin(node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() + + \ No newline at end of file diff --git a/src/planning/path_planning/path_planning/draw boundaries.py b/src/planning/path_planning/path_planning/draw boundaries.py index 14b19d4a..3020536d 100644 --- a/src/planning/path_planning/path_planning/draw boundaries.py +++ b/src/planning/path_planning/path_planning/draw boundaries.py @@ -29,4 +29,5 @@ # plt.plot(xr2,yr2,'og',label='right boundary original') plt.legend() -plt.show() \ No newline at end of file +plt.show() +plt.savefig("/home/tanish/autonomous/src/planning/path_planning/path_planning/boundaries_drawn.png") \ No newline at end of file diff --git a/src/planning/path_planning/path_planning/fasttube_planner.py b/src/planning/path_planning/path_planning/fasttube_planner.py new file mode 100644 index 00000000..fb1496fd --- /dev/null +++ b/src/planning/path_planning/path_planning/fasttube_planner.py @@ -0,0 +1,197 @@ +import rclpy +from rclpy.node import Node + +from moa_msgs.msg import Track +from geometry_msgs.msg import Pose, PoseArray + +import numpy as np +import matplotlib.pyplot as plt +from fsd_path_planning import PathPlanner, MissionTypes, ConeTypes +import math + + +class centerline_planner(Node): + def __init__(self): + super().__init__("centerline_planner") + + # parameters + self._plot = True + self.look_forward = 6 + self.left_cones = [] + self.right_cones = [] + self.path_planner = PathPlanner(MissionTypes.trackdrive) + + # subscribers + self.subscription_left_cone_map = self.create_subscription(Track, 'left_track', self.left_cone_map_callback, 10) + self.subscription_right_cone_map = self.create_subscription(Track, 'right_track', self.right_cone_map_callback, 10) + self.create_subscription(Pose, "car_position", self.set_car_position, 10) # car pose + + # publishers + self.centerline_publisher = self.create_publisher(PoseArray, "selected_trajectory", 10) + self.declare_parameters( + namespace='', + parameters=[ + ('invert_cones', False), + ] + ) + + def set_car_position(self, msg: Pose) -> None: + self.car_pose = msg + self.loop() + + def left_cone_map_callback(self, msg: Track) -> None: + self.left_cones = msg.cones + + def right_cone_map_callback(self, msg: Track) -> None: + self.right_cones = msg.cones + + def loop(self) -> None: + self.get_logger().info(f"car pose received = {hasattr(self, 'car_pose')}") + + if not hasattr(self, 'car_pose'): + return + + lb, rb = self.get_boundaries() # get boundaries (list of [x,y] points) + + # >>> DEBUG override: when only one cone color is seen + if len(lb) > 0 and len(rb) == 0: # only blue cones + self.get_logger().info("DEBUG: only blue cones -> full right") # >>> LOG + car_x = self.car_pose.position.x # >>> CHANGED + car_y = self.car_pose.position.y # >>> CHANGED + # simple right-turn trajectory + right_line = [ # >>> CHANGED + [car_x + 1.0, car_y + 1.0], + [car_x + 2.0, car_y + 2.0], + ] + msg = self.get_posearray_msg(right_line) # >>> CHANGED + self.centerline_publisher.publish(msg) # >>> CHANGED + return # >>> CHANGED + elif len(rb) > 0 and len(lb) == 0: # only yellow cones + self.get_logger().info("DEBUG: only yellow cones -> full left") # >>> LOG + car_x = self.car_pose.position.x # >>> CHANGED + car_y = self.car_pose.position.y # >>> CHANGED + # simple left-turn trajectory + left_line = [ + [car_x - 1.0, car_y + 1.0], + [car_x - 2.0, car_y + 2.0], + ] + msg = self.get_posearray_msg(left_line) # >>> CHANGED + self.centerline_publisher.publish(msg) # >>> CHANGED + return # >>> CHANGED + # <<< end DEBUG override >>> + + lblocal, _, _ = self.get_next_points(lb, self.look_forward) # get local points (list of [x,y] points) close to car + rblocal, car_position, car_orientation = self.get_next_points(rb, self.look_forward) + + if not len(lblocal) > 0 or not len(rblocal) > 0: + return + + global_cones = self.get_global_cones(lblocal, rblocal) + car_direction = self.get_car_direction(car_orientation) + + path = self.path_planner.calculate_path_in_global_frame(global_cones, car_position, car_direction) # compute centerline + + centerline = path[:, 1:3] + + if not len(centerline) > 0: + return + + # publish + msg = self.get_posearray_msg(centerline) + self.centerline_publisher.publish(msg) + + self.plot(lb, rb, centerline, self._plot) # plot + + def get_global_cones(self, lb, rb): + cones_by_type = [np.zeros((0, 2)) for _ in range(5)] + cones_by_type[ConeTypes.LEFT] = lb + cones_by_type[ConeTypes.RIGHT] = rb + return cones_by_type + + def get_car_direction(self, car_orientation): + return np.array([-math.sin(car_orientation), math.cos(car_orientation)]) + + # MAIN FUNCTIONS + def get_boundaries(self): + """Retrieves the left and right boundary from msg and returns as a [x,y] list""" + left_cones = np.array([[P.x, P.y] for P in self.left_cones]) + right_cones = np.array([[P.x, P.y] for P in self.right_cones]) + + return left_cones, right_cones + + def get_next_points(self, line, look_forward): + """Retrieves the points closest to the car + *ASSUMES THE points ARE SORTED/ORDERED + """ + car_point = np.array([self.car_pose.position.x, self.car_pose.position.y]) + car_orientation = self.car_pose.orientation.w + points = np.array(line) + + min_indx = np.argmin(np.linalg.norm(car_point - points, axis=1)) + points = self.get_local_points(min_indx, points, look_forward) + + return points, car_point, car_orientation + + def get_posearray_msg(self, line): + pose_array = PoseArray() + + for P in line: + pose = Pose() + pose.position.x = P[0] + pose.position.y = P[1] + pose_array.poses.append(pose) + + return pose_array + + def plot(self, lb, rb, centerline, to_plot=False): + """Plots the boundary and centerline points""" + if to_plot: + lb = np.array(lb) + rb = np.array(rb) + centerline = np.array(centerline) + + lbx, lby = lb[:, 0], lb[:, 1] + rbx, rby = rb[:, 0], rb[:, 1] + centx, centy = centerline[:, 0], centerline[:, 1] + car_x, car_y = [self.car_pose.position.x, self.car_pose.position.y] + + plt.ion() + plt.clf() + + plt.plot(lbx, lby, '*b', label='left') + plt.plot(rbx, rby, '*y', label='right') + plt.plot(centx, centy, '-r', label='centerline') + plt.plot(car_x, car_y, '*k', label='car position') + if not self.get_parameter('invert_cones').get_parameter_value().bool_value: + plt.plot(lbx,lby,'*b',label='left') + plt.plot(rbx,rby,'*y',label='right') + else: + plt.plot(lbx,lby,'*y',label='right') + plt.plot(rbx,rby,'*b',label='left') + + plt.plot(centx,centy,'-r',label='centerline') + plt.plot(car_x,car_y,'*k',label='car position') + + plt.pause(0.1) + plt.legend() + plt.show() + + def get_local_points(self, min_indx, points, look_forward): + remaining_points = len(points) - (min_indx + 1) + if remaining_points < look_forward: + look_forward = remaining_points + 1 + points = points[min_indx:min_indx + look_forward] + + return points + + +def main(args=None): + rclpy.init(args=args) + node = centerline_planner() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/planning/path_planning/path_planning/fasttube_without_kalman.py b/src/planning/path_planning/path_planning/fasttube_without_kalman.py new file mode 100644 index 00000000..77f0408c --- /dev/null +++ b/src/planning/path_planning/path_planning/fasttube_without_kalman.py @@ -0,0 +1,164 @@ +import rclpy +from rclpy.node import Node + +from moa_msgs.msg import Detections +from geometry_msgs.msg import Pose, PoseArray + +import numpy as np +import matplotlib.pyplot as plt +from fsd_path_planning import PathPlanner, MissionTypes, ConeTypes +import math + + +class centerline_planner(Node): + def __init__(self): + super().__init__("centerline_planner") + + # parameters + self._plot = True + self.look_forward = 4 + self.left_cones = [] + self.right_cones = [] + self.path_planner = PathPlanner(MissionTypes.trackdrive) + + # subscribers + self.subscription_cone_detection = self.create_subscription(Detections, 'cone_detection', self.cone_detection_callback, 10) + self.create_subscription(Pose, "car_position", self.set_car_position, 10) # car pose + + + # publishers + self.centerline_publisher = self.create_publisher(PoseArray, "selected_trajectory", 10) + + def set_car_position(self, msg:Pose) -> None: + self.car_pose = msg + self.loop() + + def cone_detection_callback(self, msg: Detections) -> None: + self.left_cones = msg.blue + self.right_cones = msg.yellow + + def loop(self) -> None: + self.get_logger().info(f"car pose recieved = {hasattr(self,'car_pose')}") + + if not hasattr(self,'car_pose'): + return + + lb, rb = self.get_boundaries() # get boundaries (list of [x,y] points) + + if not len(lb) > 0 or not len(rb) > 0: + return + + lblocal, _, _= self.get_next_points(lb,self.look_forward) # get local points (list of [x,y] points) close to car + rblocal, car_position, car_orientation = self.get_next_points(rb,self.look_forward) + + if not len(lblocal) > 0 or not len(rblocal) > 0: + return + + global_cones = self.get_global_cones(lblocal, rblocal) + car_direction = self.get_car_direction(car_orientation) + + path = self.path_planner.calculate_path_in_global_frame(global_cones, car_position, car_direction) # compute centerline + + centerline = path[:, 1:3] + + if not len(centerline) > 0: + return + + # publish + msg = self.get_posearray_msg(centerline) + self.centerline_publisher.publish(msg) + + self.plot(lb,rb,centerline,self._plot) # plot + + + def get_global_cones(self, lb, rb): + cones_by_type = [np.zeros((0, 2)) for _ in range(5)] + cones_by_type[ConeTypes.LEFT] = lb + cones_by_type[ConeTypes.RIGHT] = rb + return cones_by_type + + + def get_car_direction(self, car_orientation): + return np.array([-math.sin(car_orientation), math.cos(car_orientation)]) + + + # MAIN FUNCTIONS + def get_boundaries(self): + """Retrieves the left and right boundary from msg and returns as a [x,y] list""" + left_cones = np.array([[P.x, P.y] for P in self.left_cones]) + right_cones = np.array([[P.x, P.y] for P in self.right_cones]) + + return left_cones, right_cones + + def get_next_points(self,line,look_forward): + """Retrieves the points closest to the car + *ASSUMES THE points ARE SORTED/ORDERED + """ + car_point = np.array([self.car_pose.position.x,self.car_pose.position.y]) + car_orientation = self.car_pose.orientation.w + points = np.array(line) + + min_indx = np.argmin(np.linalg.norm(car_point-points, axis=1)) + # min_indx += self.is_behind_car(points[min_indx], car_point, car_orientation) + points = self.get_local_points(min_indx,points,look_forward) + + return points, car_point, car_orientation + + + def get_posearray_msg(self, line): + pose_array = PoseArray() + + for P in line: + pose = Pose() + pose.position.x = P[0] # assing x,y values to position of pose + pose.position.y = P[1] + pose_array.poses.append(pose) + + return pose_array + + + def plot(self,lb,rb,centerline,to_plot=False): + """Plots the boundary and centerline points""" + if to_plot: + # x, y points + lb = np.array(lb) + rb = np.array(rb) + centerline = np.array(centerline) + + lbx, lby = lb[:,0], lb[:,1] + rbx, rby = rb[:,0], rb[:,1] + centx, centy = centerline[:,0], centerline[:,1] + car_x, car_y = [self.car_pose.position.x, self.car_pose.position.y] + + # plot + plt.ion() + plt.clf() + + plt.plot(lbx,lby,'*b',label='left') + plt.plot(rbx,rby,'*y',label='right') + plt.plot(centx,centy,'-r',label='centerline') + plt.plot(0,0,'*k',label='car position') + + plt.pause(0.1) + plt.legend() + plt.show() + + + def get_local_points(self,min_indx,points,look_forward): + remaining_points = len(points) - (min_indx+1) # number of points forwards the car has detected + if remaining_points < look_forward: look_forward = remaining_points+1 + points = points[min_indx:min_indx+look_forward] # local points based on closest distance to car + + return points + + +def main(args=None): + rclpy.init(args=args) + node = centerline_planner() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/planning/path_planning/path_planning/reinforcement_learning/autonomous_RL.py b/src/planning/path_planning/path_planning/reinforcement_learning/autonomous_RL.py new file mode 100644 index 00000000..6c3f0370 --- /dev/null +++ b/src/planning/path_planning/path_planning/reinforcement_learning/autonomous_RL.py @@ -0,0 +1,243 @@ +#!/usr/bin/python3 + +# imports +import numpy as np + +import rclpy +from rclpy.node import Node +from moa_msgs.msg import Cone, ConeMap +from std_msgs.msg import Float32 +from geometry_msgs.msg import PoseArray, Pose + +import os +from gym import Env +from gym.spaces import Discrete, Box, Dict, Tuple, Sequence +from stable_baselines3 import PPO +from stable_baselines3.common.vec_env import DummyVecEnv +from stable_baselines3.common.evaluation import evaluate_policy + +class ReinforcementLearningEnv(Node, Env): + """reinforcement learning uses a policy to find out what action would be best for this state + The action is defined as the highest probability of it happening given this state and parameters - I THINK SO""" + def __init__ (self): + super().__init__("RL") + # ROS NODE CODE + self.declare_parameters( + namespace='', + parameters=[ + ('savebounds', False) + ] + ) + # attributes + self._saveBounds = self.get_parameter("savebounds").get_parameter_value().bool_value + self._learn = True + + # subscribers + self.create_subscription(ConeMap, "cone_map", self.callback, 10) + # publishers + self.best_steering_angle_pub = self.create_publisher(Float32, "moa/selected_steering_angle", 10) + self.path = self.create_publisher(PoseArray, "moa/chosen_actions", 10) + + # ENVIRONMENT NODE + # distance to centerline + self.action_space = Box(-45,45, shape=(1,)) + # observation? + self.observation_space = Box(-999,999,shape=(4,)) + # steering angle + self.steering_angle = 0 + self.episode_length = 100 + + def step(self, action): + # convert to radians + self.steering_angle = action[0] * np.pi / 180 + + # decrease run time + self.episode_length -= 1 + + # calculate reward + distance = self.getDistanceToCenter() + reward = -distance + + if self.episode_length <= 0: + done = True + else: + done = False + + info = {} + + # publish chosen angle + self.best_steering_angle_pub.publish(Float32(data=float(self.steering_angle))) + # self.get_logger().info(f'chosen action published {self.steering_angle}') + + return [self.center_point[0], self.center_point[1], distance, self.steering_angle], reward, done, info + + def render(self): + pass + + def reset(self): + self.steering_angle = 0 + # obs = Box(-999,999,shape=(1,)) + self.episode_length = 100 + + return self.steering_angle + + # helper methods + def getDistanceToCenter(self): + # get new point + new_point = self.getNewPoint() + + # return distance + return self.getDistance(new_point, self.center_point) + + def getNewPoint(self): + trajectory_output = PoseArray() + steering_angle = self.steering_angle + L = 1 + R = L / np.tan(steering_angle) + t_range = np.arange(0, np.pi, 0.01) + for i, individual_t in enumerate(t_range): + pose_input = Pose() + # pre transformation coordinates + x_pre_trans = np.cos(individual_t) * R - R + y_pre_trans = abs(np.sin(individual_t) * R) + # post transformation coordinates (relative to car) + current_location = [[self.car_location.position.x],[self.car_location.position.y]] + post_trans_point = self.applyTransformation(steering_angle, current_location, x_pre_trans, y_pre_trans) + x = post_trans_point[0][0] + y = post_trans_point[1][0] + pose_input.position.x = x + pose_input.position.y = y + trajectory_output.poses.append(pose_input) + + if i == 2: break + + # publish msg + # self.get_logger().info("path for action published") + self.path.publish(trajectory_output) + + return np.array([x,y]) + + def applyTransformation(self ,action, position_vector, x_pre, y_pre): + point = np.array([[x_pre], [y_pre]]) + rotation_matrix = self.getRotationMatrix(action) + # matrix multiplication for rotation then translate from car position + transformed_point = np.matmul(rotation_matrix, point) + position_vector + return transformed_point + + def getRotationMatrix(self, theta): + return np.array([[np.cos(theta), -np.sin(theta)],[np.sin(theta), np.cos(theta)]]) + + def getDistance(self, p1, p2): + return np.sqrt( sum( ( p1-p2 ) ** 2 ) ) + + + # ROS NODE + def callback(self, msg:ConeMap): + cones = msg.cones + # get car location + self.car_location = cones[0].pose.pose + + # loop through each cone + self._leftboundary = [] + self._rightboundary = [] + for i in range(len(cones)): + if i != 0: + x = cones[i].pose.pose.position.x + y = cones[i].pose.pose.position.y + # blue - left + if cones[i].colour == 0: + self._leftboundary.append([x,y]) + elif cones[i].colour == 2: + self._rightboundary.append([x,y]) + else: + self.car_pose = cones[i].pose.pose + + # get center point + self.center_point = self.getCenterPoint() + + # save coordinates for debugging + if self._saveBounds: + with open('/home/tanish/Documents/autonomous/src/planning/path_planning/path_planning/bound_coods', 'w') as fh: + xl=[i[0] for i in self._leftboundary] + yl=[i[1] for i in self._leftboundary] + xr=[i[0] for i in self._rightboundary] + yr=[i[1] for i in self._rightboundary] + for P in xl: + fh.write("{} ".format(P)) + fh.write("\n") + for P in yl: + fh.write("{} ".format(P)) + fh.write("\n") + for P in xr: + fh.write("{} ".format(P)) + fh.write("\n") + for P in yr: + fh.write("{} ".format(P)) + fh.write("\n") + fh.close() + self._saveBounds = False + + # REINFORCEMENT LEARNING + if self._learn: + # testing if envirnoment works + episodes = 5 + for episode in range(1, episodes+1): + self.reset() + done = False + score = 0 + + while not done: + self.render() + action = self.action_space.sample() + obs, reward, done, info = self.step(action) + score += reward + print(f"Episode: {episode} score: {score}") + + # self.env.close() + + log_path = os.path.join(os.getcwd(),'src/planning/path_planning/path_planning/Training','Logs') + # configure model + # env = DummyVecEnv([lambda: self]) + env = self + model = PPO('MlpPolicy', env, verbose=1, tensorboard_log=log_path, learning_rate=0.5) + + # model training + self.get_logger().info("TRAINING MODEL") + model.learn(total_timesteps=20000) + # save and load model + save_path = os.path.join(os.getcwd(),'src/planning/path_planning/path_planning/Training','SavedModels','model_ppo') + model.save(save_path) + del model + self.model = PPO.load(save_path, env) + + self._learn = False + + # model testing + print(evaluate_policy(self.model, env, n_eval_episodes=10)) + + # helper + def getCenterPoint(self): + left_point = np.array(self._leftboundary[0]) + right_point = np.array(self._rightboundary[0]) + + return (left_point + right_point) / 2 + + + +def main(args=None): + + rclpy.init(args=args) + + node = ReinforcementLearningEnv() + + rclpy.spin(node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/planning/path_planning/path_planning/simple_centerline_planner.py b/src/planning/path_planning/path_planning/simple_centerline_planner.py index a2980e20..dd4d0490 100644 --- a/src/planning/path_planning/path_planning/simple_centerline_planner.py +++ b/src/planning/path_planning/path_planning/simple_centerline_planner.py @@ -38,7 +38,7 @@ def center_line_publisher(self, msg: ConeMap): self.best_traj_pub.publish(center_line_path) - self.get_logger().info("Center line published") + #self.get_logger().info("Center line published") def get_bounds(self, msg: ConeMap): id = 1 diff --git a/src/planning/path_planning/path_planning/trajectory_generation.py b/src/planning/path_planning/path_planning/trajectory_generation.py deleted file mode 100644 index e7182558..00000000 --- a/src/planning/path_planning/path_planning/trajectory_generation.py +++ /dev/null @@ -1,188 +0,0 @@ -#!/usr/bin/python3 -import rclpy -import numpy as np -from rclpy.node import Node -from rclpy.executors import SingleThreadedExecutor - -from geometry_msgs.msg import Point, Pose, PoseArray -from moa_msgs.msg import ConeMap, AllStates, AllTrajectories -from ackermann_msgs.msg import AckermannDrive - -import numpy as np -import matplotlib.pyplot as plt - - -class trajectory_generator(Node): - def __init__(self): - super().__init__("trajectory_generation") - self.get_logger().info("Trajectory generation Node Started") - - self.declare_parameters( - namespace='', - parameters=[ - ('debug', True), - ('timer', 3.0), - ] - ) - - # attributes - self._id = 0 - self._debug = self.get_parameter('debug').get_parameter_value().bool_value - self._timer = self.get_parameter('timer').get_parameter_value().double_value - - # publishers - self.all_trajectories_publisher = self.create_publisher(AllTrajectories, "moa/trajectories", 10) - self.all_states_publisher = self.create_publisher(AllStates, "moa/states", 10) - - # subscribers - self.create_subscription(AckermannDrive, "moa/cur_vel", self.set_current_speed, 10) - self.create_subscription(ConeMap, "cone_map", self.set_cone_map, 10) - - # time in between trajectory generation - self.create_timer(self._timer, self.generate_trajectories) - - - def set_current_speed(self, msg:AckermannDrive) -> None: self._current_speed = msg.speed - - def set_cone_map(self, msg:ConeMap) -> None: self._cone_map = msg - - - # BEST TRAJECTORY PUBLISHER - def generate_trajectories(self): - '''Generates trajectories every few seconds''' - - if self._debug: self._current_speed = 0.0 - - self.get_logger().info(f"{self._timer} seconds up - generating trajectories") - self.get_logger().info(f"current speed: {hasattr(self,'_current_speed')}"\ - f" | cone map: {hasattr(self,'_cone_map')}") - - if hasattr(self,"_current_speed") and hasattr(self,"_cone_map"): - # generate trajectories - paths, states = self.my_trajectory_generator(cone_map=self._cone_map, radius=2, npoints=100) - - # publish states and trajectories - state_list = [] - for i in range(len(states)): - args = {"steering_angle": float(states[i]), - "steering_angle_velocity": 0.0, - "speed": self._current_speed, - "acceleration": 0.0, - "jerk": 0.0} - state_list.append(AckermannDrive(**args)) - - msg = AllStates(id = self._id, states = state_list) - self.all_states_publisher.publish(msg) - - msg = AllTrajectories(id = self._id, trajectories = paths) - self.all_trajectories_publisher.publish(msg) - - self.get_logger().info("msg published") - - self._id += 1 - - return - - return - - # =========================================================== - # # TRAJECTORY GENERATION - def my_trajectory_generator(self, cone_map, radius, npoints): - """generate straight trajectory based on given radius from origin (0,0)""" - trajectories = [] - # 1. initial point - first_cone = cone_map.cones[0].pose.pose.position - # cor = [first_cone.x, first_cone.y] - cor = [0,0] - # 2. radius - r = radius - # 3. x points - n = npoints - x = np.linspace(-r/3, r/3, n, endpoint=False)[1:] - n -= 1 - y = np.zeros(n) - angs = np.zeros(n) - - for i, val in enumerate(x): - tmp_path = PoseArray() - # append starting point - tmp_pose = Pose() - tmp_pose.position.x = first_cone.x - tmp_pose.position.y = first_cone.y - tmp_path.poses.append(tmp_pose) - - # 4. find y using equation of circle - y[i] = np.sqrt(np.round(r**2-(val-cor[0])**2, 3)) + cor[1] - # 5. calculate steering angle for each trajectory - dy = y[i] - cor[1] - dx = val - cor[0] - # inverse tan is in radians - angle = np.arctan(dx/dy) - angs[i] = (angle if dx < 0 else angle) - print(f"passed angle = {angs[i]} for dx = {dx}") - - # transform coordinates from fixed to car - car_position = self.get_position_of_cart(cone_map) - car_position, rotation_matrix = self.get_transformation_matrix(car_position) - post_trans_pose = self.apply_transformation(car_position, rotation_matrix, val, y[i]) - # append new points to pose array - x[i] = post_trans_pose[0][0] - y[i] = post_trans_pose[1][0] - tmp_pose2 = Pose() - tmp_pose2.position.x = x[i] - tmp_pose2.position.y = y[i] - tmp_path.poses.append(tmp_pose2) - - if x[i] is np.nan or y[i] is np.nan or angs[i] is np.nan: - print("NAN!") - - # plotting - # plt.plot([cor[0],x[i]],[cor[1],y[i]],label=f'trajectories') - - # append trajecotry - trajectories.append(tmp_path) - - # plt.show() - - # list of points as tuples - points = [(x[i],y[i]) for i in range(n)] - - return trajectories, angs - - def get_position_of_cart(self, cone_map): - # first cone - localization_data = cone_map.cones[0] - x = localization_data.pose.pose.position.x - y = localization_data.pose.pose.position.y - theta = localization_data.pose.pose.orientation.w - return x, y, theta - - def get_transformation_matrix(self, position_and_orientation): - # theta = position_and_orientation[2] - np.pi/2 - cart_x = position_and_orientation[0] - cart_y = position_and_orientation[1] - theta = position_and_orientation[2] - # 2d trasformation matrix - rotation_matrix = np.array([[np.cos(theta), -np.sin(theta)],[np.sin(theta), np.cos(theta)]]) - position_vector = np.array([[cart_x], [cart_y]]) - - return position_vector, rotation_matrix - - def apply_transformation(self, position_vector, rotation_matrix, point_x, point_y): - point = np.array([[point_x], [point_y]]) - # matrix multiplication for rotation then translate from car position - transformed_point = np.matmul(rotation_matrix, point) + position_vector - return transformed_point - - -def main(): - rclpy.init() - exe = SingleThreadedExecutor() - node = trajectory_generator() - exe.add_node(node) - exe.spin() - - rclpy.shutdown() - -if __name__ == "__main__": - main() diff --git a/src/planning/path_planning/path_planning/trajectory_generator_HRHCS.py b/src/planning/path_planning/path_planning/trajectory_generator_HRHCS.py deleted file mode 100644 index de1c3c5e..00000000 --- a/src/planning/path_planning/path_planning/trajectory_generator_HRHCS.py +++ /dev/null @@ -1,164 +0,0 @@ -#!/usr/bin/python3 - -import rclpy -import numpy as np -from rclpy.node import Node -from rclpy.executors import SingleThreadedExecutor - -from geometry_msgs.msg import Pose, PoseArray -from moa_msgs.msg import ConeMap, AllStates, AllTrajectories -from ackermann_msgs.msg import AckermannDrive - - -class trajectory_generator(Node): - def __init__(self): - super().__init__("trajectory_generation") - self.get_logger().info("Trajectory generation Node Started") - - self.declare_parameters( - namespace='', - parameters=[ - ('debug', False), - ('timer', 3.0), - ] - ) - - # attributes - self._id = 0 - self._debug = self.get_parameter('debug').get_parameter_value().bool_value - self._timer = self.get_parameter('timer').get_parameter_value().double_value - - # publishers - self.all_trajectories_publisher = self.create_publisher(AllTrajectories, "moa/trajectories", 10) - self.all_states_publisher = self.create_publisher(AllStates, "moa/states", 10) - - # subscribers - self.create_subscription(AckermannDrive, "moa/cur_vel", self.set_current_speed, 10) - self.create_subscription(ConeMap, "cone_map", self.set_cone_map, 10) - - # time in between trajectory generation - self.create_timer(self._timer, self.generate_trajectories) - - - def set_current_speed(self, msg:AckermannDrive) -> None: self._current_speed = msg.speed - - def set_cone_map(self, msg:ConeMap) -> None: self._cone_map = msg - - - # BEST TRAJECTORY PUBLISHER - def generate_trajectories(self): - '''Generates trajectories every few seconds''' - - if self._debug: self._current_speed = 0.0 - - self.get_logger().info(f"{self._timer} seconds up - generating trajectories") - self.get_logger().info(f"current speed: {hasattr(self,'_current_speed')}"\ - f" | cone map: {hasattr(self,'_cone_map')}") - - if hasattr(self,"_current_speed") and hasattr(self,"_cone_map"): - # generate trajectories - paths, states = self.trajectory_generator(self._cone_map) - # publish states and trajectories - state_list = [] - for i in range(len(states)): - args = {"steering_angle": float(states[i]), - "steering_angle_velocity": 0.0, - "speed": self._current_speed, - "acceleration": 0.0, - "jerk": 0.0} - state_list.append(AckermannDrive(**args)) - - msg = AllStates(id = self._id, states = state_list) - self.all_states_publisher.publish(msg) - - msg = AllTrajectories(id = self._id, trajectories = paths) - self.all_trajectories_publisher.publish(msg) - - self.get_logger().info("msg published") - - self._id += 1 - - return - - return - - # =========================================================== - # TRAJECTORY GENERATION - def single_trajectory_generator(self, steering_angle, position_vector, rotation_matrix): - # https://dingyan89.medium.com/simple-understanding-of-kinematic-bicycle-model-81cac6420357 is used for - # bicycle steering - L = 1; - R = L / np.tan(steering_angle); - # list of time in space - t_range = np.arange(0, np.pi/6, 0.01); - trajectory_output = PoseArray(); - for i, individual_t in enumerate(t_range): - # pose for current time in space - pose_input = Pose(); - # pre transformation coordinates - x_pre_trans = np.cos(individual_t) * R - R - y_pre_trans = abs(np.sin(individual_t) * R) - # post transformation coordinates (relative to car) - post_trans_point = self.apply_transformation(position_vector, rotation_matrix, x_pre_trans, y_pre_trans); - pose_input.position.x = post_trans_point[0][0] - pose_input.position.y = post_trans_point[1][0] - trajectory_output.poses.append(pose_input) - return trajectory_output - - def trajectory_generator(self, cone_map): - # steering angles - candidate_steering_angle = np.deg2rad(np.arange(-10, 10, 0.5)) - trajectories = [] - # get position of car (first cone in cone map data) - position_and_orientation = self.get_position_of_cart(cone_map) - # get transformation matrix - position_vector, rotation_matrix = self.get_transformation_matrix(position_and_orientation) - for steering_angle in candidate_steering_angle: - if steering_angle == 0: - steering_angle = 1e-6; - # generate trajectory for this angle - added_trajectory = self.single_trajectory_generator(steering_angle, position_vector, rotation_matrix) - trajectories.append(added_trajectory) - - return trajectories, candidate_steering_angle - - def get_position_of_cart(self, cone_map): - # first cone - localization_data = cone_map.cones[0] - x = localization_data.pose.pose.position.x - y = localization_data.pose.pose.position.y - theta = localization_data.pose.pose.orientation.w - return x, y, theta - - def get_transformation_matrix(self, position_and_orientation): - # theta = position_and_orientation[2] - np.pi/2 - cart_x = position_and_orientation[0] - cart_y = position_and_orientation[1] - theta = position_and_orientation[2] - # 2d trasformation matrix - rotation_matrix = np.array([[np.cos(theta), -np.sin(theta)],[np.sin(theta), np.cos(theta)]]) - position_vector = np.array([[cart_x], [cart_y]]) - - return position_vector, rotation_matrix - - def apply_transformation(self, position_vector, rotation_matrix, point_x, point_y): - point = np.array([[point_x], [point_y]]) - # matrix multiplication for rotation then translate from car position - transformed_point = np.matmul(rotation_matrix, point) + position_vector - return transformed_point - - -def main(): - rclpy.init() - exe = SingleThreadedExecutor() - node = trajectory_generator() - exe.add_node(node) - exe.spin() - - rclpy.shutdown() - -if __name__ == "__main__": - main() - - - diff --git a/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py b/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py deleted file mode 100644 index 7cbb8ec9..00000000 --- a/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py +++ /dev/null @@ -1,591 +0,0 @@ -#!/usr/bin/python3 -import numpy as np -from shapely import LineString, MultiPoint -from shapely import Point as shapelyPoint -from scipy import interpolate -import os - -import rclpy -from rclpy.node import Node -from rclpy.executors import SingleThreadedExecutor - -from std_msgs.msg import Header, Float32, Int16, Int32MultiArray -from builtin_interfaces.msg import Time -from geometry_msgs.msg import Pose, PoseArray, Point -from moa_msgs.msg import Cone, ConeMap, BoundaryStamped, AllTrajectories, AllStates -from ackermann_msgs.msg import AckermannDrive -from builtin_interfaces.msg import Time - - -class trajectory_optimization(Node): - def __init__(self): - super().__init__("trajectory_optimisation") - self.get_logger().info("Trajectory Optimisation Node Started") - - self.declare_parameters( - namespace='', - parameters=[ - ('debug', True) - ] - ) - - # attributes - self._debug = self.get_parameter('debug').get_parameter_value().bool_value - self._once = True - - # subscribers - self.create_subscription(AllStates, "moa/states", self.set_states, 10) - self.create_subscription(AllTrajectories, "moa/trajectories", self.delete_optimise_trajectories, 10) - self.create_subscription(AckermannDrive, "moa/cur_vel", self.set_current_speed, 10) - # self.create_subscription(BoundaryStamped, "track/bound_l", self.set_left_boundary, 10) - # self.create_subscription(BoundaryStamped, "track/bound_r", self.set_right_boundary, 10) - # only debugging - self.create_subscription(ConeMap, "cone_map", self.set_boundaries, 10) - self.best_steering_angle_pub = self.create_publisher(Float32, "moa/selected_steering_angle", 10) - - # publishers - # self.best_trajectory_publisher = self.create_publisher(AckermannDrive, "moa/selected_trajectory", 10) - self.best_trajectory_publisher = self.create_publisher(PoseArray, "moa/selected_trajectory", 10) - self.within_boundary_trajectories_publisher = self.create_publisher(AllTrajectories, 'moa/inbound_trajectories', 10) - self.best_trajectory_index = self.create_publisher(Int16, "moa/best_trajectory_index", 10) - self.out_of_bounds_indicies = self.create_publisher(Int32MultiArray, "moa/out_of_bounds", 10) - - - def set_states(self, msg: AllStates) -> None: self._state_msg = msg - - def set_current_speed(self, msg: AckermannDrive) -> None: self._current_speed = msg.speed - - def set_left_boundary(self, msg: BoundaryStamped) -> None: self._leftboundary = msg.coords - - def set_right_boundary(self, msg: BoundaryStamped) -> None: self._rightboundary = msg.coords - - def set_boundaries(self, msg: ConeMap): - if self._debug: - cones = msg.cones - # loop through each cone - leftboundary = [] - rightboundary = [] - for i in range(len(cones)): - if i != 0: - x = cones[i].pose.pose.position.x - y = cones[i].pose.pose.position.y - # blue - left - if cones[i].colour == 0: - leftboundary.append([x,y]) - elif cones[i].colour == 2: - rightboundary.append([x,y]) - - # get correspnoding right boundary based on left boundary - x1, y1, x2, y2 = leftboundary[0][0], leftboundary[0][1], rightboundary[0][0], rightboundary[0][1] - track_width = self.get_distance(x1, y1, x2, y2) - # get right boundary - # leftboundary = self.get_left_boundary(rightboundary, track_width) - print(f"left boundary = {len(leftboundary)}, right boundary = {len(rightboundary)}") - # adjust boundaries - # self._leftboundary, self._rightboundary = self.get_adjusted_boundaries(leftboundary, rightboundary) - self._leftboundary = leftboundary - self._rightboundary = rightboundary - - # print coordinate lists - if self._once: - with open(f'{os.path.dirname(__file__)}/bound_coods', 'w') as fh: - xl=[i[0] for i in self._leftboundary] - yl=[i[1] for i in self._leftboundary] - xr=[i[0] for i in self._rightboundary] - yr=[i[1] for i in self._rightboundary] - for P in xl: - fh.write("{} ".format(P)) - fh.write("\n") - for P in yl: - fh.write("{} ".format(P)) - fh.write("\n") - for P in xr: - fh.write("{} ".format(P)) - fh.write("\n") - for P in yr: - fh.write("{} ".format(P)) - fh.write("\n") - fh.close() - - with open(f'{os.path.dirname(__file__)}/bound_coods2', 'w') as fh: - xl=[i[0] for i in leftboundary] - yl=[i[1] for i in leftboundary] - xr=[i[0] for i in rightboundary] - yr=[i[1] for i in rightboundary] - for P in xl: - fh.write("{} ".format(P)) - fh.write("\n") - for P in yl: - fh.write("{} ".format(P)) - fh.write("\n") - for P in xr: - fh.write("{} ".format(P)) - fh.write("\n") - for P in yr: - fh.write("{} ".format(P)) - fh.write("\n") - fh.close() - # self._once = False - - def get_left_boundary(self, rightboundary, track_width): - angle = -90 * np.pi / 180 - leftboundary = [] - # loop through all left boundary points - for i in range(len(rightboundary)): - P = rightboundary[i] - # check what point is - if i == 0: - # forward point - vector = self.get_vector(rightboundary[i], rightboundary[i+1], True) - elif i == len(rightboundary)-1: - # backward point - vector = self.get_vector(rightboundary[i-1], rightboundary[i], True) - else: - # centeral points - vector = self.get_vector(rightboundary[i-1], rightboundary[i+1], True) - - # rotate vector - vector = self.get_rotated_vector(angle,vector) - # compute new point - new_point = P + track_width * vector - # append new point - leftboundary.append(new_point) - - return leftboundary - - # def get_adjusted_boundaries(self, leftboundaryI, rightboundaryI): - # # distance away from boundaries - # distance = 3 - # # left and right boundary lists - # leftboundary = [] - # rightboundary = [] - # # each point on the boundary - # num_points = len(leftboundaryI) - # # left - # for i in range(num_points-10): - # # if i != num_points-1: - # # # get left and right unit vector - forward - # # left_vector = self.get_vector(leftboundaryI[i], leftboundaryI[i+1], True) - # # else: - # # # backward - # # left_vector = self.get_vector(leftboundaryI[i], leftboundaryI[i-1], True) - # left_vector = self.get_vector(leftboundaryI[i], leftboundaryI[i+1], True) - - # # once vectors are found - rotate them and get new point - # angle = 90 * np.pi / 180 - # left_vector = self.get_rotated_vector(angle, left_vector) - - # # append new point - # new_left = leftboundaryI[i] + distance * left_vector - - # leftboundary.append(new_left) - - # num_points = len(rightboundaryI) - # for i in range(num_points-10): - # # if i < num_points-1: - # # # get left and right unit vector - forward - # # right_vector = self.get_vector(rightboundaryI[i], rightboundaryI[i+1], True) - # # else: - # # # backward - # # right_vector = self.get_vector(rightboundaryI[i-1], rightboundaryI[i], True) - # right_vector = self.get_vector(rightboundaryI[i], rightboundaryI[i+1], True) - - # # once vectors are found - rotate them and get new point - # angle = -90 * np.pi / 180 - # right_vector = self.get_rotated_vector(angle, right_vector) - - # # append new point - # new_right = rightboundaryI[i] + distance * right_vector - - # rightboundary.append(new_right) - - # return leftboundary, rightboundary - - - def get_vector(self, p1, p2, unit=True): - '''calculates point vector based on slope''' - vector = np.array(p2)-np.array(p1) - if unit: - return vector / self.get_magnitude(vector) - - return vector - - def get_rotated_vector(self, angle, vector): - # rotation matrix - rotate = np.array([[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]]) - - return np.dot(rotate,np.array(vector)) - - def get_magnitude(self, vector): - return np.sqrt(sum(vector**2)) - - - def delete_optimise_trajectories(self, msg: AllTrajectories): - '''deletes trajectories then optimises to find the best path for car''' - - if self._debug: self._current_speed = 0.0 - self.get_logger().info(f"all states: {hasattr(self,'_state_msg')}" - f" | current speed: {hasattr(self,'_current_speed')}" \ - f" | left boundaries: {hasattr(self,'_leftboundary')}"\ - f" | right boundaries: {hasattr(self,'_rightboundary')}") - - if hasattr(self,"_state_msg") and hasattr(self,"_current_speed") \ - and hasattr(self,"_leftboundary") and hasattr(self,"_rightboundary"): - # check ids - if self._state_msg.id != msg.id: - self.get_logger().info(f"Ids state:{self._state_msg.id} and trajectory:{msg.id} do not match") - # return - - # set trajectories and states as a list - # states = [self._state_msg.states[i].steering_angle for i in range(len(self._state_msg.states))] - states = [command.steering_angle for command in self._state_msg.states] - trajectories = msg.trajectories - - self.get_logger().info(f"number of paths before deletion = {len(trajectories)}") - self.trajectory_deletion(trajectories, states) - self.get_logger().info(f"number of paths after deletion = {len(trajectories)}") - best_trajectory_idx = self.optimisation(trajectories, states) - - # check for no trajectories - if best_trajectory_idx == None: - self.get_logger().info("no valid trajectories found") - return - self.get_logger().info(f"best state is = {states[best_trajectory_idx]}") - - # publish best trajectory - args = {"header": Header(stamp=Time(sec=0,nanosec=0), frame_id='best_trajectory'), - "poses": trajectories[best_trajectory_idx].poses} - posearray_msg = PoseArray(**args) - self.best_trajectory_publisher.publish(posearray_msg) - - # publish valid (within boundaries) trajectories including center line - ps = [Pose(position=Point(x=P[0], y=P[1], z=0.0)) for P in self._center_line_coordinates] - trajectories.append(PoseArray(poses=ps)) - alltrajectories_msg = { - "id": msg.id, - "trajectories": trajectories - } - self.within_boundary_trajectories_publisher.publish(AllTrajectories(**alltrajectories_msg)) - - # publish within boundary trajectory states - # states_msg = [] - # for sta in states: - # sargs = {"steering_angle": sta, - # "steering_angle_velocity": 0.0, - # "speed": self._current_speed, - # "acceleration": 0.0, - # "jerk": 0.0} - # states_msg.append(AckermannDrive(**sargs)) - # self.within_boundary_states_publisher.publish(AllStates(id=msg.id, states=states_msg)) - - # publish best steering angle - self.best_steering_angle_pub.publish(Float32(data=states[best_trajectory_idx])) - - self.get_logger().info("msg published") - - return - - return - - # =========================================================== - # TRAJECTORY DELETION - def trajectory_deletion(self, trajectories, states): - '''Caller for trajectory deletion if there are trajectories''' - - self.get_logger().info("Trajectory Deletion Started") - self.set_within_boundary_trajectories(trajectories, states) - - def set_within_boundary_trajectories(self, trajectories, states): - '''Deletes trajectories that are on track boundaries''' - remove_trajectories_indices = [] - - # if shapely code fails use this below (not 100%) - # for i, T in enumerate(trajectories): - # if i == 25: - # print("stop here") - # for cpose in T.poses: - # x = cpose.position.x - # y = cpose.position.y - # # compare points with boundaries - # onBound = self.compare_with_boundary(x,y,2) - # if onBound: - # rm_inds.append(i) - # # trajectories.pop([i for i in range(len(trajectories)) if T==trajectories[i]][0]) - # break - - # shapely library - self._left_boundary_linestring = LineString([(P[0], P[1]) for P in self._leftboundary]) - - pts_list = [(P[0], P[1]) for P in self._rightboundary] - if self._debug: - pts_list.pop(0) - self._right_boundary_linestring = LineString(pts_list) - - # track width - track_width = self.get_track_width() - - for i in range(len(trajectories)): - # trajectory line string - trajectory = LineString([(P.position.x, P.position.y) for P in trajectories[i].poses]) - - if i == len(trajectories)/2 - 1: - print('the long one') - - tmp = None - # check intersection - if trajectory.intersects(self._left_boundary_linestring): - # get intersection point/s - tmp = trajectory.intersection(self._left_boundary_linestring) - intersection = "left" - - elif trajectory.intersects(self._right_boundary_linestring): - tmp = trajectory.intersection(self._right_boundary_linestring) - intersection = "right" - - if tmp is not None: - # new pose list - list_of_poses = [] - if type(tmp) is MultiPoint: - tmp_x = tmp.centroid.x - tmp_y = tmp.centroid.y - else: - tmp_x = tmp.x - tmp_y = tmp.y - # go through each pose - for j, P in enumerate(trajectories[i].poses): - # get distance between points - # distance = self.get_distance(x1=tmp_x, y1=tmp_y, x2=P.position.x, y2=P.position.y) - in_bounds = self.get_in_of_bounds(inter=intersection,x1=tmp_x,y1=tmp_y,x2=P.position.x,y2=P.position.y) - if in_bounds: - list_of_poses.append(P) - else: - if len(list_of_poses) < 2: - remove_trajectories_indices.append(i) - else: - trajectories[i].poses = list_of_poses - break - - # publish indicies - print(f"removed indices = {remove_trajectories_indices}") - self.out_of_bounds_indicies.publish(Int32MultiArray(data=remove_trajectories_indices)) - - [(trajectories.pop(index), states.pop(index)) for index in list(reversed(remove_trajectories_indices))] - - def get_distance(self,x1,y1,x2,y2): - return np.sqrt(((x2-x1)**2 + (y2-y1)**2)) - - def get_in_of_bounds(self,inter,x1,y1,x2,y2): - if inter == "right": - return x2x1 and y2 shapelyPoint: - return shapelyPoint(x,y) - - def get_shapely_linestring(self, poses) -> LineString: - return LineString([(P.position.x, P.position.y) for P in poses]) - - def get_track_width(self): - return self._right_boundary_linestring.distance(self._left_boundary_linestring) - - def get_center_line(self): - '''approximates the track's center line''' - # the midpoint is the average of the coordinates - coods = [] - num_cones = min(len(self._leftboundary), len(self._rightboundary)) - for i in range(num_cones): - x1 = self._leftboundary[i][0] - y1 = self._leftboundary[i][1] - x2 = self._rightboundary[i][0] - y2 = self._rightboundary[i][1] - coods.append(self.get_avg_point(x1,y1,x2,y2)) - - # # perform extrapolation to extend line - # func = interpolate.interp1d([P[0] for P in coods], [P[1] for P in coods], kind='cubic', fill_value='extrapolate') - - # into_future_points = 0 - # into_future_distance = 0 - - # # track direction - # if coods[-1][0] > coods[-2][0]: - # # positive/right - # direction = 1 - # elif coods[-1][0] < coods[-2][0]: - # # negative/left - # direction = -1 - # elif abs(coods[-1][0] - coods[-2][0]) <= 1e-3: - # # straight - # into_future_distance = 0 - # into_future_points = 0 - - # if into_future_points != 0: - # xnew = [] - # into_future_distance += direction - # for i in range(into_future_points): - # xnew.append(coods[-1][0] + into_future_distance) - # into_future_distance += direction - # # get new pts - # ynew = func(xnew) - # # add approximated coordinates to center line - # for i in range(into_future_points): - # coods.append((xnew[i],ynew[i])) - - return LineString(coods), coods - - def get_avg_point(self, x1, y1, x2, y2): - return (((x1 + x2)/2), ((y1 + y2)/2)) - - def get_geometry_distance(self, geometry1, geometry2): - '''calculates the eucledian distance bewteen two shapely geometries''' - # this is to calculate trajectory length using only poses/points (DECAP) - # poses = trajectory.poses - # p0 = np.array([poses[0].position.x, poses[0].position.y]) - # p1 = np.array([poses[1].position.x, poses[1].position.y]) - # intL = np.linalg.norm(p1-p0,ord=2) - # arc_length = (len(poses)-1) * intL - - return geometry1.distance(geometry2) - # return frechet_distance(geometry1, geometry2) - - def get_best_trajectory_index(self, trajectory_lengths, trajectory_distances): - try: - objective_function = trajectory_distances - idx = int(np.ceil(np.argmin(objective_function))) - self.best_trajectory_index.publish(Int16(data=idx)) - return idx - except ValueError: - return None - - def DEBUG_generate_trajectories(self, n): - # list of all trajectories and states (for now just steering angle in rads) - all_traj = [] - all_states = -np.random.random(n) + np.random.random(n) - # make n pose arrays - x = y = z = 0.0 - for i in range(n): - # contains poses for ith pose array - temp = [] - # make m poses with random coordinates - for j in range(3): - pose = Pose() - pose.position.x, pose.position.y = x, y - # pose_array.poses = pose - temp.append(pose) - # calculate new x, y, z sqrt((x2-x1)^2+(y2-y1)^2) = length with x2 unknown - x = np.sqrt(0.1**2) + x - pose_array = PoseArray() - pose_array.poses = temp - # append pose array to all trajectories - all_traj.append(pose_array) - - return all_traj, all_states - -def main(): - rclpy.init() - exe = SingleThreadedExecutor() - node = trajectory_optimization() - exe.add_node(node) - exe.spin() - - rclpy.shutdown() - -if __name__ == "__main__": - main() - - - - - - - - diff --git a/src/planning/path_planning/setup.py b/src/planning/path_planning/setup.py index 4df2fe25..194d6d08 100644 --- a/src/planning/path_planning/setup.py +++ b/src/planning/path_planning/setup.py @@ -24,10 +24,11 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'trajectory_generator = path_planning.trajectory_generator_HRHCS:main', + 'centerline_planner = path_planning.centerline_planner:main', 'trajectory_optimisation = path_planning.trajectory_optimisation_CS:main', 'center_line = path_planning.simple_centerline_planner:main', - 'trajectory_generation = path_planning.trajectory_generation:main' + 'fasttube = path_planning.fasttube_planner:main', + 'fasttube_without_kalman = path_planning.fasttube_without_kalman:main' ], }, ) diff --git a/src/visualisation/base_tf/base_tf/__init__.py b/src/visualisation/base_tf/base_tf/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/visualization/foxglove_visualizer/base_tf/base_tf/base_tf.py b/src/visualisation/base_tf/base_tf/base_tf.py similarity index 100% rename from src/visualization/foxglove_visualizer/base_tf/base_tf/base_tf.py rename to src/visualisation/base_tf/base_tf/base_tf.py diff --git a/src/visualization/foxglove_visualizer/base_tf/package.xml b/src/visualisation/base_tf/package.xml similarity index 100% rename from src/visualization/foxglove_visualizer/base_tf/package.xml rename to src/visualisation/base_tf/package.xml diff --git a/src/visualisation/base_tf/resource/base_tf b/src/visualisation/base_tf/resource/base_tf new file mode 100644 index 00000000..e69de29b diff --git a/src/visualization/foxglove_visualizer/base_tf/setup.cfg b/src/visualisation/base_tf/setup.cfg similarity index 100% rename from src/visualization/foxglove_visualizer/base_tf/setup.cfg rename to src/visualisation/base_tf/setup.cfg diff --git a/src/visualization/foxglove_visualizer/base_tf/setup.py b/src/visualisation/base_tf/setup.py similarity index 100% rename from src/visualization/foxglove_visualizer/base_tf/setup.py rename to src/visualisation/base_tf/setup.py diff --git a/src/visualisation/base_tf/test/test_copyright.py b/src/visualisation/base_tf/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/src/visualisation/base_tf/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/visualisation/base_tf/test/test_flake8.py b/src/visualisation/base_tf/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/visualisation/base_tf/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/visualisation/base_tf/test/test_pep257.py b/src/visualisation/base_tf/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/visualisation/base_tf/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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/src/visualisation/cone_map_foxglove_visualiser/cone_map_foxglove_visualiser/__init__.py b/src/visualisation/cone_map_foxglove_visualiser/cone_map_foxglove_visualiser/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/cone_map_foxglove_visualizer/visualizer.py b/src/visualisation/cone_map_foxglove_visualiser/cone_map_foxglove_visualiser/visualise_cone_map.py similarity index 63% rename from src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/cone_map_foxglove_visualizer/visualizer.py rename to src/visualisation/cone_map_foxglove_visualiser/cone_map_foxglove_visualiser/visualise_cone_map.py index d78e719e..10320335 100644 --- a/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/cone_map_foxglove_visualizer/visualizer.py +++ b/src/visualisation/cone_map_foxglove_visualiser/cone_map_foxglove_visualiser/visualise_cone_map.py @@ -5,34 +5,53 @@ from visualization_msgs.msg import MarkerArray from geometry_msgs.msg import Vector3, Pose from geometry_msgs.msg import TransformStamped -from moa_msgs.msg import ConeMap -from moa_msgs.msg import Cone +from moa_msgs.msg import Track +from moa_msgs.msg import Detections import numpy as np class ConePublisher(Node): def __init__(self): + self.left_cones = [] + self.right_cones = [] + super().__init__('cone_publisher') self.markers_publisher_ = self.create_publisher(MarkerArray, 'visualization_marker_cones', 10) self.localization_marker_publisher = self.create_publisher(MarkerArray, 'visualization_marker_car', 10) self.frame_publisher_ = self.create_publisher(TransformStamped, 'base_tf', 10) - self.subscription_cone_map = self.create_subscription(ConeMap, 'cone_map', self.cone_map_callback, 10) + self.detections_publisher = self.create_publisher(MarkerArray, 'visualization_marker_detections', 10) + + + self.detections_subscriber = self.create_subscription(Detections, 'cone_detection', self.detections_callback, 10) + self.subscription_left_cone_map = self.create_subscription(Track, 'left_track', self.left_cone_map_callback, 10) + self.subscription_right_cone_map = self.create_subscription(Track, 'right_track', self.right_cone_map_callback, 10) self.subscription_localization = self.create_subscription(Pose, 'car_position', self.localization_callback, 10) - #self.marker_timer = self.create_timer(1, self.publish_cones) - #self.frame_timer = self.create_timer(1, self.publish_transform) + self.get_logger().info("Cone Map Visualization Initialization Completed") + + def left_cone_map_callback(self, msg): + self.left_cones = msg.cones - def cone_map_callback(self, msg): - # self.get_logger().info('Mapped result: "%s"' % msg.cones) - list_of_cones = msg.cones - local_cone = msg.cones[0].pose.pose - self.localization_callback(local_cone) - self.publish_cones(list_of_cones) - # self.get_logger().info('Cone map visualizing data published') + def right_cone_map_callback(self, msg): + self.right_cones = msg.cones def localization_callback(self, msg): + self.publish_cones() self.publish_transform(msg) self.publish_car(msg) - # self.get_logger().info('Car position visualizing data published') + + def detections_callback(self, msg): + Markers = MarkerArray() + is_localization = False + id_assign = 1 + Markers.markers.append(self.delete_all_cone()) + colour = 0 + for cone in msg.yellow: + Markers.markers.append(self.convert_to_visualization(cone, is_localization, id_assign, colour)) + id_assign += 1 + for cone in msg.blue: + Markers.markers.append(self.convert_to_visualization(cone, is_localization, id_assign, colour+2)) + id_assign += 1 + self.detections_publisher.publish(Markers) def convert_rotation_to_quaternion(self, angle): qw = np.cos(angle / 2) @@ -58,7 +77,6 @@ def publish_transform(self, localization_pose): t.transform.rotation.z = self.convert_rotation_to_quaternion(localization_pose.orientation.w)[2] t.transform.rotation.w = self.convert_rotation_to_quaternion(localization_pose.orientation.w)[3] - #self.broadcaster.sendTransform(t) self.frame_publisher_.publish(t) def publish_car(self, pose_of_car): @@ -66,23 +84,30 @@ def publish_car(self, pose_of_car): is_localization = True id_assign = 0 Markers.markers.append(self.delete_all_cone()) - - car_cone = Cone() - car_cone.pose.pose = pose_of_car - Markers.markers.append(self.convert_to_visualization(car_cone, is_localization, id_assign)) + Markers.markers.append(self.convert_to_visualization(pose_of_car.position, is_localization, id_assign, colour=3)) + Markers.markers[1].pose.orientation.x = self.convert_rotation_to_quaternion(pose_of_car.orientation.w)[0] + Markers.markers[1].pose.orientation.y = self.convert_rotation_to_quaternion(pose_of_car.orientation.w)[1] + Markers.markers[1].pose.orientation.z = self.convert_rotation_to_quaternion(pose_of_car.orientation.w)[2] + Markers.markers[1].pose.orientation.w = self.convert_rotation_to_quaternion(pose_of_car.orientation.w)[3] self.localization_marker_publisher.publish(Markers) - def publish_cones(self, rest_of_cones): + def publish_cones(self): Markers = MarkerArray() is_localization = False id_assign = 1 Markers.markers.append(self.delete_all_cone()) - for cone in rest_of_cones[1:]: - Markers.markers.append(self.convert_to_visualization(cone, is_localization, id_assign)) + colour = 0 + for cone in self.left_cones: + Markers.markers.append(self.convert_to_visualization(cone, is_localization, id_assign, colour)) + id_assign += 1 + for cone in self.right_cones: + Markers.markers.append(self.convert_to_visualization(cone, is_localization, id_assign, colour+2)) id_assign += 1 + + # print("COunt: ", len(Markers.markers)) self.markers_publisher_.publish(Markers) - def convert_to_visualization(self, cone, is_localization, id_assign): + def convert_to_visualization(self, cone_point, is_localization, id_assign, colour): marker = Marker() marker.header.frame_id = "global_frame" # Adjust the frame ID as needed marker.header.stamp = self.get_clock().now().to_msg() @@ -92,18 +117,16 @@ def convert_to_visualization(self, cone, is_localization, id_assign): marker.type = Marker.CUBE marker.action = Marker.ADD - marker.pose.position.x = cone.pose.pose.position.x - marker.pose.position.y = cone.pose.pose.position.y - marker.pose.position.z = cone.pose.pose.position.z + marker.pose.position = cone_point if is_localization: # Need to do later - marker.pose.orientation.x = self.convert_rotation_to_quaternion(cone.pose.pose.orientation.w)[0] - marker.pose.orientation.y = self.convert_rotation_to_quaternion(cone.pose.pose.orientation.w)[1] - marker.pose.orientation.z = self.convert_rotation_to_quaternion(cone.pose.pose.orientation.w)[2] - marker.pose.orientation.w = self.convert_rotation_to_quaternion(cone.pose.pose.orientation.w)[3] + # marker.pose.orientation.x = self.convert_rotation_to_quaternion(cone.orientation.w)[0] + # marker.pose.orientation.y = self.convert_rotation_to_quaternion(cone.orientation.w)[1] + # marker.pose.orientation.z = self.convert_rotation_to_quaternion(cone.orientation.w)[2] + # marker.pose.orientation.w = self.convert_rotation_to_quaternion(cone.orientation.w)[3] - marker.scale = Vector3(x=2.0, y=1.0, z=1.0) # Scale of the cone (x, y, z) + marker.scale = Vector3(x=0.5, y=0.2, z=0.3) # Scale of the cone (x, y, z) marker.color.r = 1.0 marker.color.g = 0.0 @@ -118,19 +141,19 @@ def convert_to_visualization(self, cone, is_localization, id_assign): marker.scale = Vector3(x=0.1, y=0.1, z=0.1) # Scale of the cone (x, y, z) - if cone.colour == 0: + if colour == 0: # Blue marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 marker.color.a = 1.0 # Alpha (opacity) - elif cone.colour == 1: + elif colour == 1: # Orange marker.color.r = 1.0 marker.color.g = 0.5 marker.color.b = 0.0 marker.color.a = 1.0 # Alpha (opacity) - elif cone.colour == 2: + elif colour == 2: # Yellow marker.color.r = 1.0 marker.color.g = 1.0 diff --git a/src/visualisation/cone_map_foxglove_visualiser/package.xml b/src/visualisation/cone_map_foxglove_visualiser/package.xml new file mode 100644 index 00000000..18334013 --- /dev/null +++ b/src/visualisation/cone_map_foxglove_visualiser/package.xml @@ -0,0 +1,26 @@ + + + + cone_map_foxglove_visualiser + 0.0.0 + TODO: Package description + dyu056 + TODO: License declaration + + ackermann_msgs + geometry_msgs + std_msgs + moa_msgs + visualization_msgs + + python3-numpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/visualisation/cone_map_foxglove_visualiser/resource/cone_map_foxglove_visualiser b/src/visualisation/cone_map_foxglove_visualiser/resource/cone_map_foxglove_visualiser new file mode 100644 index 00000000..e69de29b diff --git a/src/visualisation/cone_map_foxglove_visualiser/setup.cfg b/src/visualisation/cone_map_foxglove_visualiser/setup.cfg new file mode 100644 index 00000000..3cfd26df --- /dev/null +++ b/src/visualisation/cone_map_foxglove_visualiser/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/cone_map_foxglove_visualiser +[install] +install_scripts=$base/lib/cone_map_foxglove_visualiser diff --git a/src/visualization/pure_pursuit_visualizer/setup.py b/src/visualisation/cone_map_foxglove_visualiser/setup.py similarity index 82% rename from src/visualization/pure_pursuit_visualizer/setup.py rename to src/visualisation/cone_map_foxglove_visualiser/setup.py index 2bb99d01..6107f5bf 100644 --- a/src/visualization/pure_pursuit_visualizer/setup.py +++ b/src/visualisation/cone_map_foxglove_visualiser/setup.py @@ -1,6 +1,6 @@ from setuptools import find_packages, setup -package_name = 'pure_pursuit_visualizer' +package_name = 'cone_map_foxglove_visualiser' setup( name=package_name, @@ -20,7 +20,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'visualizer = pure_pursuit_visualizer.visualizer:main', + 'visualiser = cone_map_foxglove_visualiser.visualise_cone_map:main', ], }, ) diff --git a/src/visualisation/cone_map_foxglove_visualiser/test/test_copyright.py b/src/visualisation/cone_map_foxglove_visualiser/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/src/visualisation/cone_map_foxglove_visualiser/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/visualisation/cone_map_foxglove_visualiser/test/test_flake8.py b/src/visualisation/cone_map_foxglove_visualiser/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/visualisation/cone_map_foxglove_visualiser/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/visualisation/cone_map_foxglove_visualiser/test/test_pep257.py b/src/visualisation/cone_map_foxglove_visualiser/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/visualisation/cone_map_foxglove_visualiser/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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/src/visualization/path_planning_visualization/package.xml b/src/visualisation/path_planning_visualiser/package.xml similarity index 62% rename from src/visualization/path_planning_visualization/package.xml rename to src/visualisation/path_planning_visualiser/package.xml index 36807084..b754cebb 100644 --- a/src/visualization/path_planning_visualization/package.xml +++ b/src/visualisation/path_planning_visualiser/package.xml @@ -1,12 +1,21 @@ - path_planning_visualization + path_planning_visualiser 0.0.0 - Package for path planning visulization + Package for path planning visulisation Tanish Bhatt TODO: License declaration + ackermann_msgs + geometry_msgs + std_msgs + moa_msgs + foxglove_msgs + visualization_msgs + + python3-numpy + ament_copyright ament_flake8 ament_pep257 diff --git a/src/visualisation/path_planning_visualiser/path_planning_visualiser/ImageThrollerNode.py b/src/visualisation/path_planning_visualiser/path_planning_visualiser/ImageThrollerNode.py new file mode 100644 index 00000000..b807c57d --- /dev/null +++ b/src/visualisation/path_planning_visualiser/path_planning_visualiser/ImageThrollerNode.py @@ -0,0 +1,48 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image + +class ImageThrottlerNode(Node): + def __init__(self, *vargs, **kwargs): + super().__init__('image_throttled_node', *vargs, **kwargs) + + # parameter n is one image outputted per n input images(default 30) + self.declare_parameter('n', 20) + self.n = self.get_parameter('n').get_parameter_value().integer_value + + self.subscriber = self.create_subscription( + Image, + 'image', + self.image_callback, + 10 + ) + + self.publisher = self.create_publisher(Image, 'image_throttled', 10) + + self.counter = 0 + self.get_logger().info(f'ImageThrottlerNode started, passing every {self.n}th image.') + + def image_callback(self, msg): + self.counter += 1 + if self.counter >= self.n: + self.publisher.publish(msg) + self.get_logger().info(f'Publishing image #{self.counter}') + self.counter = 0 + + +def main(args=None): + rclpy.init(args=args) + node = ImageThrottlerNode() + 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/src/visualisation/path_planning_visualiser/path_planning_visualiser/__init__.py b/src/visualisation/path_planning_visualiser/path_planning_visualiser/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/visualisation/path_planning_visualiser/path_planning_visualiser/visualise_action_demo.py b/src/visualisation/path_planning_visualiser/path_planning_visualiser/visualise_action_demo.py new file mode 100644 index 00000000..bf83e972 --- /dev/null +++ b/src/visualisation/path_planning_visualiser/path_planning_visualiser/visualise_action_demo.py @@ -0,0 +1,70 @@ +#!/usr/bin/python3 +from foxglove_msgs.msg import LinePrimitive, Color, SceneEntity, SceneUpdate, ArrowPrimitive, SpherePrimitive, PoseInFrame, PosesInFrame +from geometry_msgs.msg import Point, Quaternion, Pose, Vector3, Quaternion, PoseArray +#from moa_msgs.msg import AllTrajectories, AllStates +from moa_msgs.msg import AllTrajectories +from ackermann_msgs.msg import AckermannDrive + +from builtin_interfaces.msg import Time, Duration +from std_msgs.msg import Int16, Int32MultiArray + +import rclpy +from rclpy.node import Node +import numpy as np + +class pub_viz(Node): + def __init__(self): + super().__init__("publish_actions") + self.get_logger().info("publish action node started") + + self.pubviz = self.create_publisher(SceneUpdate, 'visualization_trajectories', 10) + # sub to all trajectories points and states + self.create_subscription(PoseArray, "moa/chosen_actions", self.show_paths, 10) + + self.id = 1 + + def show_paths(self, msg: PoseArray): + poses = msg.poses + + points = [] + for j in range(len(poses)): + # get a particular pose + _ = poses[j].position + points.append(_) + + args = {'type': LinePrimitive.LINE_STRIP, + 'pose': Pose(position=Point(x=0.0,y=0.0,z=0.0), orientation=Quaternion(x=0.0,y=0.0,z=0.0,w=0.0)), + 'thickness': 2.0, + 'scale_invariant': True, + 'points': points, + 'color': Color(r=0.0,g=255.0,b=0.0,a=1.0)} + tmp = LinePrimitive(**args) + + + # scene entity encapsulates these primitive objects + sargs = {'timestamp': Time(sec=0,nanosec=0), + 'frame_id': 'global_frame', + 'id': f'{self.id}', + 'lifetime': Duration(sec=2,nanosec=100), + 'frame_locked': False, + 'lines': [tmp]} + + # scene update is a wrapper for scene entity + scene_update_msg = SceneUpdate(entities=[SceneEntity(**sargs)]) + + self.pubviz.publish(scene_update_msg) + self.get_logger().info("Published msg") + + self.id += 1 + return + + +def main(): + rclpy.init() + nde = pub_viz() + rclpy.spin(nde) + nde.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/visualisation/path_planning_visualiser/path_planning_visualiser/visualise_trajectories.py b/src/visualisation/path_planning_visualiser/path_planning_visualiser/visualise_trajectories.py new file mode 100644 index 00000000..cd90d992 --- /dev/null +++ b/src/visualisation/path_planning_visualiser/path_planning_visualiser/visualise_trajectories.py @@ -0,0 +1,165 @@ +#!/usr/bin/python3 +from foxglove_msgs.msg import LinePrimitive, Color, SceneEntity, SceneUpdate, ArrowPrimitive, SpherePrimitive, PoseInFrame, PosesInFrame +from geometry_msgs.msg import Point, Quaternion, Pose, Vector3, Quaternion, PoseArray +#from moa_msgs.msg import AllTrajectories, AllStates +from visualization_msgs.msg import Marker, MarkerArray +from moa_msgs.msg import AllTrajectories +from ackermann_msgs.msg import AckermannDrive + +from builtin_interfaces.msg import Time, Duration +from std_msgs.msg import Int16, Int32MultiArray,ColorRGBA + +import rclpy +from rclpy.node import Node +import numpy as np + +class pub_viz(Node): + def __init__(self): + super().__init__("publish_path_planning_msgs") + self.get_logger().info("path planning visulisation node started") + + self.next_destination_vis = [] + + self.pubviz = self.create_publisher(SceneUpdate, 'visualization_trajectories', 10) + # sub to all trajectories points and states + self.create_subscription(AllTrajectories, "moa/inbound_trajectories", self.set_inbound_trajectories, 10) + self.create_subscription(AllTrajectories, "moa/trajectories", self.show_paths, 10) + self.create_subscription(Int16, "moa/best_trajectory_index", self.get_chosen_trajectory, 10) + + # center line publisher + self.centerline_pub = self.create_publisher(MarkerArray, "visualization_centerline", 10) + + self.id = 1 + + + def get_chosen_trajectory(self, msg: Int16) -> None: + print(f"chosen idx got={msg.data}") + self.chosen_trajectory = msg.data + + def set_inbound_trajectories(self, msg: AllTrajectories) -> None: + self.inbounds = msg + + # def set_out_of_bounds_indicies(self, msg: Int32MultiArray) -> None: + # self.invalid_bounds_indicies = msg.data + + def show_paths(self, msg: AllTrajectories): + if hasattr(self,"chosen_trajectory") and hasattr(self, "inbounds"): + line_list = [] + paths = msg.trajectories + paths.append(self.inbounds.trajectories[-1]) # append center line + + for i in range(len(paths)-1): + # choose color + # chosen + if i == self.chosen_trajectory: + # green + tcols = Color(r=0.0, g=255.0, b=0.0, a=1.0) + thickness = 5.0 + # center line + elif i == len(paths) - 1: + # blue + # tcols = Color(r=0.0, g=0.0, b=255.0, a=1.0) + # thickness = 3.0 + # self.get_logger().info(f"center pts: {len(pths[i].poses)}") + break + # other lines + else: + tcols = Color(r=255.0, g=255.0, b=255.0, a=0.8) + thickness = 1.0 + + # get points + points = [] + for j in range(len(paths[i].poses)): + points.append(paths[i].poses[j].position) + args = {'type': LinePrimitive.LINE_STRIP, + 'pose': Pose(position=Point(x=0.0,y=0.0,z=0.0), orientation=Quaternion(x=0.0,y=0.0,z=0.0,w=0.0)), + 'thickness': thickness, + 'scale_invariant': True, + 'points': points, + 'color': tcols} + line_list.append(LinePrimitive(**args)) + + + # scene entity encapsulates these primitive objects + sargs = {'timestamp': Time(sec=0,nanosec=0), + 'frame_id': 'global_frame', + 'id': f'{self.id}', + 'lifetime': Duration(sec=0,nanosec=500000000), + 'frame_locked': False, + 'lines': line_list} + + # show centerline + centerline_markers = [] + centerline_markers.append(self.delete_all_markers()) + idt = 0 + for pose in paths[-1].poses: + centerline_markers.append(self.get_marker_from_pose(idt, pose)) + idt += 1 + + # scene update is a wrapper for scene entity + self.pubviz.publish(SceneUpdate(entities=[SceneEntity(**sargs)])) + self.centerline_pub.publish(MarkerArray(markers=centerline_markers)) + self.get_logger().info("Published msg") + + self.id += 1 + return + + self.get_logger().info("attributes not initialized") + return + + + def get_marker_from_pose(self, id, pose): + marker = Marker() + marker.header.frame_id = "global_frame" # Adjust the frame ID as needed + marker.header.stamp = self.get_clock().now().to_msg() + + marker.ns = "global_frame" + marker.id = id + marker.type = Marker.CUBE + marker.action = Marker.ADD + marker.pose = pose + marker.scale = Vector3(x=0.3,y=0.3,z=0.3) + marker.color = ColorRGBA(r=1.0,g=0.0,b=0.0,a=1.0) + marker.lifetime.sec = 0 + + return marker + + def delete_all_markers(self): + marker = Marker() + marker.header.frame_id = "global_frame" # Adjust the frame ID as needed + marker.header.stamp = self.get_clock().now().to_msg() + + marker.ns = "global_frame" + marker.id = -1 + marker.type = Marker.CUBE + marker.action = Marker.DELETEALL + + marker.pose.position.x = 0.0 + marker.pose.position.y = 0.0 + marker.pose.position.z = 0.0 + + marker.pose.orientation.x = 0.0 + marker.pose.orientation.y = 0.0 + marker.pose.orientation.z = 0.0 + marker.pose.orientation.w = 1.0 + + marker.scale = Vector3(x=0.3, y=0.3, z=0.3) + + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + marker.color.a = 1.0 # Alpha (opacity) + + marker.lifetime.sec = 0 + + return marker + +def main(): + rclpy.init() + nde = pub_viz() + rclpy.spin(nde) + nde.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/visualization/path_planning_visualization/path_planning_visualization/visualise_trajectories_demo.py b/src/visualisation/path_planning_visualiser/path_planning_visualiser/visualise_trajectories_demo.py similarity index 92% rename from src/visualization/path_planning_visualization/path_planning_visualization/visualise_trajectories_demo.py rename to src/visualisation/path_planning_visualiser/path_planning_visualiser/visualise_trajectories_demo.py index e82940e9..d815a220 100644 --- a/src/visualization/path_planning_visualization/path_planning_visualization/visualise_trajectories_demo.py +++ b/src/visualisation/path_planning_visualiser/path_planning_visualiser/visualise_trajectories_demo.py @@ -23,12 +23,6 @@ def __init__(self): # sub to all trajectories points and states self.all_paths = self.create_subscription(AllTrajectories, "moa/inbound_trajectories", self.set_inbound_trajectories, 10) self.create_subscription(AllTrajectories, "moa/trajectories", self.show_paths, 10) - #self.all_states = self.create_subscription(AllStates, "moa/inbound_states", self.get_all_states, 5) - # selected path - #self.chosen_states = self.create_subscription(AckermannDrive, "moa/selected_trajectory", self.get_chosen_state_idx, 5) - # self.create_subscription(PoseArray, "moa/selected_trajectory", self.get_chosen_trajectory, 5) - # self.chosen_path = self.create_subscription(PoseArray, "moa/selected_trajectory", self.show_chosen_paths, 5) - # self.next_destination = self.create_subscription(Pose, "moa/next_destination", self.save_next_destination, 5) self.create_subscription(Int16, "moa/best_trajectory_index", self.get_chosen_trajectory, 10) self.create_subscription(Int32MultiArray,"moa/out_of_bounds",self.set_out_of_bounds_indicies, 10) @@ -69,6 +63,7 @@ def show_paths(self, msg: AllTrajectories): # blue tcols = Color(r=0.0, g=0.0, b=255.0, a=1.0) thickness = 3.0 + self.get_logger().info(f"center pts: {len(pths[i].poses)}") # out of bounds elif i in self.invalid_bounds_indicies: appeneded += 1 diff --git a/src/visualisation/path_planning_visualiser/resource/path_planning_visualiser b/src/visualisation/path_planning_visualiser/resource/path_planning_visualiser new file mode 100644 index 00000000..e69de29b diff --git a/src/visualisation/path_planning_visualiser/setup.cfg b/src/visualisation/path_planning_visualiser/setup.cfg new file mode 100644 index 00000000..02772c2f --- /dev/null +++ b/src/visualisation/path_planning_visualiser/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/path_planning_visualiser +[install] +install_scripts=$base/lib/path_planning_visualiser diff --git a/src/visualization/path_planning_visualization/setup.py b/src/visualisation/path_planning_visualiser/setup.py similarity index 68% rename from src/visualization/path_planning_visualization/setup.py rename to src/visualisation/path_planning_visualiser/setup.py index 22461578..e2d7f634 100644 --- a/src/visualization/path_planning_visualization/setup.py +++ b/src/visualisation/path_planning_visualiser/setup.py @@ -1,6 +1,6 @@ from setuptools import find_packages, setup -package_name = 'path_planning_visualization' +package_name = 'path_planning_visualiser' setup( name=package_name, @@ -20,8 +20,9 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'visualize = path_planning_visualization.visualise_trajectories:main', - 'visualize2 = path_planning_visualization.visualise_trajectories_demo:main', + 'visualiser = path_planning_visualiser.visualise_trajectories:main', + 'visualiser2 = path_planning_visualiser.visualise_trajectories_demo:main', + 'image_throttler = path_planning_visualiser.ImageThrolleNode:main', ], }, ) diff --git a/src/visualisation/path_planning_visualiser/test/test_copyright.py b/src/visualisation/path_planning_visualiser/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/src/visualisation/path_planning_visualiser/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/visualisation/path_planning_visualiser/test/test_flake8.py b/src/visualisation/path_planning_visualiser/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/visualisation/path_planning_visualiser/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/visualisation/path_planning_visualiser/test/test_pep257.py b/src/visualisation/path_planning_visualiser/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/visualisation/path_planning_visualiser/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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/src/visualisation/pure_pursuit_visualiser/launch/pure_pursuit_visualiser_launch.py b/src/visualisation/pure_pursuit_visualiser/launch/pure_pursuit_visualiser_launch.py new file mode 100644 index 00000000..e5f11471 --- /dev/null +++ b/src/visualisation/pure_pursuit_visualiser/launch/pure_pursuit_visualiser_launch.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python3 + +# no parameters, run ros2 launch pure_pursuit_visualiser pure_pursuit_visualiser_launch.py + + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='pure_pursuit_visualiser', + executable='visualiser', + name='publish_pure_pursuit_msgs', + output='screen' + ), + ]) diff --git a/src/visualisation/pure_pursuit_visualiser/package.xml b/src/visualisation/pure_pursuit_visualiser/package.xml new file mode 100644 index 00000000..fa4887e2 --- /dev/null +++ b/src/visualisation/pure_pursuit_visualiser/package.xml @@ -0,0 +1,27 @@ + + + + pure_pursuit_visualiser + 0.0.0 + TODO: Package description + dyu056 + TODO: License declaration + + ackermann_msgs + geometry_msgs + std_msgs + moa_msgs + foxglove_msgs + + python3-numpy + python3-matplotlib + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/visualisation/pure_pursuit_visualiser/pure_pursuit_visualiser/__init__.py b/src/visualisation/pure_pursuit_visualiser/pure_pursuit_visualiser/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/visualization/pure_pursuit_visualizer/pure_pursuit_visualizer/visualizer.py b/src/visualisation/pure_pursuit_visualiser/pure_pursuit_visualiser/visualise_pure_pursuit.py similarity index 99% rename from src/visualization/pure_pursuit_visualizer/pure_pursuit_visualizer/visualizer.py rename to src/visualisation/pure_pursuit_visualiser/pure_pursuit_visualiser/visualise_pure_pursuit.py index c1273663..93ca4528 100644 --- a/src/visualization/pure_pursuit_visualizer/pure_pursuit_visualizer/visualizer.py +++ b/src/visualisation/pure_pursuit_visualiser/pure_pursuit_visualiser/visualise_pure_pursuit.py @@ -1,7 +1,7 @@ #!/usr/bin/python3 from foxglove_msgs.msg import LinePrimitive, Color, SceneEntity, SceneUpdate, ArrowPrimitive, SpherePrimitive, PoseInFrame, PosesInFrame from geometry_msgs.msg import Point, Quaternion, Pose, Vector3, Quaternion, PoseArray -from moa_msgs.msg import ConeMap +# from moa_msgs.msg import ConeMap from ackermann_msgs.msg import AckermannDrive import math @@ -158,7 +158,7 @@ def show_chosen_path(self, msg: PoseArray): scene_update_msg = SceneUpdate(entities=[SceneEntity(**sargs)]) self.pubviz.publish(scene_update_msg) - self.get_logger().info("Published msg") + #self.get_logger().info("Published msg") self.id += 1 diff --git a/src/visualisation/pure_pursuit_visualiser/resource/pure_pursuit_visualiser b/src/visualisation/pure_pursuit_visualiser/resource/pure_pursuit_visualiser new file mode 100644 index 00000000..e69de29b diff --git a/src/visualisation/pure_pursuit_visualiser/setup.cfg b/src/visualisation/pure_pursuit_visualiser/setup.cfg new file mode 100644 index 00000000..028103a0 --- /dev/null +++ b/src/visualisation/pure_pursuit_visualiser/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/pure_pursuit_visualiser +[install] +install_scripts=$base/lib/pure_pursuit_visualiser diff --git a/src/visualisation/pure_pursuit_visualiser/setup.py b/src/visualisation/pure_pursuit_visualiser/setup.py new file mode 100644 index 00000000..48d66583 --- /dev/null +++ b/src/visualisation/pure_pursuit_visualiser/setup.py @@ -0,0 +1,30 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = 'pure_pursuit_visualiser' + +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']), + # include the launch directory + (os.path.join('share', package_name, 'pure_pursuit_visualiser'), glob('launch/*.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='dyu056', + maintainer_email='yudaniel888@hotmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'visualiser = pure_pursuit_visualiser.visualise_pure_pursuit:main', + ], + }, +) diff --git a/src/visualisation/pure_pursuit_visualiser/test/test_copyright.py b/src/visualisation/pure_pursuit_visualiser/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/src/visualisation/pure_pursuit_visualiser/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/visualisation/pure_pursuit_visualiser/test/test_flake8.py b/src/visualisation/pure_pursuit_visualiser/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/visualisation/pure_pursuit_visualiser/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/visualisation/pure_pursuit_visualiser/test/test_pep257.py b/src/visualisation/pure_pursuit_visualiser/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/visualisation/pure_pursuit_visualiser/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/setup.cfg b/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/setup.cfg deleted file mode 100644 index a116675f..00000000 --- a/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/cone_map_foxglove_visualizer -[install] -install_scripts=$base/lib/cone_map_foxglove_visualizer diff --git a/src/visualization/path_planning_visualization/path_planning_visualization/visualise_trajectories.py b/src/visualization/path_planning_visualization/path_planning_visualization/visualise_trajectories.py deleted file mode 100644 index b0c5603b..00000000 --- a/src/visualization/path_planning_visualization/path_planning_visualization/visualise_trajectories.py +++ /dev/null @@ -1,160 +0,0 @@ -#!/usr/bin/python3 -from foxglove_msgs.msg import LinePrimitive, Color, SceneEntity, SceneUpdate, ArrowPrimitive, SpherePrimitive, PoseInFrame, PosesInFrame -from geometry_msgs.msg import Point, Quaternion, Pose, Vector3, Quaternion, PoseArray -#from moa_msgs.msg import AllTrajectories, AllStates -from moa_msgs.msg import AllTrajectories -from ackermann_msgs.msg import AckermannDrive - -from builtin_interfaces.msg import Time, Duration - -import rclpy -from rclpy.node import Node -import numpy as np - -class pub_viz(Node): - def __init__(self): - super().__init__("publish_path_planning_msgs") - self.get_logger().info("path planning visulisation node started") - - self.next_destination_vis = [] - - self.pubviz = self.create_publisher(SceneUpdate, 'visualization_trajectories', 5) - # sub to all trajectories points and states - #self.all_paths = self.create_subscription(AllTrajectories, "moa/inbound_trajectories", self.show_paths, 5) - # self.all_paths = self.create_subscription(AllTrajectories, "moa/trajectories", self.show_paths, 5) - #self.all_states = self.create_subscription(AllStates, "moa/inbound_states", self.get_all_states, 5) - # selected path - #self.chosen_states = self.create_subscription(AckermannDrive, "moa/selected_trajectory", self.get_chosen_state_idx, 5) - #self.chosen_path = self.create_subscription(PoseArray, "moa/selected_trajectory", self.get_chosen_trajectory, 5) - self.chosen_path = self.create_subscription(PoseArray, "moa/selected_trajectory", self.show_chosen_paths, 5) - self.next_destination = self.create_subscription(Pose, "moa/next_destination", self.save_next_destination, 5) - - self.id = 1 - - # def get_all_states(self, msg:AllStates) -> None: self.states = [i.steering_angle for i in msg.states] - - def get_chosen_state_idx(self, msg:AckermannDrive) -> None: - if hasattr(self, "states"): - self.chosen_idx = np.where(np.isclose(self.states, msg.steering_angle, 1e-3))[0][0] - - def get_chosen_trajectory(self, msg: PoseArray) -> None: - self.chosen_trajectory = msg - - - def show_paths(self, msg: AllTrajectories): - #if not hasattr(self,"chosen_trajectory"): - # self.get_logger().info("attribute not initialized") - # return - - line_list = [] - # list of pose array - pths = msg.trajectories - pths.append(self.chosen_trajectory) - for i in range(len(pths)): - if i == len(pths) - 1: - tcols = Color(r=255.0, g=255.0, b=255.0, a=1.0) - elif i == len(pths) - 2: - tcols = Color(r=0.0, g=255.0, b=0.0, a=1.0) - else: - tcols = Color(r=255.0, g=0.0, b=0.0, a=1.0) - pts = [] - for j in range(len(pths[i].poses)): - # get a particular pose - _ = pths[i].poses[j].position - pts.append(_) - args = {'type': LinePrimitive.LINE_STRIP, - 'pose': Pose(position=Point(x=0.0,y=0.0,z=0.0), orientation=Quaternion(x=0.0,y=0.0,z=0.0,w=0.0)), - 'thickness': 2.0, - 'scale_invariant': True, - 'points': pts, - 'color': tcols} - line_list.append(LinePrimitive(**args)) - - # arrow primitive code if needed - # args = {'pose': Pose(position=Point(x=1.0,y=0.0,z=0.0), orientation=Quaternion(x=0.0,y=0.0,z=0.0,w=0.0)), - # 'shaft_length': 1.0, - # 'shaft_diameter': 0.1, - # 'head_length': 2.5, - # 'head_diameter': 0.5, - # 'color': Color(r=67.0,g=125.0,b=100.0,a=1.0)} - # msg = ArrowPrimitive(**args) - - # scene entity encapsulates these primitive objects - sargs = {'timestamp': Time(sec=0,nanosec=0), - 'frame_id': 'global_frame', - 'id': f'{self.id}', - 'lifetime': Duration(sec=3,nanosec=0), - 'frame_locked': False, - 'lines': line_list, - 'spheres': self.next_destination_vis} - # scene update is a wrapper for scene entity - scene_update_msg = SceneUpdate(entities=[SceneEntity(**sargs)]) - - self.pubviz.publish(scene_update_msg) - self.get_logger().info("Published msg") - - self.id += 1 - - def show_chosen_paths(self, msg: PoseArray): - tcols = Color(r=0.0, g=255.0, b=0.0, a=1.0) - pts = [] - line_list = []; - for j in range(len(msg.poses)): - # get a particular pose - _ = msg.poses[j].position - pts.append(_) - args = {'type': LinePrimitive.LINE_STRIP, - 'pose': Pose(position=Point(x=0.0, y=0.0, z=0.0), - orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=0.0)), - 'thickness': 2.0, - 'scale_invariant': True, - 'points': pts, - 'color': tcols} - line_list.append(LinePrimitive(**args)) - - # arrow primitive code if needed - # args = {'pose': Pose(position=Point(x=1.0,y=0.0,z=0.0), orientation=Quaternion(x=0.0,y=0.0,z=0.0,w=0.0)), - # 'shaft_length': 1.0, - # 'shaft_diameter': 0.1, - # 'head_length': 2.5, - # 'head_diameter': 0.5, - # 'color': Color(r=67.0,g=125.0,b=100.0,a=1.0)} - # msg = ArrowPrimitive(**args) - - # scene entity encapsulates these primitive objects - sargs = {'timestamp': Time(sec=0,nanosec=0), - 'frame_id': 'global_frame', - 'id': f'{self.id}', - 'lifetime': Duration(sec=3,nanosec=0), - 'frame_locked': False, - 'lines': line_list, - 'spheres': self.next_destination_vis} - - # scene update is a wrapper for scene entity - scene_update_msg = SceneUpdate(entities=[SceneEntity(**sargs)]) - - self.pubviz.publish(scene_update_msg) - self.get_logger().info("Published msg") - - self.id += 1 - - def save_next_destination(self, msg : Pose): - tcols = Color(r=255.0, g=255.0, b=0.0, a=1.0) - args = {'pose': msg, - 'size': Vector3(x=1.0, y=1.0, z=1.0), - 'color': tcols} - if len(self.next_destination_vis) == 0: - self.next_destination_vis.append(SpherePrimitive(**args)) - else: - self.next_destination_vis[0] = SpherePrimitive(**args) - - -def main(): - rclpy.init() - nde = pub_viz() - rclpy.spin(nde) - nde.destroy_node() - rclpy.shutdown() - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/src/visualization/path_planning_visualization/setup.cfg b/src/visualization/path_planning_visualization/setup.cfg deleted file mode 100644 index 858519dd..00000000 --- a/src/visualization/path_planning_visualization/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/path_planning_visualization -[install] -install_scripts=$base/lib/path_planning_visualization diff --git a/src/visualization/pure_pursuit_visualizer/setup.cfg b/src/visualization/pure_pursuit_visualizer/setup.cfg deleted file mode 100644 index 66e8f9cd..00000000 --- a/src/visualization/pure_pursuit_visualizer/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/pure_pursuit_visualizer -[install] -install_scripts=$base/lib/pure_pursuit_visualizer diff --git a/unity_qucik_start.sh b/unity_qucik_start.sh deleted file mode 100644 index ef7970d4..00000000 --- a/unity_qucik_start.sh +++ /dev/null @@ -1,13 +0,0 @@ -#!/bin/bash -ros2 topic pub --once /race_controller/create std_msgs/msg/String "data: 'test'" - -gnome-terminal -- bash -c "source run_cone_map.sh; exec bash" -gnome-terminal -- bash -c "source run_cone_map_visualizer.sh; exec bash" -gnome-terminal -- bash -c "source run_controller_visualizer.sh; exec bash" -gnome-terminal -- bash -c "source run_controller.sh; exec bash" -gnome-terminal -- bash -c "source run_foxglove_ros.sh; exec bash" -gnome-terminal -- bash -c "source run_localization.sh; exec bash" -gnome-terminal -- bash -c "source run_path_planning_visualizer.sh; exec bash" -gnome-terminal -- bash -c "source run_path_planning.sh; exec bash" -gnome-terminal -- bash -c "source run_simulation_car_control.sh; exec bash" -