def main_auto():
# initialize ROS node
init_node('auto_mode', anonymous=True)
nh = Publisher('ecu', ECU, queue_size=10)
# set node rate
rateHz = get_param("controller/rate")
rate = Rate(rateHz)
dt = 1.0 / rateHz
t_i = 0
# get experiment parameters
t_0 = get_param("controller/t_0") # time to start test
t_f = get_param("controller/t_f") # time to end test
FxR_target = get_param("controller/FxR_target")
d_f_target = pi / 180 * get_param("controller/d_f_target")
# main loop
while not is_shutdown():
# get command signal
(FxR, d_f) = circular(t_i, t_0, t_f, d_f_target, FxR_target)
# send command signal
ecu_cmd = ECU(FxR, d_f)
nh.publish(ecu_cmd)
# wait
t_i += dt
rate.sleep()
def run(self):
publishRate = 30 # Hz
rosRate = Rate(publishRate)
while not is_shutdown():
if not self._multiArray:
continue
covMat = self._convertToMatrix(self._multiArray)
covMatNpTp = np.transpose(np.array(covMat))
(eigValues, eigVectors) = np.linalg.eig(covMatNpTp)
loginfo(eigVectors)
quaternion = self._getRotation(eigVectors)
scaledEigenValues = self._getScaledEigenvalues(
eigValues, 0.05, 0.25)
ellipsoid = self._createEllipsoidMsg([1, 1, 1], quaternion,
scaledEigenValues)
self._publisher.publish(ellipsoid)
# loginfo(ellipsoid)
loginfo(10 * "---")
rosRate.sleep()
def main_auto():
# initialize ROS node
init_node('auto_mode', anonymous=True)
nh = Publisher('ecu', ECU, queue_size = 10)
# set node rate
rateHz = get_param("controller/rate")
rate = Rate(rateHz)
dt = 1.0 / rateHz
t_i = 0
# get experiment parameters
t_0 = get_param("controller/t_0") # time to start test
t_f = get_param("controller/t_f") # time to end test
FxR_target = get_param("controller/FxR_target")
d_f_target = pi/180*get_param("controller/d_f_target")
# main loop
while not is_shutdown():
# get command signal
(FxR, d_f) = circular(t_i, t_0, t_f, d_f_target, FxR_target)
# send command signal
ecu_cmd = ECU(FxR, d_f)
nh.publish(ecu_cmd)
# wait
t_i += dt
rate.sleep()
def local_robot_state_publisher(self, publish_rate=20):
if self.publish_robot_state:
rate = Rate(publish_rate) # 20hz
t = currentThread()
while not is_shutdown() and getattr(t, "do_run", True):
local_transformer.update_robot_state()
rate.sleep()
return True
def run(self):
rate = Rate(1)
while not rospy.is_shutdown():
cameraMessage = self._createCameraInfoMessage()
self._cameraInfoPublisher.publish(cameraMessage)
rate.sleep()
def run(self):
rosRate = Rate(30)
while not is_shutdown():
message = self._setForces()
loginfo(message)
self._publisher.publish(message)
rosRate.sleep()
def run(self):
rosRate = Rate(30)
while not is_shutdown():
self._broadcastTransform(self._markerPose, self._parentFrame,
self._childFrame) #map2center
rosRate.sleep()
def local_robot_pose_publisher(self, publish_rate=20):
"Publishes the base_frame of the robot in reference to the odom frame to tf."
if self.publish_robot_base_pose:
rate = Rate(publish_rate) # 20hz
t = currentThread()
while not is_shutdown() and getattr(t, "do_run", True):
local_transformer.update_robot_pose()
rate.sleep()
return True
def do_sample():
pub = Publisher('turtle1/cmd_vel', Twist, queue_size=10)
init_node('sample_talker')
rate = Rate(10)
print_instruction()
while not is_shutdown():
move = get_move()
if move is not None:
pub.publish(move)
rate.sleep()
def tank_drive(leftFrontVel, leftBackVel, rightFrontVel, rightBackVel):
vel_publisher = Publisher("/robot_drive_controller/command", Float64MultiArray, queue_size=10)
rospy.init_node("drive_velocity_setpoint", anonymous=True)
vel_array = Float64MultiArray()
# left front, left_back, right_front right_back
vel_array.data = [leftFrontVel, leftBackVel, rightFrontVel, rightBackVel]
rate = Rate(10)
while not rospy.is_shutdown():
rospy.loginfo(vel_array)
vel_publisher.publish(vel_array)
rate.sleep()
def run(self):
rosRate = Rate(60)
while not is_shutdown():
buttonMsg = self._setButton()
positionMsg = self._setLockState()
self._pubPosition.publish(positionMsg)
self._pubButton.publish(buttonMsg)
rosRate.sleep()
def main(args):
init()
ihm = Ihm()
rospy.init_node('ihm', anonymous=True)
try:
rate = Rate(1)
rate.sleep()
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
def local_object_pose_publisher(self, publish_rate=20):
"""
Publishes the given pose of the object of given name in reference to the map frame to tf.
:type name: str
:type pose: list or Pose
"""
if self.publish_objects_poses:
rate = Rate(publish_rate)
t = currentThread()
while not is_shutdown() and getattr(t, "do_run", True):
local_transformer.update_objects()
rate.sleep()
return True
def wifi_signal_monitor():
init_node('bthere_wifi_signal_monitor', anonymous=False)
pub = Publisher('/bthere/wifi_signal', WifiData, queue_size=10)
loginfo('Outputting to /bthere/wifi_signal')
test_output = get_param('~test_output', False)
update_period = get_param('~update_period', 15.0)
quiet = get_param('~quiet', False)
rate = Rate(1 / float(update_period))
loginfo('Publishing rate: ' + str(1 / float(update_period)) + 'hz')
while not is_shutdown():
if (test_output):
output_test_data(rate, pub, quiet)
else:
output_wifi(rate, pub, quiet)
rate.sleep()
def run(self):
publishRate = 30 # Hz
rosRate = Rate(publishRate)
# initialize stiffness feedback class
CalcOmniFeedbackForce = CalcHDFeedbackForce([0, 1000], [-3.3, 3.3],
[-60, 60])
# [0.5, 0.5, 0.5], [5, 5, 5],
# [-3.3,-3.3,-3.3], [3.3,3.3,3.3],
# [-60,-60,-60],[60,60,60])
while not is_shutdown():
if not (self._stiffnessMessage and self._omniPositionMessage):
continue
# Get/Set stiffness matrix
loginfo(self._omniPositionMessage)
stiffnessMatrix = self._convertArrayToMatrix(
self._stiffnessMessage)
CalcOmniFeedbackForce.setCurrentRobotStiffness(stiffnessMatrix)
# Get/Set omni current_position
currentPosition = self._omniPositionMessage.current_position
CalcOmniFeedbackForce.setCurrentHDPosition(
[currentPosition.x, currentPosition.y, currentPosition.z])
# Get/Set omni lock_position position
lockPosition = self._omniPositionMessage.lock_position
CalcOmniFeedbackForce.setLockPositionHD(
[lockPosition.x, lockPosition.y, lockPosition.z])
# Do the calculations
CalcOmniFeedbackForce.calcForce()
# get the feedbackForces
feedBackForce = CalcOmniFeedbackForce.getHDForces()
# Set feedback force message
message = self._setOmniFeedbackMessage(feedBackForce)
# publish feedback force message
self._publisher.publish(message)
loginfo(10 * "---")
rosRate.sleep()
def run(self):
rosRate = Rate(30)
broadcaster = StaticTransformBroadcaster()
while not is_shutdown():
rot = Rotation(self._rotMatrixArray[0], self._rotMatrixArray[1],
self._rotMatrixArray[2], self._rotMatrixArray[3],
self._rotMatrixArray[4], self._rotMatrixArray[5],
self._rotMatrixArray[6], self._rotMatrixArray[7],
self._rotMatrixArray[8])
quat = rot.GetQuaternion()
staticTransform = self._setTransform(self._robotBaseFrame,
self._HDFrame, quat)
broadcaster.sendTransform(staticTransform)
rosRate.sleep()
def local_static_odom_publisher(self, publish_rate=20):
if self.publish_static_odom:
rate = Rate(publish_rate) # 20hz
t = currentThread()
while not is_shutdown() and getattr(t, "do_run", True):
published = local_transformer.publish_pose(
local_transformer.map_frame,
local_transformer.projection_namespace + '/' +
local_transformer.odom_frame,
[[0, 0, 0], [0, 0, 0, 1]],
static=True)
if not published:
logerr(
"(publisher) Could not publish static map to odom tf."
)
rate.sleep()
return True
def main_auto():
global read_yaw0, yaw_local
# initialize ROS node
init_node('auto_mode', anonymous=True)
ecu_pub = Publisher('ecu', ECU, queue_size=10)
se_sub = Subscriber('imu', Imu, imu_callback)
# set node rate
rateHz = get_param("controller/rate")
rate = Rate(rateHz)
dt = 1.0 / rateHz
# get PID parameters
p = get_param("controller/p")
i = get_param("controller/i")
d = get_param("controller/d")
pid = PID(P=p, I=i, D=d)
setReference = False
# get experiment parameters
t_0 = get_param("controller/t_0") # time to start test
t_f = get_param("controller/t_f") # time to end test
FxR_target = get_param("controller/FxR_target")
t_params = (t_0, t_f, dt)
while not is_shutdown():
# OPEN LOOP
if read_yaw0:
# set reference angle
if not setReference:
pid.setPoint(yaw_local)
setReference = True
t_i = 0.0
print('yaw_local', yaw_local)
# apply open loop command
else:
(FxR, d_f) = straight(t_i, pid, t_params, FxR_target)
ecu_cmd = ECU(FxR, -d_f)
ecu_pub.publish(ecu_cmd)
t_i += dt
# wait
rate.sleep()
def motorMaster():
'''
The Master Node defined as a function. Using a multiarray message, the 4 angles are calculated
by the functions brought in from motorControls and then published to a single topic. Each motor
pulls its respective command from that multiarray.
'''
angPub = Publisher('motAngs', DOFArray, queue_size=10)
init_node('master', anonymous=True)
while not is_shutdown():
for i in dataFrame.values:
angles = DOFArray()
angles.baseAng, angles.mainAng, \
angles.secAng, angles.toolAng = genAngles(list(i))
rate = Rate(.5)
loginfo("\n" + str(angles))
angPub.publish(angles)
rate.sleep()
def imu():
#define publisher
pub0 = Publisher('imu_data', Imu, queue_size=10)
pub1 = Publisher('yaw_rate', Float64WithHeader, queue_size=10)
pub2 = Publisher('yaw_rate_control', Float64, queue_size=10)
init_node('imu', anonymous=True)
rate = Rate(10) # 10hz
print('Running IMU node.')
while not is_shutdown():
#
# Read IMU data.
data = imu_t.get_data()
imu_data.header.stamp = Time.now()
imu_data.header.stamp = get_rostime()
#
# Get quaternion.
roll = 180 * math.atan(data[3] / math.sqrt(data[4]**2 + data[5]**2)) / math.pi
pitch = 180 * math.atan(data[4] / math.sqrt(data[3]**2 + data[5]**2)) / math.pi
yaw = 180 * math.atan(data[5] / math.sqrt(data[3]**2 + data[5]**2)) / math.pi
quaternion = quaternion_from_euler(roll,pitch,yaw)
#
# Fill message.
imu_data.orientation.w = quaternion[0]
imu_data.orientation.x = quaternion[1]
imu_data.orientation.y = quaternion[2]
imu_data.orientation.z = quaternion[3]
imu_data.linear_acceleration.x = data[3]
imu_data.linear_acceleration.y = data[4]
imu_data.linear_acceleration.z = data[5]
imu_data.linear_acceleration_covariance[0] = -1
imu_data.angular_velocity.x = data[0]
imu_data.angular_velocity.y = data[1]
imu_data.angular_velocity.z = data[2]
imu_data.angular_velocity_covariance[0] = -1
pub0.publish(imu_data)
yaw_rate = Float64WithHeader()
yaw_rate.header.stamp = get_rostime()
yaw_rate.float.data = data[2]
pub1.publish(yaw_rate)
pub2.publish(data[2]/250.0) #data2 is yaw rate, 1 is pitch rate, 0 roll, have to fix the imudata
# loginfo(imu_data)
# loginfo(yaw_rate)
rate.sleep()
def main_auto():
global read_yaw0, yaw_local
# initialize ROS node
init_node('auto_mode', anonymous=True)
ecu_pub = Publisher('ecu', ECU, queue_size = 10)
se_sub = Subscriber('imu/data', Imu, imu_callback)
# set node rate
rateHz = get_param("controller/rate")
rate = Rate(rateHz)
dt = 1.0 / rateHz
# get PID parameters
p = get_param("controller/p")
i = get_param("controller/i")
d = get_param("controller/d")
pid = PID(P=p, I=i, D=d)
setReference = False
# get experiment parameters
t_0 = get_param("controller/t_0") # time to start test
t_f = get_param("controller/t_f") # time to end test
FxR_target = get_param("controller/FxR_target")
t_params = (t_0, t_f, dt)
while not is_shutdown():
# OPEN LOOP
if read_yaw0:
# set reference angle
if not setReference:
pid.setPoint(yaw_local)
setReference = True
t_i = 0.0
# apply open loop command
else:
(FxR, d_f) = straight(t_i, pid, t_params, FxR_target)
ecu_cmd = ECU(FxR, d_f)
ecu_pub.publish(ecu_cmd)
t_i += dt
# wait
rate.sleep()
def step():
publisher = Publisher('turtle1/cmd_vel', Twist, queue_size=10)
init_node('control')
rate = Rate(100)
while not is_shutdown():
keys = {
"fwd": get_param('fwd'),
"bwd": get_param('bwd'),
"rht": get_param('rht'),
"lft": get_param('lft')
}
system('clear')
display(keys)
move = getMove(keys)
if move == 'quit':
return True
break
if move is not None:
publisher.publish(move)
rate.sleep()
def main_auto():
# initialize ROS node
init_node('auto_mode', anonymous=True)
ecu_pub = Publisher('ecu', ECU, queue_size=10)
# set node rate
rateHz = get_param("controller/rate")
rate = Rate(rateHz)
dt = 1.0 / rateHz
# get experiment parameters
FxR_target = get_param("controller/FxR_target")
while not is_shutdown():
# OPEN LOOP
ecu_cmd = ECU(FxR_target, 0)
ecu_pub.publish(ecu_cmd)
# wait
rate.sleep()
def main_auto():
global throttle, steering
# initialize the ROS node
init_node('manual_control', anonymous=True)
Subscriber('rc_inputs', ECU, rc_inputs_callback)
nh = Publisher('ecu_pwm', ECU, queue_size = 10)
# set node rate
rateHz = 50
dt = 1.0/rateHz
rate = Rate(rateHz)
throttle = 90
steering = 90
# main loop
while not is_shutdown():
ecu_cmd = ECU(throttle, steering)
nh.publish(ecu_cmd)
rate.sleep()
def main_auto():
global throttle, steering
# initialize the ROS node
init_node('manual_control', anonymous=True)
Subscriber('rc_inputs', ECU, rc_inputs_callback)
nh = Publisher('ecu_pwm', ECU, queue_size=10)
# set node rate
rateHz = 50
dt = 1.0 / rateHz
rate = Rate(rateHz)
throttle = 90
steering = 90
# main loop
while not is_shutdown():
ecu_cmd = ECU(throttle, steering)
nh.publish(ecu_cmd)
rate.sleep()
def sonar():
pub = Publisher('sonar_meas', Float64WithHeader, queue_size=10)
pub1 = Publisher('sonar_meas_control', Float64, queue_size=10)
init_node('sonar', anonymous=True)
rate = Rate(20) # 10hz
while not is_shutdown():
GPIO.output(TRIG, True)
time.sleep(0.00001)
GPIO.output(TRIG, False)
count = 0
while GPIO.input(ECHO) == 0:
if count > 1000:
break
pulse_start = time.time()
count += 1
count = 0
while GPIO.input(ECHO) == 1:
if count > 1000:
break
pulse_end = time.time()
count += 1
pulse_duration = pulse_end - pulse_start
distance = pulse_duration * 17150
distance = distance / 100.0
sonar_data = Float64WithHeader()
sonar_data.header.stamp = get_rostime()
if abs(distance) < 2.0:
sonar_data.float.data = distance
pub1.publish(distance)
else:
sonar_data.float.data = 0.0
pub1.publish(0.0)
pub.publish(sonar_data)
rate.sleep()
def cpu_monitor():
"""Publishes CPU data to /bthere/cpu_data."""
architecture = uname()[
4] # This will return 'x86_64', 'aarc64' (for 64 bit arm), etc.
if (not architecture in SUPPORTED_ARCHITECTURES):
logerr(
"This architecture doesn't appear to be one that is supported. Consider adding it and openning"
+ " a pull request on github!")
exit()
init_node("bthere_cpu_monitor", anonymous=False)
pub = Publisher("/bthere/cpu_data", CPUData, queue_size=10)
loginfo("Outputting to /bthere/cpu_data")
#update period should to be somewhat small since the cpu load data is average since you last checked,
#a slower update rate will be less accurate for bursty loads and may introduce more lag than expected
#if a load is added later in the time between updates for example.
update_period = get_param('~update_period', 1.0)
rate = Rate(1 / float(update_period))
loginfo("Publishing rate: " + str(1.0 / update_period) + " hz")
quiet = get_param("~quiet", False)
#since the temperature-getting seems likely to be failure prone, try it once to check.
able_to_get_temps = True
if (isnan(get_cpu_temps(architecture)[0])):
logwarn("Unable to get CPU temperatures")
able_to_get_temps = False
last_cpu_times = []
while not is_shutdown():
data = CPUData()
gated_loginfo(quiet, "------ CPU Data ------")
if (able_to_get_temps):
# If temperature data can be collected, add it to the CPUData to be published and log
package_temp, core_temps = get_cpu_temps(architecture)
gated_loginfo(quiet, "CPU Package temp. (C): " + str(package_temp))
data.package_temp = package_temp
if (len(core_temps) > 0):
for core in range(len(core_temps)):
gated_loginfo(
quiet, "CPU Core " + str(core) + "temp. (C): " +
str(core_temps[core]))
data.core_temps = core_temps
else:
# If the data is unavailable just publish NaN and log
gated_loginfo(quiet, "CPU temperatures unavailable")
data.package_temp = float("NaN")
data.core_temps = [float("NaN")]
if (len(last_cpu_times) == 0):
# If this hasn't been initialized, we just won't publish this info yet and init.
# last_cpu_times can't just be initialized before the loop because it should (for consistency) be the same
# time between data collections and getting the initial data before the loop would make the time between
# data collections small and potentially make the data misleading due to burst loads.
last_cpu_times = get_load_data()
gated_loginfo(quiet, "CPU load not yet available")
else:
overall_load, per_cores, last_cpu_times = get_cpu_load(
last_cpu_times)
gated_loginfo(
quiet,
"Overall CPU load: " + str(round(overall_load * 100, 1)) + "%")
data.overall_cpu_load = overall_load
if (len(per_cores) > 0):
for core in range(len(per_cores)):
gated_loginfo(
quiet, "CPU core " + str(core) + " load: " +
str(round(per_cores[core] * 100, 1)) + "%")
data.core_loads = per_cores
# Add the header information:
header = Header(stamp=Time.now())
# The frame_id property seems to be to do with tf frames of reference. That isn't useful for something like
# this, so just leave it empty. (this might be the wrong way to do this, but I don't have any other info.)
# The sequential id is apparently set by the publisher.
data.header = header
pub.publish(data)
rate.sleep()
def sonar():
init_node('sonar', anonymous=True)
rate = Rate(20) # 10hz
while not is_shutdown():
pi.gpio_trigger(TRIGGER, 10)
rate.sleep()
class Node:
def __init__(self):
init_node(NODE_NAME, anonymous=True)
Service(SERVICE_NAME, Planning, self.callback)
rospy.Subscriber("pose", Float32MultiArray, self.pose_callback)
self.planner = Planner()
self.navigator = Navigator()
self.position = Position()
self.status = IDLE
self.publisher = Publisher(PUBLISH_TO,
Float32MultiArray,
queue_size=10)
self.rate = Rate(10)
self.goal = None
self.calculating = False
def callback(self, request):
commands = loads(request.req)
response = {'status': 'type error'}
if commands['type'] == SET_GOAL:
self.planner.update_goal(
(commands['goal'][0], commands['goal'][1]))
self.goal = commands['goal']
self.calculating = True
self.planner.plan()
self.position.update_plan(self.planner.planned)
self.calculating = False
response['status'] = 'ok'
elif commands['type'] == MOVE:
if commands['status'] == COMMAND_PAUSE:
self.status = PAUSED
response['status'] = 'ok'
elif commands['status'] == COMMAND_GO:
self.status = WALKING
response['status'] = 'ok'
elif commands['status'] == COMMAND_CANCEL:
self.planner.clear()
self.navigator.clear()
self.status = IDLE
response['status'] = 'ok'
elif commands['type'] == SET_MAP:
self.planner.update_map(commands['map'])
response['state'] = self.status
response['target'] = self.goal
response['path'] = self.planner.get_path()
response['status'] = 'ok'
return dumps(response)
def operate(self):
if self.calculating:
self.publisher.publish(Float32MultiArray(data=[0, 0]))
return
planned_position = self.position.get_position(
*self.planner.current_position)
if self.position.threshold_check(POSITION_ERROR_THRES):
self.publisher.publish(Float32MultiArray(data=[0, 0]))
print('Node : Hm... where am I?')
self.planner.plan()
self.position.update_plan(self.planner.planned)
planned_position = self.position.get_position(
*self.planner.current_position)
return
if not planned_position:
self.status = IDLE
self.publisher.publish(Float32MultiArray(data=[0, 0]))
return
self.publisher.publish(
Float32MultiArray(data=self.navigator.get_motor_speed(
self.planner.current_position, planned_position, time())))
def pose_callback(self, input):
new_pose = (input.data[3], input.data[4],
(input.data[2] * 180 / np.pi + 360) % 360)
self.planner.update_position(new_pose)
def publish(self):
while not is_shutdown():
if self.status == IDLE or self.status == PAUSED:
self.publisher.publish(Float32MultiArray(data=[0, 0]))
elif self.status == WALKING:
if not validate_planned(self.planner.planned,
self.planner.map):
self.planner.plan()
self.position.update_plan(self.planner.planned)
self.operate()
self.rate.sleep()
def run(self):
self.publish()
elif key in vSpeedBindings.keys():
fixedV = fixedV*vSpeedBindings[key][0]
elif key in wSpeedBindings.keys():
fixedW = fixedW*wSpeedBindings[key][0]
elif key in altitudeBindings.keys():
z += altitudeBindings[key]
z = max(min(1.75,z),0.2)
else:
v = 0.0
w = 0.0
if (key == '\x03'):
break
print 'v %f w %f z %f m \n'%(v,w,z)
pub_a.publish(z)
pub_v.publish(v)
pub_w.publish(w)
rate.sleep()
except ROSInterruptException:
pass
finally:
print 'IM SHOOKETH'
pub_a.publish(z)
pub_v.publish(v)
pub_w.publish(w)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
def control():
global read_yaw0, yaw_local, est_yaw
# global location
# initialize ROS node
init_node('adas', anonymous=True)
Subscriber('imu/data', Imu, imu_callback)
Subscriber('scan', LaserScan, laser_callback)
# Subscriber('vel_est', Encoder, vel_callback)
# Subscriber('range', Float32MultiArray, range_callback)
Subscriber('pose_estimate', Vector3, pose_callback)
ecu_pub = Publisher('ecu', ECU, queue_size=1)
encoder_pub = Publisher('filtered_vel', Float32, queue_size=1)
yaw_pub = Publisher('filtered_yaw', Float32, queue_size=1)
scenario = (get_param("adas/scenario"))
(p, i, d) = (get_param("adas/PID")[0], get_param("adas/PID")[1],
get_param("adas/PID")[2])
stime = 0.02 # simulation time step
rate = Rate(1 / stime)
aeb = False
setReference = False
setReference1 = False
trigger = False
pid = PID(P=p, I=i, D=d)
t_i = 0.0
(t0, tf, ttc) = (0, 0, 0)
while not is_shutdown():
t0 = rospy.get_time()
if scenario == "AEB":
# if 0.0 < location < 15.0: # for camera based detection
if dist < 1:
if not aeb:
aeb = True
else:
aeb = False
if aeb:
if not setReference:
pid.setPoint(0)
setReference = True
(F, dx) = (-1 / dist * 3, 0)
else:
setReference = False
(F, dx) = (2.0, 0.0)
elif scenario == "Lane Change":
if t_i >= 50:
(F, dx) = (0, 0)
else:
# if 0.0 < location < 15.0: # for camera based detection
if dist < 1:
if not trigger:
trigger = True
x1 = dist
ttc = vel_avg * x1
net_time = ttc + t_i
if trigger:
if net_time >= t_i:
if not setReference:
pid.setPoint(30 / 57.3)
setReference = True
setReference1 = False
(F, dx) = (3, pid.update(est_yaw, stime))
else:
if not setReference1:
pid.setPoint(0)
setReference1 = True
setReference = False
(F, dx) = (3, pid.update(est_yaw, stime))
else:
(F, dx) = (3, 0)
trigger = False
setReference1 = False
setReference = False
ecu_pub.publish(ECU(F, dx))
encoder_pub.publish(Float32(vel_avg))
yaw_pub.publish(Float32(yaw_local))
rate.sleep()
tf = rospy.get_time()
t_elapsed = tf - t0
t_i += t_elapsed
print('time elapsed=' + str(t_i) + '\nloop time=' + str(t_elapsed))
print('ttc=' + str(ttc))
print('vel avg=' + str(vel_avg))
print('range=' + str(dist))