Commit e4e882c7 authored by Brian Hou's avatar Brian Hou
Browse files

Update code for Python 2 compatibility.

This should make it easier to run code on the physical MuSHR platform.
parent c6d4c827
<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>
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):
......
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():
......
from __future__ import division
import numpy as np
from cse478 import utils
......
"""Simple parametric paths."""
from __future__ import division
import numpy as np
......
from __future__ import division
import numpy as np
from control.controller import BaseController
......
from __future__ import division
import numpy as np
from control.controller import BaseController
......
#!/usr/bin/env python
from __future__ import division
import numpy as np
import rosunit
import unittest
......
#!/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)
......
<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" />
......
#!/usr/bin/env python
from __future__ import division
import numpy as np
import rosunit
import unittest
......
#!/usr/bin/env python
from __future__ import division
import numpy as np
import rosunit
import unittest
......
#!/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)
<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)" />
......
......@@ -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
......
File mode changed from 100644 to 100755
<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"/>
......
......@@ -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)
......
......@@ -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(
......
<!-- 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>
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment