FROM osrf/ros:humble-desktop

# Set up environment variables
ENV DEBIAN_FRONTEND=noninteractive
ENV ROS_WS=/root/interbotix_ws
ENV DISPLAY=:0
ENV RESOLUTION=1920x1080

# Install core build dependencies, ROS 2 utilities, and GUI/VNC tools
RUN apt-get update && apt-get install -y \
    python3-pip python3-rosdep python3-colcon-common-extensions \
    ros-humble-gazebo-ros-pkgs ros-humble-ros2-control ros-humble-ros2-controllers \
    ros-humble-xacro ros-humble-moveit \
    build-essential git supervisor xvfb x11vnc fluxbox novnc net-tools \
    && rm -rf /var/lib/apt/lists/*

# Symlink noVNC index file
RUN ln -s /usr/share/novnc/vnc.html /usr/share/novnc/index.html

# Set up ROS 2 workspace and clone Interbotix ROS 2 repositories
RUN mkdir -p $ROS_WS/src
WORKDIR $ROS_WS/src
RUN git clone -b humble https://github.com/Interbotix/interbotix_ros_core.git
RUN git clone -b humble https://github.com/Interbotix/interbotix_ros_manipulators.git
RUN git clone -b humble https://github.com/Interbotix/interbotix_ros_toolboxes.git
# X-Series driver + its dependency: cloned as ROS 2 Ament packages (branch "ros2"),
# NOT the standalone C++ library on "main" - only the ros2 branch produces an
# ament-compatible CMake config that interbotix_xs_sdk's find_package() can locate.
RUN git clone -b ros2 https://github.com/Interbotix/interbotix_xs_driver.git
RUN git clone -b ros2 https://github.com/Interbotix/dynamixel-workbench.git
RUN find $ROS_WS/src -name COLCON_IGNORE -delete

# Patch xsarm_moveit.launch.py to declare 'use_gazebo_gui' (default 'false') and forward it to xsarm_gz_classic.launch.py
# so gzclient is disabled by default in headless Xvfb software rendering environments.
RUN python3 -c " \
p = '$ROS_WS/src/interbotix_ros_manipulators/interbotix_ros_xsarms/interbotix_xsarm_moveit/launch/xsarm_moveit.launch.py'; \
c = open(p).read(); \
c = c.replace('use_sim_time_param = determine_use_sim_time_param(', \"use_gazebo_gui_launch_arg = LaunchConfiguration('use_gazebo_gui')\n    use_sim_time_param = determine_use_sim_time_param(\"); \
c = c.replace(\"'use_sim_time': use_sim_time_param,\n        }.items(),\n        condition=LaunchConfigurationEquals(\", \"'use_sim_time': use_sim_time_param,\n            'use_gazebo_gui': use_gazebo_gui_launch_arg,\n        }.items(),\n        condition=LaunchConfigurationEquals(\"); \
c = c.replace(\"declared_arguments.append(\n        DeclareLaunchArgument(\n            'world_filepath'\", \"declared_arguments.append(\n        DeclareLaunchArgument(\n            'use_gazebo_gui',\n            default_value='false',\n            choices=('true', 'false'),\n            description='launches the Gazebo GUI if true.',\n        )\n    )\n    declared_arguments.append(\n        DeclareLaunchArgument(\n            'world_filepath'\"); \
open(p, 'w').write(c); \
"

# Initialize rosdep and install workspace dependencies
WORKDIR $ROS_WS
RUN rosdep update || true
RUN apt-get update && rosdep install --from-paths src --ignore-src -r -y && rm -rf /var/lib/apt/lists/*

# Skip legacy ROS1/catkin-only packages that colcon/ROS 2 cannot build
# (e.g. interbotix_xsarm_pid, interbotix_xsarm_diagnostic_tool)
RUN for f in $(grep -rl "find_package(catkin" --include=CMakeLists.txt $ROS_WS/src); do \
        touch "$(dirname "$f")/COLCON_IGNORE"; \
    done

# Build the colcon workspace (build ROS 2 message packages first to ensure CMake exports exist).
# Both build steps use --symlink-install for consistency (avoids conflicting install layouts).
# Slate mobile-base packages are skipped (require the unavailable trossen_slate library and
# are not needed for the wx250s arm simulation).
# NOTE: this builds the FULL workspace, including interbotix_xsarm_sim (Gazebo Classic) and
# interbotix_xsarm_moveit - so this same image works for both the xs_sdk-simulation variant
# (docker_container/) and the Gazebo+MoveIt variant (docker_container_gz/); only the launch
# command in supervisord.conf differs between the two.
RUN /bin/bash -c "source /opt/ros/humble/setup.bash && colcon build --packages-select interbotix_xs_msgs --symlink-install && colcon build --symlink-install --packages-skip-regex '.*slate.*'"

# Setup bash environment for interactive sessions
RUN echo "source /opt/ros/humble/setup.bash" >> /root/.bashrc
RUN echo "source $ROS_WS/install/setup.bash" >> /root/.bashrc

# Install JupyterLab into the same Python environment used by ROS 2,
# so notebooks can import rclpy / interbotix_xs_modules directly.
RUN pip3 install --no-cache-dir jupyterlab ipykernel modern_robotics "tornado<6.5.9"

# Copy supervisor configuration file
COPY supervisord.conf /etc/supervisor/conf.d/supervisord.conf

# Start Supervisord
CMD ["/usr/bin/supervisord", "-c", "/etc/supervisor/conf.d/supervisord.conf"]
