Commit 0c64c4c5 authored by Brian Hou's avatar Brian Hou
Browse files

Release Project 1: Introduction.

parents
Pipeline #404174 failed with stage
in 7 minutes and 30 seconds
[flake8]
max-line-length = 120
max-complexity = 10
# Files that Git should ignore
# Python files #
################
__pycache__
*.pyc
# OS generated files #
######################
.DS_Store
.DS_Store?
._*
.Spotlight-V100
.Trashes
Icon?
ehthumbs.db
Thumbs.db
# Emacs files #
###############
*~
\#*\#
.\#*
# Vim files #
#############
*.swp
cache:
key: "${CI_JOB_NAME}" # https://docs.gitlab.com/ee/ci/caching/#sharing-caches-across-different-branches
paths:
- ccache
lint:
tags: [python-ci]
image: python:3.8-buster
script:
- pip3 -q install flake8 pep8-naming typing
- python3 -m flake8
test_packages:
tags: [industrial-ci]
image: docker:git
services:
- docker:dind
variables:
ADDITIONAL_DEBS: cython3
CATKIN_LINT: 'true'
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"
CCACHE_DIR: ${CI_PROJECT_DIR}/ccache
before_script:
- apk add --update bash coreutils tar openssh-client
- 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="."
artifacts:
when: always
reports:
junit: ${BASEDIR}/**/test_results/**/*.xml
- git:
local-name: serial
uri: https://github.com/wjwwood/serial.git
- git:
local-name: mushr
uri: https://github.com/prl-mushr/mushr.git
version: nick/noetic
- git:
local-name: push_button_utils
uri: https://github.com/prl-mushr/push_button_utils.git
- git:
local-name: ydlidar
uri: https://github.com/prl-mushr/ydlidar.git
- git:
local-name: mushr_base
uri: https://github.com/prl-mushr/mushr_base.git
version: nick/python3-support
- git:
local-name: mushr_sim
uri: https://github.com/prl-mushr/mushr_sim.git
version: nick/python3-support
- git:
local-name: vesc
uri: https://github.com/prl-mushr/vesc.git
version: nick/python3-support
\ No newline at end of file
# mushr478
## Usage
This repository is meant to live in your ROS workspace's `src` directory. It expects to be overlaid on another workspace containing MuSHR dependency packages.
## Running Tests
Each package contains a `test` directory with unit and integration tests built with the [rosunit](https://wiki.ros.org/rosunit) framework. To run all of these tests:
catkin test
If you just want to run tests for a particular package, add the package name onto the command as in this example:
catkin test introduction
<details>
<summary>Advanced Test Usage</summary>
`catkin test` provides a summary view of all test results. You may need to see more detailed logs if you are, for instance, diagnosing why test isn't being run. To run tests for a package and see more log output:
roscd introduction; catkin run_tests --no-deps --this
It is possible to run tests by individual file. The command differs by the types of tests; for tests that use ROS (they start a node, usually to publish or subscribe to topics from the code under test), use `rostest` to run the launch file for the test:
rostest introduction pose_listener.test --text
For plain Python unit tests, simply run the file:
python3 $(rospack find introduction)/test/test_norms.py
</details>
cmake_minimum_required(VERSION 3.0.2)
project(cse478)
find_package(catkin REQUIRED COMPONENTS
rospy
)
catkin_package()
catkin_python_setup()
catkin_add_env_hooks(${PROJECT_NAME} SHELLS sh DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks)
if (CATKIN_ENABLE_TESTING)
find_package(roslaunch REQUIRED)
roslaunch_add_file_check(launch)
endif()
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Map1/Orientation1
Splitter Ratio: 0.5
Tree Height: 775
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 0.5
Class: rviz/Grid
Color: 54; 205; 196
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 400
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 0.300000012
Class: rviz/Map
Color Scheme: map
Draw Behind: true
Enabled: true
Name: Map
Topic: /map
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
back_left/wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
back_right/wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_footprint:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
camera_bottom_screw_frame:
Alpha: 1
Show Axes: false
Show Trail: false
camera_color_frame:
Alpha: 1
Show Axes: false
Show Trail: false
camera_color_optical_frame:
Alpha: 1
Show Axes: false
Show Trail: false
camera_depth_frame:
Alpha: 1
Show Axes: false
Show Trail: false
camera_depth_optical_frame:
Alpha: 1
Show Axes: false
Show Trail: false
camera_left_ir_frame:
Alpha: 1
Show Axes: false
Show Trail: false
camera_left_ir_optical_frame:
Alpha: 1
Show Axes: false
Show Trail: false
camera_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
camera_right_ir_frame:
Alpha: 1
Show Axes: false
Show Trail: false
camera_right_ir_optical_frame:
Alpha: 1
Show Axes: false
Show Trail: false
front_left/wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front_left/wheel_steer_link:
Alpha: 1
Show Axes: false
Show Trail: false
front_right/wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front_right/wheel_steer_link:
Alpha: 1
Show Axes: false
Show Trail: false
insets_link:
Alpha: 1
Show Axes: false
Show Trail: false
laser_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
platform_link:
Alpha: 1
Show Axes: false
Show Trail: false
Name: RobotModel
Robot Description: car/robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 255; 22; 144
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 22; 144
Max Intensity: 4096
Min Color: 255; 22; 144
Min Intensity: 0
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.0299999993
Style: Points
Topic: /car/scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 92; 38; 134
Default Light: true
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/ThirdPersonFollower
Distance: 9.53489304
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -0.363669515
Y: 0.845769167
Z: -4.76837158e-07
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.345398068
Target Frame: base_link
Value: ThirdPersonFollower (rviz)
Yaw: 1.98539793
Saved:
- Angle: 0
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: TopDownOrtho
Near Clip Distance: 0.00999999978
Scale: 206.591095
Target Frame: base_link
Value: TopDownOrtho (rviz)
X: 0
Y: 0
Window Geometry:
Displays:
collapsed: false
Height: 1056
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1855
X: 65
Y: 24
mviz() {
rviz -d $(rospack find cse478)/config/default.rviz
}
\ No newline at end of file
<!-- -*- mode: XML -*- -->
<launch>
<!-- Set to 1 if you want to run the map_server -->
<arg name="map_server" value = "1" />
<!-- We take a map path either as an environment variable or an argument.
The environment variable has precedence. -->
<arg name="_env_map" default="$(optenv MAP)" />
<arg name="map" default="$(find mushr_sim)/maps/sandbox.yaml"/>
<arg name="car_name" default="car" />
<!-- 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>
<!-- 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 -->
<arg name="racecar_color" value="" />
<arg name="car_name" value="$(arg car_name)" />
<arg name="use_tf_prefix" value="false" />
</include>
</group>
</launch>
image: cse2_2.pgm
resolution: 0.050000
origin: [0, 0, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
<?xml version="1.0"?>
<package format="3">
<name>cse478</name>
<version>0.0.0</version>
<description>The cse478 package</description>
<author>Gilwoo Lee</author>
<author>Kay Ke</author>
<author>Johan Michalove</author>
<author>Matthew Rockett</author>
<author>Matt Schmittle</author>
<author>Nick Walker</author>
<maintainer email="mushr@cs.washington.edu">UW CSE Robotics Teaching Staff</maintainer>
<url>https://courses.cs.washington.edu/courses/cse478/</url>
<url>https://mushr.io/</url>
<license>Educational Use Only</license>
<test_depend>roslaunch</test_depend>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>map_server</exec_depend>
<exec_depend>mushr_sim</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>ackermann_msgs</exec_depend>
<export>
</export>
</package>
# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
from setuptools import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(packages=["cse478"], package_dir={"": "src"})
setup(**setup_args)
"""ROS message collector."""
import rospy
try:
import queue
except ImportError:
import Queue as queue # noqa
class MessageCollector:
"""Collect all messages published to a ROS topic."""
def __init__(self, topic, msg_type, queue_size=10):
self._topic = topic
self._msg_type = msg_type
self._queue_size = queue_size
def start(self, duration=None, max_msgs=-1):
self.msgs = queue.Queue(maxsize=max_msgs)
self.__cmd_sub = rospy.Subscriber(
self._topic, self._msg_type, self._msg_callback, queue_size=self._queue_size
)
if duration:
rospy.sleep(duration)
return self.stop()
def stop(self):
self.__cmd_sub.unregister()
self.__cmd_sub = None
return list(self.msgs.queue)
def _msg_callback(self, msg):
self.msgs.put(msg)
#!/usr/bin/env python
"""ROS geometry and map utility functions."""
import numpy as np
import rospy
import tf.transformations as tr
from geometry_msgs.msg import (
Pose,
PoseStamped,
Transform,
TransformStamped,
Quaternion,
Point32,
)
from nav_msgs.srv import GetMap
from std_msgs.msg import Header
def pose_to_pq(msg):
"""Convert a C{geometry_msgs/Pose} into position/quaternion np.arrays.
Args:
msg: ROS message to be converted
Returns:
p: position as a np.array
q: quaternion as a np.array (order = [x, y, z, w])
"""
p = np.array([msg.position.x, msg.position.y, msg.position.z])
q = np.array(
[msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]
)
return p, q
def pose_stamped_to_pq(msg):
"""Convert a C{geometry_msgs/PoseStamped} into position/quaternion np.arrays.
Args:
msg: ROS message to be converted
Returns:
p: position as a np.array
q: quaternion as a np.array (order = [x, y, z, w])
"""
return pose_to_pq(msg.pose)
def transform_to_pq(msg):
"""Convert a C{geometry_msgs/Transform} into position/quaternion np.arrays.
Args:
msg: ROS message to be converted
Returns:
p: position as a np.array
q: quaternion as a np.array (order = [x, y, z, w])
"""
p = np.array([msg.translation.x, msg.translation.y, msg.translation.z])
q = np.array([msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w])
return p, q
def transform_stamped_to_pq(msg):
"""Convert a C{geometry_msgs/TransformStamped} into position/quaternion np.arrays.
Args:
msg: ROS message to be converted