Scenario gui

.rosinstall

@@ -18,10 +18,10 @@
     uri: https://github.com/ignc-research/stable-baselines3
     version: master
 
-- git:
-    local-name: ../forks/navigation/local_planner/teb
-    uri: https://github.com/rst-tu-dortmund/teb_local_planner
-    version: melodic-devel
+#- git:
+#    local-name: ../forks/navigation/local_planner/teb
+#    uri: https://github.com/rst-tu-dortmund/teb_local_planner
+#    version: melodic-devel
 - git:
     local-name: ../forks/navigation/local_planner/mpc
     uri: https://github.com/rst-tu-dortmund/mpc_local_planner
@@ -30,3 +30,7 @@
     local-name: ../forks/navigation/msgs/ford
     uri: https://bitbucket.org/acl-swarm/ford_msgs/src/master/
     version: master
+- git:
+    local-name: ../forks/stable-baselines3
+    uri: https://github.com/tuananhroman/stable-baselines3
+    version: master

Docker/Dockerfile

@@ -0,0 +1,79 @@
+#################################################################
+# Dockerfile for Arena-Rosnav
+
+# URL: https://github.com/ignc-research/arena-rosnav
+
+# Based on Ubuntu 18.04 + ROS-Melodic-Desktop-Full Version
+
+# Author: Yi Sun
+
+#################################################################
+
+# This image includes additional meta-packages such for desktop installations than offical image
+FROM osrf/ros:melodic-desktop-full
+
+# Change the default shell from /bin/sh to /bin/bash
+SHELL ["/bin/bash","-c"]
+
+# 1. Install additinal pkgs
+RUN apt-get -y update && apt-get install -y \
+    libqt4-dev \
+    libopencv-dev \
+    liblua5.2-dev \
+    screen \
+    python3.6 \
+    python3.6-dev \
+    libpython3.6-dev \
+    python3-catkin-pkg-modules \
+    python3-rospkg-modules \
+    python3-empy \
+    python3-setuptools \
+    python3-pip \
+    ros-melodic-navigation \
+    ros-melodic-teb-local-planner \
+    ros-melodic-mpc-local-planner \
+&& echo $'\n\
+source /opt/ros/melodic/setup.sh' >> /root/.bashrc
+
+# 2. Prepare virtual environment
+RUN pip3 install --upgrade pip \
+&& pip3 install virtualenv  virtualenvwrapper\
+&& cd /root \
+&& mkdir .python_env \
+&& echo $'\n\
+export WORKON_HOME=/root/.python_env \n\
+export VIRTUALENVWRAPPER_PYTHON=/usr/bin/python3 \n\
+export VIRTUALENVWRAPPER_VIRTUALENV=/usr/local/bin/virtualenv \n\
+source /usr/local/bin/virtualenvwrapper.sh' >> /root/.bashrc
+
+ENV WORKON_HOME=/root/.python_env \
+    VIRTUALENVWRAPPER_PYTHON=/usr/bin/python3 \
+    VIRTUALENVWRAPPER_VIRTUALENV=/usr/local/bin/virtualenv
+
+# 3. Create python virtualenv and install dependencies
+RUN . /usr/local/bin/virtualenvwrapper.sh \
+&& mkvirtualenv --python=python3.6 rosnav \ 
+&& workon rosnav \
+&& pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag tf tf2_ros --ignore-installed \
+&& pip install pyyaml catkin_pkg netifaces pathlib \
+&& pip --no-cache-dir install stable-baselines3
+
+# 4. Install Arena-Rosnav repo and compile
+RUN git clone --depth 1 https://github.com/ignc-research/arena-rosnav /root/catkin_ws/src/arena-rosnav \
+&& . /usr/local/bin/virtualenvwrapper.sh \
+&& . /opt/ros/melodic/setup.sh \
+&& workon rosnav \
+&& pip install -r /root/catkin_ws/src/arena-rosnav/arena_navigation/arena_local_planner/model_based/cadrl_ros/requirements_cadrl.txt \
+&& pip install gym \
+&&  cd /root/catkin_ws/src/arena-rosnav \
+&& rosws update \
+&& . /opt/ros/melodic/setup.sh \
+&& cd /root/catkin_ws \
+&& catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python3 \
+&& echo $'source /root/catkin_ws/devel/setup.bash' >> /root/.bashrc
+
+# 5. Install geometry2 pkg
+RUN . /root/.bashrc \
+&& . /opt/ros/melodic/setup.sh \
+&& cd /root/catkin_ws/src/arena-rosnav/ \
+&& . geometry2_install.sh

Docker/README.md

@@ -0,0 +1,71 @@
+## Install docker
+Ubuntu:
+  https://cnvrg.io/how-to-setup-docker-and-nvidia-docker-2-0-on-ubuntu-18-04/
+
+  Mac:
+https://docs.docker.com/docker-for-mac/install/
+
+  Windows:
+  https://docs.docker.com/docker-for-windows/install/
+
+### Install docker-compose 
+https://docs.docker.com/compose/install/
+
+## Build image and setup container
+1. clone the whole repositary
+```
+git clone https://github.com/ignc-research/arena-rosnav
+```
+2. Open local terminal under the path where you just clone the repo
+3. Compose the docker
+```
+docker-compose up -d
+```
+
+## Start /Enter /Stop the container
+1. Check if the arena-rosnav and novnc container is already started
+```
+docker ps
+```
+2. Start container (under the path of arena-rosnav)
+```
+docker-compose start
+```
+3. Enter the ros contaier 
+```
+docker exec -it arena-rosnav bash
+```
+4. Stop the container (under the path of arena-rosnav)
+```
+docker-compose stop 
+```
+
+## Show the GUI app in ROS by browser
+1. Open 'http://localhost:8080/vnc.html'
+2. You can adjust the window size to automatic local scale or set fullscreen though the side bar on the left
+
+## Test
+1. After entering the container by `docker exec -it arena-rosnav bash` activate the venv by
+```
+workon rosnav
+```
+2. Roslaunch in venv
+```
+roslaunch arena_bringup start_arena_flatland.launch train_mode:=false use_viz:=true local_planner:=dwa map_file:=map1 obs_vel:=0.3
+
+```
+3. Open http://localhost:8080/vnc.html you will see the rviz window in browser
+3. By setting a 2D Nav Goal manually on white goal point you will see the robot move to the goal automatically
+
+## Develop in VS Code
+1. Install plugin Remote - Containers in VS Code
+2. Open the arena-rosnav folder in VS Code
+3. Click the green icon in the lower left corner (or type `F1`) 
+4. Select `Remote-Containers: Reopen in Container`
+5. Select `From docker-compose.yml` 
+6. Select `ros`
+7. Modify the files which are automatically created by VS Code
+    * devcontainer.json in .devcontainer folder  
+    You can change the workspaceFolder to "workspaceFolder": "/root/catkin_ws/src/arena-rosnav"
+    * docker-compose.yml in .devcontainer folder  
+    You can comment the "volumes:- .:/workspace:cached", which is redundant

Docker/novnc.env

@@ -0,0 +1,3 @@
+DISPLAY_WIDTH=1920
+DISPLAY_HEIGHT=1080
+RUN_XTERM=no

Docker/ros.env

@@ -0,0 +1,4 @@
+# ROS_MASTER_URI=[IP or HOSTNAME]:11311
+# ROS_HOSTNAME=ros
+# TB3_MODEL=waffle_pi
+# TURTLEBOT3_MODEL=waffle_pi

arena_bringup/launch/sublaunch/amcl.launch

@@ -0,0 +1,42 @@
+<launch>
+    <arg name="scan_topic" default="scan"/>
+    <arg name="initial_pose_x" default="0.0"/>
+    <arg name="initial_pose_y" default="0.0"/>
+    <arg name="initial_pose_a" default="0.0"/>
+
+    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
+        <param name="min_particles"             value="500"/>
+        <param name="max_particles"             value="3000"/>
+        <param name="kld_err"                   value="0.02"/>
+        <param name="update_min_d"              value="0.20"/>
+        <param name="update_min_a"              value="0.20"/>
+        <param name="resample_interval"         value="1"/>
+        <param name="transform_tolerance"       value="0.5"/>
+        <param name="recovery_alpha_slow"       value="0.00"/>
+        <param name="recovery_alpha_fast"       value="0.00"/>
+        <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
+        <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
+        <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
+        <param name="gui_publish_rate"          value="50.0"/>
+
+        <remap from="scan"                      to="$(arg scan_topic)"/>
+        <param name="laser_max_range"           value="3.5"/>
+        <param name="laser_max_beams"           value="180"/>
+        <param name="laser_z_hit"               value="0.5"/>
+        <param name="laser_z_short"             value="0.05"/>
+        <param name="laser_z_max"               value="0.05"/>
+        <param name="laser_z_rand"              value="0.5"/>
+        <param name="laser_sigma_hit"           value="0.2"/>
+        <param name="laser_lambda_short"        value="0.1"/>
+        <param name="laser_likelihood_max_dist" value="2.0"/>
+        <param name="laser_model_type"          value="likelihood_field"/>
+
+        <param name="odom_model_type"           value="diff"/>
+        <param name="odom_alpha1"               value="0.1"/>
+        <param name="odom_alpha2"               value="0.1"/>
+        <param name="odom_alpha3"               value="0.1"/>
+        <param name="odom_alpha4"               value="0.1"/>
+        <param name="odom_frame_id"             value="odom"/>
+        <param name="base_frame_id"             value="base_footprint"/>
+    </node>
+</launch>

arena_bringup/launch/sublaunch/fake_localization.launch

@@ -0,0 +1,19 @@
+
+<launch>
+  <arg name="global_frame_id"/>
+  <arg name="odom_frame_id"/>
+  <arg name="base_frame_id" />
+  <arg name="odom_ground_truth"/>
+
+<!-- send fake static transform map_to_odom, which will be used to get localization from odom to map -->
+<node pkg="tf2_ros" type="static_transform_publisher" name="map_to_odom_tfpublisher" args="0 0 0 0 0 0 $(arg global_frame_id) $(arg odom_frame_id)"/>
+
+<!-- fake localization node, which will now not be used to send location topic -->
+<!-- <node pkg="arena_fake_localization" type="fake_localization" name="fake_localization" output="screen">
+  <param name="global_frame_id" value="$(arg global_frame_id)"/>
+  <param name="odom_frame_id" value="$(arg odom_frame_id)"/>
+  <param name="base_frame_id" value="$(arg base_frame_id)"/>
+  <param name="base_pose_ground_truth" value="$(arg odom_ground_truth)"/>
+</node> -->
+
+</launch>

arena_bringup/launch/sublaunch/flatland_simulator.launch

@@ -0,0 +1,49 @@
+
+<launch>
+
+  <!--  ******************** flatland********************  -->
+  <arg name="world_path"/>
+  <arg name="update_rate" />
+  <arg name="step_size" />
+  <arg name="show_viz" />
+  <arg name="viz_pub_rate"/>
+  <arg name="use_rviz"/>  
+  <arg name="rviz_file" default="nav_LP"/>  
+  <arg name="train_mode"/>
+  <env name="ROSCONSOLE_FORMAT" value="[${severity} ${time} ${logger}]: ${message}" />
+
+  <!-- launch flatland server -->
+  <node name="flatland_server" pkg="flatland_server" type="flatland_server" output="screen">  
+    <param name="world_path" value="$(arg world_path)" />
+    <param name="update_rate" value="$(arg update_rate)" />
+    <param name="step_size" value="$(arg step_size)" />
+    <param name="show_viz" value="$(arg show_viz)" />
+    <param name="viz_pub_rate" value="$(arg viz_pub_rate)" />
+    <param name="train_mode" value="$(arg train_mode)" />
+  </node>
+
+
+  <!--  ***************** Robot Model *****************  -->
+  <arg name="initial_pose_x" default="0.0"/>
+  <arg name="initial_pose_y" default="0.0"/>
+  <arg name="initial_pose_a" default="0.0"/>
+
+  <node name="spawn_model" pkg="rosservice" type="rosservice"
+    args="call --wait /spawn_model &quot;{
+      yaml_path: '$(find simulator_setup)/robot/myrobot.model.yaml',
+      name: 'myrobot',
+      ns: '',
+      pose: {x: $(arg initial_pose_x), y: $(arg initial_pose_y), theta: $(arg initial_pose_a)}}&quot;"
+    />
+
+  <!--  ************** flatland viz  ***************  -->
+  <group if="$(arg show_viz)">
+    <node name="flatland_viz" pkg="flatland_viz" type="flatland_viz" output="screen" required="false" unless="$(arg use_rviz)"/>
+  </group>
+
+  <!--  ************** rviz  ***************  -->
+  <group if="$(arg use_rviz)">
+    <node name="flatland_rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find arena_bringup)/rviz/$(arg rviz_file).rviz"/>
+  </group>    
+
+</launch>

arena_bringup/launch/sublaunch/move_base.launch

@@ -0,0 +1,15 @@
+<launch>
+
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
+    <!-- <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" /> -->
+    <rosparam file="$(find arena_bringup)/config/config_movebase/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find arena_bringup)/config/config_movebase/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find arena_bringup)/config/config_movebase/costmap_local_params.yaml" command="load" />
+    <rosparam file="$(find arena_bringup)/config/config_movebase/costmap_global_params.yaml" command="load" />
+
+    <rosparam file="$(find arena_bringup)/config/config_movebase/move_base_params.yaml" command="load" />
+    <rosparam file="$(find arena_bringup)/config/config_movebase/global_planner_params.yaml" command="load" />
+    <rosparam file="$(find arena_bringup)/config/config_movebase/local_planner_dwa_params.yaml" command="load" />
+  </node>
+
+</launch>

arena_bringup/launch/sublaunch/move_base/move_base_cadrl.launch

@@ -0,0 +1,42 @@
+<launch>
+
+    <!-- move base -->
+    <arg name="model" default="burger"/>
+    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+        <rosparam file="$(find conventional)/config/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
+        <rosparam file="$(find conventional)/config/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />   
+        <rosparam file="$(find conventional)/config/local_costmap_params.yaml" command="load" />
+        <rosparam file="$(find conventional)/config/global_costmap_params.yaml" command="load" />
+    </node>
+
+
+    <arg name="tb3_speed" default="0.3"/>
+    <arg name="file" default="cadrl_node_tb3.py"/>
+    <!-- Launch neural net ros wrapper -->
+    <node pkg="cadrl_ros" type="cadrl_node_tb3.py" name="cadrl_node" output="screen" ns="/cadrl">
+
+        <!-- Publications -->
+        <remap from="~other_vels" to="other_vels"/>
+        <remap from="~nn_cmd_vel" to="/cmd_vel"/>
+        <remap from="~pose_marker" to="pose_marker"/>
+        <remap from="~path_marker" to="path_marker"/>
+        <remap from="~goal_path_marker" to="goal_path_marker"/>
+        <remap from="~agent_marker" to="other_agents_marker"/>
+        <remap from="~agent_markers" to="other_agents_markers"/>
+
+        <!-- Subscriptions -->
+        <remap from="~pose" to="/odom"/>
+        <remap from="~velocity" to="velocity"/>
+        <remap from="~safe_actions" to="local_path_finder/safe_actions"/>
+        <remap from="~planner_mode" to="planner_fsm/mode"/>
+        <remap from="~goal" to="/goal"/>
+        <remap from="~subgoal" to="/plan_manager/subgoal"/>
+        <remap from="~clusters" to="/obst_odom"/>
+        <remap from="~peds" to="ped_manager/ped_recent"/>
+
+        <!-- Parameters -->
+        <param name="~tb3_speed" value="$(arg tb3_speed)"/>
+
+    </node>
+
+</launch>

arena_bringup/launch/sublaunch/move_base/move_base_dwa.launch

@@ -0,0 +1,27 @@
+<!-- -->
+<launch>
+  <!-- Arguments -->
+  <arg name="model" default="burger"/>
+  <arg name="cmd_vel_topic" default="/cmd_vel" />
+  <arg name="odom_topic" default="odom" />
+  <arg name="move_forward_only" default="false"/>
+
+  <!-- move_base -->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
+    <rosparam file="$(find conventional)/config/tb3/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find conventional)/config/tb3/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find conventional)/config/tb3/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find conventional)/config/tb3/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find conventional)/config/tb3/move_base_params.yaml" command="load" />
+
+
+    <rosparam file="$(find conventional)/config/tb3/dwa_local_planner_params_burger.yaml" command="load" />
+    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
+    <remap from="odom" to="$(arg odom_topic)"/>
+    <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
+    <param name="controller_frequency" value="5.0" />
+    <param name="planner_frequency" value="0.0" />
+
+  </node>
+</launch>