def __init__(self):
self.car_position = Position()
# PURE PURSUIT PARAMS
self.pure_pursuit = pure_pursuit
self.pure_pursuit.k = 0.1 # look forward gain
self.pure_pursuit.Lfc = 0.2 # look-ahead distance
self.pure_pursuit.L = 0.324 # [m] wheel base of vehicle
self.speed_limit, self.cx, self.cy = init()
self.cont_intf = ConstraintsInterface(vehicle_name).start()
# MAP PARAMS
self.obstacle_map = None
self.resolution = None
self.width = None
self.height = None
self.future_x = None
self.future_y = None
self.origin = Point()
# EMERGENCY PARAMS
self.emergency_flag = False
while not self.cont_intf.is_ready and not rospy.is_shutdown():
rospy.sleep(0.1)
self.brake_controller = BrakeControl(vehicle_name)
def main():
"""
Initializes the parameters from launch file.
Starts the instance of ConstraintsInterface and BrakeControl.
When operated through remote: If the car senses the emergency, it stops.
Otherwise, it runs with limited velocity.
"""
rospy.init_node('velocity_constraint_node')
# Gets the current speed limit from the launch file.
speed_limit_param = rospy.search_param('speed_limit')
speed_limit = rospy.get_param(speed_limit_param)
speed_limit = float(speed_limit)
# Gets the trigger sensitivity from the launch file
trigger_param = rospy.search_param('trigger_sensitivity')
trigger_sensitivity = rospy.get_param(trigger_param)
trigger_sensitivity = float(trigger_sensitivity)
vehicle_name = rospy.get_param(rospy.get_name() + '/vehicle_name')
# starts instance of control interface with speed constraints functionality
cont_intf = ConstraintsInterface(vehicle_name, speed_limit,
trigger_sensitivity).start()
# starts instance of brake control interface.
brake_controller = BrakeControl(vehicle_name)
rospy.sleep(2)
rate = rospy.Rate(30)
# sends commands from controller but with constrained speed.
# if there is an emergency the car brakes.
while not rospy.is_shutdown():
if not brake_controller.is_emergency:
error = cont_intf.control_error()
velocity = cont_intf.pid_controller(error)
cont_intf.send_control(
cont_intf.remote_steering,
velocity,
0, # zero brake force
cont_intf.remote_transmission)
else:
brake_controller.brake_car()
cont_intf.integral = 0 # reset integrator in PID
rate.sleep()
def main():
rospy.init_node('geofence_node')
# start instance of brake_controller
brake_controller = BrakeControl('geofence')
# publishers
car_poly_pub = rospy.Publisher("/3D_car", PolygonStamped, queue_size=1)
pose_pub = rospy.Publisher("/robot_pose", PoseStamped, queue_size=1)
path_plan_pub = rospy.Publisher("/path_plan", Path, queue_size=1)
past_path_pub = rospy.Publisher("/past_path", Path, queue_size=1)
target_pub = rospy.Publisher("/target", PointStamped, queue_size=1)
# starting car position subscriber
car_position = StateSubscriber().start()
# starting geofence node
geofence = Geofence().start()
rospy.sleep(1)
# define the geofence
geofence.set_center()
geofence.set_radius()
# publish the geofence to rviz
geofence.define_marker()
geofence.publish_marker()
# control loop
rate = rospy.Rate(30)
while not rospy.is_shutdown():
norm = np.linalg.norm([
car_position.x - geofence.center.x,
car_position.y - geofence.center.y
], 2)
if norm > geofence.radius:
brake_controller.brake_car()
rate.sleep()
def main():
"""
Initializes parameters for doing donuts.
Starts instance of ConstraintsInterface and BrakeControl.
Creates publishers for visualization.
Starts instance of Geofence.
Then waits for a pose estimate for the car and geofence/donut center point to be given from rviz.
Smooth circular or donut path is created.
Car will start to follow path after start signal is given through /initialpose topic from rviz.
As soon as the car completes a lap, a new circular path with a shift in starting point is created.
Pure pursuit control is used for path tracking, PID is used for velocity control.
Geofence, circular track path and car's tracking path are visualized in rviz.
"""
rospy.init_node('donuts_node')
speed_limit, path_radius, geofence_radius, gear, target_laps = init()
# vehicle name
# initialize constraints interface
cont_intf = ConstraintsInterface(vehicle_name, speed_limit).start()
cont_intf.k_d = 100
# starts instance of brake control interface.
brake_controller = BrakeControl(vehicle_name)
rospy.sleep(2)
# start publishers
car_poly_pub = rospy.Publisher("/3D_car", PolygonStamped, queue_size=1)
pose_pub = rospy.Publisher("/robot_pose", PoseStamped, queue_size=1)
path_plan_pub = rospy.Publisher("/path_plan", Path, queue_size=1)
past_path_pub = rospy.Publisher("/past_path", Path, queue_size=1)
target_pub = rospy.Publisher("/target", PointStamped, queue_size=1)
rospy.sleep(0.2)
# start geofence
geofence = Geofence().start()
# starting car position subscriber
car_position = StateSubscriber().start()
rospy.loginfo('Waiting for initial pose')
rospy.wait_for_message('/initialpose', PoseWithCovarianceStamped)
rospy.loginfo('Received initial pose')
rospy.sleep(1)
# define the geofence
geofence.set_center_radius(geofence_radius)
# create circular path
cx, cy, cyaw = smooth_donut_path(geofence.center, path_radius)
publish_path(path_plan_pub, cx, cy)
# publish the geofence to rviz
geofence.define_marker()
geofence.publish_marker()
# wait for the initial position of the car
rospy.loginfo('Waiting for start signal')
rospy.wait_for_message('/clicked_point', PointStamped)
rospy.loginfo('Received start signal')
while not car_position.x:
print('waiting for initial car position')
# initialize pure pursuit variables
lastIndex = len(cx) - 1
# target_ind = pure_pursuit.calc_target_index(car_position, cx, cy)
target_ind = pure_pursuit.calc_target_index(car_position, cx, cy)
x = []
y = []
yaw = []
# donut loop
steering = 0.0
rate = rospy.Rate(30)
laps = 0
while laps < target_laps and not rospy.is_shutdown():
cx, cy, cyaw = smooth_donut_path(geofence.center, path_radius,
laps * math.pi / 3)
lastIndex = len(cx) - 1
target_ind = 0
while lastIndex > target_ind and not rospy.is_shutdown():
# compute control input via pure pursuit
steering, target_ind = \
pure_pursuit.pure_pursuit_control(car_position, cx, cy, target_ind)
if geofence.is_outside_geofence(car_position):
brake_controller.brake_car(steering)
cont_intf.integral = 0 # reset integrator in PID
else:
cont_intf.send_constrained_control(steering, 0, gear, 1, 1)
# publish stuff for rviz.
x.append(car_position.x)
y.append(car_position.y)
yaw.append(car_position.yaw)
publish_3Dcar(car_poly_pub, pose_pub, car_position.x,
car_position.y, car_position.yaw)
publish_path(path_plan_pub, cx, cy)
publish_path(past_path_pub, x, y, yaw)
publish_target(target_pub, cx[target_ind], cy[target_ind])
rate.sleep()
laps = laps + 1
rospy.loginfo('finished lap')
if not rospy.is_shutdown():
rospy.loginfo("Trajectory finished.")
class Purepursuit():
"""
Class for doing pure pursuit on given path.
"""
def __init__(self):
self.car_position = Position()
# PURE PURSUIT PARAMS
self.pure_pursuit = pure_pursuit
self.pure_pursuit.k = 0.1 # look forward gain
self.pure_pursuit.Lfc = 0.2 # look-ahead distance
self.pure_pursuit.L = 0.324 # [m] wheel base of vehicle
self.speed_limit, self.cx, self.cy = init()
self.cont_intf = ConstraintsInterface(vehicle_name).start()
# MAP PARAMS
self.obstacle_map = None
self.resolution = None
self.width = None
self.height = None
self.future_x = None
self.future_y = None
self.origin = Point()
# EMERGENCY PARAMS
self.emergency_flag = False
while not self.cont_intf.is_ready and not rospy.is_shutdown():
rospy.sleep(0.1)
self.brake_controller = BrakeControl(vehicle_name)
def start(self):
"""
Spins up ROS background thread; must be called to start
receiving and sending data.
:return: itself
:rtype: Purepursuit
"""
Thread(target=self._init_and_spin_ros, args=()).start()
return self
def _init_and_spin_ros(self):
"""
Initializes subscribers and publishers.
"""
self.car_poly_pub = rospy.Publisher("/3D_car",
PolygonStamped,
queue_size=1)
self.pose_pub = rospy.Publisher("/robot_pose",
PoseStamped,
queue_size=1)
self.path_plan_pub = rospy.Publisher("/path_plan", Path, queue_size=1)
self.target_pub = rospy.Publisher("/target",
PointStamped,
queue_size=1)
self.longer_pub = rospy.Publisher("/plan_longer", Bool, queue_size=1)
rospy.Subscriber('/robot_pose',
PoseStamped,
self.update_pose,
queue_size=1)
# publishes the new cx cy from the pathplanner when obstacle is detected
rospy.Subscriber("/path_floor_2",
path_pure_pursuit,
self._update_path,
queue_size=1)
rospy.Subscriber('/map_obstacle',
OccupancyGrid,
self._get_obsmap_cb,
queue_size=1)
rospy.sleep(3)
## Initial Pose
print('Waiting for initial pose')
rospy.wait_for_message('/initialpose', PoseWithCovarianceStamped)
print('Received initial pose')
while not self.car_position.x:
print('waiting for initial car position')
rospy.sleep(1)
rospy.spin()
def update_pose(self, msg):
"""
Callback that updates pose of car.
:param msg: car's current pose
:type msg: PoseStamped
"""
self.car_position.x = msg.pose.position.x
self.car_position.y = msg.pose.position.y
quaternions = (msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w)
euler = euler_from_quaternion(quaternions)
self.car_position.yaw = euler[2]
def _update_path(self, msg):
"""
Callback to update the new a-star path to avoid obstacle.
:param msg: path details
:type msg: path_pure_pursuit
"""
self.cx = msg.cx
self.cy = msg.cy
self.lastIndex = len(self.cx) - 1
def calc_xyindex(self, x, y):
"""
Calculates indexes from x,y position.
:param x: x-coordinate in map frame
:type x: float
:param y: y-coordinate in map frame
:type y: float
:return: x,y grid coordinates
:rtype: tuple
"""
grid_x = int(round((x - self.origin.x) / self.resolution))
grid_y = int(round((y - self.origin.y) / self.resolution))
return (grid_x, grid_y)
def _get_obsmap_cb(self, msg):
"""
Callback that updates the occupancy map.
:param msg: Obstacle map data
:type msg: OccupancyGrid
"""
temp_map = np.array(msg.data)
self.width = msg.info.width
self.height = msg.info.height
self.obstacle_map = np.array(temp_map).reshape(self.height, self.width)
self.resolution = msg.info.resolution
self.origin = msg.info.origin.position
def visualize_everything(self):
"""
Publish car, path and target index for rviz.
"""
publish_3Dcar(self.car_poly_pub, self.pose_pub, self.car_position.x,
self.car_position.y, self.car_position.yaw)
publish_path(self.path_plan_pub, self.cx, self.cy)
publish_target(self.target_pub, self.cx[self.target_ind],
self.cy[self.target_ind])
def run_pure_pursuit(self):
"""
Pure pursuit tracks the dynamic path created for distant obstacles.
If the car senses the obstacles too close, it brakes and reverses,
and retries the same path forward 3 times.
If the result is the same even during the 3rd consecutive retry,
it starts to plan the new path taking longer time.
Visualizes publishers in rviz.
"""
self.car_position.v = self.cont_intf.odometry_node.get_velocity()
# initialize pure pursuit variables
self.lastIndex = len(self.cx) - 1
self.target_ind = pure_pursuit.calc_target_index(
self.car_position, self.cx, self.cy)
# simualtion + animation loop
steering = 0.0
rate = rospy.Rate(30)
tries = 0
extra_dist = 0
while self.lastIndex > self.target_ind and not rospy.is_shutdown():
pure_pursuit.Lfc = 0.4
# compute control input via pure pursuit
self.car_position.v = self.cont_intf.odometry_node.get_velocity()
self.target_ind = pure_pursuit.calc_target_index(
self.car_position, self.cx, self.cy)
if tries == 3 and self.future_path_path_check(self.cx, self.cy):
continue
steering, self.target_ind = self.pure_pursuit.pure_pursuit_control(
self.car_position, self.cx, self.cy, self.target_ind)
if self.brake_controller.is_emergency or self.emergency_flag:
if not self.emergency_flag: # the first time we are in emergency
print(
"--------------------------EMERGENCY--------------------------"
)
self.brake_controller.brake_car(0)
car_x = self.car_position.x
car_y = self.car_position.y
self.emergency_flag = True
self.cont_intf.integral = 0
self.cont_intf.last_error = 0
tries = (tries + 1) % 4
if tries == 3: # the third time it drives in reverse extra 0.2 m
extra_dist = 0.2
messa = Bool()
messa.data = True
self.longer_pub.publish(
messa
) # after two unsuccesful tries publishes message for the obst avoidance to plan longer
# go reverse
self.cont_intf.send_control(-steering * 2 / 3, -0.5, 0, 0)
# if went backwards reset everything
if np.hypot(car_x - self.car_position.x,
car_y - self.car_position.y) >= 0.3 + extra_dist:
self.emergency_flag = False
self.cont_intf.integral = 0
self.cont_intf.last_error = 0
rospy.sleep(2)
else:
error = self.speed_limit - self.car_position.v
velocity = self.cont_intf.pid_controller(error)
self.cont_intf.send_control(steering, velocity, 0,
0) #0 break force and low gear
self.visualize_everything()
rate.sleep()
if not rospy.is_shutdown():
rospy.loginfo("Trajectory finished.")
self.brake_controller.brake_car(0)
def future_path_path_check(self, cx, cy):
"""
Checks if future path is valid (0.7m).
:param cx: The x-coordinates list of the track path
:type cx: list
:param cy: The y-coordinates list of the track path
:type cy: list
:return: flag indicating valid (true) or invalid (false) future path
:rtype: bool
"""
# Index nearest to car
dx = [self.car_position.x - icx for icx in cx]
dy = [self.car_position.y - icy for icy in cy]
d = [abs(math.sqrt(idx**2 + idy**2)) for (idx, idy) in zip(dx, dy)]
index = d.index(min(d)) # index of point nearest to car
# Index 0.7 m ahead
d_temp = [abs(x - 0.7) for x in d[index:]]
t_ind = d_temp.index(min(d_temp)) + index
# check all points of path
for i in range(index, t_ind):
if self.check_point(cx[i], cy[i]):
return True
return False
def check_point(self, x, y):
"""
Checks if the point is valid for the robot to be.
:param x: x-coordinate of the track path
:type x: float
:param y: y-coordinate of the track path
:type y: float
:return: flag indicating valid (true) or invalid (false) point
:rtype: bool
"""
grid_x = round((x - self.origin.x) / self.resolution)
grid_y = round((y - self.origin.y) / self.resolution)
rradius = int(0.17 / self.resolution)
if (self.obstacle_map[grid_y - rradius:grid_y + rradius,
grid_x - rradius:grid_x + rradius] == 1).any():
return True
return False
def main():
"""
Initializes the parameters from launch file.
Creates publishers for visualization.
Starts the instances of ConstraintsInterface, BrakeControl, Geofence and StateSubscriber.
Waits for center point and radius to be given in rviz.
Creates geofence, can be seen in rviz.
When operated through remote: If the car senses the emergency, it stops.
Or if the car is outside the geofence, it stops.
Otherwise, it runs with limited velocity.
"""
rospy.init_node('velocity_constraint_node')
# Gets the current speed limit from the launch file.
speed_limit_param = rospy.search_param('speed_limit')
speed_limit = rospy.get_param(speed_limit_param)
speed_limit = float(speed_limit)
# Gets the trigger sensitivity from the launch file
trigger_param = rospy.search_param('trigger_sensitivity')
trigger_sensitivity = rospy.get_param(trigger_param)
trigger_sensitivity = float(trigger_sensitivity)
car_poly_pub = rospy.Publisher("/3D_car", PolygonStamped, queue_size=1)
pose_pub = rospy.Publisher("/robot_pose", PoseStamped, queue_size=1)
# wait for the initial position of the car
rospy.loginfo('Waiting for initial pose')
rospy.wait_for_message('/initialpose', PoseWithCovarianceStamped)
rospy.loginfo('Received initial pose')
vehicle_name = rospy.get_param(rospy.get_name() + '/vehicle_name')
# starts instance of control interface with speed constraints functionality
cont_intf = ConstraintsInterface(vehicle_name, speed_limit,
trigger_sensitivity).start()
# starts instance of brake control interface.
brake_controller = BrakeControl(vehicle_name)
# start geofence
geofence = Geofence().start()
# starting car position subscriber
car_position = StateSubscriber().start()
rospy.sleep(1)
# define the geofence
geofence.set_center()
geofence.set_radius()
# publish the geofence to rviz
geofence.define_marker()
geofence.publish_marker()
rate = rospy.Rate(30)
# sends commands from controller but with constrained speed.
# if there is an emergency the car brakes.
while not rospy.is_shutdown():
if brake_controller.is_emergency:
brake_controller.brake_car()
print('brake')
cont_intf.integral = 0 # reset integrator in PID
if geofence.is_outside_geofence(car_position):
brake_controller.brake_car_extreme()
cont_intf.integral = 0 # reset integrator in PID
else:
error = cont_intf.control_error()
velocity = cont_intf.pid_controller(error)
cont_intf.send_control(
cont_intf.remote_steering,
velocity,
0, # zero brake force
cont_intf.remote_transmission)
publish_3Dcar(car_poly_pub, pose_pub, car_position.x, car_position.y,
car_position.yaw)
rate.sleep()
def main():
"""
Initializes parameters for pure pursuit.
Starts instance of ConstraintsInterface and BrakeControl.
Starts instance of StateSubscriber.
Creates publishers for visualization.
Then waits for a pose estimate for the car to be given.
Car will start to follow path after start signal is given,
which is done by publishing to the /clicked_point topic.
"""
global short_look_ahead, long_look_ahead
rospy.init_node('floor2_pure_pursuit')
vehicle_name = "SVEA3"
target_speed, cx, cy, gear, long_look_ahead, short_look_ahead = init()
final_point = (-8.45582103729, -5.04866838455)
cx.append(final_point[0])
cy.append(final_point[1])
cont_intf = ConstraintsInterface(vehicle_name, target_speed).start()
cont_intf.k_d = 100
while not cont_intf.is_ready and not rospy.is_shutdown():
rospy.sleep(0.1)
# start emergency brake node
brake_controller = BrakeControl(vehicle_name)
# start car position and velocity subscriber
car_state = StateSubscriber().start()
car_poly_pub = rospy.Publisher("/3D_car", PolygonStamped, queue_size=1)
pose_pub = rospy.Publisher("/robot_pose", PoseStamped, queue_size=1)
path_plan_pub = rospy.Publisher("/path_plan", Path, queue_size=1)
past_path_pub = rospy.Publisher("/past_path", Path, queue_size=1)
target_pub = rospy.Publisher("/target", PointStamped, queue_size=1)
rospy.sleep(3)
print('Waiting for initial pose')
rospy.wait_for_message('/initialpose', PoseWithCovarianceStamped)
print('Received initial pose')
publish_path(path_plan_pub, cx, cy)
print('Waiting for start signal')
rospy.wait_for_message('/clicked_point', PointStamped)
print('Received start signal')
while not car_state.x:
print('waiting for initial car position')
# PURE PURSUIT PARAMS ########################################################
pure_pursuit.k = 0.1 # look forward gain
pure_pursuit.Lfc = long_look_ahead # look-ahead distance
pure_pursuit.L = 0.324 # [m] wheel base of vehicle
###############################################################################
lastIndex = len(cx) - 2
target_ind = pure_pursuit.calc_target_index(car_state, cx, cy)
x = []
y = []
yaw = []
# pure pursuit loop
# follow path until second to last index.
steering = 0.0
rate = rospy.Rate(30)
cont_intf.send_control(steering, 0, 0, gear) # send low gear
while lastIndex > target_ind and not rospy.is_shutdown():
pure_pursuit.Lfc = calc_look_ahead(car_state)
# compute control input via pure pursuit
steering, target_ind = \
pure_pursuit.pure_pursuit_control(car_state, cx, cy, target_ind)
if not brake_controller.is_emergency:
cont_intf.send_constrained_control(
steering,
0, # zero brake force
gear) # send low gear
else:
brake_controller.brake_car(steering)
# publish car, path and target index for rviz
x.append(car_state.x)
y.append(car_state.y)
yaw.append(car_state.yaw)
publish_3Dcar(car_poly_pub, pose_pub, car_state.x, car_state.y,
car_state.yaw)
publish_path(path_plan_pub, cx, cy)
publish_path(past_path_pub, x, y, yaw)
publish_target(target_pub, cx[target_ind], cy[target_ind])
rate.sleep()
# create a straight path for points inbetween the last two points
# points to project goal onto
p1 = (-2.33859300613, 3.73132610321)
p2 = (-2.86509466171, 3.14682602882)
line = (p2[0] - p1[0], p2[1] - p1[1])
proj_p2 = np.dot(p2, line)
cx = cx[:-1]
cy = cy[:-1]
new_x = np.linspace(cx[-1], final_point[0], 50)
new_y = np.linspace(cy[-1], final_point[1], 50)
cx.extend(new_x)
cy.extend(new_y)
# continue following final point until goal is reached.
while not rospy.is_shutdown():
steering, target_ind = \
pure_pursuit.pure_pursuit_control(car_state, cx, cy, target_ind)
# calculate car projection on line
proj_car = np.dot((car_state.x, car_state.y), line)
if brake_controller.is_emergency:
brake_controller.brake_car(steering)
elif proj_car < proj_p2:
cont_intf.send_constrained_control(steering, 0, gear)
else:
# brake if goal is reached.
brake_controller.brake_car(steering)
rospy.sleep(0.1)
break
# update visualizations
x.append(car_state.x)
y.append(car_state.y)
yaw.append(car_state.yaw)
publish_3Dcar(car_poly_pub, pose_pub, car_state.x, car_state.y,
car_state.yaw)
publish_path(path_plan_pub, cx, cy)
publish_path(past_path_pub, x, y, yaw)
publish_target(target_pub, cx[target_ind], cy[target_ind])
rate.sleep()
if not rospy.is_shutdown():
rospy.loginfo("Trajectory finished.")
while brake_controller.is_emergency:
brake_controller.brake_car(steering)
rospy.spin()