Commit 83338f3c authored by Brian Hou's avatar Brian Hou
Browse files

Release Project 3: Control.

parent 29e0bab8
Pipeline #417412 failed with stage
in 6 minutes and 22 seconds
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()
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
<?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>
pid:
kp: 0.5
kd: 0.5
pp:
distance_lookahead: 0.1
mpc:
K: 2
T: 1
\ No newline at end of file
<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>
<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>
<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
<?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>
#!/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()
#!/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 can run at the same time
rospy.init_node("controller_runner", [], anonymous=True)
parser = argparse.ArgumentParser()
parser.add_argument(
"path_name", type=str, choices=path_generators, help="Name of path to generate"
)
parser.add_argument(
"--speed", type=float, default=2, help="Max speed along the path"
)