Fix ROS bugs in state estimators

.docker/Dockerfile

@@ -13,7 +13,8 @@ RUN apt-get update && apt-get install -y \
     ros-noetic-robot-pose-ekf \
     ros-noetic-ackermann-msgs \
     ros-noetic-urg-node \
-    ros-noetic-realsense2-camera
+    ros-noetic-realsense2-camera \
+    ros-noetic-foxglove-bridge
 
 RUN pip3 install numpy "jax[cpu]" pyyaml rospkg transforms3d equinox scipy matplotlib
 RUN pip3 install --upgrade numpy
@@ -74,4 +75,4 @@ RUN source /opt/ros/noetic/setup.sh && catkin_make
 
 # Add lines to the bashrc file that source ROS
 RUN echo "source /opt/ros/noetic/setup.bash" >> /root/.bashrc
-RUN echo "source /catkin_ws/devel/setup.bash" >> /root/.bashrc
+RUN echo "source /catkin_ws/devel/setup.bash" >> /root/.bashrc

README.md

@@ -61,4 +61,12 @@ After putting all robots in their start positions:
 5. **Start experiments:** Attach to the bash service `docker attach bash`
     a. Enable teleop control of F1Tenth by holding R1 on the controller
     b. Start the experiment by `rostopic pub -1 /start_control`
-    c. Stop the experiment by `rostopic pub -1 /stop_control`
+    c. Stop the experiment by `rostopic pub -1 /stop_control`
+
+## TODO
+
+Configure environment variables
+```
+ENV ROS_MASTER_URI=http://localhost:11311
+ENV ROS_IP=192.168.0.166
+```

docker-compose.simulators.yml

@@ -0,0 +1,20 @@
+version: "3"
+
+services:
+  simulators:
+    image: realm_gc:latest
+    build:
+      context: .
+      dockerfile: .docker/Dockerfile
+    environment:
+      - DISPLAY=${DISPLAY} # allows GUI access
+    volumes:
+      - /tmp/.X11-unix:/tmp/.X11-unix # allows GUI access
+      - ./realm_gc:/catkin_ws/src/realm_gc # allows access to the ROS package in the realm directory
+    network_mode: host
+    privileged: true
+    stdin_open: true # docker run -i
+    tty: true # docker run -t
+    depends_on:
+      - roscore
+    command: /bin/bash -c 'source /catkin_ws/devel/setup.bash && roslaunch --wait rgc all_simulators.launch'

docker-compose.yml

@@ -25,22 +25,35 @@ services:
       - roscore
     command: /bin/bash -c '. /catkin_ws/devel/setup.bash && roslaunch --wait vicon_bridge vicon.launch datastream_hostport:=192.168.0.9'
 
-  rviz:
+  foxglove_bridge:
     image: realm_gc:latest
     build:
       context: .
       dockerfile: .docker/Dockerfile
-    environment:
-      - DISPLAY=${DISPLAY}
     volumes:
-      - /tmp/.X11-unix:/tmp/.X11-unix
+      - ./realm_gc:/catkin_ws/src/realm_gc # allows access to the ROS package in the realm directory
+    network_mode: host
+    privileged: true
+    stdin_open: true # docker run -i
+    tty: true # docker run -t
+    depends_on:
+      - roscore
+    command: /bin/bash -c 'catkin_make && . /catkin_ws/devel/setup.bash && roslaunch --wait --screen foxglove_bridge foxglove_bridge.launch port:=8765'
+
+  robots:
+    image: realm_gc:latest
+    build:
+      context: .
+      dockerfile: .docker/Dockerfile
+    volumes:
+      - ./realm_gc:/catkin_ws/src/realm_gc # allows access to the ROS package in the realm directory
     network_mode: host
     privileged: true
     stdin_open: true # docker run -i
     tty: true # docker run -t
     depends_on:
       - roscore
-    command: /bin/bash -c '. /catkin_ws/devel/setup.bash && rviz'
+    command: /bin/bash -c 'catkin_make && source /catkin_ws/devel/setup.bash && roslaunch --wait rgc tro_all.launch'
 
   bash:
     image: realm_gc:latest

realm_gc/rgc/launch/sim/all_simulators.launch

@@ -0,0 +1,26 @@
+<!--
+    Launch simulators for two turtlebots and an f1tenth platform
+-->
+
+<launch>
+    <!-- Start the F1Tenth simulator -->
+    <!-- <include file="$(find rgc)/launch/tro_f1tenth.launch">
+        <arg name="position_topic" default="/vicon/realm_f1tenth/realm_f1tenth"/>
+        <arg name="control_topic" default="/vesc/high_level/ackermann_cmd_mux/input/nav_0"/>
+    </include> -->
+
+    <!-- Start the Turtlebot simulators -->
+    <group ns="turtle1">
+        <include file="$(find rgc)/launch/sim/turtlebot_simulator.launch">
+            <arg name="position_topic" default="/vicon/realm_turtle_1/realm_turtle_1"/>
+            <arg name="control_topic" default="/turtle1/cmd_vel"/>
+        </include>
+    </group>
+
+    <group ns="turtle2">
+        <include file="$(find rgc)/launch/sim/turtlebot_simulator.launch">
+            <arg name="position_topic" default="/vicon/realm_turtle_2/realm_turtle_2"/>
+            <arg name="control_topic" default="/turtle2/cmd_vel"/>
+        </include>
+    </group>
+</launch>

realm_gc/rgc/launch/sim/turtlebot_simulator.launch

@@ -0,0 +1,11 @@
+<!-- This launch file starts simulators for the turtlebot -->
+
+<launch>
+    <arg name="position_topic" default="/vicon/realm_turtle0/realm_turtle0"/>
+    <arg name="control_topic" default="/turtle1/cmd_vel"/>
+
+    <node name="turtlebot_simulator" pkg="rgc_simulator" type="turtlebot_simulator.py" output="screen">
+        <param name="position_topic" type="string" value="$(arg position_topic)"/>
+        <param name="control_topic" type="string" value="$(arg control_topic)"/>
+    </node>
+</launch>

realm_gc/rgc/launch/tro_all.launch

@@ -5,30 +5,31 @@
 
 <launch>
     <!-- Start the F1Tenth stack -->
-    <include file="$(find rgc)/launch/tro_f1tenth.launch">
+    <!-- TODO uncomment -->
+    <!-- <include file="$(find rgc)/launch/tro_f1tenth.launch">
         <arg name="base_path" default="$(find rgc_control)/saved_policies"/>
         <arg name="traj_filepath" default="base/ego_traj.eqx"/>
         <arg name="mlp_filepath" default="base/mlp.eqx"/>
 
         <arg name="vicon_topic" default="/vicon/realm_f1tenth/realm_f1tenth"/>
-    </include>
+    </include> -->
 
     <!-- Start the Turtlebot stacks -->
-    <group ns="turtle0">
+    <group ns="turtle1">
         <include file="$(find rgc)/launch/tro_turtlebot.launch">
             <arg name="base_path" default="$(find rgc_control)/saved_policies"/>
             <arg name="traj_filepath" default="base/non_ego_traj_0.eqx"/>
 
-            <arg name="vicon_topic" default="/vicon/realm_turtle0/realm_turtle0"/>
+            <arg name="vicon_topic" default="/vicon/realm_turtle_1/realm_turtle_1"/>
         </include>
     </group>
 
-    <group ns="turtle1">
+    <group ns="turtle2">
         <include file="$(find rgc)/launch/tro_turtlebot.launch">
             <arg name="base_path" default="$(find rgc_control)/saved_policies"/>
             <arg name="traj_filepath" default="base/non_ego_traj_1.eqx"/>
 
-            <arg name="vicon_topic" default="/vicon/realm_turtle1/realm_turtle1"/>
+            <arg name="vicon_topic" default="/vicon/realm_turtle_2/realm_turtle_2"/>
         </include>
     </group>
 </launch>

realm_gc/rgc/launch/tro_turtlebot.launch

@@ -4,21 +4,20 @@
     <arg name="base_path" default="$(find rgc_control)/saved_policies"/>
     <arg name="traj_filepath" default="base/non_ego_traj_0.eqx"/>
 
-    <arg name="vicon_topic" default="/vicon/realm_turtle0/realm_turtle0"/>
+    <arg name="vicon_topic" default="/vicon/realm_turtle_1/realm_turtle_1"/>
 
     <!-- State estimator -->
     <node name="turtlebot_state_estimator" pkg="rgc_state_estimators" type="turtlebot_ekf.py" output="screen">
         <param name="position_topic" type="string" value="$(arg vicon_topic)"/>
+        <param name="obs_noise_cov" type="double" value="0.1"/>
+        <param name="process_noise_cov" type="double" value="0.1"/>
     </node>
 
     <!-- Controller -->
-    <node name="turtlebot_controller" pkg="rgc_control" type="turtlebot_control.py" output="screen">
-        <!-- Info on where to find the saved trajectory file -->
+    <!-- TODO uncomment -->
+    <!-- <node name="turtlebot_controller" pkg="rgc_control" type="turtlebot_control.py" output="screen">
         <param name="trajectory/base_path" type="string" value="$(arg base_path)"/>
         <param name="trajectory/filename" type="string" value="$(arg traj_filepath)"/>
-
-        <!-- Match up with state estimator topic name -->
         <param name="state_estimate_topic" type="string" value="turtlebot_state_estimator"/>
-    </node>
-
+    </node> -->
 </launch>

realm_gc/rgc_control/src/rgc_control/f1tenth_control.py

@@ -1,4 +1,4 @@
-#!usr/bin/env python
+#!/usr/bin/env python3
 """Define class for robot control """
 import os
 

realm_gc/rgc_control/src/rgc_control/robot_control.py

@@ -23,7 +23,6 @@ def __init__(self):
         self.control_pub = None
 
         # Initialize the control
-        self.reset_control()
         self.ctrl_c = False
 
         # Create a subscriber for starting/stopping the controller

realm_gc/rgc_control/src/rgc_control/turtlebot_control.py

@@ -1,4 +1,4 @@
-#!usr/bin/env python
+#!/usr/bin/env python3
 """Define control for turtlebot using LQR """
 import numpy as np
 import rospy