WIP Visualization using Foxglove

.gitignore

@@ -4,6 +4,7 @@
 *.log
 *.csv
 *.ipynb
+*.dot
 dependencies/
 pylot/prediction/prediction/
 

data_gatherer.py

@@ -162,7 +162,8 @@ def main(argv):
                 traffic_light_depth_camera_stream,
                 traffic_light_segmented_camera_stream,
                 pose_stream)
-        pylot.operator_creator.add_bounding_box_logging(traffic_lights_stream)
+        pylot.operator_creator.add_bounding_box_logging(
+            traffic_lights_stream, 'tl-bboxes')
 
     if FLAGS.log_left_right_cameras:
         (left_camera_stream, right_camera_stream, _,
@@ -186,7 +187,8 @@ def main(argv):
             depth_camera_stream, center_camera_stream, segmented_stream,
             pose_stream, ground_obstacles_stream,
             ground_speed_limit_signs_stream, ground_stop_signs_stream)
-        pylot.operator_creator.add_bounding_box_logging(obstacles_stream)
+        pylot.operator_creator.add_bounding_box_logging(
+            obstacles_stream, 'bboxes')
 
     if FLAGS.log_multiple_object_tracker:
         pylot.operator_creator.add_multiple_object_tracker_logging(
@@ -300,8 +302,6 @@ def main(argv):
         stream.send(erdos.WatermarkMessage(erdos.Timestamp(is_top=True)))
 
     try:
-        if pylot.flags.must_visualize():
-            pylot.utils.run_visualizer_control_loop(control_display_stream)
         node_handle.wait()
     except KeyboardInterrupt:
         node_handle.shutdown()

docker/Dockerfile

@@ -72,3 +72,35 @@ RUN echo "if [ -f ~/.bashrc ]; then . ~/.bashrc ; fi" >> ~/.bash_profile
 # Set up ssh access to the container.
 RUN cd ~/ && ssh-keygen -q -t rsa -N '' -f ~/.ssh/id_rsa <<<y 2>&1 >/dev/null
 RUN sudo sed -i 's/#X11UseLocalhost yes/X11UseLocalhost no/g' /etc/ssh/sshd_config
+
+# WIP (for visualization in Foxglove)
+
+# Set up for Foxglove
+RUN sudo apt-get -y install wget
+
+# Install Foxglove Studio
+RUN cd /tmp
+RUN wget https://github.com/foxglove/studio/releases/download/v0.12.0/foxglove-studio-0.12.0-linux-amd64.deb
+RUN sudo apt install -y ./foxglove-studio-*.deb
+RUN rm ./foxglove-studio-*.deb
+
+# Set up for ROS
+RUN sudo apt -y update
+RUN sudo apt install -y lsb-release
+RUN sudo apt install -y curl
+
+# Download ROS
+# From, http://wiki.ros.org/melodic/Installation/Ubuntu:
+# Setup your computer to accept software from packages.ros.org: 
+RUN sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
+# Set up your keys
+# RUN sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
+RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
+# Make sure your Debian package index is up-to-date
+RUN sudo apt -y update
+# Desktop-Full Install
+RUN sudo apt -y install ros-melodic-desktop-full
+# Set up ROS environment variables
+RUN echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
+RUN source ~/.bashrc
+

install.sh

@@ -78,6 +78,11 @@ mkdir -p tracking/center_track ; cd tracking/center_track
 # COCO model
 ~/.local/bin/gdown https://drive.google.com/uc?id=1tJCEJmdtYIh8VuN8CClGNws3YO7QGd40
 
+###### Download QDTrack models ######
+cd $PYLOT_HOME/dependencies/models
+mkdir -p tracking/qd_track ; cd tracking/qd_track
+~/.local/bin/gdown https://drive.google.com/uc?id=1YNAQgd8rMqqEG-fRj3VWlO4G5kdwJbxz
+
 ##### Download AnyNet depth estimation models #####
 echo "[x] Downloading the depth estimation models..."
 cd $PYLOT_HOME/dependencies/models
@@ -140,6 +145,15 @@ sudo apt-get install llvm-9
 export LLVM_CONFIG=/usr/bin/llvm-config-9
 python3 setup.py build develop --user
 
+###### Install QDTrack ######
+cd $PYLOT_HOME/dependencies/
+git clone https://github.com/mageofboy/qdtrack.git
+git clone https://github.com/open-mmlab/mmdetection.git
+cd mmdetection
+python3 setup.py develop #need to add mmcv
+cd $PYLOT_HOME/dependencies/qdtrack
+python3 setup.py develop
+
 ##### Download the Lanenet code #####
 echo "[x] Cloning the lanenet lane detection code..."
 cd $PYLOT_HOME/dependencies/

pylot.py

@@ -218,7 +218,7 @@ def driver():
                 prediction_stream, waypoints_stream, control_stream)
         streams_to_send_top_on += ingest_streams
 
-    node_handle = erdos.run_async()
+    node_handle = erdos.run_async('pylot.dot')
 
     for stream in streams_to_send_top_on:
         stream.send(erdos.WatermarkMessage(erdos.Timestamp(is_top=True)))
@@ -260,8 +260,6 @@ def main(args):
             client.start_recorder(FLAGS.simulation_recording_file)
         node_handle, control_display_stream = driver()
         signal.signal(signal.SIGINT, shutdown)
-        if pylot.flags.must_visualize():
-            pylot.utils.run_visualizer_control_loop(control_display_stream)
         node_handle.wait()
     except KeyboardInterrupt:
         shutdown_pylot(node_handle, client, world)

pylot/component_creator.py

@@ -354,6 +354,11 @@ def add_obstacle_tracking(center_camera_stream,
             obstacles_wo_history_tracking_stream = \
                 pylot.operator_creator.add_center_track_tracking(
                     center_camera_stream, center_camera_setup)
+        elif FLAGS.tracker_type == 'qd_track':
+            logger.debug('Using QDTrack obstacle tracker...')
+            obstacles_wo_history_tracking_stream = \
+                pylot.operator_creator.add_qd_track_tracking(
+                    center_camera_stream, center_camera_setup)
         else:
             logger.debug('Using obstacle tracker...')
             obstacles_wo_history_tracking_stream = \

pylot/debug/ros_camera_publisher.py

@@ -0,0 +1,34 @@
+import sys
+import rospy
+import numpy as np
+from sensor_msgs.msg import Image
+
+
+class ROSCameraPublisher:
+    """Class that stores a ROS publisher node that publishes ROS Image messages
+
+    Args:
+        topic: the name of the topic published to
+
+    Attributes:
+        image_pub: ROS publisher node
+    """
+    def __init__(self, topic: str):
+        self.image_pub = rospy.Publisher(topic, Image, queue_size=10)
+
+    def publish(self, img_arr):
+        """Publishes a sensor_msgs/Image message (constructed from input)
+
+        Args:
+            img_arr: A numpy array storing a frame (e.g. camera, depth, etc.)
+        """
+        img_msg = Image(encoding='rgb8')
+        if type(img_arr[0][0][0]) != np.int8:
+            img_arr = img_arr.astype(np.int8)
+        img_msg.height, img_msg.width, channels = img_arr.shape
+        img_msg.data = img_arr.tobytes()
+        img_msg.step = img_msg.width * img_msg.height
+        img_msg.is_bigendian = (img_arr.dtype.byteorder == '>'
+                                or img_arr.dtype.byteorder == '='
+                                and sys.byteorder == 'big')
+        self.image_pub.publish(img_msg)

pylot/debug/ros_lidar_publisher.py

@@ -0,0 +1,45 @@
+import sys
+import rospy
+import numpy as np
+from sensor_msgs.msg import PointCloud2, PointField
+
+
+class ROSLIDARPublisher:
+    """Class that stores a ROS publisher node that publishes ROS Point Cloud messages
+
+    Args:
+        topic: the name of the topic published to
+
+    Attributes:
+        point_cloud_pub: ROS publisher node
+    """
+    def __init__(self, topic: str):
+        self.point_cloud_pub = rospy.Publisher(topic,
+                                               PointCloud2,
+                                               queue_size=10)
+
+    def publish(self, points):
+        """Publishes a sensor_msgs/PointCloud2 message (constructed from input)
+
+        Args:
+            points: A numpy array storing a point cloud (see pylot.pylot.perception.point_cloud)
+        """
+
+        points = points.astype(np.float32)
+        points_byte_array = points.tobytes()
+        row_step = len(points_byte_array)
+        point_step = len(points[0].tobytes())
+        fields = [
+            PointField('x', 0, PointField.FLOAT32, 1),
+            PointField('y', 4, PointField.FLOAT32, 1),
+            PointField('z', 8, PointField.FLOAT32, 1)
+        ]
+        point_cloud_msg = PointCloud2(height=1,
+                                      width=len(points),
+                                      is_dense=True,
+                                      is_bigendian=False,
+                                      fields=fields,
+                                      point_step=point_step,
+                                      row_step=row_step,
+                                      data=points_byte_array)
+        self.point_cloud_pub.publish(point_cloud_msg)