does this push

.gitignore

@@ -9,6 +9,9 @@ bin/*
 srv_gen/*
 src/graph_navigation/srv/*
 src/graph_navigation/__init__.py
+terrain_models
+terrain_models/*
+*.bag
 
 # Prerequisites
 *.d

CMakeLists.txt

@@ -91,37 +91,21 @@ target_precompile_headers(navigation
 ADD_EXECUTABLE(navigation_tests
     src/navigation/navigation_tests.cc
     src/navigation/motion_primitives.cc
+    src/navigation/load_model_test.cc
 )
 TARGET_LINK_LIBRARIES(navigation_tests
     gtest
     gtest_main
-    ${libs})
-
-ROSBUILD_ADD_EXECUTABLE(social_nav
-                        src/navigation/social_nav.cc
-                        src/navigation/social_main.cc
-                        src/navigation/motion_primitives.cc
-                        src/navigation/ackermann_motion_primitives.cc
-                        src/navigation/constant_curvature_arcs.cc
-                        src/navigation/image_tiler.cc
-                        src/navigation/linear_evaluator.cc
-                        src/navigation/deep_cost_map_evaluator.cc
-                        src/navigation/image_based_evaluator.cc
-                        src/navigation/navigation.cc
-                        src/navigation/terrain_evaluator.cc)
-TARGET_LINK_LIBRARIES(social_nav
-  shared_library
-  ${libs}
-  ${TORCH_LIBRARIES}
-  ${OpenCV_LIBS}
-  ${Boost_LIBRARIES}
-)
-target_precompile_headers(social_nav
+    ${TORCH_LIBRARIES}
+    ${OpenCV_LIBS}
+      ${libs})
+target_precompile_headers(navigation
   PRIVATE
     <torch/torch.h>
     <torch/script.h>
     <opencv2/opencv.hpp>
 )
 
+
 add_dependencies(navigation ${catkin_EXPORTED_TARGETS})
-add_dependencies(social_nav ${catkin_EXPORTED_TARGETS})
+add_dependencies(navigation_tests ${catkin_EXPORTED_TARGETS})

Dockerfile

@@ -0,0 +1,58 @@
+FROM registry.hub.docker.com/library/ros:noetic
+
+# Install apt dependencies
+RUN apt-get update && \
+    apt-get install -y git libgflags-dev libpopt-dev \
+                       libgoogle-glog-dev liblua5.1-0-dev \
+                       libboost-all-dev libqt5websockets5-dev \
+                       python-is-python3 libeigen3-dev sudo tmux wget unzip
+
+# Install ROS apt dependencies
+RUN apt-get install -y ros-noetic-tf ros-noetic-angles ros-noetic-cv-bridge ros-noetic-image-transport
+
+# Install libtorch
+WORKDIR /tmp
+RUN wget https://download.pytorch.org/libtorch/cpu/libtorch-cxx11-abi-shared-with-deps-1.10.0%2Bcpu.zip && \
+    unzip libtorch-cxx11-abi-shared-with-deps-1.10.0+cpu.zip -d /opt && \
+    rm libtorch-cxx11-abi-shared-with-deps-1.10.0+cpu.zip
+
+
+# Create and configure the user
+ARG HOST_UID
+RUN useradd dev -m -s /bin/bash -u $HOST_UID -G sudo && \
+    echo "dev ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers && \
+    mkdir -p /home/dev && \
+    chown -R dev:dev /home/dev
+
+USER dev
+WORKDIR /home/dev
+RUN rosdep update
+
+# clone deps
+RUN git clone https://github.com/ut-amrl/amrl_maps.git && \
+    git clone https://github.com/ut-amrl/amrl_msgs.git && \
+    git clone https://github.com/ut-amrl/ut_automata.git --recurse-submodules
+
+# set up .bashrc
+RUN echo "source /opt/ros/noetic/setup.sh\n" \
+"export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:~/ut_automata\n" \
+"export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:~/graph_navigation\n" \
+"export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:~/amrl_maps\n" \
+"export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:~/amrl_msgs" >> ~/.profile
+RUN echo "source /opt/ros/noetic/setup.bash\n" \
+"export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:~/ut_automata\n" \
+"export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:~/graph_navigation\n" \
+"export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:~/amrl_maps\n" \
+"export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:~/amrl_msgs" >> ~/.bashrc
+
+
+# build deps
+RUN /bin/bash -lc "cd amrl_msgs && make"
+RUN /bin/bash -lc "cd ut_automata && make"
+
+# add launcher
+ENV CS393R_DOCKER_CONTEXT 1
+COPY --chown=dev:dev ./tmux_session.sh /home/dev/tmux_session.sh
+RUN chmod u+x /home/dev/tmux_session.sh
+CMD [ "/home/dev/tmux_session.sh" ]
+ENTRYPOINT [ "/bin/bash", "-l", "-c" ]

Makefile

@@ -8,6 +8,25 @@ all: build/CMakeLists.txt.copy
 	$(info Build_type is [${build_type}])
 	$(MAKE) --no-print-directory -C build
 
+
+# fix the docker commands
+docker_all: docker_build_q
+	docker run --rm --volume "$(shell pwd)":/home/dev/graph_navigation graph_navigation "cd graph_navigation && make -j"
+
+docker_shell: docker_build_q
+	if [ $(shell docker ps -a -f name=graph_navigation_shell | wc -l) -ne 2 ]; then docker run -dit --name graph_navigation_shell --volume "$(shell pwd)":/home/dev/graph_navigation --workdir /home/dev/graph_navigation -p 10272:10272 graph_navigation; fi
+	docker exec -it graph_navigation_shell bash -l
+
+docker_stop:
+	docker container stop graph_navigation_shell
+	docker container rm graph_navigation_shell
+
+docker_build:
+	docker build --build-arg HOST_UID=$(shell id -u) -t graph_navigation .
+
+docker_build_q:
+	docker build -q --build-arg HOST_UID=$(shell id -u) -t graph_navigation .
+
 # Sets the build type to Debug.
 set_debug:
 	$(eval build_type=Debug)

config/navigation_spot_icl.lua

@@ -0,0 +1,70 @@
+function deg2rad(deg)
+  return deg * (math.pi / 180)
+end
+
+NavigationParameters = {
+  laser_topic = "velodyne_2dscan_highbeams";
+  odom_topic = "odom";
+  localization_topic = "localization";
+  image_topic = "/bev/single/compressed";
+  init_topic = "initialpose";
+  enable_topic = "autonomy_arbiter/enabled";
+  laser_loc = {
+    x = 0.065;
+    y = 0;
+  };
+  dt = 0.025;
+  max_linear_accel = 0.5;
+  max_linear_decel = 0.5;
+  -- max_linear_speed = 1.4;
+  max_linear_speed = 1.5;
+  max_angular_accel = 0.5;
+  max_angular_decel = 0.5;
+  max_angular_speed = 1.0;
+  carrot_dist = 250;  -- large carrot distance so the goal does not latch onto the map
+  system_latency = 0.24;
+  obstacle_margin = 0.15;
+  num_options = 31;
+  -- num_options = 15;
+  robot_width = 0.44;
+  robot_length = 0.5;
+  base_link_offset = 0;
+  -- max_free_path_length = 4.0;
+  max_free_path_length = 3.5;  -- ahg demo
+  max_clearance = 1.0;
+  can_traverse_stairs = false;
+  evaluator_type = "terrain2";
+  camera_calibration_path = "config/camera_calibration_kinect.yaml";
+  model_path = "";
+  local_fov = deg2rad(180);  -- do not turn in place when the goal is behind the robot
+  target_dist_tolerance = 0.5;
+  target_vel_tolerance = 0.2;
+  target_angle_tolerance = deg2rad(20);
+  use_map_speed = true;
+  use_kinect = false;
+};
+
+AckermannSampler = {
+  max_curvature = 2.5;
+  clearance_path_clip_fraction = 0.8;
+};
+
+TerrainEvaluator = {
+  patch_size_pixels = 64;
+  bev_pixels_per_meter = 150;
+  min_cost = 0.0;
+  max_cost = 1;
+  discount_factor = 0.9;
+  -- discount_factor = 0.8; -- ahg demo
+  rollout_density = 10;
+
+  model_path = "/home/dev/graph_navigation/terrain_models/model.pt";
+
+  -- dist_to_goal_weight = -0.2;
+  -- dist_to_goal_weight = -0.7;
+  dist_to_goal_weight = -2.0;
+
+  clearance_weight = 0; -- -0.25;
+  fpl_weight = 0; -- -0.75;
+  terrain_weight = 4.0;
+}

tmux_session.sh

@@ -0,0 +1,77 @@
+#!/bin/bash
+
+tmux -V > /dev/null 2>&1
+if [ $? -eq 127 ]
+then
+    echo "tmux is not installed! Cannot continue"
+    exit 127
+fi
+
+session="ROS"
+
+tmux has-session -t $session > /dev/null 2>&1
+RUNNING=$?
+
+ARG=$1
+if [[ "${ARG}" == "stop" ]]
+then
+    if [ $RUNNING -ne 0 ]
+    then
+        echo "tmux session is not running."
+        exit 0
+    fi
+
+    echo "Killing running tmux session"
+    tmux kill-session -t $session > /dev/null 2>&1
+    exit 0
+fi
+
+if [ $RUNNING -eq 0 ]
+then
+    echo "ERROR: tmux session is already running. Kill current session with ./tmux_session.sh stop"
+    exit 127
+fi
+
+
+if [ "${ARG}" == "-d" ]
+then
+    INSTALL_PATH=$2
+    if [ -z "${INSTALL_PATH}" ]
+    then
+        INSTALL_PATH="${HOME}/"
+    fi
+
+    # check for /  at the end
+    if [ ${INSTALL_PATH: -1} != '/' ]
+    then
+        INSTALL_PATH="${INSTALL_PATH}/"
+    fi
+else
+    INSTALL_PATH="${HOME}/"
+fi
+
+if ! [[ -d "${INSTALL_PATH}ut_automata" ]]
+then
+    echo "${INSTALL_PATH}ut_automata does not exist, please provide a valid path using ./tmux_session.sh -d <INSTALL_PATH>"
+    exit 127
+fi
+
+tmux new-session -d -s $session 
+tmux set mouse on
+window=0
+
+tmux rename-window -t $session:$window 'roscore'
+tmux send-keys -t $session:$window 'roscore' C-m
+
+window=1
+tmux new-window -t $session:$window -n 'simulator'
+tmux send-keys -t $session:$window "cd ${INSTALL_PATH}/ut_automata && ./bin/simulator --localize" C-m
+
+window=2
+tmux new-window -t $session:$window -n 'websocket'
+tmux send-keys -t $session:$window "cd ${INSTALL_PATH}/ut_automata && ./bin/websocket" C-m
+
+if ! [ -z ${CS393R_DOCKER_CONTEXT+x} ]
+then
+    /bin/bash
+fi