Release Project 2: Localization.

.flake8

 [flake8]
 max-line-length = 120
 max-complexity = 10
+ignore = E203,W503

.gitlab-ci.yml

@@ -18,18 +18,23 @@ test_packages:
   services:
   - docker:dind
   variables:
-    ADDITIONAL_DEBS: cython3
+    ADDITIONAL_DEBS: cython3 wget
     CATKIN_LINT: 'true'
+    DOCKER_IMAGE: zoeyc/cse478:latest
     DOCKER_TLS_CERTDIR: ""
     UPSTREAM_WORKSPACE: '.gitlab/.rosinstall'
     AFTER_INSTALL_TARGET_DEPENDENCIES: '(cd ../; git clone https://github.com/prl-mushr/range_libc.git; cd range_libc; cd pywrapper; python3 setup.py install);'
     BASEDIR: ${CI_PROJECT_DIR}/.workspaces
-    TMPDIR: "${CI_PROJECT_DIR}.tmp"
+    TMPDIR: ${CI_PROJECT_DIR}.tmp
     CCACHE_DIR: ${CI_PROJECT_DIR}/ccache
+    ROS_DISTRO: noetic
   before_script:
-  - apk add --update bash coreutils tar openssh-client
+  - apk add --update bash coreutils tar openssh-client wget xz
+  - wget -q https://courses.cs.washington.edu/courses/cse478/data/localization_bags.tar.xz --directory-prefix localization/bags/
+  - tar -xvf localization/bags/localization_bags.tar.xz --directory localization
+  - rm localization/bags/localization_bags.tar.xz
   - git clone --quiet --depth 1 https://github.com/ros-industrial/industrial_ci .industrial_ci -b master
-  script: .industrial_ci/gitlab.sh ROS_DISTRO=noetic TARGET_WORKSPACE="."
+  script: .industrial_ci/gitlab.sh TARGET_WORKSPACE="."
   artifacts:
     when: always
     reports:

cse478/CMakeLists.txt

@@ -9,7 +9,10 @@ catkin_package()
 
 catkin_python_setup()
 
-catkin_add_env_hooks(${PROJECT_NAME} SHELLS sh DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks)
+install(DIRECTORY config launch maps
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+catkin_add_env_hooks(${PROJECT_NAME} SHELLS bash zsh DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks)
 
 if (CATKIN_ENABLE_TESTING)
   find_package(roslaunch REQUIRED)

cse478/config/env.sh

+#!/usr/bin/env sh
+
+# Env vars we want access to in the shell tools
\ No newline at end of file

cse478/env-hooks/cse478.bash.em

+mviz() {
+    rviz -d $(rospack find cse478)/config/default.rviz
+}
+
+export UW_TOOLS_SHELL_FLAVOR=bash
+@[if DEVELSPACE]@
+. @(CMAKE_CURRENT_SOURCE_DIR)/config/env.sh
+export SOURCES_ROOT="@(CMAKE_CURRENT_SOURCE_DIR)/src/uw_tools"
+
+ export PATH="@(CMAKE_CURRENT_SOURCE_DIR)/scripts/available_client/:$PATH"
+. "@(CMAKE_CURRENT_SOURCE_DIR)/scripts/uw_tools_client"
+
+@[else]@
+# TODO(nickswalker): Add install space support
+@[end if]@
\ No newline at end of file

cse478/env-hooks/cse478.sh

-mviz() {
-    rviz -d $(rospack find cse478)/config/default.rviz
-}
\ No newline at end of file

cse478/env-hooks/cse478.zsh.em

+mviz() {
+    rviz -d $(rospack find cse478)/config/default.rviz
+}
+
+export UW_TOOLS_SHELL_FLAVOR=zsh
+@[if DEVELSPACE]@
+. @(CMAKE_CURRENT_SOURCE_DIR)/config/env.sh
+export SOURCES_ROOT="@(CMAKE_CURRENT_SOURCE_DIR)/src/uw_tools"
+
+export PATH="@(CMAKE_CURRENT_SOURCE_DIR)/scripts/available_client/:$PATH"
+. "@(CMAKE_CURRENT_SOURCE_DIR)/scripts/uw_tools_client"
+
+@[else]@
+# TODO(nickswalker): Add install space support
+@[end if]@
\ No newline at end of file

cse478/launch/teleop.launch

@@ -8,30 +8,45 @@
     <arg name="_env_map" default="$(optenv MAP)" />
     <arg name="map" default="$(find mushr_sim)/maps/sandbox.yaml"/>
     <arg name="car_name" default="car" />
+    <arg name="fake_localization" default="true"/>
+    <arg name="initial_x" default="0"/>
+    <arg name="initial_y" default="0"/>
+    <arg name="initial_theta" default="0"/>
 
     <!-- Launch  map server-->
-    <group if="$(arg map_server)">
-        <include file="$(find mushr_sim)/launch/map_server.launch">
-            <arg name="map" value="$(eval _env_map if _env_map else map)" />
-        </include>
-    </group>
+    <include  if="$(arg map_server)" file="$(find mushr_sim)/launch/map_server.launch">
+        <arg name="map" value="$(eval _env_map if _env_map else map)" />
+    </include>
 
     <!-- Launch car -->
-
     <group ns="$(arg car_name)">
         <remap from="/$(arg car_name)/initialpose" to="/initialpose" />
 
         <include file="$(find mushr_sim)/launch/single_car.launch">
             <!-- Could be racecar-mit, racecar-uw-tx2, or racecar-uw-nano -->
             <arg name="racecar_version" value="racecar-uw-nano" />
-            <!-- The colors of the racecar, should be of the form "-<platform_color>-<inset_color>" -->
-            <!-- An empty string will result in the default URDF -->
-              <!-- Check CMakeLists.txt of mushr_description for appropriate values -->
+             <!-- The colors of the racecar, should be of the form "-<platform_color>-<inset_color>" -->
+             <!-- An empty string will result in the default URDF -->
+             <!-- Check CMakeLists.txt of mushr_description for appropriate values -->
             <arg name="racecar_color" value="" />
 
             <arg name="car_name" value="$(arg car_name)" />
             <arg name="use_tf_prefix" value="false" />
+            <arg name="fake_localization" value="$(arg fake_localization)" />
          </include>
     </group>
 
+    <node pkg="mushr_sim" type="clicked_point_to_reposition" name="clicked_point_to_reposition" />
+    <node pkg="mushr_sim" type="mushr_sim" name="mushr_sim">
+        <param name="n_cars" value="1"/>
+        <param name="car_names" value="['car']"/>
+        <param name="initial_x" value="$(arg initial_x)"/>
+        <param name="initial_y" value="$(arg initial_y)"/>
+        <param name="initial_theta" value="$(arg initial_theta)"/>
+
+        <remap from="~$(arg car_name)/scan" to="/$(arg car_name)/scan" />
+        <remap from="~$(arg car_name)/joint_states" to="/$(arg car_name)/joint_states"/>
+        <remap from="~$(arg car_name)/car_pose" to="/$(arg car_name)/car_pose"/>
+    </node>
+
 </launch>

cse478/maps/cse_006.yaml

+free_thresh: 0.196
+image: cse_006.pgm
+negate: 0
+occupied_thresh: 0.65
+origin:
+- -2.5874989999999993
+- -7.912499
+- 0.0
+resolution: 0.025

cse478/maps/maze_0.yaml

+image: maze_0.pgm
+resolution: 0.050000
+origin: [-12.200000, -12.200000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

cse478/maps/shapes_world.yaml

+image: shapes_world.pgm
+resolution: 0.025000
+origin: [0, 0, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

cse478/scripts/available_client/whichws

+#!/usr/bin/env bash
+###
+# Prints the path to the root of the currently sourced workspace
+###
+
+if [[ -z "$CMAKE_PREFIX_PATH" ]]; then
+    exit 1
+fi
+
+# Read in the CMake path. The first path in this list is the latest sourced workspace
+IFS=':' read -ra paths <<< "$CMAKE_PREFIX_PATH"
+
+# Resolve the path, in case it's a symlink
+current=$(readlink -f $paths[0])
+# The path is actually to the devel folder, so we'll pop up one level
+current=$(dirname $current)
+
+# If we found one, print the path. Otherwise exit
+if [[ ! -z "$current" ]]; then
+    echo $current
+else
+    exit 1
+fi
\ No newline at end of file

cse478/scripts/uw_tools_client

+#!/usr/bin/env bash
+. "$SOURCES_ROOT/common/util.bash"
+case "$UW_TOOLS_SHELL_FLAVOR" in
+    zsh) source "$SOURCES_ROOT/common/modes.zsh";;
+    bash) source "$SOURCES_ROOT/common/modes.bash";;
+esac
+
+sim-mode
\ No newline at end of file

cse478/scripts/whichws

+#!/usr/bin/env bash
+###
+# Prints the path to the root of the currently sourced workspace
+###
+
+if [[ -z "$CMAKE_PREFIX_PATH" ]]; then
+    exit 1
+fi
+
+# Read in the CMake path. The first path in this list is the latest sourced workspace
+IFS=':' read -ra paths <<< "$CMAKE_PREFIX_PATH"
+
+# Resolve the path, in case it's a symlink
+current=$(readlink -f $paths[0])
+# The path is actually to the devel folder, so we'll pop up one level
+current=$(dirname $current)
+
+# If we found one, print the path. Otherwise exit
+if [[ ! -z "$current" ]]; then
+    echo $current
+else
+    exit 1
+fi
\ No newline at end of file

cse478/src/cse478/collector.py

 """ROS message collector."""
 
+import message_filters
 import rospy
 
 try:
@@ -32,3 +33,38 @@ class MessageCollector:
 
     def _msg_callback(self, msg):
         self.msgs.put(msg)
+
+
+class SynchronizedMessageCollector:
+    """Collect approximately synchronized messages published to a set of ROS topics."""
+
+    def __init__(self, topics, msg_types, queue_size=10, sync_tolerance=0.1):
+        self._topics = topics
+        self._msg_types = msg_types
+        self._queue_size = queue_size
+        self._sync_tolerance = sync_tolerance
+
+    def start(self, duration=None, max_msgs=-1):
+        self.msgs = queue.Queue(maxsize=max_msgs)
+
+        self.subscibers = []
+        for topic, msg_type in zip(self._topics, self._msg_types):
+            sub = message_filters.Subscriber(topic, msg_type)
+            self.subscibers.append(sub)
+
+        self.__ts = message_filters.ApproximateTimeSynchronizer(
+            self.subscibers, self._queue_size, self._sync_tolerance
+        )
+        self.__ts.registerCallback(self._msg_callback)
+
+        if duration:
+            rospy.sleep(duration)
+            return self.stop()
+
+    def stop(self):
+        [sub.sub.unregister() for sub in self.subscibers]
+        self.__ts = None
+        return list(self.msgs.queue)
+
+    def _msg_callback(self, *args):
+        self.msgs.put(args)

cse478/src/cse478/utils.py

@@ -4,7 +4,9 @@
 
 import numpy as np
 import rospy
-import tf.transformations as tr
+
+from threading import Timer
+from tf_conversions import transformations
 
 from geometry_msgs.msg import (
     Pose,
@@ -110,7 +112,7 @@ def msg_to_se3(msg):
         )
     elif np.abs(norm - 1.0) > 1e-6:
         q = q / norm
-    g = tr.quaternion_matrix(q)
+    g = transformations.quaternion_matrix(q)
     g[0:3, -1] = p
     return g
 
@@ -124,7 +126,7 @@ def angle_to_quaternion(angle):
     Returns:
       An equivalent C{geometry_msgs/Quaternion} message
     """
-    return Quaternion(*tr.quaternion_from_euler(0, 0, angle))
+    return Quaternion(*transformations.quaternion_from_euler(0, 0, angle))
 
 
 def quaternion_to_angle(q):
@@ -136,7 +138,7 @@ def quaternion_to_angle(q):
     Returns:
       The equivalent yaw angle (radians)
     """
-    _, _, yaw = tr.euler_from_quaternion((q.x, q.y, q.z, q.w))
+    _, _, yaw = transformations.euler_from_quaternion((q.x, q.y, q.z, q.w))
     return yaw
 
 
@@ -323,3 +325,30 @@ def world_to_map(poses, map_info):
     poses[:, 0] = c * poses[:, 0] - s * poses[:, 1]
     poses[:, 1] = s * temp + c * poses[:, 1]
     poses[:, 2] += angle
+
+
+def estimation_error(estimates, references):
+    """Compute the error between the estimated and ground truth states.
+
+    Args:
+        estimates: Estimated states (from particle filter)
+        references: Ground truth states
+
+    Returns:
+        position_errors: Positional errors at each timestep
+        abs_angular_error: Absolute angular errors at each timestep
+    """
+    position_errors = np.linalg.norm(estimates[:, :2] - references[:, :2], axis=1)
+    estimate_angles = estimates[:, 2]
+    reference_angles = references[:, 2]
+    angular_errors = np.arctan2(
+        np.sin(reference_angles - estimate_angles),
+        np.cos(reference_angles - estimate_angles),
+    )
+    return position_errors, np.abs(angular_errors)
+
+
+class RepeatTimer(Timer):
+    def run(self):
+        while not self.finished.wait(self.interval):
+            self.function(*self.args, **self.kwargs)

cse478/src/uw_tools/common/modes.bash

+#!/usr/bin/env bash
+###
+# Custom shell modes that control the current ROS master
+##
+
+# If no mode is set, set it to none. Otherwise, keep the current mode
+export UW_TOOLS_MODE=${UW_TOOLS_MODE:-"none"}
+
+function __prompt_command {
+    UW_TOOLS_PROMPT_ROS_STATUS=$(get_ros_status)
+}
+
+function sim-mode {
+    if [[ "$UW_TOOLS_MODE" == "none" ]]; then
+        export original_PROMPT_COMMAND="$PROMPT_COMMAND"
+        export original_PS1="$PS1"
+        export original_ROS_MASTER_URI="$ROS_MASTER_URI"
+        export original_ROS_IP="$ROS_IP"
+    fi
+
+    export ROS_MASTER_URI=http://localhost:11311
+    set_rosip lo
+    export UW_TOOLS_MODE="sim"
+    PROMPT_COMMAND=__prompt_command
+    PS1="\[\033[41;1;37m\]lcl \$UW_TOOLS_PROMPT_ROS_STATUS\[\033[0m\]:\[\033[01;34m\]\w\[\033[00m\]\$ "
+}
+
+function exit-mode {
+    export ROS_MASTER_URI="$original_ROS_MASTER_URI"
+    export ROS_IP="$original_ROS_IP"
+    PS1="$original_PS1"
+    PROMPT_COMMAND="$original_PROMPT_COMMAND"
+    export UW_TOOLS_MODE="none"
+}