Commit 1025d23a authored by Brian Hou's avatar Brian Hou
Browse files

Release Project 4: Planning.

parent 70829590
Pipeline #424155 failed with stage
in 14 minutes and 38 seconds
......@@ -28,6 +28,14 @@ catkin_package(
std_msgs
)
install(
DIRECTORY
config
launch
DESTINATION
${CATKIN_PACKAGE_SHARE_DESTINATION}
)
catkin_install_python(
PROGRAMS
scripts/control_node
......
......@@ -318,6 +318,13 @@ def time_parameterize_ramp_up_ramp_down(path_xyt, speed):
Returns:
path_xytv: np.array of states and speed with shape L x 4
"""
# For paths with fewer waypoints than necessary to ramp up and ramp down,
# just set the desired speed directly (with a non-zero final speed, to
# guarantee forward movement).
if path_xyt.shape[0] < 4:
speeds = speed * np.ones(path_xyt.shape[0])
return np.hstack([path_xyt, speeds[:, np.newaxis]])
ramp_distance = 0.5
displacement_vectors = np.diff(path_xyt[:, [0, 1]], axis=0)
displacements = np.linalg.norm(displacement_vectors, axis=1)
......
......@@ -223,11 +223,6 @@ class ModelPredictiveController(BaseController):
self.bbox_map = np.zeros((self.K * self.T, 2, 4))
self.collisions = np.zeros(self.K * self.T, dtype=bool)
self.obstacle_map = ~self.permissible_region
self.map_x = self.map_info.origin.position.x
self.map_y = self.map_info.origin.position.y
self.map_angle = utils.quaternion_to_angle(self.map_info.origin.orientation)
self.map_c = np.cos(self.map_angle)
self.map_s = np.sin(self.map_angle)
car_l = self.car_length
car_w = self.car_width
self.car_bbox = (
......
......@@ -2,9 +2,10 @@
"""ROS geometry and map utility functions."""
from __future__ import division
import heapq
import numpy as np
import rospy
from geometry_msgs.msg import (
Pose,
PoseStamped,
......@@ -150,7 +151,7 @@ def rotation_matrix(theta):
The equivalent 2x2 rotation matrix as a np.array
"""
c, s = np.cos(theta), np.sin(theta)
return np.matrix([[c, -s], [s, c]])
return np.array([[c, -s], [s, c]])
def particle_to_pose(particle):
......@@ -342,3 +343,27 @@ def estimation_error(estimates, references):
np.cos(reference_angles - estimate_angles),
)
return position_errors, np.abs(angular_errors)
class PriorityQueue(object):
def __init__(self):
self.min_heap = []
self.pop_counter = 0
def __len__(self):
return len(self.min_heap)
def push(self, elem):
heapq.heappush(self.min_heap, elem)
def peek(self):
if not self.min_heap:
raise IndexError("no elements to peek")
return self.min_heap[0][1]
def pop(self):
if not self.min_heap:
raise IndexError("no elements to pop")
elem = heapq.heappop(self.min_heap)
self.pop_counter += 1
return elem
cmake_minimum_required(VERSION 3.0.2)
project(planning)
find_package(catkin REQUIRED COMPONENTS
rospy
)
catkin_python_setup()
catkin_package()
install(
DIRECTORY
launch
DESTINATION
${CATKIN_PACKAGE_SHARE_DESTINATION}
)
catkin_install_python(
PROGRAMS
scripts/roadmap
scripts/run_search
scripts/planner
DESTINATION
${CATKIN_PACKAGE_BIN_DESTINATION}
)
if(CATKIN_ENABLE_TESTING)
find_package(roslaunch REQUIRED)
# find_package(rostest REQUIRED)
roslaunch_add_file_check(launch)
# catkin_add_nosetests tests Python code directly
catkin_add_nosetests(test/samplers.py)
catkin_add_nosetests(test/roadmap.py)
catkin_add_nosetests(test/astar.py)
catkin_add_nosetests(test/shortcut.py)
# add_rostest tests the ROS integrations
endif()
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Map1/Orientation1
Splitter Ratio: 0.5
Tree Height: 648
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: LaserScan
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 0; 0; 0
Enabled: true
Line Style:
Line Width: 0.029999999329447746
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.30000001192092896
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_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
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: false
Invert Rainbow: false
Max Color: 255; 22; 144
Min Color: 255; 22; 144
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.029999999329447746
Style: Points
Topic: /car/scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Arrow Length: 0.30000001192092896
Axes Length: 0.30000001192092896
Axes Radius: 0.009999999776482582
Class: rviz/PoseArray
Color: 255; 25; 0
Enabled: true
Head Length: 0.07000000029802322
Head Radius: 0.029999999329447746
Name: Real Path
Queue Size: 10
Shaft Length: 0.23000000417232513
Shaft Radius: 0.009999999776482582
Shape: Arrow (Flat)
Topic: /car/controller/real_path/poses
Unreliable: false
Value: true
- Alpha: 1
Arrow Length: 0.30000001192092896
Axes Length: 0.30000001192092896
Axes Radius: 0.009999999776482582
Class: rviz/PoseArray
Color: 138; 226; 52
Enabled: true
Head Length: 0.07000000029802322
Head Radius: 0.029999999329447746
Name: Reference Path
Queue Size: 10
Shaft Length: 0.23000000417232513
Shaft Radius: 0.009999999776482582
Shape: Arrow (Flat)
Topic: /car/controller/path/poses
Unreliable: false
Value: true
- Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Class: rviz/Pose
Color: 0; 0; 255
Enabled: true
Head Length: 0.20000000298023224
Head Radius: 0.10000000149011612
Name: Reference State
Queue Size: 10
Shaft Length: 0.20000000298023224
Shaft Radius: 0.02500000037252903
Shape: Arrow
Topic: /car/controller/path/reference_state
Unreliable: false
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /car/controller/rollouts
Name: Rollouts
Namespaces:
{}
Queue Size: 100
Value: true
- Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Class: rviz/Pose
Color: 255; 25; 0
Enabled: false
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Name: Pose
Queue Size: 10
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Shape: Arrow
Topic: /move_base_simple/goal
Unreliable: false
Value: false
- Alpha: 1
Arrow Length: 0.30000001192092896
Axes Length: 0.30000001192092896
Axes Radius: 0.009999999776482582
Class: rviz/PoseArray
Color: 0; 0; 0
Enabled: true
Head Length: 0.07000000029802322
Head Radius: 0.10000000149011612
Name: Planning Vertices
Queue Size: 10
Shaft Length: 0.10000000149011612
Shaft Radius: 0.019999999552965164
Shape: Arrow (3D)
Topic: /car/planner/vertices
Unreliable: false
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /car/planner/edges
Name: Planning Edges
Namespaces:
"": true
Queue Size: 100
Value: true
Enabled: true
Global Options:
Background Color: 255; 255; 255
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
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Angle: 0
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Scale: 20
Target Frame: <Fixed Frame>
X: 20
Y: 40
Saved:
- Angle: 0
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: TopDownOrtho
Near Clip Distance: 0.009999999776482582
Scale: 206.59109497070312
Target Frame: base_link
X: 0
Y: 0
- Angle: 0
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: TopDownOrtho
Near Clip Distance: 0.009999999776482582
Scale: 20
Target Frame: <Fixed Frame>
X: 20
Y: 40
Window Geometry:
Displays:
collapsed: false
Height: 945
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000018a000005d1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000005d1000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000005d1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000005d1000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005580000003efc0100000002fb0000000800540069006d0065010000000000000558000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002b3000005d100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1720
X: 72
Y: 27
<launch>
<arg name="car_name" default="car"/>
<arg name="map" default="$(find cse478)/maps/maze_0.yaml"/>
<arg name="initial_x" default="0"/>
<arg name="initial_y" default="0"/>
<arg name="initial_theta" default="0"/>
<arg name="fake_localization" default="false" />
<arg name="teleop" default="false"/>
<arg name="sampler" default="halton"/>
<arg name="problem" default="se2"/>
<arg name="num_vertices" default="200"/>
<arg name="connection_radius" default="100.0"/>
<arg name="curvature" default="0.05"/>
<include file="$(find cse478)/launch/teleop.launch">
<arg name="fake_localization" value="$(arg fake_localization)" />
<arg name="map" value="$(arg map)"/>
<arg name="initial_x" value="$(arg initial_x)"/>
<arg name="initial_y" value="$(arg initial_y)"/>
<arg name="initial_theta" value="$(arg initial_theta)"/>
<arg name="teleop" value="$(arg teleop)" />
</include>
<group ns="$(arg car_name)">
<node pkg="planning" type="roadmap" name="roadmap"
args="--use-ros-map --lazy --sampler $(arg sampler) --problem $(arg problem) --num-vertices $(arg num_vertices) --connection-radius $(arg connection_radius) --curvature $(arg curvature)" output="screen"/>
</group>
</launch>
<launch>
<arg name="car_name" default="car"/>
<arg name="map" default="$(find cse478)/maps/maze_0.yaml"/>
<arg name="initial_x" default="0"/>
<arg name="initial_y" default="0"/>
<arg name="initial_theta" default="0"/>
<arg name="fake_localization" default="false" />
<arg name="controller" default="mpc"/>
<arg name="teleop" default="true"/>
<arg name="rviz" default="true" />
<arg name="num_vertices" default="200"/>
<arg name="connection_radius" default="100.0"/>
<arg name="curvature" default="0.05"/>
<include file="$(find cse478)/launch/teleop.launch">
<arg name="fake_localization" value="$(arg fake_localization)" />
<arg name="map" value="$(arg map)"/>
<arg name="initial_x" value="$(arg initial_x)"/>
<arg name="initial_y" value="$(arg initial_y)"/>
<arg name="initial_theta" value="$(arg initial_theta)"/>
<arg name="teleop" value="$(arg teleop)" />
</include>
<group ns="$(arg car_name)">
<include file="$(find localization)/launch/include/particle_filter.xml">
<arg name="publish_tf" value="$(eval not fake_localization)" />
<arg name="initial_x" value="$(arg initial_x)"/>
<arg name="initial_y" value="$(arg initial_y)"/>
<arg name="initial_theta" value="$(arg initial_theta)"/>
</include>
<node pkg="planning" type="planner" name="planner" output="screen">
<param name="num_vertices" value="$(arg num_vertices)"/>
<param name="connection_radius" value="$(arg connection_radius)"/>
<param name="curvature" value="$(arg curvature)"/>
</node>
</group>
<include file="$(find control)/launch/controller.launch">
<arg name="type" default="$(arg controller)"/>
</include>
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find planning)/config/planning.rviz"/>
</launch>
<?xml version="1.0"?>
<package format="3">
<name>planning</name>
<version>1.0.0</version>
<description>CSE 478 MuSHR Planning Project</description>
<author>Brian Hou</author>
<author>Gilwoo Lee</author>
<author>Mohit Shridhar</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_depend>std_msgs</build_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>cse478</exec_depend>
<exec_depend>localization</exec_depend>
<exec_depend>control</exec_depend>
<exec_depend condition="$ROS_PYTHON_VERSION == 2">ipython</exec_depend>
<exec_depend condition="$ROS_PYTHON_VERSION == 3">ipython3</exec_depend>
<exec_depend condition="$ROS_PYTHON_VERSION == 2">python-matplotlib</exec_depend>
<exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-matplotlib</exec_depend>
<exec_depend condition="$ROS_PYTHON_VERSION == 2">python-networkx</exec_depend>
<exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-networkx</exec_depend>
<exec_depend condition="$ROS_PYTHON_VERSION == 2">python-numpy</exec_depend>
<exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-numpy</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>rviz</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>tf</exec_depend>
<exec_depend>visualization_msgs</exec_depend>
<export>
</export>
</package>
#!/usr/bin/env python
import rospy
from planning.planner_ros import PlannerROS
if __name__ == "__main__":
rospy.init_node("planner")
PlannerROS(
num_vertices=rospy.get_param("~num_vertices"),
connection_radius=float(rospy.get_param("~connection_radius")),
curvature=float(rospy.get_param("~curvature")),
tf_prefix=rospy.get_param("~tf_prefix", ""),
)
rospy.spin()
#!/usr/bin/env python
"""Script for generating and visualizing roadmaps."""