Update code for Python 2 compatibility.

This should make it easier to run code on the physical MuSHR platform.

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
+</launch>

control/src/control/control_ros.py

+from __future__ import division
 import threading
 
 import matplotlib.cm as cm
@@ -69,10 +70,11 @@ class ControlROS:
             if self.controller.path is None:
                 self._real_poses = []
                 with self.controller.path_condition:
-                    self.controller.path_condition.wait_for(
-                        lambda: self.controller.path is not None
-                        or self.controller.shutdown_event.is_set()
-                    )
+                    while (
+                        self.controller.path is None
+                        and not self.controller.shutdown_event.is_set()
+                    ):
+                        self.controller.path_condition.wait()
                 if self.controller.shutdown_event.is_set():
                     break
             latest_pose = self._get_car_pose()
@@ -356,6 +358,8 @@ def rollouts_to_markers_cmap(poses, costs, ns="paths", cmap="cividis", scale=0.0
     max_c = np.max(costs)
     min_c = np.min(costs)
     norm = colors.Normalize(vmin=min_c, vmax=max_c)
+    if cmap not in cm.cmaps_listed.keys():
+        cmap = "viridis"
     cmap = cm.get_cmap(name=cmap)
 
     def colorfn(cost):

control/src/control/controller.py

+from __future__ import division
 import numpy as np
 import threading
 import time
@@ -19,7 +20,7 @@ def compute_position_in_frame(p, frame):
     # END QUESTION 1.2
 
 
-class BaseController:
+class BaseController(object):
     def __init__(self, **kwargs):
         self.__properties = {
             "frequency",
@@ -158,9 +159,8 @@ class BaseController:
             # on it for the with context
             with self.path_condition:
                 # We hold the lock. Give it up until a path is set
-                self.path_condition.wait_for(
-                    lambda: self.path is not None or self.shutdown_event.is_set()
-                )
+                while self.path is None and not self.shutdown_event.is_set():
+                    self.path_condition.wait()
                 # We have a path, and we're guaranteed exclusive access now
                 # Check to see if we got killed while waited for a path
                 if self.shutdown_event.is_set():

control/src/control/mpc.py

+from __future__ import division
 import numpy as np
 
 from cse478 import utils

control/src/control/path_generator.py

 """Simple parametric paths."""
-
+from __future__ import division
 import numpy as np
 
 

control/src/control/pid.py

+from __future__ import division
 import numpy as np
 
 from control.controller import BaseController

control/src/control/purepursuit.py

+from __future__ import division
 import numpy as np
 
 from control.controller import BaseController

control/test/controller.py

 #!/usr/bin/env python
-
+from __future__ import division
 import numpy as np
 import rosunit
 import unittest

control/test/controller_performance.py

 #!/usr/bin/env python
-
+from __future__ import division
 import numpy as np
 import rospy
 import rostest
@@ -42,10 +42,10 @@ class TestControllerPerformance(unittest.TestCase):
         end_time = time.time() + duration
         car_trace = []
         while time.time() < end_time and self.controller.controller.path is not None:
-            heard_loop_event = self.controller.controller.looped_event.wait(timeout=1.0)
-            self.assertTrue(
-                heard_loop_event, msg="No control was received for one second"
-            )
+            start_stamp = time.time()
+            self.controller.controller.looped_event.wait(timeout=1.0)
+            elapsed = time.time() - start_stamp
+            self.assertLess(elapsed, 1.0, msg="No control was received for one second")
             car_trace.append(self.controller._get_car_pose())
         car_trace = np.array(car_trace)
         finished, errored = self.controller.wait_for_finish(timeout=20.0)

control/test/controller_performance.test

 <launch>
     <arg name="type" />
-    <arg name="plot" default="false"/>
     <arg name="rviz" default="false" />
 
     <include file="$(find cse478)/launch/teleop.launch">
@@ -10,7 +9,6 @@
 
     <test pkg="control" type="controller_performance.py" test-name="controller" ns="car">
         <param name="type" value="$(arg type)" />
-        <param name="plot" value="$(arg plot)"/>
         <rosparam command="load" file="$(find control)/config/parameters.yaml" />
         <!-- Grab motion model params -->
         <rosparam command="load" file="$(find localization)/config/parameters.yaml" />

control/test/pid.py

 #!/usr/bin/env python
-
+from __future__ import division
 import numpy as np
 import rosunit
 import unittest

control/test/purepursuit.py

 #!/usr/bin/env python
-
+from __future__ import division
 import numpy as np
 import rosunit
 import unittest

cse478/src/cse478/utils.py

 #!/usr/bin/env python
 
 """ROS geometry and map utility functions."""
-
-from threading import Timer
-
+from __future__ import division
 import numpy as np
 import rospy
 
@@ -344,9 +342,3 @@ def estimation_error(estimates, references):
         np.cos(reference_angles - estimate_angles),
     )
     return position_errors, np.abs(angular_errors)
-
-
-class RepeatTimer(Timer):
-    def run(self):
-        while not self.finished.wait(self.interval):
-            self.function(*self.args, **self.kwargs)

introduction/launch/path_publisher.launch

 <launch>
-    <arg name="control_topic" default="/car/mux/ackermann_cmd_mux/input/navigation" />
-    <arg name="init_pose_topic" default="/initialpose" />
     <arg name="plan_file" default="$(find introduction)/plans/straight_line.txt" />
     <arg name="buffer_size" default="500" />
 
     <node pkg="introduction" type="path_publisher" name="path_publisher" output="screen">
-        <param name="control_topic" value="$(arg control_topic)" />
-        <param name="init_pose_topic" value="$(arg init_pose_topic)" />
         <param name="plan_file" value="$(arg plan_file)" />
+        <remap from="control" to="/car/mux/ackermann_cmd_mux/input/navigation" />
     </node>
     <node pkg="introduction" type="pose_listener" name="pose_listener" output="screen">
         <param name="buffer_size" value="$(arg buffer_size)" />

introduction/scripts/path_publisher

@@ -60,16 +60,12 @@ class PathPublisher:
     def _setup_publishers(self):
         """Set up publishers: one for the initial pose, one for each command."""
 
-        init_pose_topic = rospy.get_param("~init_pose_topic", "/initialpose")
         self.init_pose_publisher = rospy.Publisher(
-            init_pose_topic, PoseWithCovarianceStamped, queue_size=1
+            "initialpose", PoseWithCovarianceStamped, queue_size=1
         )
 
-        control_topic = rospy.get_param(
-            "~control_topic", "/car/mux/ackermann_cmd_mux/input/navigation"
-        )
         self.control_publisher = rospy.Publisher(
-            control_topic, AckermannDriveStamped, queue_size=1
+            "control", AckermannDriveStamped, queue_size=1
         )
 
         # Publishers sometimes need time to warm up. You can also wait until there

introduction/test/path_publisher.test

 <launch>
   <node pkg="introduction" type="path_publisher" name="path_publisher">
-    <param name="control_topic" value="mocked_control" />
-    <param name="init_pose_topic" value="mocked_init_pose" />
     <param name="plan_file" value="$(find introduction)/plans/straight_line.txt" />
   </node>
   <test test-name="test_publisher" pkg="introduction" type="test_path_publisher.py"/>

introduction/test/test_norms.py

@@ -19,7 +19,8 @@ def gen_prim_pyth_trips(limit=None):
         m = m.reshape(-1, 3)
         if limit:
             m = m[m[:, 2] <= limit]
-        yield from m
+        for item in m:
+            yield item
         m = np.dot(m, uad)
 
 

introduction/test/test_path_publisher.py

@@ -15,11 +15,11 @@ from cse478.utils import pose_stamped_to_pq
 class TestPublisher(unittest.TestCase):
     def test_path_publisher(self):
         rospy.init_node("test_introduction_publisher")
-        control_collector = MessageCollector("/mocked_control", AckermannDriveStamped)
+        control_collector = MessageCollector("/control", AckermannDriveStamped)
         control_collector.start()
 
         car_pose_cov = rospy.wait_for_message(
-            "/mocked_init_pose", PoseWithCovarianceStamped, timeout=3
+            "/initialpose", PoseWithCovarianceStamped, timeout=3
         )
         self.assertIsNotNone(car_pose_cov)
         np.testing.assert_equal(

localization/launch/include/particle_filter.xml

 <!-- Shared launch file component for configuring particle_filter_node. -->
 <launch>
     <arg name="publish_tf" default="true" />
-    <arg name="n_particles" default="1000"/>
+    <arg name="n_particles" default="10000"/>
     <arg name="n_viz_particles" default="60" />
-    <arg name="odometry_topic" default="vesc/odom" />
-    <arg name="motor_state_topic" default="vesc/sensors/core" />
-    <arg name="servo_state_topic" default="vesc/sensors/servo_position_command" />
     <arg name="scan_topic" default="scan"/>
     <arg name="laser_ray_step" default="18"/>
     <arg name="exclude_max_range_rays" default="true"/>
     <arg name="max_range_meters" default="11.0" />
-    <arg name="initial_x" default="0"/>
-    <arg name="initial_y" default="0"/>
-    <arg name="initial_theta" default="0"/>
+    <arg name="initial_x" default="NaN"/>
+    <arg name="initial_y" default="NaN"/>
+    <arg name="initial_theta" default="NaN"/>
 
     <node pkg="localization" type="particle_filter_node" name="particle_filter" output="screen">
         <param name="publish_tf" value="$(arg publish_tf)" />
         <param name="n_particles" value="$(arg n_particles)"/>
         <param name="n_viz_particles" value="$(arg n_viz_particles)"/>
-        <param name="odometry_topic" value="$(arg odometry_topic)"/>
-        <param name="motor_state_topic" value="$(arg motor_state_topic)" />
-        <param name="servo_state_topic" value="$(arg servo_state_topic)" />
         <param name="scan_topic" value="$(arg scan_topic)"/>
         <param name="laser_ray_step" value="$(arg laser_ray_step)"/>
         <param name="exclude_max_range_rays" value="$(arg exclude_max_range_rays)" />
@@ -28,6 +22,8 @@
         <param name="initial_x" value="$(arg initial_x)"/>
         <param name="initial_y" value="$(arg initial_y)"/>
         <param name="initial_theta" value="$(arg initial_theta)"/>
+        <remap from="servo_state" to="vesc/sensors/servo_position_command" />
+        <rosparam command="load" file="$(find localization)/config/parameters.yaml" />
     </node>
 </launch>