diff --git a/.gitignore b/.gitignore index 615a7fe2..b64ba3bd 100644 --- a/.gitignore +++ b/.gitignore @@ -15,6 +15,8 @@ install/ log/ simulator_old/ +e2e_planner/data/ +e2e_planner/runs/ #ファイル名で指定 AGENTS.md CLAUDE.md diff --git a/docker/Dockerfile b/docker/Dockerfile index 4248ff44..bf1de17a 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -1,39 +1,8 @@ -# cudaやcuDNNを利用したい場合は公式サイトを参考に以下のように変更してください -# 例 FROM nvidia/cuda:12.2.0-devel-ubuntu22.04 -FROM ubuntu:22.04 -# maintainer -LABEL maintainer="Kenta Ishizeki" -# ROS_DIDTRO -ENV ROS_DISTRO=humble -# username 自分の好きな名前に変更 -ENV USER_NAME=developer -ENV DEBIAN_FRONTEND=noninteractive -# workspace 好きな名前に変更 -ENV MYWORKSPACE_NAME=ros2_ws -# basic packages -RUN apt-get update && \ - apt-get upgrade -y && \ - apt-get install -y \ - wget \ - curl \ - git \ - build-essential \ - vim \ - sudo \ - lsb-release \ - locales \ - bash-completion \ - tzdata \ - python3-pip \ - python3-dev \ - software-properties-common && \ - rm -rf /var/lib/apt/lists/* -# locale -RUN locale-gen en_US en_US.UTF-8 -ENV LANG=en_US.UTF-8 -ENV LANGUAGE=en_US:en -ENV LC_ALL=en_US.UTF-8 +FROM kyo0221/cudagl:12.6.0-devel-ubuntu22.04-torch-humble + +ENV USER_NAME=nvidia +ENV MYWORKSPACE_NAME=formula_ws # create user RUN useradd -m -s /bin/bash ${USER_NAME} && \ @@ -45,48 +14,22 @@ USER ${USER_NAME} WORKDIR /home/${USER_NAME} ENV HOME=/home/${USER_NAME} -# ROS2 Humble install -RUN sudo apt-get update && \ - sudo apt-get install -y software-properties-common && \ - sudo add-apt-repository universe && \ - sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ - -o /usr/share/keyrings/ros-archive-keyring.gpg && \ - echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \ - http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | \ - sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null - -# Other install -RUN sudo apt-get update && \ - sudo apt-get install -y \ - ros-humble-desktop \ - python3-colcon-common-extensions \ - python3-rosdep \ - python3-vcstool \ - python3-argcomplete \ - ros-dev-tools \ - ros-humble-joint-state-publisher \ - ros-humble-rqt-* \ - ros-humble-xacro \ - ros-$ROS_DISTRO-ros-gz-sim \ - ros-$ROS_DISTRO-ros-gz-bridge - -# rosdep -RUN sudo rosdep init || true -RUN rosdep update - -# python packages -RUN pip3 install --upgrade pip -RUN pip3 install pyserial +# install aiformula repository +RUN mkdir ~/$MYWORKSPACE_NAME && \ + cd ~/$MYWORKSPACE_NAME && \ + git clone --recursive -b fix/docker https://github.com/open-rdc/aiformula src -# Layout of terminal -RUN echo "PS1='\[\e[1;31m\]\u@\h\[\e[00m\]:\[\e[1;34m\]\w\[\e[00m\]\$ '" >> ~/.bashrc +# download YOLOPv2 weights +RUN wget -q https://github.com/CAIC-AD/YOLOPv2/releases/download/V0.0.1/yolopv2.pt \ + -O ~/$MYWORKSPACE_NAME/src/perception/road_detector/data/weights/yolopv2.pt -# ROS environment -RUN echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc && \ - echo "source/ros2_ws/install/setup.bash" >> ~/.bashrc +# config setting +COPY config/.bashrc /home/${USER_NAME}/.bashrc +COPY config/.vimrc /home/${USER_NAME}/.vimrc +COPY config/.tmux.conf /home/${USER_NAME}/.tmux.conf -RUN mkdir -p ~/$MYWORKSPACE_NAME/src && \ - cd ~/$MYWORKSPACE_NAME/src && \ - git clone --recursive -b main https://github.com/open-rdc/aiformula +# install python dependencies +COPY config/requirements.txt /tmp/requirements.txt +RUN pip3 install --no-cache-dir -r /tmp/requirements.txt CMD ["/bin/bash"] diff --git a/docker/README.md b/docker/README.md index b176843a..9e0abe87 100644 --- a/docker/README.md +++ b/docker/README.md @@ -1,45 +1,96 @@ -# aiformula2026_docker -これはAIForemulaにおける環境構築のためのDockerfileです。 -こちらを使用する前にDockerをインストールしてください。 -以下が手順になります。 +# docker + +AIFormulaの開発環境を構築するための Docker 環境です。 + +## 前提 + +- [Docker](https://docs.docker.com/get-docker/) がインストール済みであること +- GPU を使う場合は [NVIDIA Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html) がインストール済みであること + +## 使い方 + +以下のスクリプトはすべて `docker/` ディレクトリ内で実行してください。 + +### 1. イメージのビルド + +```bash +./build.sh +``` + +`aiformula:humble` というイメージが作成されます。 + +### 2. コンテナの起動 + +```bash +# GPU を使わない場合 +./run.sh + +# GPU を使う場合 +./run_gpu.sh +``` + +`my-aiformula-humble` という名前でコンテナが起動し、そのまま bash に入ります。 +（GUIが表示されない場合はホスト側で `xhost +local:docker` を実行してください）。 + +### 3. ワークスペースのビルド（初回のみ） + +コンテナ内で以下を実行します。 -## 手順 ```bash -#ホスト側 -git clone https://github.com/open-rdc/aiformula -cd docker -docker build -t . -#例 -docker run -it -d --name -docker exec -it /bin/bash -#コンテナ内 -cd ~/ros2_ws -colcon build +cd ~/formula_ws +cb # colcon build --symlink-install のエイリアス source ~/.bashrc ``` -## GPUをお使いの場合 -GPUを使っており、cudaやcuDNNを利用したい場合は以下のサイトを参考にベースイメージ名を変更してください。 -- [参考サイト](https://hub.docker.com/r/nvidia/cuda/) +### コンテナへの再接続 + +`exit` するとコンテナは停止します。再度入る場合は次のようにします。 -GPUをお使いの場合の実行例は以下のようになります。 ```bash -docker run -it \ - --net=host \ - --ipc=host \ - --pid=host \ - --gpus all \ - --name \ - -e DISPLAY=$DISPLAY \ - -e WAYLAND_DISPLAY=$WAYLAND_DISPLAY \ - -e XDG_RUNTIME_DIR=$XDG_RUNTIME_DIR \ - -e PULSE_SERVER=$PULSE_SERVER \ - -e XAUTHORITY=/tmp/.docker.xauth \ - -v $HOME/.Xauthority:/tmp/.docker.xauth \ - -v /mnt/wslg:/mnt/wslg \ - -v /tmp/.X11-unix:/tmp/.X11-unix \ - -v $(realpath ):/home// \ - - -xhost +local:docker > /dev/null && docker start > /dev/null && docker exec -it bash +# 停止中のコンテナを起動して入る +docker start -i my-aiformula-humble + +# 起動中のコンテナに別シェルから入る +docker exec -it my-aiformula-humble bash ``` + +## 補足 + +### シェルの便利機能 + +#### bash（エイリアス / 関数） + +| コマンド | 内容 | +| --- | --- | +| `cb` | `colcon build --symlink-install` | +| `cbcl` | `install/ build/ log/` を削除してからクリーンビルド | +| `bashrc` | `~/.bashrc` を再読み込み | +| `ros_make` | ワークスペースへ移動してビルドし、元のディレクトリへ戻る | + +- プロンプトにカレントディレクトリの git ブランチを表示します。 +- 起動時に `/opt/ros/humble/setup.bash` と `~/formula_ws/install/setup.bash` を自動で source します。 +- `ROS_DOMAIN_ID=10` を設定済みです。 + +#### tmux + +| 操作 | キー | +| --- | --- | +| prefix | `C-a`（`C-b` から変更） | +| ペインを垂直分割 | `prefix` + `\` | +| ペインを水平分割 | `prefix` + `-` | +| ペイン移動 | `C-h` / `C-j` / `C-k` / `C-l`（`C-o` で順送り） | +| ペインのリサイズ | `prefix` + `H` / `J` / `K` / `L` | +| ウィンドウ切り替え | `Shift` + `←` / `→` | +| 設定のリロード | `prefix` + `r` | + +- マウス操作が有効です。 +- ステータスラインは 256 色対応、ウィンドウ一覧は右寄せ表示です。 + +#### vim + +- 行番号を表示します（`set number`）。 + +### ホストとのファイル共有 + +`docker/docker_share/` がコンテナ内の `/home/host_files` にマウントされます。 +ホストとコンテナの間でファイルをやり取りしたいときはこのディレクトリを使ってください。 \ No newline at end of file diff --git a/docker/build.sh b/docker/build.sh new file mode 100755 index 00000000..3f629806 --- /dev/null +++ b/docker/build.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +eval "docker image build -t aiformula:humble ." \ No newline at end of file diff --git a/docker/config/.bashrc b/docker/config/.bashrc new file mode 100644 index 00000000..8ed72cbb --- /dev/null +++ b/docker/config/.bashrc @@ -0,0 +1,131 @@ +# ~/.bashrc: executed by bash(1) for non-login shells. +# see /usr/share/doc/bash/examples/startup-files (in the package bash-doc) +# for examples + +# If not running interactively, don't do anything +[ -z "$PS1" ] && return + +# don't put duplicate lines in the history. See bash(1) for more options +# ... or force ignoredups and ignorespace +HISTCONTROL=ignoredups:ignorespace + +# append to the history file, don't overwrite it +shopt -s histappend + +# for setting history length see HISTSIZE and HISTFILESIZE in bash(1) +HISTSIZE=1000 +HISTFILESIZE=2000 + +# check the window size after each command and, if necessary, +# update the values of LINES and COLUMNS. +shopt -s checkwinsize + +# make less more friendly for non-text input files, see lesspipe(1) +[ -x /usr/bin/lesspipe ] && eval "$(SHELL=/bin/sh lesspipe)" + +# set variable identifying the chroot you work in (used in the prompt below) +if [ -z "$debian_chroot" ] && [ -r /etc/debian_chroot ]; then + debian_chroot=$(cat /etc/debian_chroot) +fi + +# set a fancy prompt (non-color, unless we know we "want" color) +case "$TERM" in + xterm-color) color_prompt=yes;; +esac + +# uncomment for a colored prompt, if the terminal has the capability; turned +# off by default to not distract the user: the focus in a terminal window +# should be on the output of commands, not on the prompt +#force_color_prompt=yes + +if [ -n "$force_color_prompt" ]; then + if [ -x /usr/bin/tput ] && tput setaf 1 >&/dev/null; then + # We have color support; assume it's compliant with Ecma-48 + # (ISO/IEC-6429). (Lack of such support is extremely rare, and such + # a case would tend to support setf rather than setaf.) + color_prompt=yes + else + color_prompt= + fi +fi + +if [ "$color_prompt" = yes ]; then + PS1='${debian_chroot:+($debian_chroot)}\[\033[01;32m\]\u@\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\$ ' +else + PS1='${debian_chroot:+($debian_chroot)}\u@\h:\w\$ ' +fi +unset color_prompt force_color_prompt + +# If this is an xterm set the title to user@host:dir +case "$TERM" in +xterm*|rxvt*) + PS1="\[\e]0;${debian_chroot:+($debian_chroot)}\u@\h: \w\a\]$PS1" + ;; +*) + ;; +esac + +# enable color support of ls and also add handy aliases +if [ -x /usr/bin/dircolors ]; then + test -r ~/.dircolors && eval "$(dircolors -b ~/.dircolors)" || eval "$(dircolors -b)" + alias ls='ls --color=auto' + #alias dir='dir --color=auto' + #alias vdir='vdir --color=auto' + + alias grep='grep --color=auto' + alias fgrep='fgrep --color=auto' + alias egrep='egrep --color=auto' +fi + +# some more ls aliases +alias ll='ls -alF' +alias la='ls -A' +alias l='ls -CF' +alias open='xdg-open' + +# Alias definitions. +# You may want to put all your additions into a separate file like +# ~/.bash_aliases, instead of adding them here directly. +# See /usr/share/doc/bash-doc/examples in the bash-doc package. + +if [ -f ~/.bash_aliases ]; then + . ~/.bash_aliases +fi + +# enable programmable completion features (you don't need to enable +# this, if it's already enabled in /etc/bash.bashrc and /etc/profile +# sources /etc/bash.bashrc). +#if [ -f /etc/bash_completion ] && ! shopt -oq posix; then +# . /etc/bash_completion +#fi + +############ original config ############ +parse_git_branch() { + git branch 2> /dev/null | sed -e '/^[^*]/d' -e 's/* \(.*\)/[\1]/' +} + +if [ -f /usr/lib/git-core/git-sh-prompt ]; then + source /usr/lib/git-core/git-sh-prompt +elif [ -f /etc/bash_completion.d/git-prompt ]; then + source /etc/bash_completion.d/git-prompt +fi + +export PS1='\[\033[01;32m\]\u@\h\[\033[01;33m\] \w \[\033[01;31m\]$(__git_ps1 "(%s)") \n\[\033[01;34m\]\$\[\033[00m\] ' +export ROS_DOMAIN_ID=10 + +source /opt/ros/humble/setup.bash +[ -f ~/formula_ws/install/setup.bash ] && source ~/formula_ws/install/setup.bash + +export ROS_WORKSPACE=${HOME}/formula_ws + +alias cb='colcon build --symlink-install' +alias cbcl='rm -rf install/ build/ log/ && colcon build --symlink-install' +alias bashrc='source ~/.bashrc' + +function ros_make() { + dir=$PWD; + cd $ROS_WORKSPACE; + colcon build --symlink-install; + . install/local_setup.bash; + cd $dir; +} \ No newline at end of file diff --git a/docker/config/.tmux.conf b/docker/config/.tmux.conf new file mode 100644 index 00000000..c37a6a57 --- /dev/null +++ b/docker/config/.tmux.conf @@ -0,0 +1,77 @@ +## prefixキーの変更 + +# prefixキーをC-aに変更する +set -g prefix C-a + +# デフォルトのprefixキーC-bを解除する +unbind C-b + + +## ペインのキーバインド + +# prefix+\でペインを垂直分割する +bind \\ split-window -h + +# prefix+-でペインを水平分割する +bind - split-window -v + +# ペインの移動 +bind -n C-o select-pane -t :.+ +bind -n C-h select-pane -L +bind -n C-j select-pane -D +bind -n C-k select-pane -U +bind -n C-l select-pane -R + +# ペインを最大化 + +# ペインをリサイズする +bind -r H resize-pane -L 5 +bind -r J resize-pane -D 5 +bind -r K resize-pane -U 5 +bind -r L resize-pane -R 5 + + +## ウィンドウのキーバインド + +# ウィンドウを切り替える +bind -n S-left previous-window +bind -n S-right next-window + + +## ステータスラインの見た目の変更 + +# 256色モードを有効にする +set-option -g default-terminal screen-256color +set -g terminal-overrides 'xterm:colors=256' + +# ステータスラインの色を変更 +setw -g status-style fg=colour255,bg=colour234 + +# status-leftを非表示にする +set -g status-left "" + +# status-rightを非表示にする +set -g status-right "" + +# window-statusを右寄せにする +set -g status-justify right + +# window-statusの見た目を変更 +setw -g window-status-current-format '#[bg=colour2,fg=colour255]#{?client_prefix,#[bg=colour3],} #I #W ' +setw -g window-status-format '#[fg=colour242] #I #W ' + +# windowのインデックスを1から始める +set -g base-index 1 + + +## その他 + +# マウス操作を有効にする +set-option -g mouse on + +# bash +set-option -g default-shell "/bin/bash" +set -g default-command "bash --rcfile ~/.bashrc" + +# prefix+r で設定のリロード +bind r source-file ~/.tmux.conf \; display "Reloaded!" \ No newline at end of file diff --git a/docker/config/.vimrc b/docker/config/.vimrc new file mode 100644 index 00000000..de2e53cb --- /dev/null +++ b/docker/config/.vimrc @@ -0,0 +1 @@ +set number \ No newline at end of file diff --git a/docker/config/requirements.txt b/docker/config/requirements.txt new file mode 100644 index 00000000..5687d970 --- /dev/null +++ b/docker/config/requirements.txt @@ -0,0 +1,11 @@ +numpy<2 +matplotlib==3.10.9 +opencv-python==4.11.0.86 +pymap3d==3.2.0 +pyproj==3.7.1 +schedulefree==1.4.1 +scipy==1.15.3 +setuptools==58.2.0 +tensorboard==2.20.0 +tqdm==4.68.1 +transforms3d==0.4.2 diff --git a/docker/docker_share/.gitkeep b/docker/docker_share/.gitkeep new file mode 100644 index 00000000..e69de29b diff --git a/docker/login.sh b/docker/login.sh new file mode 100755 index 00000000..2e187d95 --- /dev/null +++ b/docker/login.sh @@ -0,0 +1,4 @@ +#!/bin/bash + + +eval "docker container exec -it my-aiformula-humble /bin/bash" \ No newline at end of file diff --git a/docker/run.sh b/docker/run.sh new file mode 100755 index 00000000..e93b04d8 --- /dev/null +++ b/docker/run.sh @@ -0,0 +1,16 @@ +#!/bin/bash + +eval "docker container run \ +--network host \ +-it \ +--name my-aiformula-humble \ +-e DISPLAY=$DISPLAY \ +--volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ +-v $PWD/docker_share:/home/host_files \ +--privileged \ +-v /dev:/dev \ +--env="XAUTHORITY=$XAUTH" \ +-v "$XAUTH:$XAUTH" \ +--env="QT_X11_NO_MITSHM=1" \ +--ipc=host \ +aiformula:humble" diff --git a/docker/run_gpu.sh b/docker/run_gpu.sh new file mode 100755 index 00000000..edd541f7 --- /dev/null +++ b/docker/run_gpu.sh @@ -0,0 +1,17 @@ +#!/bin/bash + +eval "docker container run \ +--network host \ +--gpus all \ +-it \ +--name my-aiformula-humble \ +-e DISPLAY=$DISPLAY \ +--volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ +-v $PWD/docker_share:/home/host_files \ +--privileged \ +-v /dev:/dev \ +--env="XAUTHORITY=$XAUTH" \ +-v "$XAUTH:$XAUTH" \ +--env="QT_X11_NO_MITSHM=1" \ +--ipc=host \ +aiformula:humble" diff --git a/main_executor/config/main_params.yaml b/main_executor/config/main_params.yaml index 55a91b3c..fbd5d65f 100644 --- a/main_executor/config/main_params.yaml +++ b/main_executor/config/main_params.yaml @@ -1,17 +1,17 @@ launch: #起動パラメータ ros__parameters: joy: true - vectornav : true - odrive : true - socketcan : true - zed : true - sim : false + vectornav : false + odrive : false + socketcan : false + zed : false + sim : true /**: #ワイルドカード(ここのパラメータは全ノードから読める: <名前に注意>) ros__parameters: # 並進 速度[m/s],加速度[m/s^2],加減速度[-m/s^2],躍度[m/s^3] linear_max: - vel : 3.0 + vel : 2.0 acc: 5.0 jer: 0.0 # 舵角 位置[deg] @@ -145,6 +145,7 @@ lane_planner_node: vector_map_topic : "/vector_map" localization_pose_topic : "/localization/pose" nav_cmd_topic : "/planning/nav_cmd" + lane_change_topic : "/flag" default_nav_cmd : "straight" nav_cmd_fallback_order : ["straight", "left", "right"] global_path_topic : "/planner/global_path" @@ -209,7 +210,7 @@ controller_server_node: base_frame_id : "base_link" # PurePursuitPlugin params (linear_max.*, steering_max.*, wheelbase は /** から継承) lookahead_distance : 5.0 - steered_gain : 1.8 + steered_gain : 1.8 chassis_driver_node: ros__parameters: diff --git a/main_executor/src/main.cpp b/main_executor/src/main.cpp index 2467bc1d..8bb5f935 100644 --- a/main_executor/src/main.cpp +++ b/main_executor/src/main.cpp @@ -39,11 +39,13 @@ int main(int argc, char * argv[]){ auto controller_server_node = std::make_shared(nodes_option); auto object_detector_node = std::make_shared(nodes_option); +#ifdef ENABLE_ZED std::shared_ptr zed_wrapper_node; if (use_zed) { zed_wrapper_node = std::make_shared(nodes_option); exec.add_node(zed_wrapper_node); } +#endif exec.add_node(controller_node); exec.add_node(chassis_driver_node); exec.add_node(vectormap_server_node); diff --git a/perception/road_detector/data/weights/.gitkeep b/perception/road_detector/data/weights/.gitkeep new file mode 100644 index 00000000..e69de29b diff --git a/scripts/convert_sim_to_vectornav_pose.py b/scripts/convert_sim_to_vectornav_pose.py deleted file mode 100644 index 773e797e..00000000 --- a/scripts/convert_sim_to_vectornav_pose.py +++ /dev/null @@ -1,72 +0,0 @@ -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import PoseWithCovarianceStamped -from sensor_msgs.msg import NavSatFix -from sensor_msgs.msg import Imu - -import pymap3d as pm -import math -import numpy as np -from scipy.spatial.transform import Rotation -import copy - -class Converter(Node): - def __init__(self): - super().__init__("sim_to_vectornav_pose_converter_node") - - # IMU yaw offset parameter (degrees) - self.imu_yaw_offset = -176 - - self.create_subscription(NavSatFix, "/navsat", self.callback_gps, 10) - self.create_subscription(Imu, "/imu_raw", self.callback_imu, 10) - - self.publisher = self.create_publisher(PoseWithCovarianceStamped, '/vectornav/pose', 10) - - self.pub_msg = PoseWithCovarianceStamped() - - self.get_logger().info(f"シミュレータのgpsとimuからvectornav/poseを再現します．IMU yawオフセット: {self.imu_yaw_offset}度") - - - - def callback_gps(self, msg): - self.pub_msg.header = msg.header - - x,y,z = pm.geodetic2ecef(msg.latitude, msg.longitude, msg.altitude) - self.pub_msg.pose.pose.position.x = x - self.pub_msg.pose.pose.position.y = y - self.pub_msg.pose.pose.position.z = z - - - def callback_imu(self, msg): - t = msg.orientation - q = self.fix_rotation([t.x, t.y, t.z, t.w], self.imu_yaw_offset) - - self.pub_msg.pose.pose.orientation.x = q[0] - self.pub_msg.pose.pose.orientation.y = q[1] - self.pub_msg.pose.pose.orientation.z = q[2] - self.pub_msg.pose.pose.orientation.w = q[3] - - self.publisher.publish(self.pub_msg) - - - def fix_rotation(self, quaternion, yaw_degrees): - rotation = Rotation.from_quat(quaternion) - euler = rotation.as_euler('xyz', degrees=False) - euler[2] = euler[2] + np.deg2rad(yaw_degrees) - - # print(np.deg2rad(euler)) - fixed_rotation = Rotation.from_euler('xyz', euler) - return fixed_rotation.as_quat() - - -def main(): - rclpy.init() - node = Converter() - - rclpy.spin(node) - node.destroy_node() - - rclpy.shutdown() - -if __name__ == "__main__": - main() diff --git a/scripts/convert_sim_to_vectornav_velocity_body.py b/scripts/convert_sim_to_vectornav_velocity_body.py deleted file mode 100644 index a4cc3c27..00000000 --- a/scripts/convert_sim_to_vectornav_velocity_body.py +++ /dev/null @@ -1,36 +0,0 @@ -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import TwistWithCovarianceStamped -from nav_msgs.msg import Odometry - - -class Converter(Node): - def __init__(self): - super().__init__("sim_to_vectornav_velocity_body_converter_node") - - self.create_subscription(Odometry, "/odom", self.callback_odom, 10) - self.publisher = self.create_publisher(TwistWithCovarianceStamped, "/vectornav/velocity_body", 10) - - self.get_logger().info("/odom から /vectornav/velocity_body を再現して出版します．") - - def callback_odom(self, msg: Odometry): - out = TwistWithCovarianceStamped() - out.header = msg.header - - out.twist.twist = msg.twist.twist - out.twist.covariance = msg.twist.covariance - - self.publisher.publish(out) - - -def main(): - rclpy.init() - node = Converter() - - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/scripts/e2e_add_yolopv2_data.py b/scripts/e2e_add_yolopv2_data.py index 77c50e7c..af4523ee 100644 --- a/scripts/e2e_add_yolopv2_data.py +++ b/scripts/e2e_add_yolopv2_data.py @@ -7,8 +7,8 @@ from pathlib import Path from tqdm import tqdm -sys.path.append(str(Path(__file__).parent.parent / 'e2e_planner' / 'scripts')) -from util.yolop_processor import YOLOPv2Processor +sys.path.append(str(Path(__file__).parent.parent / 'e2e_planner')) +from e2e_planner.util.yolop_processor import YOLOPv2Processor def process_dataset(dataset_path: Path, yolop_weight_path: Path) -> None: diff --git a/sensing/zed_wrapper/CMakeLists.txt b/sensing/zed_wrapper/CMakeLists.txt index 5159a9f5..523a3f82 100644 --- a/sensing/zed_wrapper/CMakeLists.txt +++ b/sensing/zed_wrapper/CMakeLists.txt @@ -10,9 +10,17 @@ ament_auto_find_build_dependencies() # ZED SDK is a system library — not a ROS package set(ZED_DIR "/usr/local/zed/") -find_package(zed REQUIRED) +find_package(zed QUIET) +if(NOT zed_FOUND) + message(WARNING "ZED SDK not found at ${ZED_DIR} — skipping zed_wrapper build") + ament_auto_package() + return() +endif() find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) +# let dependent packages (main_executor) know the ZED node is available +ament_export_definitions("ENABLE_ZED") + ament_auto_add_library(zed_wrapper_node SHARED src/zed_wrapper_node.cpp ) diff --git a/simulator/launch/gazebo_ignition.launch.py b/simulator/launch/gazebo_ignition.launch.py index f2ae9791..4d2bfce3 100644 --- a/simulator/launch/gazebo_ignition.launch.py +++ b/simulator/launch/gazebo_ignition.launch.py @@ -11,8 +11,6 @@ def generate_launch_description(): - # Declare world argument (default: shihou_world.sdf) - # Available options: shihou_world.sdf, classic_world_ignition.sdf world_arg = DeclareLaunchArgument( 'world', default_value='shihou_world.sdf', @@ -29,6 +27,7 @@ def generate_launch_description(): package='ros_gz_bridge', executable='parameter_bridge', arguments=[ + '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock', # RGB camera (color image only) '/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo', '/image_raw@sensor_msgs/msg/Image@gz.msgs.Image', @@ -44,10 +43,8 @@ def generate_launch_description(): remappings=[ ('/image_raw', '/zed/zed_node/rgb/image_rect_color'), ('/depth_image', '/zed/zed_node/depth/depth_registered'), - ('/navsat', '/vectornav/gnss'), - ('/depth_image_raw/points', '/zed/zed_node/point_cloud'), - # /imu_raw は convert_sim_to_vectornav_pose.py が yaw オフセットを適用して - # /vectornav/imu へ再配信するため、bridge ではリマップしない + ('/depth_image_raw/points', '/zed/zed_node/pointcloud'), + # ('/imu_raw', '/vectornav/imu') ] ) @@ -67,9 +64,6 @@ def generate_launch_description(): executable='convert_sim_to_vectornav_pose.py', output='screen', parameters=[{ - # /imu_raw から /vectornav/imu へ変換するときのyaw補正。 - # /vectornav/imuのyawは実機同様に北基準headingとして扱い， - # localization/odom側でROS ENU yawへ変換する。 'imu_frame_id': 'vectornav', }] ) @@ -83,6 +77,12 @@ def generate_launch_description(): }] ) + urdf_path = os.path.join( + get_package_share_directory('simulator'), + 'models', + 'ai_car1', + 'model.urdf', + ) ros2_control_src = os.path.join( get_package_share_directory('simulator'), 'models', @@ -91,6 +91,18 @@ def generate_launch_description(): ) ros2_control_dst = '/tmp/simulator_ai_car1_ros2_control.yaml' shutil.copyfile(ros2_control_src, ros2_control_dst) + with open(urdf_path, 'r', encoding='utf-8') as urdf_file: + robot_description = urdf_file.read() + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[{ + 'robot_description': robot_description, + 'use_sim_time': True, + }], + output='screen', + ) caster_yaw_position_spawner = Node( package='controller_manager', @@ -101,6 +113,8 @@ def generate_launch_description(): '/controller_manager', '--controller-manager-timeout', '60', + '--switch-timeout', + '60', ], output='screen', ) @@ -114,9 +128,10 @@ def generate_launch_description(): ('gz_args', [world_file_path, ' -r'])] ), steered_to_twist, + bridge, + robot_state_publisher, convert_vectornav_pose, convert_vectornav_velocity_body, - bridge, TimerAction( period=2.0, actions=[caster_yaw_position_spawner], diff --git a/simulator/models/ai_car1/model.urdf b/simulator/models/ai_car1/model.urdf index b650083e..279dd900 100644 --- a/simulator/models/ai_car1/model.urdf +++ b/simulator/models/ai_car1/model.urdf @@ -192,4 +192,4 @@ - + \ No newline at end of file diff --git a/simulator/scripts/convert_sim_to_vectornav_pose.py b/simulator/scripts/convert_sim_to_vectornav_pose.py index dece042d..a43e1ab4 100755 --- a/simulator/scripts/convert_sim_to_vectornav_pose.py +++ b/simulator/scripts/convert_sim_to_vectornav_pose.py @@ -12,8 +12,9 @@ class SimVectornavConverter(Node): """ シミュレータのセンサデータを実機 VectorNav 相当に変換して再配信する。 - - /imu_raw → yaw オフセット適用 → /vectornav/imu - - /vectornav/gnss + /vectornav/imu(補正後) → ECEF 位置 + 補正姿勢 → /vectornav/pose + - /imu_raw → yaw オフセット適用 → /vectornav/imu + - /navsat → そのまま再配信 → /vectornav/gnss + - /navsat + /vectornav/imu(補正後) → ECEF 位置 + 補正姿勢 → /vectornav/pose """ def __init__(self): @@ -26,15 +27,16 @@ def __init__(self): self._latest_corrected_imu: Imu | None = None self.create_subscription(Imu, "/imu_raw", self._imu_callback, 10) - self.create_subscription(NavSatFix, "/vectornav/gnss", self._gnss_callback, 10) + self.create_subscription(NavSatFix, "/navsat", self._gnss_callback, 10) self._imu_pub = self.create_publisher(Imu, "/vectornav/imu", 10) + self._gnss_pub = self.create_publisher(NavSatFix, "/vectornav/gnss", 10) self._pose_pub = self.create_publisher(PoseWithCovarianceStamped, "/vectornav/pose", 10) self.get_logger().info( f"yaw オフセット {self.yaw_offset_deg} 度を適用し，" f"imu_frame_id={self.imu_frame_id} として " - f"/vectornav/imu と /vectornav/pose を配信します" + f"/vectornav/imu, /vectornav/gnss, /vectornav/pose を配信します" ) # ------------------------------------------------------------------ @@ -59,6 +61,8 @@ def _imu_callback(self, msg: Imu): self._imu_pub.publish(out) def _gnss_callback(self, msg: NavSatFix): + self._gnss_pub.publish(msg) + if self._latest_corrected_imu is None: return @@ -92,4 +96,4 @@ def main(): if __name__ == "__main__": - main() + main() \ No newline at end of file diff --git a/simulator/scripts/convert_sim_to_vectornav_velocity_body.py b/simulator/scripts/convert_sim_to_vectornav_velocity_body.py index 6c47fd91..ca015b23 100755 --- a/simulator/scripts/convert_sim_to_vectornav_velocity_body.py +++ b/simulator/scripts/convert_sim_to_vectornav_velocity_body.py @@ -37,4 +37,4 @@ def main(): if __name__ == "__main__": - main() + main() \ No newline at end of file