Commit 78941775 authored by Brian Hou's avatar Brian Hou
Browse files

Release Project 2: Localization.

parent 0c64c4c5
Pipeline #409088 failed with stage
in 5 minutes and 10 seconds
[flake8]
max-line-length = 120
max-complexity = 10
ignore = E203,W503
......@@ -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:
......
......@@ -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)
......
#!/usr/bin/env sh
# Env vars we want access to in the shell tools
\ No newline at end of file
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
mviz() {
rviz -d $(rospack find cse478)/config/default.rviz
}
\ No newline at end of file
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
......@@ -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>
free_thresh: 0.196
image: cse_006.pgm
negate: 0
occupied_thresh: 0.65
origin:
- -2.5874989999999993
- -7.912499
- 0.0
resolution: 0.025
image: maze_0.pgm
resolution: 0.050000
origin: [-12.200000, -12.200000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
image: shapes_world.pgm
resolution: 0.025000
origin: [0, 0, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
#!/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
#!/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
#!/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
"""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)
......@@ -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)
#!/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"
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment