def updatePath():
global x, y, theta, env, outImage, started, reprojMatrix, pp, transformMatrix, initFinished, goalPoint, goalChanged, following, newPath
realTimeObs = True
replanOnError = True
while True:
if initFinished and frame is not None:
obstacles = getObstaclePositions(frame, transformMatrix)
if realTimeObs or not started:
env.setObstacles(np.squeeze(obstacles))
if not started:
started = True
if (realTimeObs and env.newObstacles()) or (
replanOnError and pp.highError) or goalChanged:
goalChanged = False
newPath = True
try:
env.generatePath(goalPoint)
if env.splinePoints is None or len(env.splinePoints) == 0:
pp = PurePursuit([(x, y)], LOOKAHEAD, 10000.0)
else:
pp = PurePursuit(env.splinePoints, LOOKAHEAD, 0.5)
following = True
started = True
except:
pass
time.sleep(0.1)
def test_calculate_velocity(
self): # only functions with 'test_'-prefix will be run!
# Set up path
pp = PurePursuit()
pp.goal_margin = 0.1
# Set up test inputs
goals = [
np.array([1., 0.]),
np.array([1., 1.]),
np.array([-0.5, -0.5]),
np.array([0., -0.2]),
np.array([0., 0.05])
]
# Desired outputs
v_true = [0.22, 0.203703703704, -0.189655172414, 0.122222222222, 0.]
w_true = [0., 0.203703703704, 0.379310344828, -1.22222222222, 0.]
# Ensure that calculated outputs match desired outputs
err_msg = "test goal ({},{}) has the velocity ({}, {}) instead of ({}, {})"
for i in range(0, len(goals)):
(v, w) = pp.calculate_velocity(goals[i])
self.assertTrue(
np.abs(v - v_true[i]) < 1e-6 and np.abs(w - w_true[i]) < 1e-6,
err_msg.format(goals[i][0], goals[i][1], v, w, v_true[i],
w_true[i]))
def __init__(self):
rospy.loginfo("Initializing %s" % rospy.get_name())
self.pursuit = PurePursuit()
self.pursuit.set_look_ahead_distance(0.2)
self.target_global = None
self.listener = tf.TransformListener()
self.pub_goal_marker = rospy.Publisher(
"goal_marker", Marker, queue_size=1)
self.pub_target_marker = rospy.Publisher(
"target_marker", Marker, queue_size=1)
self.pub_pid_goal = rospy.Publisher(
"pid_control/goal", PoseStamped, queue_size=1)
self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan)
self.sub_box = rospy.Subscriber(
"artifact_pose", PoseStamped, self.cb_pose, queue_size=1)
self.sub_goal = rospy.Subscriber(
"/move_base_simple/goal", PoseStamped, self.cb_goal, queue_size=1)
self.timer = rospy.Timer(rospy.Duration(0.2), self.tracking)
def move_straight(self, start, end, reverse):
dist_thresh = 0.03
dist = np.sqrt((start[1] - end[1])**2 + (start[0] - end[0])**2)
k = 2
# timeout = 2 * dist / self.lin_vel_max
timeout = 10
pp = PurePursuit(start, end)
start_time = time.time()
timer = time.time()
while (abs(dist) > dist_thresh) and (time.time() - start_time <
timeout):
if (time.time() - timer) > self.dt:
timer = time.time()
dist = np.sqrt((self.state[1] - end[1])**2 +
(self.state[0] - end[0])**2)
v = min(self.lin_vel_max, dist * k)
if reverse:
v = v * -1
w = pp.get_ang_vel(self.state, v)
self.send_vels(v, w)
self.send_vels(0, 0)
def generate_pure_pursuit_path():
global pp
pp = PurePursuit()
for i in range(len(instructions)):
# add x,y coords from each point in the generated trajectory as waypoints.
# this is better than just adding the 5 nodes as waypoints.
pp.add_point(instructions[i][1], instructions[i][2])
interpolate_path(i) #TODO comment out when not debugging.
def __init__(self):
self.node_name = rospy.get_name()
self.dis4constV = 3. # Distance for constant velocity
self.pos_ctrl_max = 0.7
self.pos_ctrl_min = 0
self.ang_ctrl_max = 0.5
self.ang_ctrl_min = -0.5
self.pos_station_max = 0.5
self.pos_station_min = -0.5
self.cmd_ctrl_max = 0.7
self.cmd_ctrl_min = -0.7
self.station_keeping_dis = 0.5 # meters
self.is_station_keeping = False
self.start_navigation = False
self.stop_pos = []
self.final_goal = None # The final goal that you want to arrive
self.goal = self.final_goal
self.robot_position = None
rospy.loginfo("[%s] Initializing " % (self.node_name))
self.sub_goal = rospy.Subscriber("/path_points",
PoseArray,
self.path_cb,
queue_size=1)
#self.sub_goal = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=1)
rospy.Subscriber('/odometry/ground_truth',
Odometry,
self.odom_cb,
queue_size=1,
buff_size=2**24)
self.pub_cmd = rospy.Publisher("/X1/cmd_vel", Twist, queue_size=1)
self.pub_lookahead = rospy.Publisher("/lookahead_point",
Marker,
queue_size=1)
self.station_keeping_srv = rospy.Service("/station_keeping", SetBool,
self.station_keeping_cb)
self.navigate_srv = rospy.Service("/navigation", SetBool,
self.navigation_cb)
self.pos_control = PID_control("Position")
self.ang_control = PID_control("Angular")
self.ang_station_control = PID_control("Angular_station")
self.pos_station_control = PID_control("Position_station")
self.purepursuit = PurePursuit()
self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position")
self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular")
self.pos_station_srv = Server(pos_PIDConfig, self.pos_station_pid_cb,
"Angular_station")
self.ang_station_srv = Server(ang_PIDConfig, self.ang_station_pid_cb,
"Position_station")
self.lookahead_srv = Server(lookaheadConfig, self.lookahead_cb,
"LookAhead")
self.initialize_PID()
def generate_hard_path():
global pp
pp = PurePursuit()
# this creates a path manually set by changing waypoints here.
# used for testing the robot on a small course by the lab.
pp.add_point(0, -1)
pp.add_point(2, -1)
pp.add_point(2, 1)
pp.add_point(0, 1)
def __init__(self):
rospy.loginfo("Initializing %s" % rospy.get_name())
## parameters
self.map_frame = rospy.get_param("~map_frame", 'odom')
self.bot_frame = None
self.switch_thred = 1 # change to next lane if reach next
self.pursuit = PurePursuit()
self.pursuit.set_look_ahead_distance(0.1)
## node path
while not rospy.has_param("/guid_path"):
rospy.loginfo("Wait for /guid_path")
rospy.sleep(0.5)
self.guid_path = rospy.get_param("/guid_path")
self.last_node = -1
self.all_anchor_ids = rospy.get_param("/all_anchor_ids")
self.joy_lock = False
## set first tracking lane
self.set_lane(True)
# variable
self.target_global = None
self.listener = tf.TransformListener()
# markers
self.pub_goal_marker = rospy.Publisher("goal_marker",
Marker,
queue_size=1)
self.pub_target_marker = rospy.Publisher("target_marker",
Marker,
queue_size=1)
# publisher, subscriber
self.pub_pid_goal = rospy.Publisher("pid_control/goal",
PoseStamped,
queue_size=1)
self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan)
self.sub_box = rospy.Subscriber("anchor_pose",
PoseDirectional,
self.cb_goal,
queue_size=1)
sub_joy = rospy.Subscriber('joy_teleop/joy',
Joy,
self.cb_joy,
queue_size=1)
self.timer = rospy.Timer(rospy.Duration(0.1), self.tracking)
def __init__(self):
self.node_name = rospy.get_name()
self.dis4constV = 1. # Distance for constant velocity
self.pos_ctrl_max = 1
self.pos_ctrl_min = 0.0
self.pos_station_max = 0.5
self.pos_station_min = -0.5
self.cmd_ctrl_max = 0.95
self.cmd_ctrl_min = -0.95
self.station_keeping_dis = 1
self.is_station_keeping = False
self.start_navigation = False
self.stop_pos = []
self.final_goal = None # The final goal that you want to arrive
self.goal = self.final_goal
self.robot_position = None
#parameter
self.sim = rospy.get_param('sim', False)
rospy.loginfo("[%s] Initializing " %(self.node_name))
self.sub_goal = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=1)
rospy.Subscriber('odometry', Odometry, self.odom_cb, queue_size = 1, buff_size = 2**24)
if self.sim:
from duckiepond_vehicle.msg import UsvDrive
self.pub_cmd = rospy.Publisher("cmd_drive", UsvDrive, queue_size = 1)
else :
self.pub_cmd = rospy.Publisher("cmd_drive", MotorCmd, queue_size = 1)
self.pub_lookahead = rospy.Publisher("lookahead_point", Marker, queue_size = 1)
self.station_keeping_srv = rospy.Service("station_keeping", SetBool, self.station_keeping_cb)
self.navigate_srv = rospy.Service("navigation", SetBool, self.navigation_cb)
self.pos_control = PID_control("Position")
self.ang_control = PID_control("Angular")
self.ang_station_control = PID_control("Angular_station")
self.pos_station_control = PID_control("Position_station")
self.purepursuit = PurePursuit()
self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position")
self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular")
self.pos_station_srv = Server(pos_PIDConfig, self.pos_station_pid_cb, "Angular_station")
self.ang_station_srv = Server(ang_PIDConfig, self.ang_station_pid_cb, "Position_station")
self.lookahead_srv = Server(lookaheadConfig, self.lookahead_cb, "LookAhead")
self.initialize_PID()
def __init__(self):
rospy.loginfo("Initializing %s" % rospy.get_name())
self.end = np.array([[0.3, 2.2, 1.2, 2.2],
[4.1, 3.9, 3, 3.9],
[4.1, 2.2, 3, 2.2],
[4.1, 0.5, 3, 0.5]])
self.pursuit = PurePursuit()
self.pursuit.set_look_ahead_distance(0.2)
self.pose = None
self.pub_cmd = rospy.Publisher("cmd_vel", Twist, queue_size=1)
self.pub_goal_marker = rospy.Publisher(
"goal_marker", Marker, queue_size=1)
self.pub_pid_goal = rospy.Publisher(
"pid_control/goal", PoseStamped, queue_size=1)
self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan)
self.sub_pose = rospy.Subscriber(
"pose", PoseStamped, self.cb_pose, queue_size=1)
self.srv_topos = rospy.Service("to_position", GoToPos, self.to_pos)
def __init__(self):
self.node_name = rospy.get_name()
self.station_keeping_dis = 1
self.is_station_keeping = False
self.start_navigation = False
self.stop_pos = []
self.goals = []
self.diving_points = []
self.diving_points_hold = []
self.get_path = False
self.yaw = 0
self.dive = False
self.finish_diving = True
self.final_goal = None # The final goal that you want to arrive
self.goal = self.final_goal
self.robot_position = None
self.dive_dis = 5
self.cycle = rospy.get_param("~cycle", True)
rospy.loginfo("[%s] Initializing " % (self.node_name))
# self.pub_lookahead = rospy.Publisher("lookahead_point", Marker, queue_size = 1)
self.pub_robot_goal = rospy.Publisher("robot_goal",
RobotGoal,
queue_size=1)
self.path_srv = rospy.Service("set_path", SetRobotPath, self.path_cb)
self.finish_diving_srv = rospy.Service("finish_diving", SetBool,
self.finish_diving_cb)
# self.lookahead_srv = Server(lookaheadConfig, self.lookahead_cb, "LookAhead")
self.purepursuit = PurePursuit()
self.purepursuit.set_lookahead(5)
self.odom_sub = rospy.Subscriber("odometry",
Odometry,
self.odom_cb,
queue_size=1,
buff_size=2**24)
self.imu_sub = rospy.Subscriber("imu/data",
Imu,
self.imu_cb,
queue_size=1,
buff_size=2**24)
def test_find_goal(
self): # only functions with 'test_'-prefix will be run!
# Set up path
pp = PurePursuit()
pp.path = Path()
pose0 = PoseStamped()
pose0.pose.position.x = 0.0
pose0.pose.position.y = 0.0
pp.path.poses.append(pose0)
pose1 = PoseStamped()
pose1.pose.position.x = 1.0
pose1.pose.position.y = 0.0
pp.path.poses.append(pose1)
pose2 = PoseStamped()
pose2.pose.position.x = 2.0
pose2.pose.position.y = 1.0
pp.path.poses.append(pose2)
pp.lookahead = 1.0
# Set up test inputs
x = [np.array([-0.25, 0.0]), np.array([0.5, 0.]), np.array([1.5, 0.5])]
# Desired outputs
goals_true = [
np.array([0.75, 0.]),
np.array([1.41143782777, 0.411437827766]),
np.array([2., 1.])
]
# Ensure that calculated outputs match desired outputs
err_msg = "test point ({},{}) has the wrong goal ({}, {}) instead of ({}, {})"
for i in range(0, len(x)):
(pt, dist, seg) = pp.find_closest_point(x[i])
goal = pp.find_goal(x[i], pt, dist, seg)
self.assertTrue(
np.linalg.norm(goal - goals_true[i]) < 1e-6,
err_msg.format(x[i][0], x[i][1], goal[0], goal[1],
goals_true[i][0], goals_true[i][1]))
def __init__(self):
rospy.loginfo("Initializing %s" % rospy.get_name())
self.pursuit = PurePursuit()
self.pursuit.set_look_ahead_distance(0.2)
self.target_global = None
self.listener = tf.TransformListener()
# robot pose w.r.t the pose when plan pid_goal
self.robot_now_pose = PoseStamped()
self.pub_goal_marker = rospy.Publisher("goal_marker",
Marker,
queue_size=1)
self.pub_target_marker = rospy.Publisher("target_marker",
Marker,
queue_size=1)
self.pub_pid_goal = rospy.Publisher("pid_control/goal",
PoseStamped,
queue_size=1)
self.pub_pid_pose = rospy.Publisher("pid_control/pose",
PoseStamped,
queue_size=1)
# tracking robot pose before next self.tracking loop
self.map_frame = rospy.get_param("~map_frame", 'map')
self.robot_frame = rospy.get_param("~robot_frame", 'base_link')
self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan)
self.sub_box = rospy.Subscriber("anchor_position",
PoseDirectional,
self.cb_pose,
queue_size=1)
self.timer = rospy.Timer(rospy.Duration(0.2), self.tracking)
#!/usr/bin/env python
import rospy
from pure_pursuit import PurePursuit
if __name__ == '__main__':
rospy.init_node('pure_pursuit')
pp = PurePursuit()
rospy.spin()
def __init__(self):
rospy.loginfo("Initializing %s" % rospy.get_name())
## parameters
self.map_frame = rospy.get_param("~map_frame", 'odom')
self.bot_frame = 'base_link'
self.switch_thred = 1.5 # change to next lane if reach next
self.pursuit = PurePursuit()
self.lka = rospy.get_param("~lookahead", 0.5)
self.pursuit.set_look_ahead_distance(self.lka)
## node path
while not rospy.has_param("/guid_path") and not rospy.is_shutdown():
rospy.loginfo("Wait for /guid_path")
rospy.sleep(0.5)
self.guid_path = rospy.get_param("/guid_path")
self.tag_ang_init = rospy.get_param("/guid_path_ang_init")
self.last_node = -1
self.next_node_id = None
self.all_anchor_ids = rospy.get_param("/all_anchor_ids")
self.joy_lock = False
self.get_goal = True
self.joint_ang = None
## set first tracking lane
self.pub_robot_speech = rospy.Publisher("speech_case",
String,
queue_size=1)
self.pub_robot_go = rospy.Publisher("robot_go", Bool, queue_size=1)
rospy.sleep(1) # wait for the publisher to be set well
self.set_lane(True)
# variable
self.target_global = None
self.listener = tf.TransformListener()
# markers
self.pub_goal_marker = rospy.Publisher("goal_marker",
Marker,
queue_size=1)
self.pub_target_marker = rospy.Publisher("target_marker",
Marker,
queue_size=1)
# publisher, subscriber
self.pub_pid_goal = rospy.Publisher("pid_control/goal",
PoseStamped,
queue_size=1)
self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan)
self.sub_box = rospy.Subscriber("anchor_position",
PoseDirectional,
self.cb_goal,
queue_size=1)
sub_joy = rospy.Subscriber('joy_teleop/joy',
Joy,
self.cb_joy,
queue_size=1)
sub_fr = rospy.Subscriber('front_right/ranges',
DeviceRangeArray,
self.cb_range,
queue_size=1)
sub_joint = rospy.Subscriber('/dynamixel_workbench/joint_states',
JointState,
self.cb_joint,
queue_size=1)
#Don't update goal too often
self.last_update_goal = None
self.goal_update_thred = 0.001
self.hist_goal = np.array([])
self.normal = True
self.get_goal = True
self.timer = rospy.Timer(rospy.Duration(0.1), self.tracking)
# print("init done")
# prevent sudden stop
self.last_plan = None
# keep searching until find path or reach search end
self.search_angle = 10 * math.pi / 180
self.search_max = 5
### stupid range drawing
# for using tf to draw range
br1 = tf2_ros.StaticTransformBroadcaster()
br2 = tf2_ros.StaticTransformBroadcaster()
# stf.header.frame_id = "uwb_back_left"
# stf.child_frame_id = "base_link"
# stf.transform.translation.x = -1*r_tag_points[0, 0]
# stf.transform.translation.y = -1*r_tag_points[0, 1]
# br1.sendTransform(stf)
# stf2 = stf
# stf2.header.stamp = rospy.Time.now()
# stf2.header.frame_id = "uwb_front_right"
# stf2.transform.translation.x = -1*r_tag_points[1, 0]
# stf2.transform.translation.y = -1*r_tag_points[1, 1]
# br2.sendTransform(stf2)
stf = TransformStamped()
stf.header.stamp = rospy.Time.now()
stf.transform.rotation.w = 1
stf.header.frame_id = "base_link"
stf.child_frame_id = "uwb_back_left"
stf.transform.translation.x = r_tag_points[0, 0]
stf.transform.translation.y = r_tag_points[0, 1]
stf2 = TransformStamped()
stf2.header.stamp = rospy.Time.now()
stf2.transform.rotation.w = 1
stf2.header.frame_id = "base_link"
stf2.child_frame_id = "uwb_front_right"
stf2.transform.translation.x = r_tag_points[1, 0]
stf2.transform.translation.y = r_tag_points[1, 1]
goalPoint = (-10000, -10000)
goalChanged = False
x = y = theta = None
outImage = np.zeros((1, 1, 3), dtype=np.uint8)
frame = None
reprojMatrix = None
transformMatrix = None
started = False
initFinished = False
following = False
newPath = False
env = Environment(4.0)
env.generatePath(goalPoint)
pp = PurePursuit([(0.0, 0.0)], LOOKAHEAD, 0.5)
lastReset = 0
def angleDiff(a, b):
diff = abs(a - b) % math.pi
if diff > math.pi:
diff = 2.0 * math.pi - diff
if (a - b >= 0 and a - b <= math.pi) or (a - b <= -math.pi
and a - b >= -2.0 * math.pi):
diff = 1 * diff
else:
diff = -1 * (math.pi - diff)
def test_find_closest_point(
self): # only functions with 'test_'-prefix will be run!
# Set up path
pp = PurePursuit()
pp.path = Path()
pose0 = PoseStamped()
pose0.pose.position.x = 0.0
pose0.pose.position.y = 0.0
pp.path.poses.append(pose0)
pose1 = PoseStamped()
pose1.pose.position.x = 1.0
pose1.pose.position.y = 0.0
pp.path.poses.append(pose1)
pose2 = PoseStamped()
pose2.pose.position.x = 2.0
pose2.pose.position.y = 1.0
pp.path.poses.append(pose2)
# Set up test inputs
x = [
np.array([-1., 0.]),
np.array([0., 0.1]),
np.array([0.5, 0.5]),
np.array([0.9, -1.]),
np.array([1.5, 0.5]),
np.array([2., 0.5]),
np.array([3., 3.])
]
#x = np.array([-1., 0., 0.5, 0.9, 1.5, 2.0, 3.0])
#y = np.array([ 0., 0.1, 0.5, -1., 0.5, 0.5, 3.0])
# Desired outputs
pts_true = [
np.array([0., 0.]),
np.array([0., 0.]),
np.array([0.5, 0.]),
np.array([0.9, 0.]),
np.array([1.5, 0.5]),
np.array([1.75, 0.75]),
np.array([2., 1.])
]
dists_true = [1., 0.1, 0.5, 1.0, 0., 0.25 * np.sqrt(2.), np.sqrt(5.)]
segs_true = [0, 0, 0, 0, 1, 1, 1]
# Ensure that calculated outputs match desired outputs
err_msg_pt = "test point ({},{}) has the wrong closest point ({}, {}) instead of ({}, {})"
err_msg_dist = "test point ({},{}) has the wrong distance ({} instead of {})"
err_msg_seg = "test point ({},{}) has the wrong segment ({} instead of {})"
for i in range(0, len(x)):
(pt, dist, seg) = pp.find_closest_point(x[i])
self.assertTrue(
np.linalg.norm(pt - pts_true[i]) < 1e-6,
err_msg_pt.format(x[i][0], x[i][1], pt[0], pt[1],
pts_true[i][0], pts_true[i][1]))
self.assertTrue(
np.abs(dist - dists_true[i]) < 1e-6,
err_msg_dist.format(x[i][0], x[i][1], dist, dists_true[i]))
self.assertEqual(
seg, segs_true[i],
err_msg_seg.format(x[i][0], x[i][1], seg, segs_true[i]))
import math
import numpy as np
from environment import Environment, Spline
from pure_pursuit import PurePursuit
from obstacleDetector import getObstaclePositions
from imgUtils import overlayImages
x = y = theta = None
cte = None
env = Environment(3.0)
#path = [(x, 0.0) for x in np.linspace(-10, 10, 200)]
#env.waypoints = [(-10, 0), (10, 0)]
#env.splinePoints = path # TODO remove
env.generatePath((-18.0, -6.0))
pp = PurePursuit([(0.0, 0.0)], 2.5, 0.5)
def updateRobotPos():
global cte, x, y, theta, env
frame = cv2.imread("ObstacleNN/TrainingData1/image183.png")
print("Setting up...")
setupImgs = []
for i in range(15):
setupImgs.append(frame)
time.sleep(0.1)
transformMatrix, reprojMatrix = setup(setupImgs)
def __init__(self):
self.node_name = rospy.get_name()
self.state = "normal"
self.station_keeping_dis = 1
self.is_station_keeping = False
self.start_navigation = False
self.over_bridge_count = 4
self.stop_pos = []
self.goals = []
self.full_goals = []
self.get_path = False
self.final_goal = None # The final goal that you want to arrive
self.goal = self.final_goal
self.robot_position = None
self.cycle = rospy.get_param("~cycle", True)
self.gazebo = rospy.get_param("~gazebo", False)
self.lookahead = rospy.get_param("~lookahead", 2.2)
rospy.loginfo("LookAhead: " + str(self.lookahead))
self.over_bridge_counter = 0
self.satellite_list = []
self.satellite_thres = 15
self.imu_angle = 0
self.angle_thres = 0.85
self.pre_pose = []
self.bridge_mode = False
self.stop_list = []
self.stop_start_timer = rospy.get_time()
self.stop_end_timer = rospy.get_time()
self.satellite_avg = 0
self.satellite_curr = 0
self.log_string = ""
rospy.loginfo("[%s] Initializing " % (self.node_name))
# self.pub_lookahead = rospy.Publisher("lookahead_point", Marker, queue_size = 1)
self.pub_robot_goal = rospy.Publisher("robot_goal",
RobotGoal,
queue_size=1)
self.pub_fake_goal = rospy.Publisher("fake_goal", Marker, queue_size=1)
self.pub_log_str = rospy.Publisher("log_str", String, queue_size=1)
self.path_srv = rospy.Service("set_path", SetRobotPath, self.path_cb)
self.reset_srv = rospy.Service("reset_goals", SetBool, self.reset_cb)
# self.lookahead_srv = Server(lookaheadConfig, self.lookahead_cb, "LookAhead")
self.purepursuit = PurePursuit()
self.purepursuit.set_lookahead(self.lookahead)
rospy.Subscriber("odometry",
Odometry,
self.odom_cb,
queue_size=1,
buff_size=2**24)
rospy.Subscriber("/mavros/global_position/raw/satellites",
UInt32,
self.satellite_cb,
queue_size=1,
buff_size=2**24)
import time
from final_base import start, end, tankDrive, turn, forward, getFloor, getProximity, stop, beepSync, setMusicNote, updateImage, updateImage2
import math
import numpy as np
from environment import Environment, Spline
from pure_pursuit import PurePursuit
x = y = theta = None
cte = None
env = Environment()
#path = [(x, 0.0) for x in np.linspace(-10, 10, 200)]
#env.waypoints = [(-10, 0), (10, 0)]
#env.splinePoints = path # TODO remove
env.generatePath(None)
pp = PurePursuit(env.splinePoints, 2.5, 0.5)
def updateRobotPos():
global cte, x, y, theta, env
cap = WebcamVideoStream(src=int(sys.argv[1]))
cap.start()
print("Setting up...")
setupImgs = []
for i in range(15):
frame = cap.read()
setupImgs.append(frame)
time.sleep(0.1)
#!/usr/bin/env python3
import rospy
import numpy as np
from segway.msg import BaseCommand,Odometry,Path
from pure_pursuit import PurePursuit
from math import copysign
from util import RunningAverage,clip
pp=PurePursuit(.18,.18,.03)#.1588 is wheel separation
def pathCB(msg):
global pp
pp.updatePath(msg)
rospy.init_node("waypoint_following")
rospy.Subscriber("waypoints",Path,pathCB,queue_size=1)
def odomCB(msg):
global pp
pp.updateOdom(msg)
rospy.Subscriber('odometry',Odometry,odomCB,queue_size=1)
cmdpub=rospy.Publisher('target_vel',BaseCommand,queue_size=1)
rate=rospy.Rate(30)
stopped=False
while not rospy.is_shutdown():
rate.sleep()
v,w=pp.getControl(.3)
c=BaseCommand()
c.header.stamp=rospy.get_rostime()
c.velocity=v
c.omega=clip(w,-3,3)
if not stopped:
cmdpub.publish(c)