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.")
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()