Release Project 3: Control.

control/CMakeLists.txt

+cmake_minimum_required(VERSION 3.0.2)
+project(control)
+
+find_package(catkin REQUIRED COMPONENTS
+  message_generation
+  nav_msgs
+  rospy
+  std_msgs
+)
+
+add_service_files(
+  FILES
+  FollowPath.srv
+)
+
+catkin_python_setup()
+
+generate_messages(
+  DEPENDENCIES
+  nav_msgs
+  std_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+  message_runtime
+  nav_msgs
+  std_msgs
+)
+
+catkin_install_python(
+  PROGRAMS
+  scripts/control_node
+  scripts/path_sender
+  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/controller.py)
+  catkin_add_nosetests(test/pid.py)
+  catkin_add_nosetests(test/purepursuit.py)
+  catkin_add_nosetests(test/mpc.py)
+
+  # add_rostest tests the ROS integrations
+
+  add_rostest(test/controller_performance.test ARGS type:=pid)
+  add_rostest(test/controller_performance.test ARGS type:=pp)
+  add_rostest(test/controller_performance.test ARGS type:=mpc)
+
+endif()

control/config/control.rviz

+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Map1/Orientation1
+      Splitter Ratio: 0.5
+    Tree Height: 601
+  - 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: ""
+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
+  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: 146.04165649414062
+      Target Frame: <Fixed Frame>
+      X: 2.478355884552002
+      Y: 0.4150882661342621
+    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
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 898
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001d5000002e4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002e4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002e4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005580000003efc0100000002fb0000000800540069006d0065010000000000000558000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000268000002e400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1368
+  X: 72
+  Y: 0

control/config/control_juggler.xml

+<?xml version='1.0' encoding='UTF-8'?>
+<root version="2.3.8">
+ <tabbed_widget parent="main_window" name="Main Window">
+  <Tab tab_name="Controller Error" containers="1">
+   <Container>
+    <DockSplitter orientation="-" sizes="0.50;0.50" count="2">
+     <DockSplitter orientation="|" sizes="0.50;0.50" count="2">
+      <DockSplitter orientation="-" sizes="0.50;0.50" count="2">
+       <DockArea name="...">
+        <plot style="Lines" mode="TimeSeries">
+         <range right="74.963884" bottom="-2.527011" top="2.354175" left="64.813944"/>
+         <limitY/>
+         <curve name="/car/car_pose/position/x" color="#790079"/>
+         <curve name="/car/car_pose/position/y" color="#008282"/>
+         <curve name="/car/controller/path/reference_state/position/x" color="#ff00ff"/>
+         <curve name="/car/controller/path/reference_state/position/y" color="#00ffff"/>
+        </plot>
+       </DockArea>
+       <DockArea name="...">
+        <plot style="Lines" mode="TimeSeries">
+         <range right="74.963884" bottom="17.394411" top="17.394429" left="64.813944"/>
+         <limitY/>
+         <curve name="/car/car_pose/orientation/yaw_deg" color="#ff0000">
+          <transform name="Scale/Offset" alias="/car/car_pose/orientation/yaw_deg[Scale/Offset]">
+           <options value_scale="0.017453" time_offset="0" value_offset="0"/>
+          </transform>
+         </curve>
+         <curve name="/car/controller/path/reference_state/orientation/yaw_deg" color="#00ff00"/>
+        </plot>
+       </DockArea>
+      </DockSplitter>
+      <DockArea name="...">
+       <plot style="Lines" mode="XYPlot">
+        <range right="-2.410739" bottom="2.237857" top="2.237958" left="-2.410831"/>
+        <limitY/>
+        <curve name="Real Car Path" curve_y="/car/car_pose/position/y" curve_x="/car/car_pose/position/x" color="#ff0000"/>
+        <curve name="Reference State Path" curve_y="/car/controller/path/reference_state/position/y" curve_x="/car/controller/path/reference_state/position/x" color="#00ff00"/>
+       </plot>
+      </DockArea>
+     </DockSplitter>
+     <DockSplitter orientation="|" sizes="0.500489;0.499511" count="2">
+      <DockArea name="...">
+       <plot style="Lines" mode="TimeSeries">
+        <range right="0.000000" bottom="-0.500000" top="0.500000" left="0.000000"/>
+        <limitY min="-0.5" max="0.5"/>
+        <curve name="/car/controller/error/data" color="#0000ff"/>
+       </plot>
+      </DockArea>
+      <DockArea name="...">
+       <plot style="Lines" mode="TimeSeries">
+        <range right="0.000000" bottom="-0.100000" top="0.100000" left="0.000000"/>
+        <limitY/>
+        <curve name="/car/mux/ackermann_cmd_mux/input/navigation/drive/speed" color="#9467bd"/>
+        <curve name="/car/mux/ackermann_cmd_mux/input/navigation/drive/steering_angle" color="#17becf"/>
+       </plot>
+      </DockArea>
+     </DockSplitter>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <currentTabIndex index="0"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+  <plugin ID="DataLoad CSV">
+   <default time_axis=""/>
+  </plugin>
+  <plugin ID="DataLoad ROS bags">
+   <use_header_stamp value="false"/>
+   <use_renaming_rules value="true"/>
+   <discard_large_arrays value="true"/>
+   <max_array_size value="100"/>
+  </plugin>
+  <plugin ID="DataLoad ULog"/>
+  <plugin ID="MQTT Subscriber"/>
+  <plugin ID="ROS Topic Subscriber">
+   <use_header_stamp value="false"/>
+   <use_renaming_rules value="true"/>
+   <discard_large_arrays value="true"/>
+   <max_array_size value="100"/>
+  </plugin>
+  <plugin ID="UDP Server"/>
+  <plugin ID="WebSocket Server"/>
+  <plugin ID="ZMQ Subscriber"/>
+  <plugin ID="ROS /rosout Visualization" status="idle"/>
+  <plugin ID="ROS Topic Re-Publisher" status="idle"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <previouslyLoaded_Datafiles/>
+ <previouslyLoaded_Streamer name="ROS Topic Subscriber"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <customMathEquations/>
+ <snippets/>
+ <!-- - - - - - - - - - - - - - - -->
+</root>
+

control/config/parameters.yaml

+pid:
+  kp: 0.5
+  kd: 0.5
+pp:
+  distance_lookahead: 0.1
+mpc:
+  K: 2
+  T: 1
\ No newline at end of file

control/launch/controller.launch

+<launch>
+    <arg name="type" default=""/>
+    <group ns="car">
+    <node pkg="control" type="control_node" name="controller" output="screen">
+        <param name="type" value="$(arg type)" />
+        <rosparam command="load" file="$(find control)/config/parameters.yaml" />
+        <!-- Grab motion model params -->
+        <rosparam command="load" file="$(find localization)/config/parameters.yaml" />
+    </node>
+    </group>
+</launch>

control/launch/controller_sim.launch

+<launch>
+    <arg name="car_name" default="car"/>
+    <arg name="use_namespace" default="true" />
+    <arg name="map" default="$(find mushr_sim)/maps/sandbox.yaml"/>
+    <arg name="initial_x" default="0"/>
+    <arg name="initial_y" default="0"/>
+    <arg name="initial_theta" default="0"/>
+    <arg name="type" default="YOU_MUST_SPECIFY_A_CONTROLLER_TYPE"/>
+    <arg name="fake_localization" default="true" />
+    <arg name="rviz" default="true" />
+    <arg name="teleop" default="false" />
+
+    <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>
+
+    <include unless="$(arg fake_localization)" file="$(find localization)/launch/particle_filter_sim.launch">
+        <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="publish_tf" default="true" />
+    </include>
+
+    <include file="$(find control)/launch/controller.launch">
+        <arg name="type" value="$(arg type)" />
+    </include>
+
+    <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find control)/config/control.rviz"/>
+</launch>

control/launch/plotjuggler.launch

+<launch>
+    <node pkg="plotjuggler" type="plotjuggler" name="plot_juggler" args="-l $(find control)/config/control_juggler.xml" />
+    <node pkg="control" type="path_sender" name="$(anon path_sender)" args="line --length 10" />
+</launch>
\ No newline at end of file

control/package.xml

+<?xml version="1.0"?>
+<package format="3">
+  <name>control</name>
+  <version>1.0.0</version>
+  <description>CSE 478 MuSHR Control Project</description>
+
+  <author>Zoey Chen</author>
+  <author>Brian Hou</author>
+  <author>Kay Ke</author>
+  <author>Gilwoo Lee</author>
+  <author>Johan Michalove</author>
+  <author>Matthew Rockett</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>
+  <test_depend>rostest</test_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_export_depend>nav_msgs</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>ackermann_msgs</exec_depend>
+  <exec_depend>cse478</exec_depend>
+  <exec_depend>localization</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>mushr_sim</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>plotjuggler</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-numpy</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-numpy</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>rqt_gui</exec_depend>
+  <exec_depend>rviz</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>tf2_geometry_msgs</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>topic_tools</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+
+  <export>
+
+  </export>
+</package>

control/scripts/control_node

+#!/usr/bin/env python
+import signal
+
+import rospy
+from control.control_ros import (
+    ControlROS,
+    get_ros_params,
+    controllers,
+)
+
+
+def interrupt_signal(x, y):
+    node.shutdown()
+    exit(0)
+
+
+if __name__ == "__main__":
+    rospy.init_node("controller", disable_signals=True)
+    controller_type, params = get_ros_params()
+    rospy.loginfo("Using {} controller".format(controller_type))
+
+    controller = controllers[controller_type](**params)
+
+    node = ControlROS(controller)
+    signal.signal(signal.SIGINT, interrupt_signal)
+    node.start()
+    rospy.spin()

control/scripts/path_sender

+#!/usr/bin/env python
+
+import argparse
+import rospy
+
+from geometry_msgs.msg import PoseStamped
+from mushr_sim.srv import CarPose
+from nav_msgs.msg import Path
+from std_msgs.msg import Header
+
+from cse478 import utils
+from control.path_generator import line, wave, circle, left_turn, right_turn, saw
+from control.srv import FollowPath
+
+path_generators = {
+    "line": line,
+    "circle": circle,
+    "left-turn": left_turn,
+    "right-turn": right_turn,
+    "wave": wave,
+    "saw": saw,
+}
+
+
+if __name__ == "__main__":
+    # anonymous so multiple ca