Skip to content
Snippets Groups Projects
Commit 119f09c8 authored by Davis, Duane T's avatar Davis, Duane T
Browse files

Updated robot.py to use RLock to prevent concurrent odom access 

Also added static method to GeometryMap to generate circle world map
parent a3a670b6
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/cs4313_utilities/maps.py
+ 59
− 0
View file @ 119f09c8
......@@ -392,6 +392,65 @@ class GeometryMap(Map):
# Locally defined methods
#------------------------
@staticmethod
def circleWorldMapFactory():
''' CS4313-specific static method that creates and returns a map
of the cs4313_worlds/circles.world Gazebo environment
'''
north_wall = geometry.LineSegment((-28.0, -25.0), (35.0, 35.0))
east_wall = geometry.LineSegment((35.0, 35.0), (35.0, -35.0))
south_wall = geometry.LineSegment((-35.0, -35.0), (35.0, -35.0))
west_wall = geometry.LineSegment((-35.0, -35.0), (-35.0, 35.0))
c1 = geometry.Circle((-28.0, -25.0), 3.0)
c2 = geometry.Circle((-20.0, -25.0), 1.0)
c3 = geometry.Circle((-12.0, -30.0), 2.0)
c4 = geometry.Circle((-12.0, -15.0), 4.0)
c5 = geometry.Circle((-12.0, -5.0), 1.0)
c6 = geometry.Circle((-23.0, -15.0), 2.0)
c7 = geometry.Circle((-21.0, -2.0), 2.0)
c8 = geometry.Circle((-28.0, 6.0), 3.0)
c9 = geometry.Circle((-28.0, 15.0), 2.0)
c10 = geometry.Circle((-16.0, 15.0), 1.0)
c11 = geometry.Circle((-5.0, 8.0), 5.0)
c12 = geometry.Circle((-5.0, 21.0), 2.0)
c13 = geometry.Circle((-3.0, -6.0), 2.0)
c14 = geometry.Circle((0.0, -25.0), 5.0)
c15 = geometry.Circle((10.0, -25.0), 2.0)
c16 = geometry.Circle((6.0, -10.0), 3.0)
c17 = geometry.Circle((5.0, 8.0), 1.0)
c18 = geometry.Circle((8.0, 24.0), 4.0)
c19 = geometry.Circle((15.0, 15.0), 2.0)
c20 = geometry.Circle((15.0, 2.0), 3.0)
c21 = geometry.Circle((20.0, -9.0), 2.0)
c22 = geometry.Circle((20.0, -18.0), 1.0)
c23 = geometry.Circle((13.0, -18.0), 2.0)
c24 = geometry.Circle((25.0, -27.0), 3.0)
c25 = geometry.Circle((28.0, -15.0), 2.0)
c26 = geometry.Circle((28.0, 0.0), 3.0)
c27 = geometry.Circle((26.0, 12.0), 1.0)
c28 = geometry.Circle((26.0, 25.0), 3.0)
w12 = geometry.LineSegment((-25.0, -25.0), (-21.0, -25.0))
w45 = geometry.LineSegment((-12.0, -6.0), (-12.0, -11.0))
w46 = geometry.LineSegment((-16.0, -15.0), (-21.0, -15.0))
w89 = geometry.LineSegment((-28.0, 9.0), (-28.0, 13.0))
w910 = geometry.LineSegment((-26.0, 15.0), (-17.0, 15.0))
w1112 = geometry.LineSegment((-5.0, 13.0), (-5.0, 19.0))
w1415 = geometry.LineSegment((5.0, -25.0), (8.0, -25.0))
w1117 = geometry.LineSegment((0.0, 8.0), (4.0, 8.0))
w1920 = geometry.LineSegment((11.0, 9.0), (19.0, 9.0))
w2122 = geometry.LineSegment((20.0, -11.0), (20.0, -17.0))
w2223 = geometry.LineSegment((15.0, -18.0), (19.0, -18.0))
w2526 = geometry.LineSegment((28.0, -3.0), (28.0, -13.0))
w2728 = geometry.LineSegment((26.0, 22.0), (26.0, 13.0))
geometries = ( north_wall, east_wall, south_wall, west_wall, \
c1, c2, c3, c4, c5, c6, c7, c8, c9, c10, \
c11, c12, c13, c14, c15, c16, c17, c18, c19, \
c20, c21, c22, c23, c24, c25, c26, c27, c28, \
w12, w45, w46, w89, w910, w1112, w1415, \
w1117, w1920, w2122, w2223, w2526, w2728)
return GeometryMap((-35.01, -35.01), (35.01, 35.01), geometries)
@staticmethod
def arenaMapFactory():
''' CS4313-specific static method that creates and returns a map
......
src/cs4313_utilities/robot.py
+ 47
− 43
View file @ 119f09c8
......@@ -2,6 +2,7 @@
# Standard python imports
import math
import threading
# ROS imports
import rospy
......@@ -38,6 +39,7 @@ class SimpleRobot(object):
baseToOdomTF: ROS broadcaster to publish odometry transforms
mapToOdomTF: ROS listener for map transforms
timer: ROS timer for control loop iteration
odomLock: lock object to prevent concurrent access to the odometry
Class methods:
spinOnce: runs a single iteration of the vehicle's control loop
......@@ -94,7 +96,7 @@ class SimpleRobot(object):
rospy.Subscriber("ground_truth", nav_msgs.Odometry, self.processGroundTruth, queue_size=1)
self.timer = rospy.Rate(10)
self.cmdPub = rospy.Publisher(cmdTopic, geometry_msgs.Twist, queue_size=1)
self.odomLock = threading.RLock()
def setControlConstants(self, newCaptureDist=1.5, newMaxXDot=1.0, \
newMaxThetaDot=0.77, newXCmdC1=2.0, newXCmdC2=1.0, \
......@@ -129,16 +131,17 @@ class SimpleRobot(object):
from the simulation (if using ground truth)
@param groundTruthData: nav_msgs/Odometry object containing ground truth data
'''
(_, _, yaw) = quat.quat_to_euler( (groundTruthData.pose.pose.orientation.x, \
groundTruthData.pose.pose.orientation.y, \
groundTruthData.pose.pose.orientation.z, \
groundTruthData.pose.pose.orientation.w) )
self.state.pose[0] = groundTruthData.pose.pose.position.x
self.state.pose[1] = groundTruthData.pose.pose.position.y
self.state.pose[2] = yaw
self.state.velocity[0] = groundTruthData.twist.twist.linear.x
self.state.velocity[1] = groundTruthData.twist.twist.linear.y
self.state.velocity[2] = groundTruthData.twist.twist.angular.z
with self.odomLock:
(_, _, yaw) = quat.quat_to_euler( (groundTruthData.pose.pose.orientation.x, \
groundTruthData.pose.pose.orientation.y, \
groundTruthData.pose.pose.orientation.z, \
groundTruthData.pose.pose.orientation.w) )
self.state.pose[0] = groundTruthData.pose.pose.position.x
self.state.pose[1] = groundTruthData.pose.pose.position.y
self.state.pose[2] = yaw
self.state.velocity[0] = groundTruthData.twist.twist.linear.x
self.state.velocity[1] = groundTruthData.twist.twist.linear.y
self.state.velocity[2] = groundTruthData.twist.twist.angular.z
def processOdometry(self, odomData):
......@@ -146,39 +149,40 @@ class SimpleRobot(object):
(does not make any control calculations)
@param odomData: nav_msgs/Odometry object containing odometry data
'''
(_, _, yaw) = quat.quat_to_euler( (odomData.pose.pose.orientation.x, \
odomData.pose.pose.orientation.y, \
odomData.pose.pose.orientation.z, \
odomData.pose.pose.orientation.w) )
self.state.odom[0] = odomData.pose.pose.position.x
self.state.odom[1] = odomData.pose.pose.position.y
self.state.odom[2] = yaw
self.baseToOdomTF.sendTransform((self.state.odom[0], self.state.odom[1], 0.0),
tf.transformations.quaternion_from_euler(0.0, 0.0, self.state.odom[2]),
rospy.Time.now(), "base_link", "odom")
if not src.USE_GROUND_TRUTH:
try:
(trans, rot) = self.mapToOdomTF.lookupTransform('/map', '/odom', rospy.Time(0))
(_, _, delta_rot) = quat.quat_to_euler( (rot[0], rot[1], rot[2], rot[3]) )
self.state.pose[0] = self.state.odom[0] + trans[0]
self.state.pose[1] = self.state.odom[1] + trans[1]
self.state.pose[2] = rm.normalize(self.state.odom[2] + delta_rot)
course = math.atan2(odomData.twist.twist.linear.y, \
odomData.twist.twist.linear.x) + delta_rot
speed = math.hypot(odomData.twist.twist.linear.y, \
odomData.twist.twist.linear.x)
self.state.velocity[0] = speed * math.cos(course)
self.state.velocity[1] = speed * math.sin(course)
self.state.velocity[2] = odomData.twist.twist.angular.z
with self.odomLock:
(_, _, yaw) = quat.quat_to_euler( (odomData.pose.pose.orientation.x, \
odomData.pose.pose.orientation.y, \
odomData.pose.pose.orientation.z, \
odomData.pose.pose.orientation.w) )
self.state.odom[0] = odomData.pose.pose.position.x
self.state.odom[1] = odomData.pose.pose.position.y
self.state.odom[2] = yaw
self.baseToOdomTF.sendTransform((self.state.odom[0], self.state.odom[1], 0.0),
tf.transformations.quaternion_from_euler(0.0, 0.0, self.state.odom[2]),
rospy.Time.now(), "base_link", "odom")
if not src.USE_GROUND_TRUTH:
try:
(trans, rot) = self.mapToOdomTF.lookupTransform('/map', '/odom', rospy.Time(0))
(_, _, delta_rot) = quat.quat_to_euler( (rot[0], rot[1], rot[2], rot[3]) )
self.state.pose[0] = self.state.odom[0] + trans[0]
self.state.pose[1] = self.state.odom[1] + trans[1]
self.state.pose[2] = rm.normalize(self.state.odom[2] + delta_rot)
course = math.atan2(odomData.twist.twist.linear.y, \
odomData.twist.twist.linear.x) + delta_rot
speed = math.hypot(odomData.twist.twist.linear.y, \
odomData.twist.twist.linear.x)
self.state.velocity[0] = speed * math.cos(course)
self.state.velocity[1] = speed * math.sin(course)
self.state.velocity[2] = odomData.twist.twist.angular.z
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
self.state.pose[0] = self.state.odom[0]
self.state.pose[1] = self.state.odom[1]
self.state.pose[2] = self.state.odom[2]
self.state.velocity[0] = odomData.twist.twist.linear.x
self.state.velocity[1] = odomData.twist.twist.linear.y
self.state.velocity[2] = odomData.twist.twist.angular.z
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
self.state.pose[0] = self.state.odom[0]
self.state.pose[1] = self.state.odom[1]
self.state.pose[2] = self.state.odom[2]
self.state.velocity[0] = odomData.twist.twist.linear.x
self.state.velocity[1] = odomData.twist.twist.linear.y
self.state.velocity[2] = odomData.twist.twist.angular.z
def pdWaypointControl(self):
......
src/cs4313_utilities/robot_math.py
+ 7
− 7
View file @ 119f09c8
......@@ -131,7 +131,7 @@ def normalize_pi(angle):
# Sensor and motion model functions
def odometryMotionModel(state, odom1, odom2, \
alpha1=0.2, alpha2=0.1, alpha3=0.1, alpha4=0.2):
alpha1=0.1, alpha2=0.02, alpha3=0.1, alpha4=0.1):
''' Implements the CS4313 odometry-based motion model (lecture 3.2)
@param state: hypothetical state of the form ( x, y, theta )
@param odom1: odometry value at time t-1
......@@ -149,12 +149,12 @@ def odometryMotionModel(state, odom1, odom2, \
d_rot2 = normalize_pi(odom2[2] - odom1[2] - d_rot1)
# Add noise to compute "true" motion
d_trans_hat = d_trans + random.gauss(0.0, alpha3 * abs(d_trans) + \
alpha4 * abs(d_rot1 + d_rot2))
d_rot1_hat = d_rot1 + random.gauss(0.0, alpha1 * abs(d_rot1) + \
alpha2 * abs(d_trans))
d_rot2_hat = d_rot2 + random.gauss(0.0, alpha1 * abs(d_rot2) + \
alpha2 * abs(d_trans))
t_noise_sigma = alpha3 * abs(d_trans) + alpha4 * abs(d_rot1 + d_rot2)
r1_noise_sigma = alpha1 * abs(d_rot1) + alpha2 * abs(d_trans)
r2_noise_sigma = alpha1 * abs(d_rot2) + alpha2 * abs(d_trans)
d_trans_hat = d_trans + random.gauss(0.0, t_noise_sigma)
d_rot1_hat = d_rot1 + random.gauss(0.0, r1_noise_sigma)
d_rot2_hat = d_rot2 + random.gauss(0.0, r2_noise_sigma)
# Apply the computed motion to the original state
x_t = state[0] + d_trans_hat * math.cos(odom1[2] + d_rot1_hat)
......
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