Release Project 4: Planning.

control/CMakeLists.txt

@@ -28,6 +28,14 @@ catkin_package(
   std_msgs
 )
 
+install(
+  DIRECTORY
+  config
+  launch
+  DESTINATION
+  ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
 catkin_install_python(
   PROGRAMS
   scripts/control_node

control/src/control/controller.py

@@ -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)

control/src/control/mpc.py

@@ -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 = (

cse478/src/cse478/utils.py

@@ -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

planning/CMakeLists.txt

+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()

planning/config/planning.rviz

+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

planning/launch/make_roadmap.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="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>

planning/launch/planner_sim.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>

planning/package.xml

+<?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>

planning/scripts/planner

+#!/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()

planning/scripts/roadmap

+#!/usr/bin/env python
+
+"""Script for generating and visualizing roadmaps."""