diff --git a/.devcontainer.json b/.devcontainer.json
index 994e6cf..c3a5a4c 100644
--- a/.devcontainer.json
+++ b/.devcontainer.json
@@ -11,6 +11,7 @@
"--ipc=host",
"--privileged"
],
+ "containerUser": "cev",
"containerEnv": {
"DISPLAY": "${localEnv:DISPLAY}"
},
@@ -20,10 +21,8 @@
"customizations": {
"vscode": {
"extensions": [
- "ms-iot.vscode-ros",
- "ms-vscode.cpptools-extension-pack",
"ms-python.black-formatter",
- "xaver.clang-format"
+ "llvm-vs-code-extensions.vscode-clangd"
],
"settings": {
"terminal.integrated.defaultProfile.linux": "bash",
@@ -33,7 +32,7 @@
}
},
"[cpp]": {
- "editor.defaultFormatter": "xaver.clang-format"
+ "editor.defaultFormatter": "llvm-vs-code-extensions.vscode-clangd"
},
"[python]": {
"diffEditor.ignoreTrimWhitespace": false,
diff --git a/.github/workflows/docker-image.yml b/.github/workflows/docker-image.yml
index c3ffaa4..534edf9 100644
--- a/.github/workflows/docker-image.yml
+++ b/.github/workflows/docker-image.yml
@@ -7,17 +7,28 @@ on:
branches: [ "main" ]
jobs:
-
- build:
-
+ build-deploy:
runs-on: ubuntu-22.04
-
steps:
- name: Fetch recursively
uses: actions/checkout@v4
with:
submodules: recursive
fetch-depth: 0
+ - name: Set up Docker Buildx
+ uses: docker/setup-buildx-action@v3
+ - name: Build the Docker image
+ run: docker build . --file Dockerfile.deploy --tag rc-brain-deploy:$(date +%s)
+ build-dev:
+ runs-on: ubuntu-22.04
+ steps:
+ - name: Fetch recursively
+ uses: actions/checkout@v4
+ with:
+ submodules: recursive
+ fetch-depth: 0
+ - name: Set up Docker Buildx
+ uses: docker/setup-buildx-action@v3
- name: Build the Docker image
- run: docker build . --file Dockerfile --tag my-image-name:$(date +%s)
+ run: docker build . --file Dockerfile.dev --tag rc-brain-dev:$(date +%s)
diff --git a/.gitmodules b/.gitmodules
index cd316c6..6d65ecd 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,6 +1,3 @@
-[submodule "cev_msgs"]
- path = cev_msgs
- url = https://github.com/cornellev/cev_msgs.git
[submodule "external/serial-ros2"]
path = external/serial-ros2
url = https://github.com/RoverRobotics-forks/serial-ros2.git
@@ -13,24 +10,12 @@
[submodule "external/bond"]
path = external/bond
url = https://github.com/ros/bond_core.git
-[submodule "cev/cev_localization_ros2"]
- path = cev/cev_localization_ros2
- url = https://github.com/cornellev/cev_localization_ros2.git
-[submodule "cev/cev_planner_ros2"]
- path = cev/cev_planner_ros2
- url = https://github.com/cornellev/cev_planner_ros2.git
-[submodule "cev/cev_autobrake_ros2"]
- path = cev/cev_autobrake_ros2
- url = https://github.com/cornellev/cev_autobrake_ros2.git
-[submodule "cev/cev_teleop_ros2"]
- path = cev/cev_teleop_ros2
- url = https://github.com/cornellev/cev_teleop_ros2.git
-[submodule "cev/cev_vision_ros2"]
- path = cev/cev_vision_ros2
- url = https://github.com/cornellev/cev_vision_ros2.git
-[submodule "cev/cev_encoder_odometry_ros2"]
- path = cev/cev_encoder_odometry_ros2
- url = https://github.com/cornellev/cev_encoder_odometry_ros2.git
-[submodule "cev/cev_trajectory_follower_ros2"]
- path = cev/cev_trajectory_follower_ros2
- url = https://github.com/cornellev/cev_trajectory_follower_ros2.git
+[submodule "cev/all/cev_planner_ros2/cev_planner"]
+ path = cev/all/cev_planner_ros2/cev_planner
+ url = https://github.com/cornellev/cev_planner.git
+[submodule "cev/all/cev_planner_ros2/matplotlib-cpp"]
+ path = cev/all/cev_planner_ros2/tmp/matplotlib-cpp
+ url = https://github.com/lava/matplotlib-cpp.git
+[submodule "cev/all/cev_localization_ros2/cev_kalman_filter"]
+ path = cev/all/cev_localization_ros2/cev_kalman_filter
+ url = https://github.com/cornellev/cev_kalman_filter
diff --git a/Dockerfile b/Dockerfile
deleted file mode 100644
index 09c2826..0000000
--- a/Dockerfile
+++ /dev/null
@@ -1,56 +0,0 @@
-# syntax=docker/dockerfile:1.7-labs
-FROM ros:humble
-
-SHELL ["/bin/bash", "-c"]
-
-# Arguments
-ARG FOLDER_NAME=rc-brain
-ARG WORKSPACE_DIR=/home/cev
-
-# Set working directory
-WORKDIR $WORKSPACE_DIR
-
-# Source ROS2 environment automatically in every shell
-RUN echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
-RUN echo "source $WORKSPACE_DIR/install/setup.bash" >> ~/.bashrc
-
-# Install dependencies
-RUN apt-get update && apt-get install -y \
- curl \
- python3-colcon-common-extensions \
- ros-$ROS_DISTRO-ros-base \
- && apt-get clean && rm -rf /var/lib/apt/lists/*
-
-# Install clang-format
-RUN curl -fsSL https://apt.llvm.org/llvm-snapshot.gpg.key | tee /etc/apt/trusted.gpg.d/apt.llvm.org.asc && \
- echo $'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy main\n' \
- $'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-18 main\n' \
- $'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-19 main\n' \
- | tee -a /etc/apt/sources.list && \
- apt-get update && apt-get install -y clang-format clang-format-19 \
- && apt-get clean && rm -rf /var/lib/apt/lists/*
-
-# Fix libc-bin issue
-RUN rm /var/lib/dpkg/info/libc-bin.* || true && apt-get update && apt-get install -y libc-bin
-
-# Initialize rosdep
-RUN rosdep update --rosdistro $ROS_DISTRO
-
-# Create workspace directory
-RUN mkdir -p src/$FOLDER_NAME
-
-# Copy the full package source code
-COPY --parents ./* src/$FOLDER_NAME/
-RUN chmod +x src/$FOLDER_NAME/install.sh && src/$FOLDER_NAME/install.sh
-
-# Install package dependencies
-RUN source /opt/ros/$ROS_DISTRO/setup.bash && rosdep install --from-paths src -r -y
-
-# Build the ROS2 workspace
-RUN source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install
-
-# Set up entrypoint
-COPY entrypoint.sh /entrypoint.sh
-RUN chmod +x /entrypoint.sh
-
-ENTRYPOINT ["/entrypoint.sh"]
diff --git a/Dockerfile.deploy b/Dockerfile.deploy
index 03d49f2..0b8f613 100644
--- a/Dockerfile.deploy
+++ b/Dockerfile.deploy
@@ -3,6 +3,17 @@ FROM ros:humble
SHELL ["/bin/bash", "-c"]
+# Set up CEV user
+RUN useradd -m -s /bin/bash cev
+RUN usermod -aG sudo cev
+# Grant passwordless sudo access to the 'sudo' group
+RUN echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers
+USER cev
+
+# Docker doesn't do this by default
+RUN sudo chown -R cev /home/cev
+
+
ARG FOLDER_NAME=rc-brain
ARG WORKSPACE_DIR=/home/cev
WORKDIR $WORKSPACE_DIR
@@ -12,36 +23,31 @@ RUN echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
RUN echo "source $WORKSPACE_DIR/install/setup.bash" >> ~/.bashrc
# Init rosdep and update package index
-# for some reason we need to do this
-RUN rm /var/lib/dpkg/info/libc-bin.*
-RUN apt-get clean
-RUN apt-get update
-RUN apt-get install libc-bin
+RUN sudo apt-get update
RUN rosdep update --rosdistro $ROS_DISTRO
-RUN mkdir -p src/$FOLDER_NAME
-
# Install and build build-from-source packages
# We do this as a separate step from the stuff for our code, since that changes more frequently
-RUN cd src && \
- git clone https://github.com/Slamtec/sllidar_ros2.git && \
- cd sllidar_ros2 && \
- git checkout 3430009
+COPY --chown=cev --parents external/**/package.xml src/$FOLDER_NAME/
RUN source /opt/ros/$ROS_DISTRO/setup.bash && rosdep install --from-paths src -r -y
-RUN source /opt/ros/$ROS_DISTRO/setup.bash && colcon build
-# Install self dependencies with rosdep
-COPY --parents ./*/package.xml src/$FOLDER_NAME
+# Run install_extra.sh, which installs additional dependencies
+COPY --chown=cev scripts/install_extra.sh src/$FOLDER_NAME/scripts/
+RUN sudo src/$FOLDER_NAME/scripts/install_extra.sh
+
+# Finally build external packages
+COPY --chown=cev external src/$FOLDER_NAME/external/
+RUN source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install
+
+# Install all other dependencies
+COPY --chown=cev --parents **/package.xml src/$FOLDER_NAME/
RUN source /opt/ros/$ROS_DISTRO/setup.bash && rosdep install --from-paths src -r -y
# Clean up apt cache to make container smaller
-RUN apt-get clean
+RUN sudo apt-get clean
# Build
-COPY . src/$FOLDER_NAME
-RUN source /opt/ros/$ROS_DISTRO/setup.bash && colcon build
+COPY --chown=cev . src/$FOLDER_NAME
+RUN source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install
-# Copy entrypoint script
-COPY entrypoint.sh entrypoint.sh
-RUN chmod +x entrypoint.sh
-ENTRYPOINT ["/bin/bash", "/home/cev/entrypoint.sh"]
+ENTRYPOINT ["/bin/bash", "/home/cev/src/rc-brain/scripts/deployed_entrypoint.sh"]
diff --git a/Dockerfile.dev b/Dockerfile.dev
index 795e722..833878c 100644
--- a/Dockerfile.dev
+++ b/Dockerfile.dev
@@ -3,44 +3,44 @@ FROM ros:humble
SHELL ["/bin/bash", "-c"]
+# Set up CEV user
+RUN useradd -m -s /bin/bash cev
+RUN usermod -aG sudo cev
+# Grant passwordless sudo access to the 'sudo' group
+RUN echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers
+USER cev
+
ARG FOLDER_NAME=rc-brain
ARG WORKSPACE_DIR=/home/cev
WORKDIR $WORKSPACE_DIR
+# Docker doesn't do this by default
+RUN sudo chown -R cev /home/cev
+
# Source the ROS2 environment automatically when a new shell is created
RUN echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
RUN echo "source $WORKSPACE_DIR/install/setup.bash" >> ~/.bashrc
# Install dev tools
-RUN apt-get update
-RUN apt-get install -y curl
-RUN curl -fsSL https://apt.llvm.org/llvm-snapshot.gpg.key | tee /etc/apt/trusted.gpg.d/apt.llvm.org.asc
-RUN cat /etc/apt/trusted.gpg.d/apt.llvm.org.asc
-RUN echo $'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy main\n' \
- $'deb-src http://apt.llvm.org/jammy/ llvm-toolchain-jammy main\n' \
- $'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-18 main\n' \
- $'deb-src http://apt.llvm.org/jammy/ llvm-toolchain-jammy-18 main\n' \
- $'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-19 main\n' \
- $'deb-src http://apt.llvm.org/jammy/ llvm-toolchain-jammy-19 main\n' \
- | tee -a /etc/apt/sources.list
-RUN apt-get update
-RUN apt-get install -y clang-format clang-format-19
+RUN sudo apt-get update
+# Tools required to run clang install
+RUN sudo apt-get install -y \
+ curl \
+ lsb-release \
+ software-properties-common \
+ gnupg
+RUN curl -fsSL https://apt.llvm.org/llvm.sh | sudo bash -s -- 19
+RUN sudo apt-get install -y clangd-19
+RUN sudo update-alternatives --install /usr/bin/clangd clangd /usr/bin/clangd-19 100
# Init rosdep and update package index
-# for some reason we need to do this
-RUN rm /var/lib/dpkg/info/libc-bin.*
-RUN apt-get clean
-RUN apt-get update
-RUN apt-get install -y libc-bin
RUN rosdep update --rosdistro $ROS_DISTRO
-RUN mkdir -p src/$FOLDER_NAME
-
-# Copy install.sh
-COPY --parents ./install.sh src/$FOLDER_NAME
-
-# Run install.sh
-RUN src/$FOLDER_NAME/install.sh
+# Run install_extra.sh, which installs additional dependencies
+COPY --chown=cev scripts/install_extra.sh src/$FOLDER_NAME/scripts/
+RUN sudo src/$FOLDER_NAME/scripts/install_extra.sh
-COPY --parents ./*/package.xml src/$FOLDER_NAME
+# Install own dependencies
+RUN mkdir -p src/$FOLDER_NAME
+COPY --chown=cev --parents **/package.xml src/$FOLDER_NAME/
RUN source /opt/ros/$ROS_DISTRO/setup.bash && rosdep install --from-paths src -r -y
diff --git a/README.md b/README.md
index 4fc738c..740fe91 100644
--- a/README.md
+++ b/README.md
@@ -1,16 +1,9 @@
# rc-brain
One repo to rule them all
-## Installation
+## Installation (local)
`git clone --recurse-submodules https://github.com/cornellev/rc-brain.git`
-`cd rc-brain && sudo ./install.sh`
-
-## Docker
-`docker build -t cev-rc-autonomy .`
-Then run either
-`docker run --rm ros2-autonomy launch`
-or
-`docker run --rm ros2-autonomy teleop`
+`cd rc-brain && sudo ./scripts/install_extra.sh`
## Development
@@ -21,12 +14,25 @@ Run `./scripts/deploy/arduino_deploy.sh` to compile and flash code to the arduin
#### Formatting
If you're not using the DevContainer, please make sure to install `clang-format`, ideally version 19! This will help keep the formatting of our C++ code formatted consistently. Python code is formatted by `black`. The DevContainer will install the `Black` extension for you, which ships with `black`, but otherwise you'll need to install it in VSCode or whatever other IDE you're using.
+### Docker (DevContainer)
+Everything should pretty much work out of the box, except...
+> [!WARNING]
+> You must have the Docker buildx plugin installed to build the image correctly. You may get very confusing failures!
+> On Ubuntu, you can install the `docker-buildx-plugin` package.
+
### Deployment
#### Regular deployment
If you're just developing, building Docker images will probably take too long. You can deploy normally just by running `scripts/deploy.sh`. It will drop you into an interactive session at the end so you can launch whatever you want to.
#### Docker deployment
+Use `docker build . --file Dockerfile.deploy -t rc-brain-deploy:latest` to build the deploy container.
+Then run either `docker run --rm rc-brain-deploy launch` or `docker run --rm rc-brain-deploy teleop` to launch.
+
+> [!WARNING]
+> You must have the Docker buildx plugin installed to build the image correctly. You may get very confusing failures!
+> On Ubuntu, you can install the `docker-buildx-plugin` package.
+
Our Docker deployment pipeline works by pushing Docker images to a local registry and pulling them down on the mini cars. This avoids both long build times on the mini cars and the long time it takes to transfer whole images (as we can take advantage of Docker's cache).
You'll have to do a bit of setup to get your computer to use the local registry. Note that you must be connected to `cev-router`.
diff --git a/autonomy/launch/launch.py b/autonomy/launch/launch.py
index 407b59d..a8fa4fa 100644
--- a/autonomy/launch/launch.py
+++ b/autonomy/launch/launch.py
@@ -5,21 +5,18 @@
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
+
def get_path(package, dir, file):
- return os.path.join(
- get_package_share_directory(package),
- dir,
- file
- )
+ return os.path.join(get_package_share_directory(package), dir, file)
+
def launch(package, file, launch_folder="launch", arguments={}):
return IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- get_path(package, launch_folder, file)
- ),
- launch_arguments=arguments.items()
+ PythonLaunchDescriptionSource(get_path(package, launch_folder, file)),
+ launch_arguments=arguments.items(),
)
+
def generate_launch_description():
imu_config = get_path("autonomy", "config", "imu.yml")
robot_localization_config = get_path("autonomy", "config", "robot_localization.yml")
@@ -28,72 +25,95 @@ def generate_launch_description():
[
# STATIC TRANSFORMS
Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name='static_transform_publisher_world_map',
- output='screen',
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_world_map",
+ output="screen",
arguments=[
- '0', '0', '0', # Translation: x = 0.035, y = 0.04, z = 0 (meters)
- '0', '0', '0', '1', # Rotation: 0
- 'world',
- 'map'
+ "0",
+ "0",
+ "0", # Translation: x = 0.035, y = 0.04, z = 0 (meters)
+ "0",
+ "0",
+ "0",
+ "1", # Rotation: 0
+ "world",
+ "map",
],
),
Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name='static_transform_publisher_map_odom',
- output='screen',
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_map_odom",
+ output="screen",
arguments=[
- '0', '0', '0', # Translation: x = 0.035, y = 0.04, z = 0 (meters)
- '0', '0', '0', '1', # Rotation: 0
- 'map',
- 'odom'
+ "0",
+ "0",
+ "0", # Translation: x = 0.035, y = 0.04, z = 0 (meters)
+ "0",
+ "0",
+ "0",
+ "1", # Rotation: 0
+ "map",
+ "odom",
],
),
Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name='static_transform_publisher_odom_base_link',
- output='screen',
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_odom_base_link",
+ output="screen",
arguments=[
- '0', '0', '0', # Translation: x = 0.035, y = 0.04, z = 0 (meters)
- '0', '0', '0', '1', # Rotation: 0
- 'odom',
- 'base_link'
+ "0",
+ "0",
+ "0", # Translation: x = 0.035, y = 0.04, z = 0 (meters)
+ "0",
+ "0",
+ "0",
+ "1", # Rotation: 0
+ "odom",
+ "base_link",
],
),
Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name='static_transform_publisher_base_link_lidar',
- output='screen',
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_base_link_lidar",
+ output="screen",
arguments=[
- '-0.035', '-0.04', '0', # Translation: x = 0.035, y = 0.04, z = 0 (meters)
- '0', '0', '1', '0', # Rotation: M_PI
- 'base_link',
- 'laser'
+ "-0.035",
+ "-0.04",
+ "0", # Translation: x = 0.035, y = 0.04, z = 0 (meters)
+ "0",
+ "0",
+ "1",
+ "0", # Rotation: M_PI
+ "base_link",
+ "laser",
],
),
Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name='static_transform_publisher_base_link_imu',
- output='screen',
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_base_link_imu",
+ output="screen",
arguments=[
- '-0.18', '0.0', '0', # Translation: x = -0.18, y = 0.07, z = 0 (meters)
- '0', '0', '0', '1', # Rotation: 0
- 'base_link',
- 'imu'
+ "-0.18",
+ "0.0",
+ "0", # Translation: x = -0.18, y = 0.07, z = 0 (meters)
+ "0",
+ "0",
+ "0",
+ "1", # Rotation: 0
+ "base_link",
+ "imu",
],
),
# # IMU
Node(
- package='witmotion_ros',
- executable='witmotion_ros_node',
- parameters=[
- imu_config
- ]
+ package="witmotion_ros",
+ executable="witmotion_ros_node",
+ parameters=[imu_config],
),
# Robot Localization
# Node(
@@ -119,21 +139,20 @@ def generate_launch_description():
"map_update_interval": 0.05,
# "position_covariance_scale": 1.0,
# "yaw_covariance_scale": 1.0,
- "resolution": .09,
- "min_laser_range": .15,
+ "resolution": 0.09,
+ "min_laser_range": 0.15,
"max_laser_range": 12.0,
"use_scan_matching": True,
"do_loop_closing": True,
"use_scan_barycenter": True,
- "minimum_travel_distance": .05,
- "minimum_travel_heading": .05,
- "correlation_search_space_dimension": .2,
+ "minimum_travel_distance": 0.05,
+ "minimum_travel_heading": 0.05,
+ "correlation_search_space_dimension": 0.2,
"loop_search_space_dimension": 3.0,
- "angle_variance_penalty": 0.0
+ "angle_variance_penalty": 0.0,
}
- ]
+ ],
),
-
# UNITREE LIDAR
# Node(
# package='unitree_lidar_ros2',
@@ -153,11 +172,10 @@ def generate_launch_description():
# {'imu_frame': "unilidar_imu"},
# {'imu_topic': "unilidar/imu"}]
# ),
-
# CEV Localization
Node(
package="cev_localization",
- executable="ackermann_ekf",
+ executable="localization",
name="cev_localization_node",
output="screen",
parameters=[
@@ -168,7 +186,7 @@ def generate_launch_description():
}
],
),
- # Node(
+ # Node(
# package="cev_planner_ros2",
# executable="planner_node",
# name="cev_planner_ros2_node",
@@ -177,19 +195,15 @@ def generate_launch_description():
# # get_path("cev_planner_ros2", "config", "cev_planner.yaml")
# # ],
# ),
-
# Node(
# package="cev_planner_ros2",
# executable="planner_node",
# name="cev_planner_ros2_node",
# output="screen",
# ),
-
# Trajectory Launch
launch("trajectory_follower", "launch.py"),
-
## LAUNCH FILES
-
# Teleop
# launch("teleop", "launch.py"),
# Autobrake
@@ -197,7 +211,11 @@ def generate_launch_description():
# Serial Communicator
launch("serial_com", "launch.py"),
# RPLidar
- launch("sllidar_ros2", "sllidar_a1_launch.py", arguments={"serial_port": "/dev/rplidar"}),
+ launch(
+ "sllidar_ros2",
+ "sllidar_a1_launch.py",
+ arguments={"serial_port": "/dev/rplidar"},
+ ),
# Encoder Odometry (Ackermann)
launch("encoder_odometry", "launch.py"),
]
diff --git a/autonomy/launch/mux.py b/autonomy/launch/mux.py
index 2e279b5..6974f12 100644
--- a/autonomy/launch/mux.py
+++ b/autonomy/launch/mux.py
@@ -5,21 +5,18 @@
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
+
def get_path(package, dir, file):
- return os.path.join(
- get_package_share_directory(package),
- dir,
- file
- )
+ return os.path.join(get_package_share_directory(package), dir, file)
+
def launch(package, file, launch_folder="launch", arguments={}):
return IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- get_path(package, launch_folder, file)
- ),
- launch_arguments=arguments.items()
+ PythonLaunchDescriptionSource(get_path(package, launch_folder, file)),
+ launch_arguments=arguments.items(),
)
+
def generate_launch_description():
imu_config = get_path("autonomy", "config", "imu.yml")
robot_localization_config = get_path("autonomy", "config", "robot_localization.yml")
@@ -28,72 +25,95 @@ def generate_launch_description():
[
# STATIC TRANSFORMS
Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name='static_transform_publisher_world_map',
- output='screen',
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_world_map",
+ output="screen",
arguments=[
- '0', '0', '0', # Translation: x = 0.035, y = 0.04, z = 0 (meters)
- '0', '0', '0', '1', # Rotation: 0
- 'world',
- 'map'
+ "0",
+ "0",
+ "0", # Translation: x = 0.035, y = 0.04, z = 0 (meters)
+ "0",
+ "0",
+ "0",
+ "1", # Rotation: 0
+ "world",
+ "map",
],
),
Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name='static_transform_publisher_map_odom',
- output='screen',
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_map_odom",
+ output="screen",
arguments=[
- '0', '0', '0', # Translation: x = 0.035, y = 0.04, z = 0 (meters)
- '0', '0', '0', '1', # Rotation: 0
- 'map',
- 'odom'
+ "0",
+ "0",
+ "0", # Translation: x = 0.035, y = 0.04, z = 0 (meters)
+ "0",
+ "0",
+ "0",
+ "1", # Rotation: 0
+ "map",
+ "odom",
],
),
Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name='static_transform_publisher_odom_base_link',
- output='screen',
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_odom_base_link",
+ output="screen",
arguments=[
- '0', '0', '0', # Translation: x = 0.035, y = 0.04, z = 0 (meters)
- '0', '0', '0', '1', # Rotation: 0
- 'odom',
- 'base_link'
+ "0",
+ "0",
+ "0", # Translation: x = 0.035, y = 0.04, z = 0 (meters)
+ "0",
+ "0",
+ "0",
+ "1", # Rotation: 0
+ "odom",
+ "base_link",
],
),
Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name='static_transform_publisher_base_link_lidar',
- output='screen',
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_base_link_lidar",
+ output="screen",
arguments=[
- '-0.035', '-0.04', '0', # Translation: x = 0.035, y = 0.04, z = 0 (meters)
- '0', '0', '1', '0', # Rotation: M_PI
- 'base_link',
- 'laser'
+ "-0.035",
+ "-0.04",
+ "0", # Translation: x = 0.035, y = 0.04, z = 0 (meters)
+ "0",
+ "0",
+ "1",
+ "0", # Rotation: M_PI
+ "base_link",
+ "laser",
],
),
Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name='static_transform_publisher_base_link_imu',
- output='screen',
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_base_link_imu",
+ output="screen",
arguments=[
- '-0.18', '0.0', '0', # Translation: x = -0.18, y = 0.07, z = 0 (meters)
- '0', '0', '0', '1', # Rotation: 0
- 'base_link',
- 'imu'
+ "-0.18",
+ "0.0",
+ "0", # Translation: x = -0.18, y = 0.07, z = 0 (meters)
+ "0",
+ "0",
+ "0",
+ "1", # Rotation: 0
+ "base_link",
+ "imu",
],
),
# # IMU
Node(
- package='witmotion_ros',
- executable='witmotion_ros_node',
- parameters=[
- imu_config
- ]
+ package="witmotion_ros",
+ executable="witmotion_ros_node",
+ parameters=[imu_config],
),
# Robot Localization
# Node(
@@ -119,21 +139,20 @@ def generate_launch_description():
"map_update_interval": 0.05,
# "position_covareiance_scale": 1.0,
# "yaw_covariance_scale": 1.0,
- "resolution": .09,
- "min_laser_range": .15,
+ "resolution": 0.09,
+ "min_laser_range": 0.15,
"max_laser_range": 12.0,
"use_scan_matching": True,
"do_loop_closing": True,
"use_scan_barycenter": True,
- "minimum_travel_distance": .05,
- "minimum_travel_heading": .05,
- "correlation_search_space_dimension": .2,
+ "minimum_travel_distance": 0.05,
+ "minimum_travel_heading": 0.05,
+ "correlation_search_space_dimension": 0.2,
"loop_search_space_dimension": 3.0,
- "angle_variance_penalty": 0.0
+ "angle_variance_penalty": 0.0,
}
- ]
+ ],
),
-
# Node(
# package="slam_toolbox",
# executable="sync_slam_toolbox_node",
@@ -165,7 +184,6 @@ def generate_launch_description():
# }
# ]
# ),
-
# UNITREE LIDAR
# Node(
# package='unitree_lidar_ros2',
@@ -185,11 +203,10 @@ def generate_launch_description():
# {'imu_frame': "unilidar_imu"},
# {'imu_topic': "unilidar/imu"}]
# ),
-
# CEV Localization
Node(
package="cev_localization",
- executable="ackermann_ekf",
+ executable="localization",
name="cev_localization_node",
output="screen",
parameters=[
@@ -200,7 +217,7 @@ def generate_launch_description():
}
],
),
- # Node(
+ # Node(
# package="cev_planner_ros2",
# executable="planner_node",
# name="cev_planner_ros2_node",
@@ -209,21 +226,16 @@ def generate_launch_description():
# # get_path("cev_planner_ros2", "config", "cev_planner.yaml")
# # ],
# ),
-
# Node(
# package="cev_planner_ros2",
# executable="planner_node",
# name="cev_planner_ros2_node",
# output="screen",
# ),
-
# Trajectory Launch
# launch("trajectory_follower", "launch.py"),
launch("mux", "launch.py"),
-
-
## LAUNCH FILES
-
# Teleop
# launch("teleop", "launch.py"),
# Autobrake
@@ -231,7 +243,11 @@ def generate_launch_description():
# Serial Communicator
launch("serial_com", "launch.py"),
# RPLidar
- launch("sllidar_ros2", "sllidar_a1_launch.py", arguments={"serial_port": "/dev/rplidar"}),
+ launch(
+ "sllidar_ros2",
+ "sllidar_a1_launch.py",
+ arguments={"serial_port": "/dev/rplidar"},
+ ),
# Encoder Odometry (Ackermann)
launch("encoder_odometry", "launch.py"),
]
diff --git a/autonomy/launch/teleop.py b/autonomy/launch/teleop.py
index 1385bc3..e21294f 100644
--- a/autonomy/launch/teleop.py
+++ b/autonomy/launch/teleop.py
@@ -5,21 +5,18 @@
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
+
def get_path(package, dir, file):
- return os.path.join(
- get_package_share_directory(package),
- dir,
- file
- )
+ return os.path.join(get_package_share_directory(package), dir, file)
+
def launch(package, file, launch_folder="launch", arguments={}):
return IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- get_path(package, launch_folder, file)
- ),
- launch_arguments=arguments.items()
+ PythonLaunchDescriptionSource(get_path(package, launch_folder, file)),
+ launch_arguments=arguments.items(),
)
+
def generate_launch_description():
imu_config = get_path("autonomy", "config", "imu.yml")
robot_localization_config = get_path("autonomy", "config", "robot_localization.yml")
@@ -28,73 +25,98 @@ def generate_launch_description():
[
# STATIC TRANSFORMS
Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name='static_transform_publisher_world_map',
- output='screen',
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_world_map",
+ output="screen",
arguments=[
- '0', '0', '0', # Translation: x = 0.035, y = 0.04, z = 0 (meters)
- '0', '0', '0', '1', # Rotation: 0
- 'world',
- 'map'
+ "0",
+ "0",
+ "0", # Translation: x = 0.035, y = 0.04, z = 0 (meters)
+ "0",
+ "0",
+ "0",
+ "1", # Rotation: 0
+ "world",
+ "map",
],
),
Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name='static_transform_publisher_map_odom',
- output='screen',
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_map_odom",
+ output="screen",
arguments=[
- '0', '0', '0', # Translation: x = 0.035, y = 0.04, z = 0 (meters)
- '0', '0', '0', '1', # Rotation: 0
- 'map',
- 'odom'
+ "0",
+ "0",
+ "0", # Translation: x = 0.035, y = 0.04, z = 0 (meters)
+ "0",
+ "0",
+ "0",
+ "1", # Rotation: 0
+ "map",
+ "odom",
],
),
Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name='static_transform_publisher_odom_base_link',
- output='screen',
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_odom_base_link",
+ output="screen",
arguments=[
- '0', '0', '0', # Translation: x = 0.035, y = 0.04, z = 0 (meters)
- '0', '0', '0', '1', # Rotation: 0
- 'odom',
- 'base_link'
+ "0",
+ "0",
+ "0", # Translation: x = 0.035, y = 0.04, z = 0 (meters)
+ "0",
+ "0",
+ "0",
+ "1", # Rotation: 0
+ "odom",
+ "base_link",
],
),
Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name='static_transform_publisher_base_link_lidar',
- output='screen',
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_base_link_lidar",
+ output="screen",
arguments=[
- '-0.035', '-0.04', '0', # Translation: x = 0.035, y = 0.04, z = 0 (meters)
- '0', '0', '1', '0', # Rotation: M_PI
- 'base_link',
- 'laser'
+ "-0.035",
+ "-0.04",
+ "0", # Translation: x = 0.035, y = 0.04, z = 0 (meters)
+ "0",
+ "0",
+ "1",
+ "0", # Rotation: M_PI
+ "base_link",
+ "laser",
],
),
Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name='static_transform_publisher_base_link_imu',
- output='screen',
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_base_link_imu",
+ output="screen",
arguments=[
- '-0.18', '0.0', '0', # Translation: x = -0.18, y = 0.07, z = 0 (meters)
- '0', '0', '0', '1', # Rotation: 0
- 'base_link',
- 'imu'
+ "-0.18",
+ "0.0",
+ "0", # Translation: x = -0.18, y = 0.07, z = 0 (meters)
+ "0",
+ "0",
+ "0",
+ "1", # Rotation: 0
+ "base_link",
+ "imu",
],
),
# # IMU
Node(
- package='witmotion_ros',
- executable='witmotion_ros_node',
+ package="witmotion_ros",
+ executable="witmotion_ros_node",
parameters=[
# {"port": "witmotion"}
imu_config
- ]
+ ],
),
# Robot Localization
# Node(
@@ -120,21 +142,20 @@ def generate_launch_description():
"map_update_interval": 0.05,
# "position_covariance_scale": 1.0,
# "yaw_covariance_scale": 1.0,
- "resolution": .13,
- "min_laser_range": .15,
+ "resolution": 0.13,
+ "min_laser_range": 0.15,
"max_laser_range": 12.0,
"use_scan_matching": True,
"do_loop_closing": True,
"use_scan_barycenter": True,
- "minimum_travel_distance": .05,
- "minimum_travel_heading": .05,
- "correlation_search_space_dimension": .2,
+ "minimum_travel_distance": 0.05,
+ "minimum_travel_heading": 0.05,
+ "correlation_search_space_dimension": 0.2,
"loop_search_space_dimension": 3.0,
- "angle_variance_penalty": 0.0
+ "angle_variance_penalty": 0.0,
}
- ]
+ ],
),
-
# UNITREE LIDAR
# Node(
# package='unitree_lidar_ros2',
@@ -154,11 +175,10 @@ def generate_launch_description():
# {'imu_frame': "unilidar_imu"},
# {'imu_topic': "unilidar/imu"}]
# ),
-
# CEV Localization
Node(
package="cev_localization",
- executable="ackermann_ekf",
+ executable="localization",
name="cev_localization_node",
output="screen",
parameters=[
@@ -169,20 +189,21 @@ def generate_launch_description():
}
],
),
-
# Trajectory Launch
# launch("trajectory_follower", "launch.py"),
-
## LAUNCH FILES
-
# Teleop
- launch("teleop", "keyboard.py"),
+ launch("teleop", "launch.py"),
# Autobrake
# launch("autobrake", "launch.py"),
# Serial Communicator
launch("serial_com", "launch.py"),
# RPLidar
- launch("sllidar_ros2", "sllidar_a1_launch.py", arguments={"serial_port": "/dev/rplidar"}),
+ launch(
+ "sllidar_ros2",
+ "sllidar_a1_launch.py",
+ arguments={"serial_port": "/dev/rplidar"},
+ ),
# Encoder Odometry (Ackermann)
launch("encoder_odometry", "launch.py"),
]
diff --git a/cev/all/cev_autobrake_ros2/CMakeLists.txt b/cev/all/cev_autobrake_ros2/CMakeLists.txt
new file mode 100644
index 0000000..8635046
--- /dev/null
+++ b/cev/all/cev_autobrake_ros2/CMakeLists.txt
@@ -0,0 +1,56 @@
+
+cmake_minimum_required(VERSION 3.8)
+project(autobrake)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES Clang)
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(ackermann_msgs REQUIRED)
+find_package(cev_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+
+add_executable(autobrake_node src/autobrake_node.cpp)
+
+ament_target_dependencies(autobrake_node
+ rclcpp
+ std_msgs
+ sensor_msgs
+ ackermann_msgs
+ cev_msgs
+ tf2_ros
+ tf2_geometry_msgs)
+
+install(TARGETS autobrake_node
+ DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY launch/
+ DESTINATION share/${PROJECT_NAME}/launch
+)
+
+install(DIRECTORY config/
+ DESTINATION share/${PROJECT_NAME}/config
+)
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # comment the line when a copyright and license is added to all source files
+ set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
+
diff --git a/mux/config/.gitkeep b/cev/all/cev_autobrake_ros2/config/.gitkeep
similarity index 100%
rename from mux/config/.gitkeep
rename to cev/all/cev_autobrake_ros2/config/.gitkeep
diff --git a/cev/all/cev_autobrake_ros2/launch/launch.py b/cev/all/cev_autobrake_ros2/launch/launch.py
new file mode 100644
index 0000000..34b79f1
--- /dev/null
+++ b/cev/all/cev_autobrake_ros2/launch/launch.py
@@ -0,0 +1,19 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+ return LaunchDescription(
+ [
+ Node(
+ package="autobrake",
+ executable="autobrake_node",
+ name="autobrake_node",
+ )
+ ]
+ )
+
diff --git a/cev/all/cev_autobrake_ros2/package.xml b/cev/all/cev_autobrake_ros2/package.xml
new file mode 100644
index 0000000..2641a8c
--- /dev/null
+++ b/cev/all/cev_autobrake_ros2/package.xml
@@ -0,0 +1,28 @@
+
+
+
+ autobrake
+ 0.0.0
+ TODO: Package description
+ sloth
+ TODO: License declaration
+
+ ament_cmake
+
+ ament_lint_auto
+ ament_lint_common
+
+ rclcpp
+ std_msgs
+ sensor_msgs
+ ackermann_msgs
+ cev_msgs
+ nav_msgs
+ tf2
+ tf2_ros
+ tf2_geometry_msgs
+
+
+ ament_cmake
+
+
diff --git a/cev/all/cev_autobrake_ros2/src/autobrake_node.cpp b/cev/all/cev_autobrake_ros2/src/autobrake_node.cpp
new file mode 100644
index 0000000..10bed6f
--- /dev/null
+++ b/cev/all/cev_autobrake_ros2/src/autobrake_node.cpp
@@ -0,0 +1,159 @@
+#include
+#include
+#include
+#include
+#include "cev_msgs/msg/sensor_collect.hpp"
+#include
+
+using std::placeholders::_1;
+
+/** NOTES
+ * R = L / tan(steering_angle)
+ * Circle Center = (-R, 0)
+ *
+ * Obstacle = (rsin(theta), rcos(theta))
+ *
+ * Angle from center to obstacle = tan(O_y / (O_x + R))
+ * Dist from center to obstacle = sqrt((O_x + R)^2 + O_y^2)
+ *
+ * Circum dist to obstacle = R * angle from center to obstacle
+ * Time to hit obstacle = circum dist to obstacle / velocity
+ *
+ * NOTE: LIDAR'S 0 is forward, and angles increment clockwise
+ */
+
+class AutobrakeNode : public rclcpp::Node {
+public:
+ AutobrakeNode(): Node("autobrake") {
+ forward_brake_.data = MAX_VEL;
+
+ scan_sub_ = this->create_subscription("scan", 1,
+ std::bind(&AutobrakeNode::checkCollision, this, _1));
+
+ sensor_collect_sub_ = this->create_subscription(
+ "sensor_collect", 1, std::bind(&AutobrakeNode::setVars, this, _1));
+
+ rc_movement_sub_ = this->create_subscription(
+ "rc_movement_msg", 1, std::bind(&AutobrakeNode::setTargets, this, _1));
+
+ forward_brake_pub_ = this->create_publisher("auto_max_vel", 1);
+
+ timer_ = this->create_wall_timer(std::chrono::milliseconds(10),
+ std::bind(&AutobrakeNode::publishBrake, this));
+
+ RCLCPP_INFO(this->get_logger(), "Autobrake node initialized.");
+ }
+
+private: // TODO: Make these constants configurable and use transforms instead of direct lidar
+ const float VEHICLE_LENGTH = 0.185;
+ const float VEHICLE_WIDTH = 0.18;
+ const float AUTOBRAKE_TIME = 0.7;
+ const float AUTOBRAKE_DISTANCE = 0.2;
+ const float MAX_VEL = 1.5;
+ const int MIN_COLLISIONS_FOR_BRAKE = 3;
+ const float LIDAR_ROTATIONAL_OFFSET = M_PI;
+ const float LIDAR_HORIZONTAL_OFFSET = 0.035;
+
+ float steering_angle_ = 0;
+ float velocity_ = 0;
+ float target_velocity_ = 0;
+ std_msgs::msg::Float32 forward_brake_;
+
+ rclcpp::Subscription::SharedPtr scan_sub_;
+ rclcpp::Subscription::SharedPtr sensor_collect_sub_;
+ rclcpp::Subscription::SharedPtr rc_movement_sub_;
+ rclcpp::Publisher::SharedPtr forward_brake_pub_;
+ rclcpp::TimerBase::SharedPtr timer_;
+
+ // Calculate the maximum velocity based on the distance to an obstacle
+ float maxVelocity(float dist) {
+ if (dist < 0) return MAX_VEL;
+ if (dist < AUTOBRAKE_DISTANCE) return 0;
+
+ return std::max(0.0f, -3.5f + std::sqrt(49 + 40 * dist) / 2 - 0.2f);
+ }
+
+ // LIDAR'S 0 is forward, and angles increment clockwise
+ // Check for potential collisions based on laser scan data
+ void checkCollision(const sensor_msgs::msg::LaserScan::SharedPtr data) {
+ int invert_flag = 1;
+ float min_vel = MAX_VEL;
+
+ if (steering_angle_ < 0) invert_flag = -1;
+
+ // Calculate turning radius (L / tan(steering_angle)), or infinity if driving straight
+ float turning_radius = std::abs(steering_angle_) > 0.01
+ ? VEHICLE_LENGTH / std::tan(invert_flag * steering_angle_)
+ : std::numeric_limits::infinity();
+
+ // Calculate turning radii of inside and outside wheels
+ float inner_wheel_radius = turning_radius - VEHICLE_WIDTH / 2;
+ float outer_wheel_radius = turning_radius + VEHICLE_WIDTH / 2;
+
+ for (size_t i = 0; i < data->ranges.size(); ++i) {
+ if (data->ranges[i] < data->range_min || data->ranges[i] > data->range_max) continue;
+
+ float dist_to_obstacle = std::numeric_limits::infinity();
+
+ // X and Y coordinates of obstacle relative to vehicle
+ float theta = LIDAR_ROTATIONAL_OFFSET + data->angle_min + i * data->angle_increment;
+ float r = data->ranges[i];
+ float x = invert_flag * (r * std::sin(theta) + LIDAR_HORIZONTAL_OFFSET);
+ float y = r * std::cos(theta);
+
+ if (turning_radius == std::numeric_limits::infinity()) {
+ // If driving straight, check if obstacle is within the width of the vehicle
+ if (std::abs(x) < VEHICLE_WIDTH / 2)
+ dist_to_obstacle = y;
+ else
+ continue;
+ } else {
+ // For curving, calculate radius distance to obstacle from center of movement circle
+ float radius_dist_to_obstacle =
+ std::sqrt((x + turning_radius) * (x + turning_radius) + y * y);
+
+ if (outer_wheel_radius > radius_dist_to_obstacle
+ && radius_dist_to_obstacle > inner_wheel_radius) {
+ // Calculate angle from center of movement circle to obstacle
+ float angle_from_center_to_obstacle = std::atan2(y, turning_radius + x);
+ if (angle_from_center_to_obstacle < 0)
+ angle_from_center_to_obstacle += 2 * M_PI;
+ angle_from_center_to_obstacle = fmod(angle_from_center_to_obstacle, 2 * M_PI);
+
+ // Calculate circumferential distance to obstacle
+ dist_to_obstacle = turning_radius * angle_from_center_to_obstacle;
+ } else
+ continue;
+ }
+
+ // If the obstacle is within a valid distance, adjust the minimum velocity
+ if (dist_to_obstacle >= 0) min_vel = std::min(min_vel, maxVelocity(dist_to_obstacle));
+ }
+
+ // Set the brake value to the minimum calculated velocity
+ forward_brake_.data = min_vel;
+ }
+
+ // Callback to set the velocity and steering angle from the sensor_collect topic
+ void setVars(const cev_msgs::msg::SensorCollect::SharedPtr data) {
+ velocity_ = data->velocity;
+ steering_angle_ = -data->steering_angle; // Invert the steering angle
+ }
+
+ // Callback to set the target velocity from the AckermannDrive topic
+ void setTargets(const ackermann_msgs::msg::AckermannDrive::SharedPtr data) {
+ target_velocity_ = data->speed;
+ }
+
+ // Publish the brake value at a regular interval
+ void publishBrake() {
+ forward_brake_pub_->publish(forward_brake_);
+ }
+};
+
+int main(int argc, char* argv[]) {
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
diff --git a/cev/all/cev_encoder_odometry_ros2/CMakeLists.txt b/cev/all/cev_encoder_odometry_ros2/CMakeLists.txt
new file mode 100644
index 0000000..7c02eaf
--- /dev/null
+++ b/cev/all/cev_encoder_odometry_ros2/CMakeLists.txt
@@ -0,0 +1,46 @@
+
+cmake_minimum_required(VERSION 3.8)
+project(encoder_odometry)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES Clang)
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(cev_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+add_executable(odometry src/odometry.cpp)
+ament_target_dependencies(odometry rclcpp std_msgs cev_msgs nav_msgs tf2_ros)
+
+install(TARGETS
+ odometry
+ DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY launch/
+ DESTINATION share/${PROJECT_NAME}/launch
+)
+
+install(DIRECTORY config/
+ DESTINATION share/${PROJECT_NAME}/config
+)
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # comment the line when a copyright and license is added to all source files
+ set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
+
diff --git a/serial_com/config/.gitkeep b/cev/all/cev_encoder_odometry_ros2/config/.gitkeep
similarity index 100%
rename from serial_com/config/.gitkeep
rename to cev/all/cev_encoder_odometry_ros2/config/.gitkeep
diff --git a/cev/all/cev_encoder_odometry_ros2/launch/launch.py b/cev/all/cev_encoder_odometry_ros2/launch/launch.py
new file mode 100644
index 0000000..d6c5ba7
--- /dev/null
+++ b/cev/all/cev_encoder_odometry_ros2/launch/launch.py
@@ -0,0 +1,20 @@
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+ return LaunchDescription(
+ [
+ Node(
+ package="encoder_odometry",
+ executable="odometry",
+ name="encoder_odometry_node",
+ )
+ ]
+ )
+
diff --git a/cev/all/cev_encoder_odometry_ros2/package.xml b/cev/all/cev_encoder_odometry_ros2/package.xml
new file mode 100644
index 0000000..f7e7e70
--- /dev/null
+++ b/cev/all/cev_encoder_odometry_ros2/package.xml
@@ -0,0 +1,24 @@
+
+
+
+ encoder_odometry
+ 0.0.0
+ TODO: Package description
+ sloth
+ TODO: License declaration
+
+ ament_cmake
+
+ ament_lint_auto
+ ament_lint_common
+
+ rclcpp
+ std_msgs
+ cev_msgs
+ nav_msgs
+ tf2_ros
+
+
+ ament_cmake
+
+
diff --git a/cev/all/cev_encoder_odometry_ros2/src/odometry.cpp b/cev/all/cev_encoder_odometry_ros2/src/odometry.cpp
new file mode 100644
index 0000000..e1c6002
--- /dev/null
+++ b/cev/all/cev_encoder_odometry_ros2/src/odometry.cpp
@@ -0,0 +1,122 @@
+#include
+#include
+#include "cev_msgs/msg/sensor_collect.hpp"
+#include
+#include
+#include
+#include
+
+using std::placeholders::_1;
+
+class OdometryNode : public rclcpp::Node
+{
+public:
+ OdometryNode() : Node("encoder_odometry") {
+ RCLCPP_INFO(this->get_logger(), "Initializing Ackermann Odometry Node");
+
+ sensor_collect_sub_ = this->create_subscription(
+ "sensor_collect", 1, std::bind(&OdometryNode::updateOdometry, this, _1));
+
+ odom_publisher_ = this->create_publisher("/encoder_odometry", 1);
+ }
+
+private:
+ const float WHEELBASE = 0.185;
+
+ double x_ = 0.0; // Forward of base_link is x
+ double y_ = 0.0;
+
+ double yaw_ = 0.0;
+
+ double old_left_rotations_ = 0.0;
+ double new_left_rotations_ = 0.0;
+
+ double old_steering_angle_ = 0.0;
+
+ rclcpp::Time prev_time_;
+
+ bool initialized = false;
+
+ double covariance_[36] = {
+ 0.01, 0, 0, 0, 0, 0,
+ 0, 0.01, 0, 0, 0, 0,
+ 0, 0, 0.01, 0, 0, 0,
+ 0, 0, 0, 0.01, 0, 0,
+ 0, 0, 0, 0, 0.01, 0,
+ 0, 0, 0, 0, 0, 0.01
+ };
+
+ rclcpp::Subscription::SharedPtr sensor_collect_sub_;
+ rclcpp::Publisher::SharedPtr odom_publisher_;
+
+ void updateOdometry(const cev_msgs::msg::SensorCollect::SharedPtr data) {
+ if (!initialized) {
+ prev_time_ = this->now();
+ initialized = true;
+ return;
+ }
+
+ auto current_time = this->now();
+ double dt = (current_time - prev_time_).seconds();
+ if (dt < 1e-6) return;
+
+ double delta_distance = data->velocity * dt;
+
+ double new_steering_angle = data->steering_angle;
+ double average_steering_angle = new_steering_angle;
+ // double average_steering_angle = (new_steering_angle + old_steering_angle_) / 2.0;
+
+ double turning_radius = WHEELBASE / std::tan(average_steering_angle);
+
+ double delta_theta = delta_distance / turning_radius;
+
+ yaw_ += delta_theta;
+ x_ += delta_distance * std::cos(yaw_);
+ y_ += delta_distance * std::sin(yaw_);
+
+ publishOdometry(this->now(), data->velocity, delta_theta / dt);
+
+ // Update carry vars
+ old_steering_angle_ = new_steering_angle;
+ prev_time_ = current_time;
+ }
+
+ void publishOdometry(const rclcpp::Time ¤t_time, double linear_velocity, double angular_velocity)
+ {
+ /**
+ * Publish odometry message using current pose/twist along with calculated velocities.
+ */
+ auto odom_msg = nav_msgs::msg::Odometry();
+ odom_msg.header.stamp = current_time;
+ odom_msg.header.frame_id = "odom";
+ odom_msg.child_frame_id = "base_link";
+
+ odom_msg.pose.pose.position.x = x_;
+ odom_msg.pose.pose.position.y = y_;
+
+ tf2::Quaternion q;
+ q.setRPY(0.0, 0.0, yaw_);
+ odom_msg.pose.pose.orientation.x = q.x();
+ odom_msg.pose.pose.orientation.y = q.y();
+ odom_msg.pose.pose.orientation.z = q.z();
+ odom_msg.pose.pose.orientation.w = q.w();
+
+ odom_msg.twist.twist.linear.x = linear_velocity;
+ odom_msg.twist.twist.angular.z = angular_velocity;
+
+ for (int i = 0; i < 36; i++)
+ {
+ odom_msg.pose.covariance[i] = covariance_[i];
+ }
+
+ odom_publisher_->publish(odom_msg);
+ }
+};
+
+int main(int argc, char *argv[])
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
diff --git a/cev/all/cev_localization_ros2/.clang-format b/cev/all/cev_localization_ros2/.clang-format
new file mode 100644
index 0000000..1d9b175
--- /dev/null
+++ b/cev/all/cev_localization_ros2/.clang-format
@@ -0,0 +1,36 @@
+# CEV C++ setup
+---
+BasedOnStyle: Google
+IndentWidth: 4
+Language: Cpp
+UseTab: Never
+DerivePointerAlignment: false
+PointerAlignment: Left
+BreakStringLiterals: true
+ReflowComments: true
+ContinuationIndentWidth: 4
+ColumnLimit: 100
+AlignAfterOpenBracket: DontAlign
+AlignTrailingComments:
+ Kind: Always
+ OverEmptyLines: 2
+AlignOperands: Align
+BreakBeforeBinaryOperators: NonAssignment
+PenaltyBreakAssignment: 60
+PenaltyBreakOpenParenthesis: 50
+IndentCaseLabels: true
+AllowShortFunctionsOnASingleLine: Empty
+SortIncludes: false
+MacroBlockBegin: "^#if .*$"
+MacroBlockEnd: "^#endif$"
+AlignEscapedNewlines: Right
+IndentPPDirectives: BeforeHash
+IndentAccessModifiers: false
+AccessModifierOffset: -4
+SpaceBeforeRangeBasedForLoopColon: false
+SpaceBeforeCaseColon: false
+SpaceBeforeCtorInitializerColon: false
+SpaceAfterTemplateKeyword: false
+NamespaceIndentation: All
+FixNamespaceComments: false
+---
diff --git a/cev/all/cev_localization_ros2/.devcontainer/Dockerfile.dev b/cev/all/cev_localization_ros2/.devcontainer/Dockerfile.dev
new file mode 100644
index 0000000..626dbf7
--- /dev/null
+++ b/cev/all/cev_localization_ros2/.devcontainer/Dockerfile.dev
@@ -0,0 +1,77 @@
+# Start with the official ROS Humble ROS-Core image based on Ubuntu Jammy
+FROM ros:humble-ros-core-jammy
+
+SHELL ["/bin/bash", "-c"]
+
+# Install basic build tools and ROS2 development dependencies
+RUN apt-get update && apt-get install --no-install-recommends -y \
+ build-essential \
+ git \
+ python3-colcon-common-extensions \
+ python3-colcon-mixin \
+ python3-rosdep \
+ python3-vcstool \
+ && rm -rf /var/lib/apt/lists/*
+
+# Set the ROS distro to Humble
+ENV ROS_DISTRO=humble
+
+# Initialize rosdep and update for the current ROS distro
+RUN rosdep init && \
+ rosdep update --rosdistro $ROS_DISTRO
+
+# Add and update colcon mixins and metadata
+RUN 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
+
+# Install ROS2 base packages specific to Humble distribution
+RUN apt-get update && apt-get install -y --no-install-recommends \
+ ros-humble-ros-base=0.10.0-1* \
+ && rm -rf /var/lib/apt/lists/*
+
+# Set environment variables and source ROS2 Humble setup
+ENV LANG=C.UTF-8 \
+ LC_ALL=C.UTF-8
+
+# Source the ROS2 environment automatically when a new shell is created
+RUN echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
+RUN echo "source /root/ws/install/setup.bash" >> ~/.bashrc
+
+RUN mkdir -p /root/ws/src
+RUN mkdir -p /root/ws/tmp
+COPY . /root/ws/tmp
+
+WORKDIR /root/ws
+
+RUN sudo apt update
+
+# Install dev tools
+RUN apt-get update
+RUN apt-get install -y curl
+RUN curl -fsSL https://apt.llvm.org/llvm-snapshot.gpg.key | tee /etc/apt/trusted.gpg.d/apt.llvm.org.asc
+RUN cat /etc/apt/trusted.gpg.d/apt.llvm.org.asc
+RUN echo $'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy main\n' \
+ $'deb-src http://apt.llvm.org/jammy/ llvm-toolchain-jammy main\n' \
+ $'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-19 main\n' \
+ $'deb-src http://apt.llvm.org/jammy/ llvm-toolchain-jammy-19 main\n' \
+ | tee -a /etc/apt/sources.list
+RUN apt-get update
+RUN apt-get install -y clang-format clang-format-19
+
+RUN source /opt/ros/$ROS_DISTRO/setup.bash \
+ && cd /root/ws \
+ && rosdep update \
+ && rosdep install --from-paths tmp -r -y
+
+RUN rm -rf /root/ws/tmp
+
+RUN source /opt/ros/$ROS_DISTRO/setup.bash \
+ && cd /root/ws \
+ && colcon build \
+ && git clone https://github.com/cornellev/cev_msgs.git src/cev_msgs
+
+EXPOSE 4567
\ No newline at end of file
diff --git a/cev/all/cev_localization_ros2/.devcontainer/devcontainer.json b/cev/all/cev_localization_ros2/.devcontainer/devcontainer.json
new file mode 100644
index 0000000..bc954aa
--- /dev/null
+++ b/cev/all/cev_localization_ros2/.devcontainer/devcontainer.json
@@ -0,0 +1,51 @@
+{
+ "name": "KF Dev-Container",
+ "build": {
+ "dockerfile": "Dockerfile.dev"
+ },
+ "workspaceMount": "source=${localWorkspaceFolder},target=/root/ws/src/ackermann_kf,type=bind",
+ "workspaceFolder": "/root/ws/src/ackermann_kf",
+ "runArgs": [
+ "--rm",
+ "--network=host",
+ "--ipc=host",
+ "--privileged"
+ ],
+ "containerEnv": {
+ "DISPLAY": "${localEnv:DISPLAY}"
+ },
+ "mounts": [
+ "source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached"
+ ],
+ "customizations": {
+ "vscode": {
+ "settings": {
+ "terminal.integrated.defaultProfile.linux": "bash",
+ "terminal.integrated.profiles.linux": {
+ "bash": {
+ "path": "/bin/bash"
+ }
+ }
+ },
+ "[cpp]": {
+ "editor.defaultFormatter": "xaver.clang-format"
+ },
+ "[python]": {
+ "diffEditor.ignoreTrimWhitespace": false,
+ "gitlens.codeLens.symbolScopes": [
+ "!Module"
+ ],
+ "editor.formatOnType": true,
+ "editor.wordBasedSuggestions": "off",
+ "editor.defaultFormatter": "ms-python.black-formatter"
+ },
+ "extensions": [
+ "ms-iot.vscode-ros",
+ "ms-vscode.cpptools-extension-pack",
+ "ms-python.black-formatter",
+ "xaver.clang-format"
+ ]
+ }
+ },
+ "postStartCommand": "git config --global --add safe.directory ${containerWorkspaceFolder}"
+}
\ No newline at end of file
diff --git a/cev/all/cev_localization_ros2/.gitignore b/cev/all/cev_localization_ros2/.gitignore
new file mode 100644
index 0000000..463fcb4
--- /dev/null
+++ b/cev/all/cev_localization_ros2/.gitignore
@@ -0,0 +1,6 @@
+.idea/
+build/
+log/
+install/
+cmake-build-debug/
+.vscode/
\ No newline at end of file
diff --git a/cev/all/cev_localization_ros2/CMakeLists.txt b/cev/all/cev_localization_ros2/CMakeLists.txt
new file mode 100644
index 0000000..c3d2239
--- /dev/null
+++ b/cev/all/cev_localization_ros2/CMakeLists.txt
@@ -0,0 +1,70 @@
+cmake_minimum_required(VERSION 3.10)
+project(cev_localization LANGUAGES CXX)
+
+# Set the C++ standard
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(yaml-cpp REQUIRED)
+
+# CEV
+find_package(cev_msgs REQUIRED)
+
+# Add subdirectory for the library
+add_subdirectory(cev_kalman_filter)
+
+# Include directories for project headers
+include_directories(
+ include
+ cev_kalman_filter/include
+)
+
+# Source files in the main src folder
+set(SOURCES
+ src/localization.cpp
+ src/config_parser.cpp
+ src/ros_sensor.cpp
+ src/std_ros_sensors.cpp
+)
+
+# Declare the executable
+add_executable(localization ${SOURCES})
+
+# Link libraries
+ament_target_dependencies(localization
+ rclcpp
+ sensor_msgs
+ geometry_msgs
+ tf2
+ tf2_ros
+ tf2_geometry_msgs
+ nav_msgs
+ cev_msgs
+ Eigen3
+)
+
+target_link_libraries(localization cev_kalman_filter yaml-cpp)
+
+# Install targets
+install(TARGETS localization
+ DESTINATION lib/${PROJECT_NAME})
+
+# Install additional resources
+install(DIRECTORY launch config
+ DESTINATION share/${PROJECT_NAME})
+
+install(DIRECTORY rosbag
+ DESTINATION share/${PROJECT_NAME})
+
+# Ament package configuration
+ament_package()
diff --git a/cev/all/cev_localization_ros2/README.md b/cev/all/cev_localization_ros2/README.md
new file mode 100644
index 0000000..9e4ef26
--- /dev/null
+++ b/cev/all/cev_localization_ros2/README.md
@@ -0,0 +1,2 @@
+# kalman_odom
+ROS 2 package for Ackermann Model EKF-based localization (in development)
diff --git a/cev/all/cev_localization_ros2/TODO.md b/cev/all/cev_localization_ros2/TODO.md
new file mode 100644
index 0000000..5e3ae8a
--- /dev/null
+++ b/cev/all/cev_localization_ros2/TODO.md
@@ -0,0 +1,5 @@
+ - Use message covariance, allow constant override in config
+ - Incorporate steering angle in ackermann informed calcs
+ - Write a proper README
+ - Figure out how to make an official ROS2 open source project
+ - Make sure the pipelining update frequencies make sense... (should we only update the roots periodically?)
\ No newline at end of file
diff --git a/cev/all/cev_localization_ros2/cev_kalman_filter b/cev/all/cev_localization_ros2/cev_kalman_filter
new file mode 160000
index 0000000..70bff5f
--- /dev/null
+++ b/cev/all/cev_localization_ros2/cev_kalman_filter
@@ -0,0 +1 @@
+Subproject commit 70bff5f8d6eb21fd585c5109e0ea74a4a2211a8f
diff --git a/cev/all/cev_localization_ros2/config/.gitkeep b/cev/all/cev_localization_ros2/config/.gitkeep
new file mode 100644
index 0000000..e69de29
diff --git a/cev/all/cev_localization_ros2/config/conf.rviz b/cev/all/cev_localization_ros2/config/conf.rviz
new file mode 100644
index 0000000..ad6ca68
--- /dev/null
+++ b/cev/all/cev_localization_ros2/config/conf.rviz
@@ -0,0 +1,282 @@
+Panels:
+ - Class: rviz_common/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /TF1/Frames1
+ - /MeowOdom1
+ - /MeowOdom1/Shape1
+ - /FilterOdom1
+ - /FilterOdom1/Shape1
+ Splitter Ratio: 0.5
+ Tree Height: 909
+ - Class: rviz_common/Selection
+ Name: Selection
+ - Class: rviz_common/Tool Properties
+ Expanded:
+ - /2D Goal Pose1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz_common/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz_common/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz_default_plugins/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Class: rviz_default_plugins/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: false
+ map:
+ Value: false
+ meow_link:
+ Value: true
+ odom:
+ Value: true
+ world:
+ Value: false
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: false
+ Tree:
+ world:
+ map:
+ odom:
+ meow_link:
+ {}
+ Update Interval: 0
+ Value: true
+ - Angle Tolerance: 0.10000000149011612
+ Class: rviz_default_plugins/Odometry
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: false
+ Enabled: true
+ Keep: 100
+ Name: MeowOdom
+ Position Tolerance: 0.10000000149011612
+ Shape:
+ Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Color: 170; 0; 255
+ Head Length: 0.20000000298023224
+ Head Radius: 0.10000000149011612
+ Shaft Length: 0
+ Shaft Radius: 0.05000000074505806
+ Value: Arrow
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /odometry/meow
+ Value: true
+ - Angle Tolerance: 0.10000000149011612
+ Class: rviz_default_plugins/Odometry
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: false
+ Enabled: true
+ Keep: 100
+ Name: FilterOdom
+ Position Tolerance: 0.10000000149011612
+ Shape:
+ Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Color: 170; 0; 0
+ Head Length: 0.20000000298023224
+ Head Radius: 0.10000000149011612
+ Shaft Length: 0
+ Shaft Radius: 0.05000000074505806
+ Value: Arrow
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /odometry/filtered
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz_default_plugins/Interact
+ Hide Inactive Objects: true
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/Select
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Line color: 128; 128; 0
+ - Class: rviz_default_plugins/SetInitialPose
+ Covariance x: 0.25
+ Covariance y: 0.25
+ Covariance yaw: 0.06853891909122467
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /initialpose
+ - Class: rviz_default_plugins/SetGoal
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /goal_pose
+ - Class: rviz_default_plugins/PublishPoint
+ Single click: true
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /clicked_point
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
+ Value: true
+ Views:
+ Current:
+ Class: rviz_default_plugins/Orbit
+ Distance: 15.735194206237793
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.5603981018066406
+ Target Frame:
+ Value: Orbit (rviz)
+ Yaw: 0.7553980350494385
+ Saved:
+ - Class: rviz_default_plugins/Orbit
+ Distance: 15.735194206237793
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Orbit
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.5603981018066406
+ Target Frame:
+ Value: Orbit (rviz)
+ Yaw: 0.7553980350494385
+ - Class: rviz_default_plugins/Orbit
+ Distance: 15.735194206237793
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Orbit
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.5603981018066406
+ Target Frame:
+ Value: Orbit (rviz)
+ Yaw: 0.7553980350494385
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1200
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000015600000416fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000416000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010000000416fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000416000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000051e0000041600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1920
+ X: 0
+ Y: 0
diff --git a/cev/all/cev_localization_ros2/config/ekf.conf_doc b/cev/all/cev_localization_ros2/config/ekf.conf_doc
new file mode 100644
index 0000000..4b3f697
--- /dev/null
+++ b/cev/all/cev_localization_ros2/config/ekf.conf_doc
@@ -0,0 +1,37 @@
+# General settings for the Kalman Filter node output
+odometry_settings:
+ time_step: double # Time step for the filter publish (in seconds)
+ topic: str # Topic to publish the odometry data
+
+# Define the sensors used by the node
+sensors:
+ sensor_name:
+ type: sensor_type # Sensor type (IMU, GPS, etc.)
+ topic: str # Topic to subscribe to
+ frame_id: str # Frame ID for the sensor data
+
+ state: [
+ x, dx, d2x,
+ y, dy, d2y,
+ z, dz, d2z,
+ roll, droll, d2roll,
+ pitch, dpitch, d2pitch,
+ yaw, dyaw, d2yaw,
+ tau, dtau, d2tau
+ ] # State variables for the sensor. Each should be true or false
+ covariance: [double] # Covariance matrix for the sensor data
+ estimator_models: [
+ model_name_1,
+ model_name_2,
+ ...
+ ] # List of strings representing models to use for the sensor data by name
+
+# Define update models with their parameters
+update_models:
+ model_name:
+ type: model_type # Model type (ACKERMANN, CARTESIAN, etc.)
+ parameters: [
+ parameter_1,
+ parameter_2,
+ ...
+ ] # Parameters for the model
diff --git a/cev/all/cev_localization_ros2/config/ekf_real.yml b/cev/all/cev_localization_ros2/config/ekf_real.yml
new file mode 100644
index 0000000..b22943a
--- /dev/null
+++ b/cev/all/cev_localization_ros2/config/ekf_real.yml
@@ -0,0 +1,50 @@
+# Define the sensors used by the node
+sensors:
+ witmotion_imu:
+ type: "IMU" # Sensor type (IMU, GPS, etc.)
+ topic: "/imu" # Topic to subscribe to
+ frame_id: "imu_frame" # Frame ID for the sensor data
+
+ state: ["d2_x", "d2_y", "yaw"] # State variables to use from the sensor data
+ covariance_multiplier: 1.0 # Multiplier to multiply message covariance by
+ use_message_covariance: true
+ estimator_models: ["ackermann", "cartesian"] # List of strings representing models to use for the sensor data by name
+
+ encoder_odometry:
+ type: "RAW"
+ topic: "/sensor_collect"
+ frame_id: "odom_frame"
+
+ state: ["d_x", "tau"]
+ covariance_multiplier: 2.0
+ estimator_models: ["ackermann", "cartesian"]
+
+# Define update models with their parameters
+update_models:
+ ackermann:
+ type: "ACKERMANN" # Model type (ACKERMANN, CARTESIAN, etc.)
+ state: ["x", "y", "d_x", "d_y", "yaw", "tau"] # State variables to use from the model in updates
+ estimator_models: []
+
+ publish:
+ active: true
+ topic: "/odometry/ackermann"
+ child_frame: "base_link"
+ parent_frame: "odom"
+ rate: 100
+
+ publish_tf: true
+
+ cartesian:
+ type: "CARTESIAN"
+ state: ["x", "y", "d_x", "d_y", "yaw", "d_yaw"]
+ estimator_models: ["ackermann"]
+
+ publish:
+ active: false
+ topic: "/odometry/cartesian"
+ child_frame: "base_link"
+ parent_frame: "odom_cart"
+ rate: 200
+
+ publish_tf: false
diff --git a/cev/all/cev_localization_ros2/ekf_sim.py b/cev/all/cev_localization_ros2/ekf_sim.py
new file mode 100644
index 0000000..bc9cc86
--- /dev/null
+++ b/cev/all/cev_localization_ros2/ekf_sim.py
@@ -0,0 +1,278 @@
+import copy
+
+import numpy as np
+import matplotlib.pyplot as plt
+import random
+import math
+
+"""
+Simple KF for 2D robot localization
+
+Notes:
+ x, y, yaw are relative to the parent frame.
+ dx, dy, ddx, ddy, steering_angle are relative to the robot frame, where x is forward
+"""
+
+cycle_dt = .01 # Constant time step
+real_t = 10 # Time to run the simulation for
+
+WB = .3
+
+# Current state
+x = np.array([
+ 0., # x
+ 0., # y
+ .5, # x'
+ 0., # y'
+ 0., # yaw
+ .79, # steering angle
+])
+
+# State covariance
+P = np.eye(len(x)) * .1
+
+
+# Simple bicycle model update step
+def f(x, dt):
+ x_ = x[0]
+ y_ = x[1]
+ d_x = x[2]
+ d_y = x[3]
+ yaw = x[4]
+ tau = x[5]
+
+ d_yaw = d_x * np.sin(tau) / WB * dt
+ yaw_new = yaw + d_yaw
+
+ x_new = x_ + d_x * np.cos(yaw + tau) * dt
+ y_new = y_ + d_x * np.sin(yaw + tau) * dt
+
+ return np.array([x_new, y_new, d_x, d_y, yaw_new, tau])
+
+
+def F(x, dt):
+ # Generates a Jacobian matrix representing an Simple bicycle model update step
+
+ d_x = x[2]
+ yaw = x[4]
+ tau = x[5]
+
+ p_yaw_dx = dt * np.sin(tau) / WB
+ p_yaw_tau = dt * d_x * np.cos(tau) / WB
+
+ p_x_dx = dt * np.cos(yaw + tau)
+ p_x_yaw = -dt * d_x * np.sin(yaw + tau)
+ p_x_tau = -dt * d_x * np.sin(yaw + tau)
+
+ p_y_dx = dt * np.sin(yaw + tau)
+ p_y_yaw = dt * d_x * np.cos(yaw + tau)
+ p_y_tau = dt * d_x * np.cos(yaw + tau)
+
+ # Jacobian matrix representing an Ackermann update step
+ return np.array([
+ # x y dx dy yaw tau # OUTPUT
+ [1., 0., p_x_dx, 0., p_x_yaw, p_x_tau], # x
+ [0., 1., p_y_dx, 0., p_y_yaw, p_y_tau], # y
+ [0., 0., 1., 0., 0., 0.], # dx
+ [0., 0., 0., 1., 0., 0.], # dy
+ [0., 0., p_yaw_dx, 0., 1., p_yaw_tau], # yaw
+ [0., 0., 0., 0., 0., 1.], # tau
+ ])
+
+
+# Measurement jacobian matrix
+def H(x, dt, tr):
+ # `tr` is the transformation from the state space to the sensor space
+ return (tr @ F(x, dt).T).T
+
+
+# Process noise covariance (update step). Low Q value means high confidence in model
+def Q(dt):
+ return dt * np.eye(len(x)) * .1
+
+
+def predict(x, P, dt):
+ # P = current variance
+ # x = current state
+
+ F_k = F(x, dt)
+ Q_k = Q(dt)
+
+ # Generate new state prediction with prediction matrix
+ state = f(x, dt)
+
+ # Generate new covariance prediction, with additional covariance to account for process noise
+ cov = F_k @ P @ F_k.T + Q_k
+
+ return state, cov
+
+
+def sensor_update(
+ state_,
+ variance,
+ sensor_state,
+ sensor_mul_matrix,
+ sensor_variance,
+ dt,
+ update_time_elapsed
+):
+ # H_k = H(state_, update_time_elapsed, sensor_mul_matrix)
+ H_k = H(state_, update_time_elapsed, sensor_mul_matrix)
+ R_k = sensor_variance
+ h = sensor_mul_matrix
+
+ predicted_state, predicted_variance = predict(state_, variance, dt)
+
+ predicted_sensor = h @ predicted_state
+ real_sensor = sensor_state
+
+ y_k = real_sensor - predicted_sensor
+
+ S_k = H_k @ predicted_variance @ H_k.T + R_k
+ K_k = predicted_variance @ H_k.T @ np.linalg.inv(S_k)
+
+ state_ = predicted_state + K_k @ y_k
+ predicted_variance = (np.eye(len(x)) - K_k @ H_k) @ predicted_variance
+
+ return state_, predicted_variance
+
+
+def fake_imu(state):
+ return np.array([
+ 0.,
+ 0.,
+ state[2] + random.gauss(0, .01),
+ state[3] + random.gauss(0, .01),
+ state[4] + random.gauss(0, .1),
+ 0.
+ ])
+
+
+def fake_enc(state):
+ return np.array([
+ 0.,
+ 0.,
+ state[2] + random.gauss(0, .1),
+ 0.,
+ 0.,
+ state[5]
+ ])
+
+rseed = 1000
+random.seed(rseed)
+
+def root_means_squared_estimation_error(ground_truth_states, estimated_states):
+ """
+ Take an array of ground truth and estimated states. Return the mean squared error of x and y
+ """
+ x_error = 0
+ y_error = 0
+
+ for i in range(len(ground_truth_states)):
+ x_error += (ground_truth_states[i][0] - estimated_states[i][0]) ** 2
+ y_error += (ground_truth_states[i][1] - estimated_states[i][1]) ** 2
+
+ return (x_error / len(ground_truth_states) + y_error / len(ground_truth_states)) ** .5
+
+
+def main_loop():
+ state = copy.deepcopy(x)
+ variance = copy.deepcopy(P)
+ next_state = copy.deepcopy(x)
+ next_var = copy.deepcopy(P)
+
+ true_states = []
+ est_states = []
+
+ sensor_mul_matrix = {
+ "imu": np.array([
+ [0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0],
+ [0, 0, 1, 0, 0, 0],
+ [0, 0, 0, 1, 0, 0],
+ [0, 0, 0, 0, 1, 0],
+ [0, 0, 0, 0, 0, 0]
+ ]),
+ "enc": np.array([
+ [0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0],
+ [0, 0, 1, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 1]
+ ]),
+ }
+
+ last_update_time_matrix = {
+ "imu": 0,
+ "enc": 0,
+ }
+
+ sensor_state = {
+ "imu": fake_imu,
+ "enc": fake_enc,
+ }
+
+ sensor_variances = {
+ "imu": np.eye(len(x)) * 1,
+ "enc": np.eye(len(x)) * .1,
+ }
+
+ # Simulate the state change with F and a dt of .1 100 times using matplotlib
+ for i in range(int(real_t / cycle_dt)):
+ true_states.append(next_state)
+ est_states.append(state)
+
+ seed = random.random()
+
+ # Adjust steering angle continuously
+ # next_state[5] = math.sin(i * cycle_dt / 2) * .79
+ next_state[5] = math.sin(i * cycle_dt / 2) * .79
+
+ next_state, next_var = predict(next_state, next_var, cycle_dt)
+ plt.plot(next_state[0], next_state[1], 'bo')
+
+ if seed < .8:
+ state, variance = predict(state, variance, cycle_dt)
+ elif seed < .9:
+ current_time = i * cycle_dt
+ last_update_time = last_update_time_matrix["imu"]
+ update_time_elapsed = current_time - last_update_time
+ last_update_time_matrix["imu"] = current_time
+
+ state, variance = sensor_update(
+ state,
+ variance,
+ sensor_state["imu"](next_state),
+ sensor_mul_matrix["imu"],
+ sensor_variances["imu"],
+ cycle_dt,
+ update_time_elapsed
+ )
+ elif seed < 1.0:
+ current_time = i * cycle_dt
+ last_update_time = last_update_time_matrix["enc"]
+ update_time_elapsed = current_time - last_update_time
+ last_update_time_matrix["enc"] = current_time
+
+ state, variance = sensor_update(
+ state,
+ variance,
+ sensor_state["enc"](next_state),
+ sensor_mul_matrix["enc"],
+ sensor_variances["enc"],
+ cycle_dt,
+ update_time_elapsed
+ )
+
+ plt.plot(state[0], state[1], 'ro')
+
+ true_states.append(next_state)
+ est_states.append(state)
+
+ print("Root mean squared estimation error: ", root_means_squared_estimation_error(true_states, est_states))
+
+
+if __name__ == "__main__":
+ main_loop()
+ plt.show()
diff --git a/cev/all/cev_localization_ros2/gen_jacobian.py b/cev/all/cev_localization_ros2/gen_jacobian.py
new file mode 100644
index 0000000..ffa1402
--- /dev/null
+++ b/cev/all/cev_localization_ros2/gen_jacobian.py
@@ -0,0 +1,54 @@
+import sympy as sp
+
+# Define state variables
+x, y, z = sp.symbols('x y z')
+roll, pitch, yaw = sp.symbols('roll pitch yaw')
+dx, dy, dz = sp.symbols('dx dy dz')
+droll, dpitch, dyaw = sp.symbols('droll dpitch dyaw')
+ddx, ddy, ddz = sp.symbols('ddx ddy ddz')
+tau, dtau, ddtau = sp.symbols('tau dtau ddtau')
+dt, wheelbase = sp.symbols('dt wheelbase')
+
+# New state definitions
+new_x = x + dx * sp.cos(yaw + tau) * dt
+new_y = y + dx * sp.sin(yaw + tau) * dt
+new_dx = dx + ddx * dt
+new_dy = dy + ddy * dt
+new_dtau = dtau + ddtau * dt
+new_dyaw = dx * sp.sin(tau) / wheelbase * dt
+new_yaw = yaw + new_dyaw
+new_tau = tau + dtau * dt
+
+# Combine into vectors
+new_state = sp.Matrix([
+ new_x, new_y, z, roll, pitch, new_yaw,
+ new_dx, new_dy, dz, droll, dpitch, new_dyaw,
+ ddx, ddy, ddz, new_tau, new_dtau, ddtau
+])
+
+current_state = sp.Matrix([
+ x, y, z, roll, pitch, yaw,
+ dx, dy, dz, droll, dpitch, dyaw,
+ ddx, ddy, ddz, tau, dtau, ddtau
+])
+
+# Compute the Jacobian
+jacobian = new_state.jacobian(current_state)
+
+# State names for mapping
+state_names = [
+ "x__", "y__", "z__", "roll__", "pitch__", "yaw__",
+ "d_x__", "d_y__", "d_z__", "d_roll__", "d_pitch__", "d_yaw__",
+ "d2_x__", "d2_y__", "d2_z__", "tau__", "d_tau__", "d2_tau__"
+]
+
+# Generate C++ code for non-zero Jacobian entries
+cpp_code = []
+for i in range(jacobian.shape[0]):
+ for j in range(jacobian.shape[1]):
+ entry = jacobian[i, j]
+ if entry not in [0, 1]: # Skip zero entries
+ cpp_code.append(f"F_k({state_names[i]}, {state_names[j]}) = {sp.ccode(entry)};")
+
+# Print the generated C++ code
+print("\n".join(cpp_code))
diff --git a/cev/all/cev_localization_ros2/include/config_parser.h b/cev/all/cev_localization_ros2/include/config_parser.h
new file mode 100644
index 0000000..1898073
--- /dev/null
+++ b/cev/all/cev_localization_ros2/include/config_parser.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "estimator.h"
+
+namespace cev_localization {
+ namespace config_parser {
+ struct Sensor {
+ std::string type;
+ std::string topic;
+ std::optional frame_id;
+ std::vector state_mask;
+ double covariance_multiplier;
+ bool use_message_covariance;
+ std::vector estimator_models;
+
+ static Sensor loadSensor(const YAML::Node& sensorNode);
+ };
+
+ struct Publish {
+ bool active = false;
+ std::string topic = "";
+ int rate = 100;
+ std::string child_frame = "";
+ std::string parent_frame = "";
+
+ bool publish_tf = false;
+
+ static Publish loadPublish(const YAML::Node& publishNode);
+ };
+
+ struct UpdateModel {
+ std::string type;
+ std::vector state_mask;
+ std::vector estimator_models;
+
+ Publish publish;
+
+ static UpdateModel loadUpdateModel(const YAML::Node& modelNode);
+ };
+
+ struct Config {
+ // Sensors
+ std::unordered_map sensors;
+
+ // Update Models
+ std::unordered_map update_models;
+
+ static Config loadConfig(const YAML::Node& configNode);
+ };
+
+ class ConfigParser {
+ public:
+ static Config load(const std::string& filePath);
+ };
+ }
+}
diff --git a/cev/all/cev_localization_ros2/include/ros_sensor.h b/cev/all/cev_localization_ros2/include/ros_sensor.h
new file mode 100644
index 0000000..58ddd93
--- /dev/null
+++ b/cev/all/cev_localization_ros2/include/ros_sensor.h
@@ -0,0 +1,44 @@
+#pragma once
+
+#include "sensor.h"
+
+namespace cev_localization {
+
+ template
+ class RosSensor : public ckf::Sensor {
+ protected:
+ ckf::M multiplier = ckf::M::Zero();
+
+ void new_time_handler(double time) {
+ previous_update_time = most_recent_update_time;
+ most_recent_update_time = time;
+ }
+
+ public:
+ RosSensor(std::string topic, ckf::V state, ckf::M covariance,
+ std::vector> dependents)
+ : Sensor(state, covariance, dependents) {
+ this->topic = topic;
+ }
+
+ std::string topic;
+
+ /**
+ * Handle a new message. Meant to be used as or in a message subscriber.
+ */
+ void msg_handler(typename T::SharedPtr msg) {
+ ckf::StatePackage update = msg_update(msg);
+ updateInternals(update);
+ update_dependents();
+ }
+
+ /**
+ * Update the state with a new message, and return the time of the new message
+ */
+ virtual ckf::StatePackage msg_update(typename T::SharedPtr msg) = 0;
+
+ ckf::M state_matrix_multiplier() {
+ return multiplier;
+ }
+ };
+}
\ No newline at end of file
diff --git a/cev/all/cev_localization_ros2/include/std_ros_sensors.h b/cev/all/cev_localization_ros2/include/std_ros_sensors.h
new file mode 100644
index 0000000..40a99a8
--- /dev/null
+++ b/cev/all/cev_localization_ros2/include/std_ros_sensors.h
@@ -0,0 +1,52 @@
+#pragma once
+
+#include "sensor_msgs/msg/imu.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "cev_msgs/msg/sensor_collect.hpp"
+#include
+#include
+
+#include "ros_sensor.h"
+#include "estimator.h"
+
+using namespace ckf;
+
+namespace cev_localization {
+ namespace standard_ros_sensors {
+
+ class IMUSensor : public RosSensor {
+ private:
+ double pos_mod(double angle);
+
+ protected:
+ bool initialized;
+ bool relative;
+ double initial_yaw;
+ double last_reported_yaw;
+ double last_sensor_raw_yaw;
+ bool use_message_covariance = true;
+
+ public:
+ IMUSensor(std::string topic, V state, M covariance,
+ std::vector> dependents,
+ std::vector state_mask = {"d2_x", "d2_y", "yaw"},
+ bool use_message_covariance = true, bool relative = true);
+
+ StatePackage msg_update(sensor_msgs::msg::Imu::SharedPtr msg);
+ };
+
+ class RawSensor : public RosSensor {
+ protected:
+ bool use_message_covariance = true;
+
+ public:
+ RawSensor(std::string topic, V state, M covariance,
+ std::vector> dependents,
+ std::vector state_mask = {"d_x", "tau"},
+ bool use_message_covariance = true);
+
+ StatePackage msg_update(cev_msgs::msg::SensorCollect::SharedPtr msg);
+ };
+
+ }
+}
\ No newline at end of file
diff --git a/cev/all/cev_localization_ros2/launch/.gitkeep b/cev/all/cev_localization_ros2/launch/.gitkeep
new file mode 100644
index 0000000..e69de29
diff --git a/cev/all/cev_localization_ros2/launch/debug_launch.py b/cev/all/cev_localization_ros2/launch/debug_launch.py
new file mode 100644
index 0000000..50ee173
--- /dev/null
+++ b/cev/all/cev_localization_ros2/launch/debug_launch.py
@@ -0,0 +1,89 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def get_path(package, dir, file):
+ return os.path.join(get_package_share_directory(package), dir, file)
+
+
+def generate_launch_description():
+ rviz2_config = get_path("cev_localization", "config", "conf.rviz")
+
+ return LaunchDescription(
+ [
+ Node(
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_world_map",
+ output="screen",
+ arguments=[
+ "0",
+ "0",
+ "0", # Translation: x = 0.035, y = 0.04, z = 0 (meters)
+ "0",
+ "0",
+ "0",
+ "1", # Rotation: 0
+ "world",
+ "map",
+ ],
+ ),
+ Node(
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_map_odom",
+ output="screen",
+ arguments=[
+ "0",
+ "0",
+ "0", # Translation: x = 0.035, y = 0.04, z = 0 (meters)
+ "0",
+ "0",
+ "0",
+ "1", # Rotation: 0
+ "map",
+ "odom",
+ ],
+ ),
+ Node(
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_transform_publisher_odom_base_link",
+ output="screen",
+ arguments=[
+ "0",
+ "0",
+ "0", # Translation: x = 0.035, y = 0.04, z = 0 (meters)
+ "0",
+ "0",
+ "0",
+ "1", # Rotation: 0
+ "odom",
+ "meow_link",
+ ],
+ ),
+ Node(
+ package="cev_localization",
+ executable="localization",
+ name="cev_localization_node",
+ output="screen",
+ parameters=[
+ {
+ "config_file": get_path(
+ "cev_localization", "config", "ekf_real.yml"
+ )
+ }
+ ],
+ ),
+ Node(
+ package="rviz2",
+ executable="rviz2",
+ arguments=["--display-config", rviz2_config],
+ name="rviz2",
+ output="screen",
+ ),
+ ]
+ )
diff --git a/cev/all/cev_localization_ros2/launch/launch.py b/cev/all/cev_localization_ros2/launch/launch.py
new file mode 100644
index 0000000..026a960
--- /dev/null
+++ b/cev/all/cev_localization_ros2/launch/launch.py
@@ -0,0 +1,29 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def get_path(package, dir, file):
+ return os.path.join(get_package_share_directory(package), dir, file)
+
+
+def generate_launch_description():
+ return LaunchDescription(
+ [
+ Node(
+ package="cev_localization",
+ executable="localization",
+ name="cev_localization_node",
+ output="screen",
+ parameters=[
+ {
+ "config_file": get_path(
+ "cev_localization", "config", "ekf_real.yml"
+ )
+ }
+ ],
+ )
+ ]
+ )
diff --git a/cev/all/cev_localization_ros2/package.xml b/cev/all/cev_localization_ros2/package.xml
new file mode 100644
index 0000000..8c5025c
--- /dev/null
+++ b/cev/all/cev_localization_ros2/package.xml
@@ -0,0 +1,33 @@
+
+
+
+ cev_localization
+ 0.0.0
+ TODO: Package description
+ sloth
+ TODO: License declaration
+
+ ament_cmake
+
+ rclcpp
+ sensor_msgs
+ std_msgs
+ eigen3
+ tf2
+ tf2_ros
+ tf2_geometry_msgs
+ nav_msgs
+
+ cev_msgs
+ cev_kalman_filter
+
+ yaml-cpp
+
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
\ No newline at end of file
diff --git a/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_1/ackermann_recording_v_1_0.db3 b/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_1/ackermann_recording_v_1_0.db3
new file mode 100644
index 0000000..35837e6
Binary files /dev/null and b/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_1/ackermann_recording_v_1_0.db3 differ
diff --git a/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_1/metadata.yaml b/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_1/metadata.yaml
new file mode 100644
index 0000000..b86d8b9
--- /dev/null
+++ b/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_1/metadata.yaml
@@ -0,0 +1,98 @@
+rosbag2_bagfile_information:
+ version: 5
+ storage_identifier: sqlite3
+ duration:
+ nanoseconds: 49551146185
+ starting_time:
+ nanoseconds_since_epoch: 1732241210235021710
+ message_count: 18856
+ topics_with_message_count:
+ - topic_metadata:
+ name: /tf
+ type: tf2_msgs/msg/TFMessage
+ 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: 1461
+ - topic_metadata:
+ name: /rc_movement_msg
+ type: ackermann_msgs/msg/AckermannDrive
+ 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: 2841
+ - topic_metadata:
+ name: /encoder_odometry
+ type: nav_msgs/msg/Odometry
+ 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: 2283
+ - topic_metadata:
+ name: /odometry/filtered
+ type: nav_msgs/msg/Odometry
+ 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: 1461
+ - topic_metadata:
+ name: /tf_static
+ type: tf2_msgs/msg/TFMessage
+ serialization_format: cdr
+ offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\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\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\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\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\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: 5
+ - topic_metadata:
+ name: /events/write_split
+ type: rosbag2_interfaces/msg/WriteSplitEvent
+ 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: 0
+ - topic_metadata:
+ name: /auto_max_vel
+ type: std_msgs/msg/Float32
+ 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: 4885
+ - topic_metadata:
+ name: /joy
+ type: sensor_msgs/msg/Joy
+ 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: 3103
+ - topic_metadata:
+ name: /diagnostics
+ type: diagnostic_msgs/msg/DiagnosticArray
+ 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: 97
+ - topic_metadata:
+ name: /parameter_events
+ type: rcl_interfaces/msg/ParameterEvent
+ 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\n- 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\n- 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\n- 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\n- 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\n- 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\n- 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: 0
+ - topic_metadata:
+ name: /sensor_collect
+ type: cev_msgs/msg/SensorCollect
+ 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: 2325
+ - topic_metadata:
+ name: /rosout
+ type: rcl_interfaces/msg/Log
+ serialization_format: cdr
+ offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false"
+ message_count: 17
+ - topic_metadata:
+ name: /scan
+ type: sensor_msgs/msg/LaserScan
+ 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: 378
+ compression_format: ""
+ compression_mode: ""
+ relative_file_paths:
+ - ackermann_recording_v_1_0.db3
+ files:
+ - path: ackermann_recording_v_1_0.db3
+ starting_time:
+ nanoseconds_since_epoch: 1732241210235021710
+ duration:
+ nanoseconds: 49551146185
+ message_count: 18856
\ No newline at end of file
diff --git a/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_2/ackermann_recording_v_2_0.db3 b/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_2/ackermann_recording_v_2_0.db3
new file mode 100644
index 0000000..df4740b
Binary files /dev/null and b/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_2/ackermann_recording_v_2_0.db3 differ
diff --git a/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_2/metadata.yaml b/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_2/metadata.yaml
new file mode 100644
index 0000000..8ee250e
--- /dev/null
+++ b/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_2/metadata.yaml
@@ -0,0 +1,116 @@
+rosbag2_bagfile_information:
+ version: 5
+ storage_identifier: sqlite3
+ duration:
+ nanoseconds: 49878255986
+ starting_time:
+ nanoseconds_since_epoch: 1732324873007091423
+ message_count: 19079
+ topics_with_message_count:
+ - topic_metadata:
+ name: /scan
+ type: sensor_msgs/msg/LaserScan
+ 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: 329
+ - topic_metadata:
+ name: /odometry/filtered
+ type: nav_msgs/msg/Odometry
+ 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: 1497
+ - topic_metadata:
+ name: /parameter_events
+ type: rcl_interfaces/msg/ParameterEvent
+ 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\n- 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\n- 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\n- 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\n- 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\n- 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\n- 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\n- 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\n- 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\n- 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\n- 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: 0
+ - topic_metadata:
+ name: /tf_static
+ type: tf2_msgs/msg/TFMessage
+ serialization_format: cdr
+ offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\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\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\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\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\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: 5
+ - topic_metadata:
+ name: /tf
+ type: tf2_msgs/msg/TFMessage
+ 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: 1500
+ - topic_metadata:
+ name: /auto_max_vel
+ type: std_msgs/msg/Float32
+ 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: 4979
+ - topic_metadata:
+ name: /joy
+ type: sensor_msgs/msg/Joy
+ 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: 2648
+ - topic_metadata:
+ name: /sensor_collect
+ type: cev_msgs/msg/SensorCollect
+ 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: 2339
+ - topic_metadata:
+ name: /rc_movement_msg
+ type: ackermann_msgs/msg/AckermannDrive
+ 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: 2558
+ - topic_metadata:
+ name: /events/write_split
+ type: rosbag2_interfaces/msg/WriteSplitEvent
+ 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: 0
+ - topic_metadata:
+ name: /orientation
+ type: geometry_msgs/msg/Quaternion
+ 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: 0
+ - topic_metadata:
+ name: /imu
+ type: sensor_msgs/msg/Imu
+ 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: 389
+ - topic_metadata:
+ name: /diagnostics
+ type: diagnostic_msgs/msg/DiagnosticArray
+ 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: 99
+ - topic_metadata:
+ name: /rosout
+ type: rcl_interfaces/msg/Log
+ serialization_format: cdr
+ offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false"
+ message_count: 12
+ - topic_metadata:
+ name: /encoder_odometry
+ type: nav_msgs/msg/Odometry
+ 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: 2336
+ - topic_metadata:
+ name: /magnetometer
+ type: sensor_msgs/msg/MagneticField
+ 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: 388
+ compression_format: ""
+ compression_mode: ""
+ relative_file_paths:
+ - ackermann_recording_v_2_0.db3
+ files:
+ - path: ackermann_recording_v_2_0.db3
+ starting_time:
+ nanoseconds_since_epoch: 1732324873007091423
+ duration:
+ nanoseconds: 49878255986
+ message_count: 19079
\ No newline at end of file
diff --git a/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_3/ackermann_recording_v_3_0.db3 b/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_3/ackermann_recording_v_3_0.db3
new file mode 100644
index 0000000..6f3335f
Binary files /dev/null and b/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_3/ackermann_recording_v_3_0.db3 differ
diff --git a/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_3/metadata.yaml b/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_3/metadata.yaml
new file mode 100644
index 0000000..f458918
--- /dev/null
+++ b/cev/all/cev_localization_ros2/rosbag/ackermann_recording_v_3/metadata.yaml
@@ -0,0 +1,116 @@
+rosbag2_bagfile_information:
+ version: 5
+ storage_identifier: sqlite3
+ duration:
+ nanoseconds: 23004854249
+ starting_time:
+ nanoseconds_since_epoch: 1732436633915847685
+ message_count: 8397
+ topics_with_message_count:
+ - topic_metadata:
+ name: /tf
+ type: tf2_msgs/msg/TFMessage
+ 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: 633
+ - topic_metadata:
+ name: /tf_static
+ type: tf2_msgs/msg/TFMessage
+ serialization_format: cdr
+ offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\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\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\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\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\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: 6
+ - topic_metadata:
+ name: /scan
+ type: sensor_msgs/msg/LaserScan
+ 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: 172
+ - topic_metadata:
+ name: /odometry/filtered
+ type: nav_msgs/msg/Odometry
+ 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: 634
+ - topic_metadata:
+ name: /parameter_events
+ type: rcl_interfaces/msg/ParameterEvent
+ 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\n- 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\n- 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\n- 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\n- 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\n- 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\n- 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\n- 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\n- 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: 0
+ - topic_metadata:
+ name: /sensor_collect
+ type: cev_msgs/msg/SensorCollect
+ 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: 997
+ - topic_metadata:
+ name: /diagnostics
+ type: diagnostic_msgs/msg/DiagnosticArray
+ 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: 42
+ - topic_metadata:
+ name: /rosout
+ type: rcl_interfaces/msg/Log
+ serialization_format: cdr
+ offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false"
+ message_count: 93
+ - topic_metadata:
+ name: /imu
+ type: sensor_msgs/msg/Imu
+ 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: 231
+ - topic_metadata:
+ name: /orientation
+ type: geometry_msgs/msg/Quaternion
+ 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: 0
+ - topic_metadata:
+ name: /events/write_split
+ type: rosbag2_interfaces/msg/WriteSplitEvent
+ 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: 0
+ - topic_metadata:
+ name: /auto_max_vel
+ type: std_msgs/msg/Float32
+ 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: 2284
+ - topic_metadata:
+ name: /joy
+ type: sensor_msgs/msg/Joy
+ 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: 1016
+ - topic_metadata:
+ name: /magnetometer
+ type: sensor_msgs/msg/MagneticField
+ 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: 231
+ - topic_metadata:
+ name: /encoder_odometry
+ type: nav_msgs/msg/Odometry
+ 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: 1077
+ - topic_metadata:
+ name: /rc_movement_msg
+ type: ackermann_msgs/msg/AckermannDrive
+ 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: 981
+ compression_format: ""
+ compression_mode: ""
+ relative_file_paths:
+ - ackermann_recording_v_3_0.db3
+ files:
+ - path: ackermann_recording_v_3_0.db3
+ starting_time:
+ nanoseconds_since_epoch: 1732436633915847685
+ duration:
+ nanoseconds: 23004854249
+ message_count: 8397
\ No newline at end of file
diff --git a/cev/all/cev_localization_ros2/src/ackermann_ekf.cpp b/cev/all/cev_localization_ros2/src/ackermann_ekf.cpp
new file mode 100644
index 0000000..35046d5
--- /dev/null
+++ b/cev/all/cev_localization_ros2/src/ackermann_ekf.cpp
@@ -0,0 +1,206 @@
+#include
+#include
+#include
+
+#include "model.h"
+#include "ros_sensor.h"
+#include "config_parser.h"
+#include "model.h"
+#include "sensor.h"
+#include "standard_models.h"
+#include "std_ros_sensors.h"
+
+#include
+
+#include
+
+using std::placeholders::_1;
+
+using namespace ckf;
+using namespace cev_localization;
+
+class LocalizationNode : public rclcpp::Node {
+public:
+ LocalizationNode(): Node("CEVLocalizationNode") {
+ RCLCPP_INFO(this->get_logger(), "Initializing CEV Localization Node");
+
+ this->declare_parameter("config_file", "config/ekf_real.yml");
+ std::string config_file_path = this->get_parameter("config_file").as_string();
+
+ RCLCPP_INFO(this->get_logger(), "Parsing config file at %s", config_file_path.c_str());
+
+ // Load the YAML configurations
+ config = config_parser::ConfigParser::loadConfig(config_file_path);
+
+ std::unordered_map> update_models;
+
+ for (std::pair model: config.update_models) {
+ std::string name = model.first;
+ config_parser::UpdateModel& mod = model.second;
+
+ if (update_models.find(name) != update_models.end()) {
+ RCLCPP_ERROR(this->get_logger(), "Model `%s` already exists", name.c_str());
+ throw std::runtime_error("Model already exists");
+ }
+
+ if (mod.type == "ACKERMANN") {
+ update_models[name] = std::make_shared(
+ V::Zero(), M::Identity() * .1, M::Identity() * .1, .185, mod.state_mask);
+ } else if (mod.type == "CARTESIAN") {
+ update_models[name] = std::make_shared(
+ V::Zero(), M::Identity() * .1, M::Identity() * .1, mod.state_mask);
+ } else {
+ RCLCPP_ERROR(this->get_logger(), "Unknown model type: `%s`", mod.type.c_str());
+ throw std::runtime_error("Unknown model type");
+ }
+ }
+
+ for (std::pair sensor: config.sensors) {
+ std::string name = sensor.first;
+ config_parser::Sensor& sen = sensor.second;
+
+ if (sensors.find(name) != sensors.end()) {
+ RCLCPP_ERROR(this->get_logger(), "Sensor `%s` already exists", name.c_str());
+ throw std::runtime_error("Sensor already exists");
+ }
+
+ // Create array for models to bind to
+ std::vector> models;
+ for (std::string& model_name: sen.estimator_models) {
+ if (update_models.find(model_name) == update_models.end()) {
+ RCLCPP_ERROR(this->get_logger(), "Model `%s` does not exist",
+ model_name.c_str());
+ throw std::runtime_error("Model does not exist");
+ } else {
+ models.push_back(update_models[model_name]);
+ }
+ }
+
+ if (sen.type == "IMU") {
+ auto sensor = std::make_shared(sen.topic,
+ V::Zero(), M::Identity() * .1, models, sen.state_mask,
+ sen.use_message_covariance);
+
+ sensors[name] = sensor;
+
+ sensor_subscribers[name] = this->create_subscription(
+ sen.topic, 10, [sensor](const sensor_msgs::msg::Imu::SharedPtr msg) {
+ sensor->msg_handler(msg);
+ });
+
+ } else if (sen.type == "RAW") {
+ auto sensor = std::make_shared(sen.topic,
+ V::Zero(), M::Identity() * .1, models, sen.state_mask,
+ sen.use_message_covariance);
+
+ sensors[name] = sensor;
+
+ sensor_subscribers[name] = this->create_subscription(
+ sen.topic, 10, [sensor](const cev_msgs::msg::SensorCollect::SharedPtr msg) {
+ sensor->msg_handler(msg);
+ });
+ } else {
+ RCLCPP_ERROR(this->get_logger(), "Unknown sensor type: `%s`", sen.type.c_str());
+ throw std::runtime_error("Unknown sensor type");
+ }
+ }
+
+ if (update_models.find(config.main_model) == update_models.end()) {
+ RCLCPP_ERROR(this->get_logger(), "Main model `%s` does not exist",
+ config.main_model.c_str());
+ throw std::runtime_error("Main model does not exist");
+ }
+
+ RCLCPP_INFO(this->get_logger(), "Configuration file parsed! Finishing initialization.");
+
+ main_model = update_models[config.main_model].get();
+
+ timer = this->create_wall_timer(std::chrono::milliseconds((int)(config.time_step * 1000.0)),
+ std::bind(&LocalizationNode::timer_callback, this));
+
+ tf_broadcaster_ = std::make_shared(this);
+ odom_pub = this->create_publisher(config.odometry_topic, 1);
+ }
+
+private:
+ config_parser::Config config;
+
+ std::unordered_map sensor_subscribers;
+ std::unordered_map> update_models;
+ std::unordered_map> sensors;
+
+ std::shared_ptr tf_broadcaster_;
+ rclcpp::TimerBase::SharedPtr timer;
+ rclcpp::Publisher::SharedPtr odom_pub;
+
+ ckf::Model* main_model;
+
+ void timer_callback() {
+ double time = get_clock()->now().seconds();
+
+ // model->update(time);
+ V state = main_model->get_state();
+ M covariance = main_model->get_covariance();
+
+ // std::pair prediction = main_model->predict(time);
+ // V state = prediction.first;
+ // M covariance = prediction.second;
+
+ nav_msgs::msg::Odometry odom_msg;
+ odom_msg.header.stamp = this->now();
+ odom_msg.header.frame_id = config.odom_frame;
+ odom_msg.child_frame_id = config.base_link_frame;
+
+ odom_msg.pose.pose.position.x = state[ckf::state::x];
+ odom_msg.pose.pose.position.y = state[ckf::state::y];
+ odom_msg.pose.pose.position.z = 0.0;
+
+ odom_msg.pose.covariance[0] = covariance(ckf::state::x, ckf::state::x);
+ odom_msg.pose.covariance[7] = covariance(ckf::state::y, ckf::state::y);
+ odom_msg.pose.covariance[35] = covariance(ckf::state::yaw, ckf::state::yaw);
+
+ tf2::Quaternion q = tf2::Quaternion();
+ q.setRPY(0, 0, state[ckf::state::yaw]);
+ q = q.normalized();
+ odom_msg.pose.pose.orientation.x = q.x();
+ odom_msg.pose.pose.orientation.y = q.y();
+ odom_msg.pose.pose.orientation.z = q.z();
+ odom_msg.pose.pose.orientation.w = q.w();
+
+ odom_msg.twist.twist.linear.x = state[ckf::state::d_x];
+ odom_msg.twist.twist.linear.y = state[ckf::state::d_y];
+ odom_msg.twist.twist.angular.z = state[ckf::state::d_yaw];
+
+ odom_msg.twist.covariance[0] = covariance(ckf::state::d_x, ckf::state::d_x);
+ odom_msg.twist.covariance[7] = covariance(ckf::state::d_y, ckf::state::d_y);
+ odom_msg.twist.covariance[35] = covariance(ckf::state::d_yaw, ckf::state::d_yaw);
+
+ odom_pub->publish(odom_msg);
+
+ if (config.publish_tf) {
+ geometry_msgs::msg::TransformStamped transformStamped;
+
+ transformStamped.header.stamp = this->now();
+ transformStamped.header.frame_id = config.odom_frame;
+ transformStamped.child_frame_id = config.base_link_frame;
+
+ transformStamped.transform.translation.x = state[ckf::state::x];
+ transformStamped.transform.translation.y = state[ckf::state::y];
+ transformStamped.transform.translation.z = state[ckf::state::z];
+
+ transformStamped.transform.rotation.x = q.x();
+ transformStamped.transform.rotation.y = q.y();
+ transformStamped.transform.rotation.z = q.z();
+ transformStamped.transform.rotation.w = q.w();
+
+ tf_broadcaster_->sendTransform(transformStamped);
+ }
+ }
+};
+
+int main(int argc, char* argv[]) {
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
\ No newline at end of file
diff --git a/cev/all/cev_localization_ros2/src/config_parser.cpp b/cev/all/cev_localization_ros2/src/config_parser.cpp
new file mode 100644
index 0000000..6c892fa
--- /dev/null
+++ b/cev/all/cev_localization_ros2/src/config_parser.cpp
@@ -0,0 +1,91 @@
+#include "config_parser.h"
+
+using namespace cev_localization::config_parser;
+
+Sensor Sensor::loadSensor(const YAML::Node& sensorNode) {
+ if (!sensorNode["type"] || !sensorNode["topic"]) {
+ throw std::runtime_error("Sensor type and topic must be defined");
+ }
+
+ Sensor sensor;
+ sensor.type = sensorNode["type"].as();
+ sensor.topic = sensorNode["topic"].as();
+ sensor.frame_id = sensorNode["frame_id"]
+ ? std::make_optional(sensorNode["frame_id"].as())
+ : std::nullopt;
+ sensor.state_mask = sensorNode["state"] ? sensorNode["state"].as>()
+ : std::vector{};
+ sensor.covariance_multiplier = sensorNode["covariance_multiplier"].as(1.0);
+ sensor.use_message_covariance = sensorNode["use_message_covariance"].as(true);
+ sensor.estimator_models = sensorNode["estimator_models"]
+ ? sensorNode["estimator_models"].as>()
+ : std::vector{};
+
+ return sensor;
+}
+
+Publish Publish::loadPublish(const YAML::Node& publishNode) {
+ Publish publish;
+
+ if (!publishNode["active"]) {
+ return publish;
+ }
+
+ publish.active = publishNode["active"].as();
+
+ if (!publishNode["topic"] || !publishNode["child_frame"] || !publishNode["parent_frame"]) {
+ throw std::runtime_error("Publish topic and frames must be defined");
+ }
+
+ publish.topic = publishNode["topic"].as();
+ publish.rate = publishNode["rate"].as(100);
+ publish.child_frame = publishNode["child_frame"].as("");
+ publish.parent_frame = publishNode["parent_frame"].as("");
+
+ publish.publish_tf = publishNode["publish_tf"].as(false);
+
+ return publish;
+}
+
+UpdateModel UpdateModel::loadUpdateModel(const YAML::Node& modelNode) {
+ if (!modelNode["type"] || !modelNode["state"]) {
+ throw std::runtime_error("Model type must be defined");
+ }
+
+ UpdateModel model;
+ model.type = modelNode["type"].as();
+ model.state_mask = modelNode["state"].as>();
+ model.estimator_models = modelNode["estimator_models"]
+ ? modelNode["estimator_models"].as>()
+ : std::vector{};
+
+ if (!modelNode["publish"]) {
+ return model;
+ }
+
+ model.publish = Publish::loadPublish(modelNode["publish"]);
+
+ return model;
+}
+
+Config Config::loadConfig(const YAML::Node& configNode) {
+ Config config;
+
+ for (const auto& sensorEntry: configNode["sensors"]) {
+ const std::string sensorName = sensorEntry.first.as();
+ config.sensors[sensorName] = Sensor::loadSensor(sensorEntry.second);
+ }
+
+ for (const auto& modelEntry: configNode["update_models"]) {
+ const std::string modelName = modelEntry.first.as();
+ config.update_models[modelName] = UpdateModel::loadUpdateModel(modelEntry.second);
+ }
+
+ return config;
+}
+
+Config ConfigParser::load(const std::string& filePath) {
+ YAML::Node configNode = YAML::LoadFile(filePath);
+
+ return Config::loadConfig(configNode);
+}
\ No newline at end of file
diff --git a/cev/all/cev_localization_ros2/src/localization.cpp b/cev/all/cev_localization_ros2/src/localization.cpp
new file mode 100644
index 0000000..61f1eeb
--- /dev/null
+++ b/cev/all/cev_localization_ros2/src/localization.cpp
@@ -0,0 +1,201 @@
+#include
+#include
+#include
+
+#include "model.h"
+#include "ros_sensor.h"
+#include "config_parser.h"
+#include "model.h"
+#include "sensor.h"
+#include "standard_models.h"
+#include "std_ros_sensors.h"
+
+#include
+
+#include
+
+using std::placeholders::_1;
+
+using namespace ckf;
+using namespace cev_localization;
+
+class LocalizationNode : public rclcpp::Node {
+public:
+ LocalizationNode(): Node("CEVLocalizationNode") {
+ RCLCPP_INFO(this->get_logger(), "Initializing CEV Localization Node");
+
+ this->declare_parameter("config_file", "config/ekf_real.yml");
+ std::string config_file_path = this->get_parameter("config_file").as_string();
+
+ RCLCPP_INFO(this->get_logger(), "Parsing config file at %s", config_file_path.c_str());
+
+ // Load the YAML configurations
+ config = config_parser::ConfigParser::load(config_file_path);
+
+ std::unordered_map> update_models;
+
+ for (std::pair model: config.update_models) {
+ std::string name = model.first;
+ config_parser::UpdateModel& mod = model.second;
+
+ if (update_models.find(name) != update_models.end()) {
+ RCLCPP_ERROR(this->get_logger(), "Model `%s` already exists", name.c_str());
+ throw std::runtime_error("Model already exists");
+ }
+
+ if (mod.type == "ACKERMANN") {
+ update_models[name] = std::make_shared(
+ V::Zero(), M::Identity() * .1, M::Identity() * .1, .185, mod.state_mask);
+ } else if (mod.type == "CARTESIAN") {
+ update_models[name] = std::make_shared(
+ V::Zero(), M::Identity() * .1, M::Identity() * .1, mod.state_mask);
+ } else {
+ RCLCPP_ERROR(this->get_logger(), "Unknown model type: `%s`", mod.type.c_str());
+ throw std::runtime_error("Unknown model type");
+ }
+
+ if (mod.publish.active) {
+ filter_publishers[name] =
+ this->create_publisher(mod.publish.topic, 1);
+
+ ckf::Model* model_ = update_models[name].get();
+
+ // Create a timer to publish the model
+ timers[name] = this->create_wall_timer(
+ std::chrono::milliseconds((int)(1000 / mod.publish.rate)),
+ [this, model_, name]() { this->publish_model(model_, name); });
+ }
+ }
+
+ for (std::pair sensor: config.sensors) {
+ std::string name = sensor.first;
+ config_parser::Sensor& sen = sensor.second;
+
+ if (sensors.find(name) != sensors.end()) {
+ RCLCPP_ERROR(this->get_logger(), "Sensor `%s` already exists", name.c_str());
+ throw std::runtime_error("Sensor already exists");
+ }
+
+ // Create array for models to bind to
+ std::vector> models;
+ for (std::string& model_name: sen.estimator_models) {
+ if (update_models.find(model_name) == update_models.end()) {
+ RCLCPP_ERROR(this->get_logger(), "Model `%s` does not exist",
+ model_name.c_str());
+ throw std::runtime_error("Model does not exist");
+ } else {
+ models.push_back(update_models[model_name]);
+ }
+ }
+
+ if (sen.type == "IMU") {
+ auto sensor = std::make_shared(sen.topic,
+ V::Zero(), M::Identity() * .1, models, sen.state_mask,
+ sen.use_message_covariance);
+
+ sensors[name] = sensor;
+
+ sensor_subscribers[name] = this->create_subscription(
+ sen.topic, 10, [sensor](const sensor_msgs::msg::Imu::SharedPtr msg) {
+ sensor->msg_handler(msg);
+ });
+
+ } else if (sen.type == "RAW") {
+ auto sensor = std::make_shared(sen.topic,
+ V::Zero(), M::Identity() * .1, models, sen.state_mask,
+ sen.use_message_covariance);
+
+ sensors[name] = sensor;
+
+ sensor_subscribers[name] = this->create_subscription(
+ sen.topic, 10, [sensor](const cev_msgs::msg::SensorCollect::SharedPtr msg) {
+ sensor->msg_handler(msg);
+ });
+ } else {
+ RCLCPP_ERROR(this->get_logger(), "Unknown sensor type: `%s`", sen.type.c_str());
+ throw std::runtime_error("Unknown sensor type");
+ }
+ }
+
+ RCLCPP_INFO(this->get_logger(), "Configuration file parsed! Finishing initialization.");
+
+ tf_broadcaster_ = std::make_shared(this);
+ }
+
+private:
+ config_parser::Config config;
+
+ std::unordered_map sensor_subscribers;
+ std::unordered_map::SharedPtr>
+ filter_publishers;
+ std::unordered_map timers;
+
+ std::unordered_map> update_models;
+ std::unordered_map> sensors;
+
+ std::shared_ptr tf_broadcaster_;
+ rclcpp::TimerBase::SharedPtr timer;
+
+ void publish_model(ckf::Model* model_, std::string model) {
+ V state = model_->get_state();
+ M covariance = model_->get_covariance();
+
+ nav_msgs::msg::Odometry odom_msg;
+ odom_msg.header.stamp = this->now();
+ odom_msg.header.frame_id = this->config.update_models[model].publish.parent_frame;
+ odom_msg.child_frame_id = this->config.update_models[model].publish.child_frame;
+
+ odom_msg.pose.pose.position.x = state[ckf::state::x];
+ odom_msg.pose.pose.position.y = state[ckf::state::y];
+ odom_msg.pose.pose.position.z = 0.0;
+
+ odom_msg.pose.covariance[0] = covariance(ckf::state::x, ckf::state::x);
+ odom_msg.pose.covariance[7] = covariance(ckf::state::y, ckf::state::y);
+ odom_msg.pose.covariance[35] = covariance(ckf::state::yaw, ckf::state::yaw);
+
+ tf2::Quaternion q = tf2::Quaternion();
+ q.setRPY(0, 0, state[ckf::state::yaw]);
+ q = q.normalized();
+ odom_msg.pose.pose.orientation.x = q.x();
+ odom_msg.pose.pose.orientation.y = q.y();
+ odom_msg.pose.pose.orientation.z = q.z();
+ odom_msg.pose.pose.orientation.w = q.w();
+
+ odom_msg.twist.twist.linear.x = state[ckf::state::d_x];
+ odom_msg.twist.twist.linear.y = state[ckf::state::d_y];
+ odom_msg.twist.twist.angular.z = state[ckf::state::d_yaw];
+
+ odom_msg.twist.covariance[0] = covariance(ckf::state::d_x, ckf::state::d_x);
+ odom_msg.twist.covariance[7] = covariance(ckf::state::d_y, ckf::state::d_y);
+ odom_msg.twist.covariance[35] = covariance(ckf::state::d_yaw, ckf::state::d_yaw);
+
+ this->filter_publishers[model]->publish(odom_msg);
+
+ if (this->config.update_models[model].publish.publish_tf) {
+ geometry_msgs::msg::TransformStamped transformStamped;
+
+ transformStamped.header.stamp = this->now();
+ transformStamped.header.frame_id =
+ this->config.update_models[model].publish.parent_frame;
+ transformStamped.child_frame_id = this->config.update_models[model].publish.child_frame;
+
+ transformStamped.transform.translation.x = state[ckf::state::x];
+ transformStamped.transform.translation.y = state[ckf::state::y];
+ transformStamped.transform.translation.z = state[ckf::state::z];
+
+ transformStamped.transform.rotation.x = q.x();
+ transformStamped.transform.rotation.y = q.y();
+ transformStamped.transform.rotation.z = q.z();
+ transformStamped.transform.rotation.w = q.w();
+
+ this->tf_broadcaster_->sendTransform(transformStamped);
+ }
+ }
+};
+
+int main(int argc, char* argv[]) {
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
\ No newline at end of file
diff --git a/cev/all/cev_localization_ros2/src/ros_sensor.cpp b/cev/all/cev_localization_ros2/src/ros_sensor.cpp
new file mode 100644
index 0000000..8e280c7
--- /dev/null
+++ b/cev/all/cev_localization_ros2/src/ros_sensor.cpp
@@ -0,0 +1 @@
+#include "ros_sensor.h"
\ No newline at end of file
diff --git a/cev/all/cev_localization_ros2/src/std_ros_sensors.cpp b/cev/all/cev_localization_ros2/src/std_ros_sensors.cpp
new file mode 100644
index 0000000..24ae972
--- /dev/null
+++ b/cev/all/cev_localization_ros2/src/std_ros_sensors.cpp
@@ -0,0 +1,106 @@
+#include "std_ros_sensors.h"
+#include
+
+using namespace cev_localization::standard_ros_sensors;
+
+/* IMU SENSOR */
+
+double IMUSensor::pos_mod(double angle) {
+ return fmod(fmod(angle, 2 * M_PI) + 2 * M_PI, 2 * M_PI);
+}
+
+IMUSensor::IMUSensor(std::string topic, V state, M covariance,
+ std::vector> dependents, std::vector state_mask,
+ bool use_message_covariance, bool relative)
+ : RosSensor(topic, state, covariance, dependents),
+ initialized(false),
+ relative(relative),
+ initial_yaw(0),
+ last_reported_yaw(0),
+ last_sensor_raw_yaw(0) {
+ multiplier = Estimator::state_mask_to_matrix(state_mask);
+ this->use_message_covariance = use_message_covariance;
+}
+
+StatePackage IMUSensor::msg_update(sensor_msgs::msg::Imu::SharedPtr msg) {
+ StatePackage estimate = get_internals();
+ estimate.update_time = msg->header.stamp.sec + (msg->header.stamp.nanosec / 1e9);
+
+ // -1 means that the corresponding estimate is invalid
+ if (msg->linear_acceleration_covariance[0] != -1) {
+ estimate.state[ckf::state::d2_x] = msg->linear_acceleration.x;
+ estimate.state[ckf::state::d2_y] = msg->linear_acceleration.y;
+
+ if (this->use_message_covariance) {
+ estimate.covariance(ckf::state::d2_x,
+ ckf::state::d2_x) = msg->linear_acceleration_covariance[0];
+ estimate.covariance(ckf::state::d2_x,
+ ckf::state::d2_y) = msg->linear_acceleration_covariance[1];
+ estimate.covariance(ckf::state::d2_y,
+ ckf::state::d2_x) = msg->linear_acceleration_covariance[3];
+ estimate.covariance(ckf::state::d2_y,
+ ckf::state::d2_y) = msg->linear_acceleration_covariance[4];
+ }
+ }
+
+ if (msg->orientation_covariance[0] != -1) {
+ tf2::Quaternion q(msg->orientation.x, msg->orientation.y, msg->orientation.z,
+ msg->orientation.w);
+ tf2::Matrix3x3 m(q);
+ double roll, pitch, yaw;
+ m.getRPY(roll, pitch, yaw);
+
+ if (relative && !initialized) {
+ last_sensor_raw_yaw = yaw;
+ last_reported_yaw = 0.0;
+ initialized = true;
+
+ estimate.state[ckf::state::yaw] = 0.0;
+ return estimate;
+ }
+
+ double modded_yaw_diff = fmod(yaw - last_sensor_raw_yaw, 2 * M_PI);
+
+ if (modded_yaw_diff > M_PI) {
+ modded_yaw_diff -= 2 * M_PI;
+ } else if (modded_yaw_diff < -M_PI) {
+ modded_yaw_diff += 2 * M_PI;
+ }
+
+ last_reported_yaw += modded_yaw_diff;
+ last_sensor_raw_yaw = yaw;
+
+ estimate.state[ckf::state::yaw] = last_reported_yaw;
+
+ if (this->use_message_covariance) {
+ estimate.covariance(ckf::state::yaw, ckf::state::yaw) = msg->orientation_covariance[8];
+ }
+ }
+
+ return estimate;
+}
+
+/* RAW SENSOR */
+
+RawSensor::RawSensor(std::string topic, V state, M covariance,
+ std::vector> dependents, std::vector state_mask,
+ bool use_message_covariance)
+ : RosSensor(topic, state, covariance, dependents) {
+ multiplier = Estimator::state_mask_to_matrix(state_mask);
+ this->use_message_covariance = use_message_covariance;
+}
+
+StatePackage RawSensor::msg_update(cev_msgs::msg::SensorCollect::SharedPtr msg) {
+ StatePackage estimate = get_internals();
+ estimate.update_time = msg->timestamp;
+
+ estimate.state[ckf::state::d_x] = msg->velocity;
+ estimate.state[ckf::state::tau] = msg->steering_angle;
+
+ // if (this->use_message_covariance) { // TODO: Implement covariance
+ // estimate.covariance(ckf::state::d_x, ckf::state::d_x) = msg->velocity_covariance;
+ // estimate.covariance(ckf::state::tau, ckf::state::tau) = msg->steering_angle_covariance;
+ // }
+
+ return estimate;
+}
\ No newline at end of file
diff --git a/cev/all/cev_msgs/CMakeLists.txt b/cev/all/cev_msgs/CMakeLists.txt
new file mode 100644
index 0000000..1a6c8db
--- /dev/null
+++ b/cev/all/cev_msgs/CMakeLists.txt
@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.8)
+project(cev_msgs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES Clang)
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package( REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "msg/Waypoint.msg"
+ "msg/SensorCollect.msg"
+ "msg/Trajectory.msg"
+ "msg/Obstacle.msg"
+ "msg/Obstacles.msg"
+ DEPENDENCIES std_msgs sensor_msgs
+)
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # comment the line when a copyright and license is added to all source files
+ set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
+
diff --git a/cev/all/cev_msgs/README.md b/cev/all/cev_msgs/README.md
new file mode 100644
index 0000000..d8be1c1
--- /dev/null
+++ b/cev/all/cev_msgs/README.md
@@ -0,0 +1,26 @@
+Custom ROS 2 messages for Cornell Electric Vehicles.
+
+SensorCollect.msg
+ - float64 : timestamp [ Message time in s ]
+ - float32 : velocity [ Velocity in m/s]
+ - float32 : steering_angle [ Steering angle in radians ]
+
+Trajectory.msg
+ - Header : header [ Header ]
+ - Waypoint[] : waypoints [ Array of Waypoints ]
+
+Waypoint.msg
+ - float32 : x [ x-coordinate in meters ]
+ - float32 : y [ y-coordinate in meters ]
+ - float32 : v [ velocity in m/s ]
+ - float32 : tau [ steering angle in radians ]
+ - float32 : theta [ heading in radians ]
+
+Obstacle.msg
+ - float64 : x [ x-coordinate of obstacle center in meters ]
+ - float64 : y [ y-coordinate of obstacle center in meters ]
+ - float64 : z [ z-coordinate of obstacle center in meters ]
+ - float64 : max_radius [ maximum radius of obstacle in meters ]
+
+Obstacles.msg
+ - Obstacle[] : obstacles [ Array of Obstacle(s) ]
\ No newline at end of file
diff --git a/cev/all/cev_msgs/msg/Obstacle.msg b/cev/all/cev_msgs/msg/Obstacle.msg
new file mode 100644
index 0000000..601b9b6
--- /dev/null
+++ b/cev/all/cev_msgs/msg/Obstacle.msg
@@ -0,0 +1,11 @@
+# Obstacle.msg
+#
+# float64 : x
+# float64 : y
+# float64 : z
+# float64 : max_radius
+
+float64 x
+float64 y
+float64 z
+float64 max_radius
\ No newline at end of file
diff --git a/cev/all/cev_msgs/msg/Obstacles.msg b/cev/all/cev_msgs/msg/Obstacles.msg
new file mode 100644
index 0000000..83c30ca
--- /dev/null
+++ b/cev/all/cev_msgs/msg/Obstacles.msg
@@ -0,0 +1,7 @@
+# Obstacle.msg
+#
+# Obstacle[] : obstacles
+
+# cev_msgs/Obstacle[] obstacles
+
+sensor_msgs/PointCloud2[] obstacles
\ No newline at end of file
diff --git a/cev/all/cev_msgs/msg/SensorCollect.msg b/cev/all/cev_msgs/msg/SensorCollect.msg
new file mode 100644
index 0000000..4cb515a
--- /dev/null
+++ b/cev/all/cev_msgs/msg/SensorCollect.msg
@@ -0,0 +1,9 @@
+# SensorCollect.msg
+#
+# float64 : timestamp [ Message time ]
+# float32 : velocity [ Velocity in m/s]
+# float32 : steering_angle [ Steering angle in radians ]
+#
+float64 timestamp
+float32 velocity
+float32 steering_angle
diff --git a/cev/all/cev_msgs/msg/Trajectory.msg b/cev/all/cev_msgs/msg/Trajectory.msg
new file mode 100644
index 0000000..c052c17
--- /dev/null
+++ b/cev/all/cev_msgs/msg/Trajectory.msg
@@ -0,0 +1,8 @@
+# Trajectory.msg
+#
+# Header : header [ Header ]
+# Waypoint[] : waypoints [ Array of Waypoints ]
+#
+std_msgs/Header header
+cev_msgs/Waypoint[] waypoints
+float32 timestep
diff --git a/cev/all/cev_msgs/msg/Waypoint.msg b/cev/all/cev_msgs/msg/Waypoint.msg
new file mode 100644
index 0000000..8b406ca
--- /dev/null
+++ b/cev/all/cev_msgs/msg/Waypoint.msg
@@ -0,0 +1,11 @@
+# Waypoint.msg
+#
+# float32 : x [ x-coordinate in meters ]
+# float32 : y [ y-coordinate in meters ]
+# float32 : v [ velocity in m/s ]
+#
+float32 x
+float32 y
+float32 v
+float32 tau
+float32 theta
diff --git a/cev/all/cev_msgs/package.xml b/cev/all/cev_msgs/package.xml
new file mode 100644
index 0000000..b090353
--- /dev/null
+++ b/cev/all/cev_msgs/package.xml
@@ -0,0 +1,25 @@
+
+
+
+ cev_msgs
+ 0.0.0
+ TODO: Package description
+ sloth
+ TODO: License declaration
+
+ ament_cmake
+
+ std_msgs
+ sensor_msgs
+
+ rosidl_default_generators
+ rosidl_default_runtime
+ rosidl_interface_packages
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
diff --git a/cev/all/cev_planner_ros2/.clang-format b/cev/all/cev_planner_ros2/.clang-format
new file mode 100644
index 0000000..1d9b175
--- /dev/null
+++ b/cev/all/cev_planner_ros2/.clang-format
@@ -0,0 +1,36 @@
+# CEV C++ setup
+---
+BasedOnStyle: Google
+IndentWidth: 4
+Language: Cpp
+UseTab: Never
+DerivePointerAlignment: false
+PointerAlignment: Left
+BreakStringLiterals: true
+ReflowComments: true
+ContinuationIndentWidth: 4
+ColumnLimit: 100
+AlignAfterOpenBracket: DontAlign
+AlignTrailingComments:
+ Kind: Always
+ OverEmptyLines: 2
+AlignOperands: Align
+BreakBeforeBinaryOperators: NonAssignment
+PenaltyBreakAssignment: 60
+PenaltyBreakOpenParenthesis: 50
+IndentCaseLabels: true
+AllowShortFunctionsOnASingleLine: Empty
+SortIncludes: false
+MacroBlockBegin: "^#if .*$"
+MacroBlockEnd: "^#endif$"
+AlignEscapedNewlines: Right
+IndentPPDirectives: BeforeHash
+IndentAccessModifiers: false
+AccessModifierOffset: -4
+SpaceBeforeRangeBasedForLoopColon: false
+SpaceBeforeCaseColon: false
+SpaceBeforeCtorInitializerColon: false
+SpaceAfterTemplateKeyword: false
+NamespaceIndentation: All
+FixNamespaceComments: false
+---
diff --git a/cev/all/cev_planner_ros2/.devcontainer/Dockerfile.dev b/cev/all/cev_planner_ros2/.devcontainer/Dockerfile.dev
new file mode 100644
index 0000000..626dbf7
--- /dev/null
+++ b/cev/all/cev_planner_ros2/.devcontainer/Dockerfile.dev
@@ -0,0 +1,77 @@
+# Start with the official ROS Humble ROS-Core image based on Ubuntu Jammy
+FROM ros:humble-ros-core-jammy
+
+SHELL ["/bin/bash", "-c"]
+
+# Install basic build tools and ROS2 development dependencies
+RUN apt-get update && apt-get install --no-install-recommends -y \
+ build-essential \
+ git \
+ python3-colcon-common-extensions \
+ python3-colcon-mixin \
+ python3-rosdep \
+ python3-vcstool \
+ && rm -rf /var/lib/apt/lists/*
+
+# Set the ROS distro to Humble
+ENV ROS_DISTRO=humble
+
+# Initialize rosdep and update for the current ROS distro
+RUN rosdep init && \
+ rosdep update --rosdistro $ROS_DISTRO
+
+# Add and update colcon mixins and metadata
+RUN 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
+
+# Install ROS2 base packages specific to Humble distribution
+RUN apt-get update && apt-get install -y --no-install-recommends \
+ ros-humble-ros-base=0.10.0-1* \
+ && rm -rf /var/lib/apt/lists/*
+
+# Set environment variables and source ROS2 Humble setup
+ENV LANG=C.UTF-8 \
+ LC_ALL=C.UTF-8
+
+# Source the ROS2 environment automatically when a new shell is created
+RUN echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
+RUN echo "source /root/ws/install/setup.bash" >> ~/.bashrc
+
+RUN mkdir -p /root/ws/src
+RUN mkdir -p /root/ws/tmp
+COPY . /root/ws/tmp
+
+WORKDIR /root/ws
+
+RUN sudo apt update
+
+# Install dev tools
+RUN apt-get update
+RUN apt-get install -y curl
+RUN curl -fsSL https://apt.llvm.org/llvm-snapshot.gpg.key | tee /etc/apt/trusted.gpg.d/apt.llvm.org.asc
+RUN cat /etc/apt/trusted.gpg.d/apt.llvm.org.asc
+RUN echo $'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy main\n' \
+ $'deb-src http://apt.llvm.org/jammy/ llvm-toolchain-jammy main\n' \
+ $'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-19 main\n' \
+ $'deb-src http://apt.llvm.org/jammy/ llvm-toolchain-jammy-19 main\n' \
+ | tee -a /etc/apt/sources.list
+RUN apt-get update
+RUN apt-get install -y clang-format clang-format-19
+
+RUN source /opt/ros/$ROS_DISTRO/setup.bash \
+ && cd /root/ws \
+ && rosdep update \
+ && rosdep install --from-paths tmp -r -y
+
+RUN rm -rf /root/ws/tmp
+
+RUN source /opt/ros/$ROS_DISTRO/setup.bash \
+ && cd /root/ws \
+ && colcon build \
+ && git clone https://github.com/cornellev/cev_msgs.git src/cev_msgs
+
+EXPOSE 4567
\ No newline at end of file
diff --git a/cev/all/cev_planner_ros2/.devcontainer/devcontainer.json b/cev/all/cev_planner_ros2/.devcontainer/devcontainer.json
new file mode 100644
index 0000000..20109ab
--- /dev/null
+++ b/cev/all/cev_planner_ros2/.devcontainer/devcontainer.json
@@ -0,0 +1,51 @@
+{
+ "name": "Planner Dev-Container",
+ "build": {
+ "dockerfile": "Dockerfile.dev"
+ },
+ "workspaceMount": "source=${localWorkspaceFolder},target=/root/ws/src/ackermann_kf,type=bind",
+ "workspaceFolder": "/root/ws/src/cev_planner",
+ "runArgs": [
+ "--rm",
+ "--network=host",
+ "--ipc=host",
+ "--privileged"
+ ],
+ "containerEnv": {
+ "DISPLAY": "${localEnv:DISPLAY}"
+ },
+ "mounts": [
+ "source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached"
+ ],
+ "customizations": {
+ "vscode": {
+ "settings": {
+ "terminal.integrated.defaultProfile.linux": "bash",
+ "terminal.integrated.profiles.linux": {
+ "bash": {
+ "path": "/bin/bash"
+ }
+ }
+ },
+ "[cpp]": {
+ "editor.defaultFormatter": "xaver.clang-format"
+ },
+ "[python]": {
+ "diffEditor.ignoreTrimWhitespace": false,
+ "gitlens.codeLens.symbolScopes": [
+ "!Module"
+ ],
+ "editor.formatOnType": true,
+ "editor.wordBasedSuggestions": "off",
+ "editor.defaultFormatter": "ms-python.black-formatter"
+ },
+ "extensions": [
+ "ms-iot.vscode-ros",
+ "ms-vscode.cpptools-extension-pack",
+ "ms-python.black-formatter",
+ "xaver.clang-format"
+ ]
+ }
+ },
+ "postStartCommand": "git config --global --add safe.directory ${containerWorkspaceFolder}"
+}
\ No newline at end of file
diff --git a/cev/all/cev_planner_ros2/.gitignore b/cev/all/cev_planner_ros2/.gitignore
new file mode 100644
index 0000000..463fcb4
--- /dev/null
+++ b/cev/all/cev_planner_ros2/.gitignore
@@ -0,0 +1,6 @@
+.idea/
+build/
+log/
+install/
+cmake-build-debug/
+.vscode/
\ No newline at end of file
diff --git a/cev/all/cev_planner_ros2/CMakeLists.txt b/cev/all/cev_planner_ros2/CMakeLists.txt
new file mode 100644
index 0000000..3009e8f
--- /dev/null
+++ b/cev/all/cev_planner_ros2/CMakeLists.txt
@@ -0,0 +1,75 @@
+cmake_minimum_required(VERSION 3.10)
+project(cev_planner_ros2 LANGUAGES CXX)
+
+# Set the C++ standard
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(NLopt REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+# CEV
+find_package(cev_msgs REQUIRED)
+
+# Add subdirectory for the library
+add_subdirectory(cev_planner)
+
+# Include directories for project headers
+include_directories(
+ include
+ cev_planner/include
+)
+
+# Source files in the main src folder
+set(SOURCES
+ src/planner_node.cpp
+)
+
+add_executable(planner_node ${SOURCES})
+add_executable(tf_test src/tf_test.cpp)
+
+ament_target_dependencies(planner_node
+ rclcpp
+ cev_msgs
+ nav_msgs
+ Eigen3
+ tf2_ros
+ tf2_geometry_msgs
+)
+
+ament_target_dependencies(tf_test
+ rclcpp
+ cev_msgs
+ nav_msgs
+ Eigen3
+ tf2_ros
+ tf2_geometry_msgs
+)
+
+target_link_libraries(planner_node
+ cev_planner
+)
+
+target_link_libraries(tf_test
+ cev_planner
+)
+
+# Declare the executable
+install(TARGETS planner_node
+ DESTINATION lib/${PROJECT_NAME})
+
+install(TARGETS tf_test
+ DESTINATION lib/${PROJECT_NAME})
+
+# Install additional resources
+install(DIRECTORY launch config
+ DESTINATION share/${PROJECT_NAME})
+
+# Ament package configuration
+ament_package()
diff --git a/cev/all/cev_planner_ros2/README.md b/cev/all/cev_planner_ros2/README.md
new file mode 100644
index 0000000..c8af174
--- /dev/null
+++ b/cev/all/cev_planner_ros2/README.md
@@ -0,0 +1,17 @@
+### Development Setup
+
+Make a new folder for the ROS2 workspace
+and make a `src` folder inside. `cd` into the
+`src` folder and then:
+`git clone --recurse-submodules {repo_url}`
+`sudo apt install libnlopt-dev`
+`sudo apt install libnlopt-cxx-dev`
+Clone [cev_msgs](https://github.com/cornellev/cev_msgs)
+
+Then `cd` back to the workspace root and `colcon build`.
+
+---
+### Running
+In the workspace root:
+`source install/setup.bash`
+`ros2 launch cev_planner_ros2 launch.py`
\ No newline at end of file
diff --git a/cev/all/cev_planner_ros2/TODO.md b/cev/all/cev_planner_ros2/TODO.md
new file mode 100644
index 0000000..e69de29
diff --git a/cev/all/cev_planner_ros2/cev_planner b/cev/all/cev_planner_ros2/cev_planner
new file mode 160000
index 0000000..93b8bd9
--- /dev/null
+++ b/cev/all/cev_planner_ros2/cev_planner
@@ -0,0 +1 @@
+Subproject commit 93b8bd91735cdebe4d9a945eda9b485cb0a269d0
diff --git a/cev/all/cev_planner_ros2/commit_script.sh b/cev/all/cev_planner_ros2/commit_script.sh
new file mode 100755
index 0000000..05f5721
--- /dev/null
+++ b/cev/all/cev_planner_ros2/commit_script.sh
@@ -0,0 +1,4 @@
+#!/bin/bash
+
+cd cev_planner && git add . && git commit -m "$1" && cd ..
+git add . && git commit -m "$1"
diff --git a/cev/all/cev_planner_ros2/config/.gitkeep b/cev/all/cev_planner_ros2/config/.gitkeep
new file mode 100644
index 0000000..e69de29
diff --git a/cev/all/cev_planner_ros2/config/cev_planner.yaml b/cev/all/cev_planner_ros2/config/cev_planner.yaml
new file mode 100644
index 0000000..7fd0c47
--- /dev/null
+++ b/cev/all/cev_planner_ros2/config/cev_planner.yaml
@@ -0,0 +1,17 @@
+planner_node:
+ ros__parameters:
+ x: [-1000.0, 1000.0]
+ y: [-1000.0, 1000.0]
+ tau: [-.34, .34]
+ vel: [-1.0, 1.0]
+ accel: [-.5, .5]
+ dtau: [-.20, .20]
+
+cev_planner_ros2_node:
+ ros__parameters:
+ x: [-1000.0, 1000.0]
+ y: [-1000.0, 1000.0]
+ tau: [-.34, .34]
+ vel: [-1.0, 1.0]
+ accel: [-.5, .5]
+ dtau: [-.20, .20]
\ No newline at end of file
diff --git a/cev/all/cev_planner_ros2/config/igvc.yaml b/cev/all/cev_planner_ros2/config/igvc.yaml
new file mode 100644
index 0000000..b30127e
--- /dev/null
+++ b/cev/all/cev_planner_ros2/config/igvc.yaml
@@ -0,0 +1,17 @@
+planner_node:
+ ros__parameters:
+ x: [-1000.0, 1000.0]
+ y: [-1000.0, 1000.0]
+ tau: [-.586, .586]
+ vel: [-2.2, 2.2]
+ accel: [-2.5, 2.5]
+ dtau: [-.785, .785]
+
+cev_planner_ros2_node:
+ ros__parameters:
+ x: [-1000.0, 1000.0]
+ y: [-1000.0, 1000.0]
+ tau: [-.586, .586]
+ vel: [-2.2, 2.2]
+ accel: [-2.5, 2.5]
+ dtau: [-.785, .785]
diff --git a/cev/all/cev_planner_ros2/costmap.png b/cev/all/cev_planner_ros2/costmap.png
new file mode 100644
index 0000000..02b397d
Binary files /dev/null and b/cev/all/cev_planner_ros2/costmap.png differ
diff --git a/cev/all/cev_planner_ros2/launch/.gitkeep b/cev/all/cev_planner_ros2/launch/.gitkeep
new file mode 100644
index 0000000..e69de29
diff --git a/cev/all/cev_planner_ros2/launch/igvc.py b/cev/all/cev_planner_ros2/launch/igvc.py
new file mode 100644
index 0000000..3bdcaa7
--- /dev/null
+++ b/cev/all/cev_planner_ros2/launch/igvc.py
@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def get_path(package, dir, file):
+ return os.path.join(get_package_share_directory(package), dir, file)
+
+
+def generate_launch_description():
+ return LaunchDescription(
+ [
+ Node(
+ package="cev_planner_ros2",
+ executable="planner_node",
+ name="cev_planner_ros2_node",
+ output="screen",
+ parameters=[
+ get_path("cev_planner_ros2", "config", "igvc.yaml")
+ ],
+ ),
+ ]
+ )
diff --git a/cev/all/cev_planner_ros2/launch/launch.py b/cev/all/cev_planner_ros2/launch/launch.py
new file mode 100644
index 0000000..63e748b
--- /dev/null
+++ b/cev/all/cev_planner_ros2/launch/launch.py
@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def get_path(package, dir, file):
+ return os.path.join(get_package_share_directory(package), dir, file)
+
+
+def generate_launch_description():
+ return LaunchDescription(
+ [
+ Node(
+ package="cev_planner_ros2",
+ executable="planner_node",
+ name="cev_planner_ros2_node",
+ output="screen",
+ parameters=[
+ get_path("cev_planner_ros2", "config", "cev_planner.yaml")
+ ],
+ ),
+ ]
+ )
diff --git a/cev/all/cev_planner_ros2/package.xml b/cev/all/cev_planner_ros2/package.xml
new file mode 100644
index 0000000..48cd693
--- /dev/null
+++ b/cev/all/cev_planner_ros2/package.xml
@@ -0,0 +1,28 @@
+
+
+
+ cev_planner_ros2
+ 0.0.0
+ TODO: Package description
+ sloth
+ TODO: License declaration
+
+ ament_cmake
+
+ rclcpp
+ eigen3
+ nlopt
+ tf2
+ tf2_ros
+ tf2_geometry_msgs
+
+ cev_msgs
+
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
\ No newline at end of file
diff --git a/cev/all/cev_planner_ros2/src/planner_node.cpp b/cev/all/cev_planner_ros2/src/planner_node.cpp
new file mode 100644
index 0000000..5177915
--- /dev/null
+++ b/cev/all/cev_planner_ros2/src/planner_node.cpp
@@ -0,0 +1,502 @@
+#include
+#include
+#include
+#include
+#include
+#include
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2/utils.h"
+#include
+
+#include "local_planning/mpc.h"
+#include "global_planning/rrt.h"
+#include "cost_map/gaussian_conv.h"
+#include "cost_map/nearest.h"
+#include "cost_map/nothing.h"
+
+using namespace cev_planner;
+
+class PlannerNode : public rclcpp::Node {
+public:
+ PlannerNode(): Node("planner_node"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) {
+ RCLCPP_INFO(this->get_logger(), "Initializing planner node");
+
+ Dimensions dimensions = Dimensions{.3, .3, .3};
+ // Default values if not run with config file
+ // Constraints positive_constraints = Constraints{
+ // {-1000, 1000}, // x
+ // {-1000, 1000}, // y
+ // {-.34, .34}, // tau
+ // {0, .5}, // vel
+ // {0, .25}, // accel
+ // {-.34, .34} // dtau
+ // };
+
+ // Default values if not run with config file
+ Constraints full_constraints = Constraints{
+ {-1000.0, 1000.0}, // x
+ {-1000.0, 1000.0}, // y
+ {-.34, .34}, // tau
+ {-1.0, 1.0}, // vel
+ {-.5, .5}, // accel
+ {-.20, .20} // dtau
+ };
+
+ auto safe_load = [&](auto &target, const char* name){
+ try {
+ this->declare_parameter(name, rclcpp::PARAMETER_DOUBLE_ARRAY);
+ rclcpp::Parameter param = this->get_parameter(name);
+ auto v = param.as_double_array();
+ target[0] = v[0]; target[1] = v[1];
+ }
+ catch (...) { RCLCPP_WARN(this->get_logger(), "Failed to load %s constraint, using default values (%f, %f).", name, target[0], target[1]); }
+ };
+
+ safe_load(full_constraints.x, "x");
+ safe_load(full_constraints.y, "y");
+ safe_load(full_constraints.tau, "tau");
+ safe_load(full_constraints.vel, "vel");
+ safe_load(full_constraints.accel, "accel");
+ safe_load(full_constraints.dtau, "dtau");
+
+
+ // local_planner = std::make_shared(dimensions, full_constraints,
+ // std::make_shared(2, .5));
+
+ local_planner = std::make_shared(dimensions, full_constraints);
+ global_planner = std::make_shared(dimensions, full_constraints);
+
+ map_sub = this->create_subscription("map", 1,
+ std::bind(&PlannerNode::map_callback, this, std::placeholders::_1));
+
+ odom_sub = this->create_subscription("/odometry/filtered", 1,
+ std::bind(&PlannerNode::odom_callback, this, std::placeholders::_1));
+
+ target_sub = this->create_subscription("target", 1,
+ std::bind(&PlannerNode::target_callback, this, std::placeholders::_1));
+
+ path_pub = this->create_publisher("trajectory", 1);
+
+ // Global plan every 2 seconds
+ // global_plan_timer = this->create_wall_timer(std::chrono::milliseconds(2000),
+ // std::bind(&PlannerNode::global_plan_callback, this));
+
+ // RVIZ Debug
+ global_path_pub = this->create_publisher("global_path", 1);
+ local_path_pub = this->create_publisher("local_path", 1);
+
+ target_rviz_sub = this->create_subscription("goal_pose", 1,
+ std::bind(&PlannerNode::rviz_target_callback, this, std::placeholders::_1));
+ }
+
+private:
+ Grid grid = Grid();
+ State start = State();
+ State prev_start = State();
+ State target = State();
+
+ cost_map::NearestGenerator local_plan_cost_generator = cost_map::NearestGenerator(2, .5);
+ cost_map::Nothing global_plan_cost_generator = cost_map::Nothing(1, .5);
+
+ std::shared_ptr local_plan_cost;
+ std::shared_ptr global_plan_cost;
+ bool cost_map_initialized = false;
+
+ bool map_initialized = false;
+ bool odom_initialized = false;
+ bool target_initialized = false;
+
+ // Global Planner
+ std::shared_ptr global_planner;
+ Trajectory global_path;
+ double global_path_cost = 100000000;
+ bool global_path_initialized = false;
+
+ // Local Planner
+ bool second_iteration_passed = false;
+ float prev_path_cost = 100000000;
+ int current_waypoint_in_global = 0;
+ Trajectory last_path = Trajectory();
+
+ std::shared_ptr local_planner;
+
+ //// ROS
+ // TF
+ tf2_ros::Buffer tf_buffer_;
+ tf2_ros::TransformListener tf_listener_;
+
+ // Init frames odom map baselink so that we can localize base_link inside map given the odom to
+ // map transform
+
+ // Listener to the map
+ rclcpp::Subscription::SharedPtr map_sub;
+
+ // Listener to odom
+ rclcpp::Subscription::SharedPtr odom_sub;
+
+ // Listener to the target
+ rclcpp::Subscription::SharedPtr target_sub;
+
+ // Publisher for the planned path
+ rclcpp::Publisher::SharedPtr path_pub;
+
+ // Wall timer for global plan
+ // rclcpp::TimerBase::SharedPtr global_plan_timer;
+
+ // RVIZ
+ rclcpp::Publisher::SharedPtr global_path_pub;
+ rclcpp::Publisher::SharedPtr local_path_pub;
+ rclcpp::Subscription::SharedPtr target_rviz_sub;
+
+ float avg_costmap_time = 0;
+ int cost_map_iters = 0;
+ float avg_planning_time = 0;
+ int planning_iters = 0;
+
+ std::chrono::_V2::system_clock::time_point start_time =
+ std::chrono::high_resolution_clock::now();
+ // -------------------------------
+
+ void global_plan_callback() {
+ std::cout << "I am attempting a global plan." << std::endl;
+
+ std::optional optional = global_planner->plan_path(grid, start, target);
+
+ if (!optional.has_value()) {
+ std::cout << "Global Path Failed." << std::endl;
+ global_path_initialized = false;
+ } else {
+ std::cout << "Global Path Planned." << std::endl;
+ global_path.waypoints = optional.value().waypoints;
+
+ // Publish global path
+ nav_msgs::msg::Path global_nav_path;
+ global_nav_path.header.stamp = this->now();
+ global_nav_path.header.frame_id = "map";
+ global_nav_path.poses.clear();
+
+ for (State waypoint: global_path.waypoints) {
+ geometry_msgs::msg::PoseStamped pose;
+ pose.pose.position.x = waypoint.pose.x;
+ pose.pose.position.y = waypoint.pose.y;
+ pose.pose.position.z = 0;
+ pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1),
+ waypoint.pose.theta));
+ global_nav_path.poses.push_back(pose);
+ }
+
+ global_path_pub->publish(global_nav_path);
+
+ // cev_msgs::msg::Trajectory current_plan;
+
+ // current_plan.header.stamp = this->now();
+ // current_plan.header.frame_id = "map";
+ // current_plan.waypoints.clear();
+ // current_plan.timestep = global_path.timestep;
+
+ // // std::cout << "What 3" << std::endl;
+
+ // for (State waypoint: global_path.waypoints) {
+ // cev_msgs::msg::Waypoint msg;
+ // msg.x = waypoint.pose.x;
+ // msg.y = waypoint.pose.y;
+
+ // // if (std::abs(waypoint.vel) < .3
+ // // && std::abs(waypoint.vel) > 0.0) { // Car cannot move that slow lol
+ // // waypoint.vel = (waypoint.vel >= 0) ? .3 : -.3;
+ // // }
+
+ // msg.v = waypoint.vel;
+ // msg.theta = waypoint.pose.theta;
+ // msg.tau = waypoint.tau;
+ // current_plan.waypoints.push_back(msg);
+ // }
+
+ // std::cout << "Publishing path" << std::endl;
+
+ // path_pub->publish(current_plan);
+
+ global_path_initialized = true;
+ }
+ }
+
+ bool passed_waypoint(State state, State waypoint, State prev_waypoint, State next_waypoint,
+ bool last_waypoint) {
+ float dot;
+
+ if (last_waypoint) {
+ dot = (state.pose.x - waypoint.pose.x) * (waypoint.pose.x - prev_waypoint.pose.x)
+ + (state.pose.y - waypoint.pose.y) * (waypoint.pose.y - prev_waypoint.pose.y);
+ } else {
+ dot = (state.pose.x - waypoint.pose.x) * (next_waypoint.pose.x - waypoint.pose.x)
+ + (state.pose.y - waypoint.pose.y) * (next_waypoint.pose.y - waypoint.pose.y);
+ }
+
+ float dist = state.pose.distance_to(waypoint.pose);
+
+ return dist < .5 || (dist < .7 && dot > 0);
+ }
+
+ bool hits_obstacle(Trajectory path) {
+ for (State waypoint: path.waypoints) {
+ if (global_plan_cost->cost(waypoint) > .5) {
+ return true;
+ }
+ }
+
+ return false;
+ }
+
+ void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
+ // Transform into map frame
+ geometry_msgs::msg::TransformStamped transform;
+
+ geometry_msgs::msg::PoseStamped base_link_pose;
+ base_link_pose.header = msg->header;
+ base_link_pose.pose = msg->pose.pose;
+
+ geometry_msgs::msg::PoseStamped map_pose;
+
+ try {
+ transform = tf_buffer_.lookupTransform("map", "odom", tf2::TimePointZero);
+ tf2::doTransform(base_link_pose, map_pose, transform);
+ } catch (const tf2::TransformException& ex) {
+ RCLCPP_DEBUG(this->get_logger(), "Could not transform odom to map: %s", ex.what());
+ return;
+ }
+
+ float qw = map_pose.pose.orientation.w;
+ float qx = map_pose.pose.orientation.x;
+ float qy = map_pose.pose.orientation.y;
+ float qz = map_pose.pose.orientation.z;
+
+ float yaw = restrict_angle(atan2(2.0 * (qw * qz + qx * qy),
+ 1.0 - 2.0 * (qy * qy + qz * qz)));
+
+ start = State{
+ map_pose.pose.position.x, map_pose.pose.position.y, yaw, msg->twist.twist.linear.x, 0};
+
+ odom_initialized = true;
+
+ // Plan path
+ if (map_initialized && odom_initialized && target_initialized && !global_path_initialized) {
+ // std::cout << "Planning" << std::endl;
+ global_plan_callback();
+ } else {
+ // std::cout << "Map: " << map_initialized << std::endl;
+ // std::cout << "Odom: " << odom_initialized << std::endl;
+ // std::cout << "Target: " << target_initialized << std::endl;
+ // std::cout << "Global Path: " << global_path_initialized << std::endl;
+ }
+
+ // float moved_dist = start.pose.distance_to(prev_start.pose);
+
+ if (map_initialized && odom_initialized && target_initialized && global_path_initialized) {
+ // std::cout << "I am entering the local planning loop." << std::endl;
+ float dist =
+ start.pose.distance_to(global_path.waypoints[current_waypoint_in_global].pose);
+
+ float dist_to_dest = start.pose.distance_to(target.pose);
+
+ bool passed = passed_waypoint(start, target,
+ global_path.waypoints[global_path.waypoints.size() - 2], State(), true);
+
+ if (dist_to_dest < .3) { // Reached destination
+ // Write a trajectory with 0 velocity
+ cev_msgs::msg::Trajectory current_plan;
+
+ current_plan.header.stamp = msg->header.stamp;
+ current_plan.header.frame_id = "map";
+ current_plan.waypoints.clear();
+
+ cev_msgs::msg::Waypoint msg;
+ msg.x = target.pose.x;
+ msg.y = target.pose.y;
+ msg.v = 0;
+ msg.theta = target.pose.theta;
+ msg.tau = 0;
+ current_plan.waypoints.push_back(msg);
+
+ path_pub->publish(current_plan);
+
+ target_initialized = false;
+
+ std::cout << "Passed target." << std::endl;
+
+ return;
+ }
+
+ // std::cout << "I did not pass the target" << std::endl;
+
+ while (dist < .75
+ && current_waypoint_in_global
+ < global_path.waypoints.size()) { // Progress waypoint
+ current_waypoint_in_global += 1;
+ prev_path_cost = 100000000; // Reset plan costs for new waypoint
+ dist =
+ start.pose.distance_to(global_path.waypoints[current_waypoint_in_global].pose);
+ }
+
+ // Give next two waypoints to target for planner
+ Trajectory waypoints;
+
+ if (current_waypoint_in_global < global_path.waypoints.size() - 1) {
+ waypoints.waypoints.push_back(global_path.waypoints[current_waypoint_in_global]);
+
+ if ((current_waypoint_in_global + 1) < global_path.waypoints.size()) {
+ waypoints.waypoints.push_back(
+ global_path.waypoints[current_waypoint_in_global + 1]);
+ } else {
+ waypoints.waypoints.push_back(target);
+ }
+ } else {
+ waypoints.waypoints.push_back(target);
+ }
+
+ // Fast forward last path to current position
+ // for (int i = 0; i < last_path.waypoints.size(); i++) {
+ // float dist_to_waypoint = start.pose.distance_to(last_path.waypoints[i].pose);
+ // if (dist_to_waypoint < .5) {
+ // last_path.waypoints.erase(last_path.waypoints.begin(),
+ // last_path.waypoints.begin() + i);
+ // break;
+ // }
+ // }
+
+ // std::cout << "Planning local path" << std::endl;
+ Trajectory path = local_planner->plan_path(grid, start, target, waypoints, last_path,
+ local_plan_cost);
+
+ if (path.cost >= prev_path_cost
+ || hits_obstacle(path)) { // Worse path and hasn't targeted next waypoint
+ // std::cout << "Worse path" << std::endl;
+ return;
+ }
+
+ std::chrono::_V2::system_clock::time_point end_time =
+ std::chrono::high_resolution_clock::now();
+
+ avg_planning_time = std::chrono::duration_cast(end_time
+ - start_time)
+ .count();
+
+ std::cout << "Planning Update Time: " << avg_planning_time << "ms" << std::endl;
+
+ start_time = std::chrono::high_resolution_clock::now();
+ // std::cout << "Better path" << std::endl;
+
+ last_path = path;
+
+ prev_path_cost = path.cost;
+ second_iteration_passed = true;
+ prev_start = start;
+
+ cev_msgs::msg::Trajectory current_plan;
+
+ current_plan.header.stamp = msg->header.stamp;
+ current_plan.header.frame_id = "map";
+ current_plan.waypoints.clear();
+ current_plan.timestep = path.timestep;
+
+ for (State waypoint: path.waypoints) {
+ cev_msgs::msg::Waypoint msg;
+ msg.x = waypoint.pose.x;
+ msg.y = waypoint.pose.y;
+ msg.v = waypoint.vel;
+ msg.theta = waypoint.pose.theta;
+ msg.tau = waypoint.tau;
+ current_plan.waypoints.push_back(msg);
+ }
+
+ path_pub->publish(current_plan);
+
+ // Publish local path
+ nav_msgs::msg::Path nav_path;
+ nav_path.header.stamp = msg->header.stamp;
+ nav_path.header.frame_id = "map";
+ nav_path.poses.clear();
+
+ for (State waypoint: path.waypoints) {
+ geometry_msgs::msg::PoseStamped pose;
+ pose.pose.position.x = waypoint.pose.x;
+ pose.pose.position.y = waypoint.pose.y;
+ pose.pose.position.z = 0;
+ pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1),
+ waypoint.pose.theta));
+ nav_path.poses.push_back(pose);
+ }
+
+ local_path_pub->publish(nav_path);
+ }
+ }
+
+ void map_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
+ grid = Grid();
+ grid.origin = Pose{msg->info.origin.position.x, msg->info.origin.position.y, 0};
+ grid.resolution = msg->info.resolution;
+
+ grid.data = Eigen::MatrixXf(msg->info.width, msg->info.height);
+
+ for (int i = 0; i < msg->info.width; i++) {
+ for (int j = 0; j < msg->info.height; j++) {
+ // Divide by 100 to get probability of occupancy in the range [0, 1]
+ if (msg->data[j * msg->info.width + i] < 0) {
+ grid.data(i, j) = -1.0;
+ } else if (msg->data[j * msg->info.width + i] < 50) {
+ grid.data(i, j) = 0.0;
+ } else {
+ // grid.data(i, j) = std::min(msg->data[j * msg->info.width + i] / 100.0, 1.0);
+ grid.data(i, j) = 1.0;
+ }
+ }
+ }
+
+ map_initialized = true;
+
+ // auto start_time = std::chrono::high_resolution_clock::now();
+ local_plan_cost = local_plan_cost_generator.generate_cost_map(grid);
+ global_plan_cost = global_plan_cost_generator.generate_cost_map(grid);
+ // auto end_time = std::chrono::high_resolution_clock::now();
+ // avg_costmap_time +=
+ // std::chrono::duration_cast(end_time - start_time).count();
+ // cost_map_iters += 1;
+
+ // if (cost_map_iters > 100) {
+ // std::cout << "Average cost map generation time: " << avg_costmap_time /
+ // cost_map_iters
+ // << "ms" << std::endl;
+ // avg_costmap_time = 0;
+ // cost_map_iters = 0;
+ // }
+ }
+
+ void target_callback(const cev_msgs::msg::Waypoint msg) {
+ current_waypoint_in_global = 0;
+ global_path_initialized = false;
+ target = State{msg.x, msg.y, 0, msg.v, 0};
+ prev_path_cost = 100000000;
+ second_iteration_passed = false;
+ target_initialized = true;
+ }
+
+ void rviz_target_callback(const geometry_msgs::msg::PoseStamped msg) {
+ std::cout << "I received a target." << std::endl;
+
+ current_waypoint_in_global = 0;
+ global_path_initialized = false;
+ target = State{msg.pose.position.x, msg.pose.position.y, 0, 0, 0};
+ prev_path_cost = 100000000;
+ second_iteration_passed = false;
+ target_initialized = true;
+ }
+};
+
+int main(int argc, char** argv) {
+ rclcpp::init(argc, argv);
+ auto node = std::make_shared();
+ rclcpp::spin(node);
+ rclcpp::shutdown();
+ return 0;
+}
\ No newline at end of file
diff --git a/cev/all/cev_planner_ros2/src/tf_test.cpp b/cev/all/cev_planner_ros2/src/tf_test.cpp
new file mode 100644
index 0000000..828fbcb
--- /dev/null
+++ b/cev/all/cev_planner_ros2/src/tf_test.cpp
@@ -0,0 +1,53 @@
+#include
+#include
+#include
+#include
+#include
+#include
+
+class OdomToMapTransformer : public rclcpp::Node {
+public:
+ OdomToMapTransformer()
+ : Node("odom_to_map_transformer"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) {
+ RCLCPP_INFO(this->get_logger(), "starting...");
+ odom_sub_ = this->create_subscription("/odometry/filtered", 1,
+ std::bind(&OdomToMapTransformer::odom_callback, this, std::placeholders::_1));
+
+ transformed_pose_pub_ =
+ this->create_publisher("/base_link_in_map", 1);
+ }
+
+private:
+ void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
+ geometry_msgs::msg::PoseStamped base_link_pose;
+ base_link_pose.header = msg->header;
+ base_link_pose.pose = msg->pose.pose;
+
+ // Transform from "odom" to "map"
+ try {
+ geometry_msgs::msg::TransformStamped transform_stamped =
+ tf_buffer_.lookupTransform("map", "odom", tf2::TimePointZero);
+
+ geometry_msgs::msg::PoseStamped map_pose;
+ tf2::doTransform(base_link_pose, map_pose, transform_stamped);
+
+ // Publish transformed pose in "map" frame
+ transformed_pose_pub_->publish(map_pose);
+ RCLCPP_INFO(this->get_logger(), "Published transformed pose in map frame");
+ } catch (const tf2::TransformException& ex) {
+ RCLCPP_WARN(this->get_logger(), "Could not transform odom to map: %s", ex.what());
+ }
+ }
+
+ rclcpp::Subscription::SharedPtr odom_sub_;
+ rclcpp::Publisher::SharedPtr transformed_pose_pub_;
+ tf2_ros::Buffer tf_buffer_;
+ tf2_ros::TransformListener tf_listener_;
+};
+
+int main(int argc, char** argv) {
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
diff --git a/cev/all/cev_planner_ros2/tmp/Makefile b/cev/all/cev_planner_ros2/tmp/Makefile
new file mode 100644
index 0000000..a2f7d4c
--- /dev/null
+++ b/cev/all/cev_planner_ros2/tmp/Makefile
@@ -0,0 +1,18 @@
+all: build run
+
+max: maxbuild run
+
+build: chat.cpp
+ g++ -std=c++17 chat.cpp -I/usr/include/python3.10 -lpython3.10 -lnlopt -o path_planner
+
+maxbuild: chat.cpp
+ g++ -std=c++17 chat.cpp -I/usr/include/python3.10 -lpython3.10 -lnlopt -O3 -o path_planner
+
+kernel: kernel.cpp
+ g++ -std=c++17 -O3 -I/usr/include/eigen3 kernel.cpp -o kernel `pkg-config --cflags --libs opencv4`
+
+clean: ./path_planner
+ rm -f path_planner
+
+run: ./path_planner
+ ./path_planner
diff --git a/cev/all/cev_planner_ros2/tmp/chat.cpp b/cev/all/cev_planner_ros2/tmp/chat.cpp
new file mode 100644
index 0000000..36076a6
--- /dev/null
+++ b/cev/all/cev_planner_ros2/tmp/chat.cpp
@@ -0,0 +1,443 @@
+#include
+#include
+#include
+#include
+#include
+#include
+#include "matplotlib-cpp/matplotlibcpp.h"
+namespace plt = matplotlibcpp;
+
+struct Dimensions {
+ double wheelbase;
+ double width;
+ double length;
+};
+
+// Define the bounds of the state space (min, max)
+struct Bounds {
+ double x[2];
+ double y[2];
+ double tau[2];
+ double vel[2];
+ double accel[2];
+ double dtau[2];
+};
+
+struct State {
+ double x;
+ double y;
+ double theta; // Heading
+ double tau; // Steering angle
+ double vel; // Velocity
+};
+
+// Update step
+struct Input {
+ double tau;
+ double vel;
+};
+
+double bound(double x, double bound[2]) {
+ return std::clamp(x, bound[0], bound[1]);
+}
+
+/**
+ * @brief Return a `State` updated with `Input u`
+ *
+ * @return `State x` updated with `Input u` and bounded by `Bounds bounds` over timestep `dt`
+ */
+State update(State& x, Input& u, Bounds& bounds, double dt, Dimensions& dims) {
+ State _x;
+
+ double accel = bound(u.vel - x.vel, bounds.accel);
+ double dtau = bound(u.tau - x.tau, bounds.dtau);
+
+ _x.vel = bound(x.vel + accel * dt, bounds.vel);
+ _x.tau = bound(x.tau + dtau * dt, bounds.tau);
+
+ double avg_vel = (x.vel + _x.vel) / 2;
+
+ double R = dims.wheelbase / tan(x.tau);
+ double dtheta = (avg_vel / R) * dt;
+ _x.theta = x.theta + dtheta;
+
+ double avg_theta = (x.theta + _x.theta) / 2;
+ double avg_tau = (x.tau + _x.tau) / 2;
+
+ _x.x = bound(x.x + avg_vel * cos(avg_theta + avg_tau) * dt, bounds.x);
+ _x.y = bound(x.y + avg_vel * sin(avg_theta + avg_tau) * dt, bounds.y);
+
+ return _x;
+}
+
+std::string to_string(State& x) {
+ return (
+ "State(\n"
+ " x: " + std::to_string(x.x) + "\n"
+ " y: " + std::to_string(x.y) + "\n"
+ " theta: " + std::to_string(x.theta) + "\n"
+ " tau: " + std::to_string(x.tau) + "\n"
+ " vel: " + std::to_string(x.vel) + "\n"
+ ")"
+ );
+}
+
+// INITIALIZE AS IF CLASS
+std::vector waypoints = {
+ {-1.0, -1.0, 0, 0, 0}, {1.0, 2.0, 0, 0, 0}, {10.0, 10.0, 0, 0, 0}}; // Waypoints
+// std::vector> obstacles = {{1, 1}, {1, 2}, {2, 2}}; // Obstacle coordinates
+
+// 10 obstacles randomly placed
+std::vector> obstacles = {{1, 1}, {1, 2}, {2, 1}, {3, 4}, {4, 6.5}, {4, 7.5},
+ {4, 8.5}, {4, 9.5}, {4, 5}, {5, 4}, {6, 4}, {7, 8}, {8, 7}, {9, 10}};
+
+// std::vector> obstacles = {};
+
+std::vector path = {waypoints[0]};
+
+Bounds bounds = {
+ {-10, 10}, // x
+ {-10, 10}, // y
+ {-M_PI / 4, M_PI / 4}, // tau
+ {0, 10}, // vel
+ {-2, 2}, // accel
+ {-M_PI / 8, M_PI / 8} // dtau
+};
+Dimensions dims = {
+ .2, // wheelbase
+ 1, // width
+ 1 // length
+};
+double dt = .1;
+
+double distance(State a, State b) {
+ double dx = a.x - b.x;
+ double dy = a.y - b.y;
+ return sqrt(dx * dx + dy * dy);
+}
+
+double distance(State a, std::vector b) {
+ double dx = a.x - b[0];
+ double dy = a.y - b[1];
+ return sqrt(dx * dx + dy * dy);
+}
+
+double distance(const std::vector& a, const std::vector& b) {
+ double dx = a[0] - b[0];
+ double dy = a[1] - b[1];
+ return sqrt(dx * dx + dy * dy);
+}
+
+double path_obs_cost(std::vector& path, std::vector>& obstacles) {
+ double cost = 0;
+
+ for (int i = 0; i < path.size(); i++) {
+ for (int j = 0; j < obstacles.size(); j++) {
+ cost += 3 / distance(path[i], obstacles[j]);
+ // std::cout << 1 / distance({path[i].x, path[i].y}, obstacles[j]) << std::endl;
+ }
+ }
+ return cost;
+}
+
+double path_waypoints_cost(std::vector& path, std::vector& waypoints) {
+ double cost = 0;
+ for (int i = 1; i < path.size(); i++) {
+ // Waypoints
+ for (int j = 1; j < waypoints.size() - 1; j++) {
+ cost += .3 * distance(path[i], waypoints[j]);
+ }
+ // Goal
+ cost += 2 * distance(path[i], waypoints[waypoints.size() - 1]);
+ }
+ return cost;
+}
+
+std::vector decompose(State start_state, std::vector u, Bounds& bounds, double dt,
+ Dimensions& dims) {
+ std::vector path;
+ State state = start_state;
+ path.push_back(state);
+ for (int i = 0; i < u.size(); i += 2) {
+ Input input = {u[i], u[i + 1]};
+ state = update(state, input, bounds, dt, dims);
+ path.push_back(state);
+ }
+ return path;
+}
+
+// Objective function
+double objective_function(const std::vector& x, std::vector& grad, void* data) {
+ std::vector path_ = decompose(path[path.size() - 1], x, bounds, dt, dims);
+ double cost = path_obs_cost(path_, obstacles) + path_waypoints_cost(path_, waypoints);
+
+ // std::cout << "Cost: " << cost << std::endl;
+ // Path obs
+ // std::cout << "Path obs cost: " << path_obs_cost(path, obstacles) << std::endl;
+ // Path waypoints
+ // std::cout << "Path waypoints cost: " << path_waypoints_cost(path, waypoints) << std::endl;
+
+ return cost;
+}
+
+void test_state() {
+ // TEST STATE
+ State state = {
+ 0, // x
+ 0, // y
+ M_PI / 4, // theta
+ 0, // tau
+ 0 // vel
+ };
+ Input input = {
+ 0, // dtau
+ -2 // vel
+ };
+ Bounds bounds = {
+ {-10, 10}, // x
+ {-10, 10}, // y
+ {-M_PI / 4, M_PI / 4}, // tau
+ {-10, 10}, // vel
+ {-2.0, 2.0}, // accel
+ {-M_PI / 8, M_PI / 8} // dtau
+ };
+ Dimensions dims = {
+ .2, // wheelbase
+ 1, // width
+ 1 // length
+ };
+
+ // Start time measurement
+ auto start_time = std::chrono::high_resolution_clock::now();
+
+ for (int i = 0; i < 30; i++) {
+ state = update(state, input, bounds, .1, dims);
+ }
+
+ // End time measurement
+ auto end_time = std::chrono::high_resolution_clock::now();
+
+ auto elapsed = end_time - start_time;
+
+ std::cout << to_string(state) << std::endl;
+ std::cout << "State update ran for " << elapsed.count() << " seconds." << std::endl;
+
+ std::cout << "Goal: " << waypoints[1].x << ", " << waypoints[1].y << std::endl;
+ std::cout << "\n Distance to Goal: " << distance({state.x, state.y}, waypoints[1]) << std::endl;
+}
+
+void plot_path(const std::vector& path, const std::vector& waypoints,
+ const std::vector>& obstacles, double obstacle_radius = 0.3) {
+ std::vector path_x, path_y;
+ for (const auto& state: path) {
+ path_x.push_back(state.x);
+ path_y.push_back(state.y);
+ }
+
+ std::vector wp_x, wp_y;
+ for (const auto& wp: waypoints) {
+ wp_x.push_back(wp.x);
+ wp_y.push_back(wp.y);
+ }
+
+ // Plot path
+ plt::figure_size(800, 600);
+ plt::plot(path_x, path_y,
+ {{"label", "Path"}, {"color", "blue"}, {"linestyle", "-"}, {"marker", "o"}});
+
+ // Plot waypoints
+ plt::scatter(wp_x, wp_y, 100, {{"label", "Waypoints"}, {"color", "green"}});
+
+ // Draw obstacles as circles
+ for (const auto& obs: obstacles) {
+ std::vector circle_x, circle_y;
+ for (double angle = 0; angle <= 2 * M_PI; angle += 0.1) {
+ circle_x.push_back(obs[0] + obstacle_radius * cos(angle));
+ circle_y.push_back(obs[1] + obstacle_radius * sin(angle));
+ }
+ plt::plot(circle_x, circle_y, {{"color", "red"}, {"label", "Obstacle"}});
+ }
+
+ // Add legend (avoiding duplicate obstacle labels)
+ plt::legend();
+ plt::title("Path Planning Visualization");
+ plt::xlabel("X Position");
+ plt::ylabel("Y Position");
+ plt::grid(true);
+ plt::axis("equal"); // Maintain aspect ratio
+
+ plt::show();
+}
+
+void optimize_iter(nlopt::opt& opt, std::vector& initial_input) {
+ double minf;
+ nlopt::result result = opt.optimize(initial_input, minf);
+}
+
+void plan() {
+ int num_states = 10;
+
+ // nlopt::opt opt(nlopt::LN_COBYLA, num_states * 2);
+ // nlopt::opt opt(nlopt::LN_BOBYQA, num_states * 2);
+ nlopt::opt opt(nlopt::LN_NELDERMEAD, num_states * 2);
+ // nlopt::opt opt(nlopt::LN_SBPLX, num_states * 2);
+ // nlopt::opt opt(nlopt::LN_PRAXIS, num_states * 2);
+ opt.set_min_objective(objective_function, nullptr);
+ opt.set_xtol_rel(1e-4);
+
+ // Set bounds
+ std::vector lb, ub;
+ // for (int i = 0; i < num_states; i++) {
+ // lb.push_back(-M_PI / 8);
+ // lb.push_back(-2);
+ // ub.push_back(M_PI / 8);
+ // ub.push_back(2);
+ // }
+ for (int i = 0; i < num_states; i++) {
+ lb.push_back(-M_PI / 4);
+ lb.push_back(0);
+ ub.push_back(M_PI / 4);
+ ub.push_back(10);
+ }
+ // opt.set_lower_bounds(lb);
+ // opt.set_upper_bounds(ub);
+
+ std::vector x = {};
+ for (int i = 0; i < num_states; i++) {
+ x.push_back(0);
+ x.push_back(0);
+ }
+
+ std::vector time_per_iter = {};
+
+ // std::vector path = {waypoints[0]};
+
+ // Start time measurement
+ auto start_time = std::chrono::high_resolution_clock::now();
+ auto mid_time = std::chrono::high_resolution_clock::now();
+ auto end_time = std::chrono::high_resolution_clock::now();
+
+ double max_time = 0;
+
+ int num_iters = 0;
+
+ while (distance(path[path.size() - 1], waypoints[waypoints.size() - 1]) > 0.5) {
+ mid_time = std::chrono::high_resolution_clock::now();
+ optimize_iter(opt, x);
+ num_iters++;
+ end_time = std::chrono::high_resolution_clock::now();
+
+ if (std::chrono::duration(end_time - mid_time).count() > max_time) {
+ max_time = std::chrono::duration(end_time - mid_time).count();
+ }
+
+ time_per_iter.push_back(std::chrono::duration(end_time - mid_time).count());
+ std::cout << "Iteration time: "
+ << std::chrono::duration(end_time - mid_time).count() << std::endl;
+
+ std::vector new_path = decompose(path[path.size() - 1], x, bounds, dt, dims);
+ // Keep only the first 2 states of the new path
+ path.push_back(new_path[1]);
+ path.push_back(new_path[2]);
+
+ // Print cost
+ // std::cout << "Cost: " << objective_function(x, x, nullptr) << std::endl;
+
+ // Shift all inititial inputs left by two input sets
+ // x.erase(x.begin(), x.begin() + 4);
+ // // // Add two new input sets to the end
+ // x.push_back(0);
+ // x.push_back(0);
+ // x.push_back(0);
+ // x.push_back(0);
+
+ // std::cout << "Inputs: " << std::endl;
+ // for (int i = 0; i < x.size(); i += 2) {
+ // std::cout << "dtau: " << x[i] << ", accel: " << x[i + 1] << std::endl;
+ // }
+
+ // Set all x to 0
+ x.clear();
+ for (int i = 0; i < num_states; i++) {
+ x.push_back(0);
+ // Push back final velocity
+ x.push_back(path[path.size() - 1].vel);
+ }
+
+ // plot_path(path, waypoints, obstacles);
+ }
+
+ end_time = std::chrono::high_resolution_clock::now();
+
+ std::cout << "Max iteration time: " << max_time << std::endl;
+ std::cout << "Avg iteration time: "
+ << std::chrono::duration(end_time - start_time).count() / num_iters
+ << std::endl;
+
+ // std::cout << "Time Per iter: " << std::endl;
+ // for (int i = 0; i < time_per_iter.size(); i++) {
+ // std::cout << time_per_iter[i] << std::endl;
+ // }
+
+ plot_path(path, waypoints, obstacles);
+}
+
+void optimize() {
+ int num_states = 10;
+
+ nlopt::opt opt(nlopt::LN_COBYLA, num_states * 2);
+ opt.set_min_objective(objective_function, nullptr);
+ opt.set_xtol_rel(1e-4);
+
+ // Set maximum time limit to 3 seconds
+ // opt.set_maxtime(3.0);
+
+ std::vector x = {};
+ for (int i = 0; i < num_states; i++) {
+ x.push_back(0);
+ x.push_back(0);
+ }
+ double minf;
+
+ // Start time measurement
+ auto end_time = std::chrono::high_resolution_clock::now();
+ auto start_time = std::chrono::high_resolution_clock::now();
+
+ try {
+ nlopt::result result = opt.optimize(x, minf);
+ end_time = std::chrono::high_resolution_clock::now();
+ std::cout << "Optimization completed successfully." << std::endl;
+ } catch (std::exception& e) {
+ std::cerr << "Optimization failed: " << e.what() << std::endl;
+ }
+
+ // End time measurement
+ std::chrono::duration elapsed = end_time - start_time;
+
+ std::cout << "Optimization ran for " << elapsed.count() << " seconds." << std::endl;
+ std::cout << "\nCOST: " << minf << "\n" << std::endl;
+
+ std::cout << "Inputs List: " << std::endl;
+ for (int i = 0; i < x.size(); i += 2) {
+ std::cout << "dtau: " << x[i] << ", accel: " << x[i + 1] << std::endl;
+ }
+
+ std::cout << "States Table: " << std::endl;
+ std::vector path = decompose(waypoints[0], x, bounds, dt, dims);
+ // Headers with tabs
+ printf("x\ty\ttheta\ttau\tvel\n");
+
+ // Print table using tabs inside printf instead
+ for (int i = 0; i < path.size(); i++) {
+ printf("%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", path[i].x, path[i].y, path[i].theta, path[i].tau,
+ path[i].vel);
+ }
+
+ plot_path(path, waypoints, obstacles);
+}
+
+int main() {
+ // optimize();
+ plan();
+ return 0;
+}
diff --git a/cev/all/cev_planner_ros2/tmp/costmap.png b/cev/all/cev_planner_ros2/tmp/costmap.png
new file mode 100644
index 0000000..308eb54
Binary files /dev/null and b/cev/all/cev_planner_ros2/tmp/costmap.png differ
diff --git a/cev/all/cev_planner_ros2/tmp/kernel b/cev/all/cev_planner_ros2/tmp/kernel
new file mode 100755
index 0000000..106e38a
Binary files /dev/null and b/cev/all/cev_planner_ros2/tmp/kernel differ
diff --git a/cev/all/cev_planner_ros2/tmp/kernel.cpp b/cev/all/cev_planner_ros2/tmp/kernel.cpp
new file mode 100644
index 0000000..38692f1
--- /dev/null
+++ b/cev/all/cev_planner_ros2/tmp/kernel.cpp
@@ -0,0 +1,193 @@
+#include
+#include
+#include
+#include
+#include
+#include // Include OpenCV header
+#include
+#include
+#include
+
+using namespace Eigen;
+using namespace std;
+
+VectorXf createGaussianKernel(int search_radius, float sigma) {
+ int kernel_size = 2 * search_radius + 1;
+ VectorXf kernel(kernel_size);
+ float sum = 0.0f;
+
+ for (int i = 0; i < kernel_size; ++i) {
+ int dist = i - search_radius;
+ kernel(i) = exp(-(dist * dist) / (2 * sigma * sigma));
+ sum += kernel(i);
+ }
+
+ // Normalize the kernel to ensure the sum is 1
+ kernel /= sum;
+ return kernel;
+}
+
+void visualizeCostmap(const std::vector>& costmap, const MatrixXf& grid,
+ const std::string& filename = "costmap.png") {
+ int rows = costmap.size();
+ int cols = costmap[0].size();
+
+ // Flatten the 2D vector into a 1D array for cv::Mat
+ std::vector flatCostmap;
+ for (int i = 0; i < rows; ++i) {
+ for (int j = 0; j < cols; ++j) {
+ // Normalize cost values to range [0, 255] and add them to flatCostmap
+ flatCostmap.push_back(static_cast(costmap[i][j]
+ * 255.0)); // Assuming 0 <= cost <= 1
+ // cout << costmap[i][j] << endl;
+ }
+ }
+
+ // Create a cv::Mat from the flattened 1D array
+ cv::Mat image(rows, cols, CV_8UC1, flatCostmap.data()); // 8-bit single-channel image
+
+ // Apply a colormap (e.g., 'JET', 'HOT', 'PLASMA')
+ cv::Mat coloredImage;
+ cv::applyColorMap(image, coloredImage, cv::COLORMAP_HOT);
+
+ // Overlay the original grid on the colored image
+ for (int i = 0; i < grid.rows(); ++i) {
+ for (int j = 0; j < grid.cols(); ++j) {
+ if (grid(i, j) > 0) {
+ // cv::circle(coloredImage, cv::Point(j, i), 1, cv::Scalar(200, 200, 200), -1);
+ cv::rectangle(coloredImage, cv::Point(j, i), cv::Point(j + 1, i + 1),
+ cv::Scalar(200, 200, 200), -1);
+ }
+ }
+ }
+
+ // Invert the image (optional)
+ // cv::bitwise_not(coloredImage, coloredImage);
+
+ // Make the red stronger
+ // cv::Mat channels[3];
+ // cv::split(coloredImage, channels);
+ // channels[2] = channels[2] * 2.0;
+ // cv::merge(channels, 3, coloredImage);
+
+ // Save the colored image to a file
+ cv::imwrite(filename, coloredImage);
+
+ std::cout << "Costmap image saved as " << filename << std::endl;
+}
+
+// MatrixXf random_dfs(int rows, int cols) {
+// MatrixXf grid = MatrixXf::Zero(rows, cols);
+// int num_obstacles = 5000;
+
+// std::unordered_set> y;
+// }
+
+MatrixXf generate_random_obstacles(int rows, int cols) {
+ int num_obstacles = 100;
+ int obstacle_radius = 10;
+ MatrixXf grid = MatrixXf::Zero(rows, cols);
+
+ srand(1000);
+
+ for (int i = 0; i < num_obstacles; ++i) {
+ int x = rand() % rows;
+ int y = rand() % cols;
+
+ for (int j = -obstacle_radius; j <= obstacle_radius; ++j) {
+ for (int k = -obstacle_radius; k <= obstacle_radius; ++k) {
+ int new_x = x + j;
+ int new_y = y + k;
+
+ float dist = sqrt(j * j + k * k);
+
+ if (new_x >= 0 && new_x < rows && new_y >= 0 && new_y < cols
+ && dist <= obstacle_radius) {
+ grid(new_x, new_y) = 1;
+ }
+ }
+ }
+ }
+
+ return grid;
+}
+
+int main() {
+ // Example grid initialization (replace with your actual data)
+ // MatrixXf grid(5, 5); // Example 5x5 grid
+ // grid << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23,
+ // 24,
+ // 25;
+
+ // 5x5 grid with random 1s and 0s
+ MatrixXf grid = generate_random_obstacles(1000, 1000);
+ grid = (grid.array() > .5).cast();
+
+ // Print grid
+ // cout << "Grid:\n" << grid << endl;
+
+ int search_radius = 30;
+ int kernel_size = 2 * search_radius + 1;
+ float sigma = 15.0;
+
+ // Kernel initialization
+ VectorXf kernel = createGaussianKernel(search_radius, sigma);
+
+ auto start_time = chrono::high_resolution_clock::now();
+
+ // Convolution along rows
+ MatrixXf row_conv = MatrixXf::Zero(grid.rows(), grid.cols());
+ for (int i = 0; i < grid.rows(); ++i) {
+ for (int j = 0; j < grid.cols(); ++j) {
+ float sum = 0.0f;
+ for (int k = -search_radius; k <= search_radius; ++k) {
+ int idx = j + k;
+ if (idx >= 0 && idx < grid.cols()) {
+ sum += grid(i, idx) * kernel(k + search_radius);
+ }
+ }
+ row_conv(i, j) = sum;
+ }
+ }
+
+ // Convolution along columns
+ MatrixXf cost_map = MatrixXf::Zero(grid.rows(), grid.cols());
+ for (int j = 0; j < row_conv.cols(); ++j) {
+ for (int i = 0; i < row_conv.rows(); ++i) {
+ float sum = 0.0f;
+ for (int k = -search_radius; k <= search_radius; ++k) {
+ int idx = i + k;
+ if (idx >= 0 && idx < row_conv.rows()) {
+ sum += row_conv(idx, j) * kernel(k + search_radius);
+ }
+ }
+ cost_map(i, j) = sum;
+ }
+ }
+
+ auto end_time = chrono::high_resolution_clock::now();
+ chrono::duration elapsed = end_time - start_time;
+
+ // Output the cost map
+ // cout << "Cost Map:\n" << cost_map << endl;
+ cout << "Elapsed Time: " << elapsed.count() << " seconds" << endl;
+
+ // Visualize the cost map
+ // vis_cost_map(cost_map);
+ // Convert cost map to 2D vector for visualization
+
+ cout << cost_map.rows() << endl;
+
+ std::vector> cost_map_vec(cost_map.rows(),
+ std::vector(cost_map.cols()));
+
+ for (int i = 0; i < cost_map.rows(); ++i) {
+ for (int j = 0; j < cost_map.cols(); ++j) {
+ cost_map_vec[i][j] = cost_map(i, j);
+ }
+ }
+
+ visualizeCostmap(cost_map_vec, grid, "costmap.png");
+
+ return 0;
+}
diff --git a/cev/all/cev_planner_ros2/tmp/matplotlib-cpp b/cev/all/cev_planner_ros2/tmp/matplotlib-cpp
new file mode 160000
index 0000000..ef0383f
--- /dev/null
+++ b/cev/all/cev_planner_ros2/tmp/matplotlib-cpp
@@ -0,0 +1 @@
+Subproject commit ef0383f1315d32e0156335e10b82e90b334f6d9f
diff --git a/cev/all/cev_planner_ros2/tmp/path_planner b/cev/all/cev_planner_ros2/tmp/path_planner
new file mode 100755
index 0000000..199eaa8
Binary files /dev/null and b/cev/all/cev_planner_ros2/tmp/path_planner differ
diff --git a/cev/all/cev_planner_ros2/trajectory.png b/cev/all/cev_planner_ros2/trajectory.png
new file mode 100644
index 0000000..2596124
Binary files /dev/null and b/cev/all/cev_planner_ros2/trajectory.png differ
diff --git a/cev/all/cev_teleop_ros2/CMakeLists.txt b/cev/all/cev_teleop_ros2/CMakeLists.txt
new file mode 100644
index 0000000..87fd5be
--- /dev/null
+++ b/cev/all/cev_teleop_ros2/CMakeLists.txt
@@ -0,0 +1,44 @@
+
+cmake_minimum_required(VERSION 3.8)
+project(teleop)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES Clang)
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(ackermann_msgs REQUIRED)
+
+add_executable(joy_interpreter src/joy_interpreter.cpp)
+ament_target_dependencies(joy_interpreter rclcpp sensor_msgs ackermann_msgs)
+
+install(TARGETS
+ joy_interpreter
+ DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY launch/
+ DESTINATION share/${PROJECT_NAME}/launch
+)
+
+install(DIRECTORY config/
+ DESTINATION share/${PROJECT_NAME}/config
+)
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # comment the line when a copyright and license is added to all source files
+ set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
+
diff --git a/cev/all/cev_teleop_ros2/config/.gitkeep b/cev/all/cev_teleop_ros2/config/.gitkeep
new file mode 100644
index 0000000..e69de29
diff --git a/cev/all/cev_teleop_ros2/launch/launch.py b/cev/all/cev_teleop_ros2/launch/launch.py
new file mode 100644
index 0000000..fced3e0
--- /dev/null
+++ b/cev/all/cev_teleop_ros2/launch/launch.py
@@ -0,0 +1,25 @@
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+ return LaunchDescription(
+ [
+ Node(
+ package="joy",
+ executable="joy_node",
+ name="joy_node",
+ ),
+ Node(
+ package="teleop",
+ executable="joy_interpreter",
+ name="joy_interpreter",
+ )
+ ]
+ )
+
diff --git a/cev/all/cev_teleop_ros2/package.xml b/cev/all/cev_teleop_ros2/package.xml
new file mode 100644
index 0000000..92322c8
--- /dev/null
+++ b/cev/all/cev_teleop_ros2/package.xml
@@ -0,0 +1,23 @@
+
+
+
+ teleop
+ 0.0.0
+ TODO: Package description
+ root
+ TODO: License declaration
+
+ ament_cmake
+
+ joy
+ rclcpp
+ sensor_msgs
+ ackermann_msgs
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
diff --git a/cev/all/cev_teleop_ros2/src/joy_interpreter.cpp b/cev/all/cev_teleop_ros2/src/joy_interpreter.cpp
new file mode 100644
index 0000000..d03bba1
--- /dev/null
+++ b/cev/all/cev_teleop_ros2/src/joy_interpreter.cpp
@@ -0,0 +1,103 @@
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/joy.hpp"
+#include "ackermann_msgs/msg/ackermann_drive.hpp"
+#include
+#include
+
+class LogitechRead {
+public:
+ LogitechRead(const std::vector& axes, const std::vector& buttons)
+ : axes_(axes), buttons_(buttons) {}
+
+ float get_left_trigger() const {
+ return 1 - (axes_[2] + 1) / 2;
+ }
+
+ float get_right_trigger() const {
+ return 1 - (axes_[5] + 1) / 2;
+ }
+
+ float get_y_button() const {
+ return buttons_[3];
+ }
+
+ float get_right_stick_x() const {
+ return -axes_[3];
+ }
+
+private:
+ std::vector axes_;
+ std::vector buttons_;
+};
+
+class JoyInterpreter : public rclcpp::Node {
+public:
+ JoyInterpreter(): Node("joy_interpreter") {
+ movement_pub_ =
+ this->create_publisher("rc_movement_msg", 1);
+ joy_sub_ = this->create_subscription("joy", 1,
+ std::bind(&JoyInterpreter::joy_to_twist, this, std::placeholders::_1));
+
+ this->declare_parameter("joy_interpreter/teleop/deadzone", 0.1);
+ this->declare_parameter("joy_interpreter/teleop/max_turning_angle",
+ M_PI / 9); // 20 degrees in radians
+ this->declare_parameter("joy_interpreter/teleop/max_velocity", 1.5);
+
+ deadzone_ = this->get_parameter("joy_interpreter/teleop/deadzone").as_double();
+ max_turning_angle_ =
+ this->get_parameter("joy_interpreter/teleop/max_turning_angle").as_double();
+ max_velocity_ = this->get_parameter("joy_interpreter/teleop/max_velocity").as_double();
+
+ RCLCPP_INFO(this->get_logger(), "Parameter max_turning_angle=%f", max_turning_angle_);
+ RCLCPP_INFO(this->get_logger(), "Parameter max_velocity=%f", max_velocity_);
+ RCLCPP_INFO(this->get_logger(), "Parameter deadzone=%f", deadzone_);
+ }
+
+private:
+ void joy_to_twist(const sensor_msgs::msg::Joy::SharedPtr msg) {
+ LogitechRead gamepad_data(msg->axes, msg->buttons);
+
+ // Calculate turning angle
+ float turn = -gamepad_data.get_right_stick_x() * max_turning_angle_;
+ if (std::abs(turn) < deadzone_) {
+ turn = 0.0;
+ }
+
+ // Calculate velocity
+ float drive = 0.0;
+
+// if (gamepad_data.get_y_button()) {
+// drive = .3;
+// } else {
+ drive = (gamepad_data.get_right_trigger() - gamepad_data.get_left_trigger())
+ * max_velocity_;
+// }
+
+ // float drive = (gamepad_data.get_right_trigger() - gamepad_data.get_left_trigger())
+ // * max_velocity_;
+
+ // float drive = gamepad_data.get_y_button() ? .3 : 0.0;
+
+ // RCLCPP_INFO(this->get_logger(), "Button: %f", gamepad_data.get_y_button());
+
+ // Publish AckermannDrive message
+ auto ackermann_msg = ackermann_msgs::msg::AckermannDrive();
+ ackermann_msg.steering_angle = turn;
+ ackermann_msg.speed = drive;
+ movement_pub_->publish(ackermann_msg);
+ }
+
+ rclcpp::Publisher::SharedPtr movement_pub_;
+ rclcpp::Subscription::SharedPtr joy_sub_;
+
+ double deadzone_;
+ double max_turning_angle_;
+ double max_velocity_;
+};
+
+int main(int argc, char* argv[]) {
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
diff --git a/cev/all/cev_trajectory_follower_ros2/CMakeLists.txt b/cev/all/cev_trajectory_follower_ros2/CMakeLists.txt
new file mode 100644
index 0000000..2476c72
--- /dev/null
+++ b/cev/all/cev_trajectory_follower_ros2/CMakeLists.txt
@@ -0,0 +1,68 @@
+
+cmake_minimum_required(VERSION 3.8)
+project(trajectory_follower)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES Clang)
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(ackermann_msgs REQUIRED)
+find_package(cev_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+
+add_executable(trajectory_follower_node src/trajectory_follower.cpp)
+add_executable(debug_trajectory src/debug_trajectory_publish.cpp)
+
+ament_target_dependencies(trajectory_follower_node
+ rclcpp
+ std_msgs
+ sensor_msgs
+ ackermann_msgs
+ cev_msgs
+ nav_msgs
+ tf2_ros
+ tf2_geometry_msgs)
+
+ament_target_dependencies(debug_trajectory
+ rclcpp
+ std_msgs
+ sensor_msgs
+ ackermann_msgs
+ cev_msgs
+ nav_msgs
+ tf2_ros
+ tf2_geometry_msgs)
+
+install(TARGETS trajectory_follower_node debug_trajectory
+ DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY launch/
+ DESTINATION share/${PROJECT_NAME}/launch
+)
+
+install(DIRECTORY config/
+ DESTINATION share/${PROJECT_NAME}/config
+)
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # comment the line when a copyright and license is added to all source files
+ set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
+
diff --git a/cev/all/cev_trajectory_follower_ros2/config/.gitkeep b/cev/all/cev_trajectory_follower_ros2/config/.gitkeep
new file mode 100644
index 0000000..e69de29
diff --git a/cev/all/cev_trajectory_follower_ros2/launch/launch.py b/cev/all/cev_trajectory_follower_ros2/launch/launch.py
new file mode 100644
index 0000000..e51aca0
--- /dev/null
+++ b/cev/all/cev_trajectory_follower_ros2/launch/launch.py
@@ -0,0 +1,24 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+ return LaunchDescription(
+ [
+ Node(
+ package="trajectory_follower",
+ executable="trajectory_follower_node",
+ name="trajectory_node",
+ ),
+ # Node(
+ # package="controls",
+ # executable="debug_trajectory",
+ # name="debug_trajectory",
+ # )
+ ]
+ )
+
diff --git a/cev/all/cev_trajectory_follower_ros2/package.xml b/cev/all/cev_trajectory_follower_ros2/package.xml
new file mode 100644
index 0000000..712358a
--- /dev/null
+++ b/cev/all/cev_trajectory_follower_ros2/package.xml
@@ -0,0 +1,28 @@
+
+
+