def test1():
print("test 1")
global x1, y1, position1, position2, endTicks
rospy.sleep(3)
position1 = [x1, y1]
print("start position: " + str(position1))
steering_command = NormalizedSteeringCommand()
steering_command.value = 1.0
publish_steering.publish(steering_command)
rospy.sleep(1)
speed_command = SpeedCommand()
speed_command.value = 0.2
publish_speed.publish(speed_command)
rospy.sleep(5)
speed_command.value = 0.0
publish_speed.publish(speed_command)
position2 = [x1, y1]
print("end position: " + str(position2))
print("total ticks: " + str(endTicks))
euclideanDistance = euclidean_distance(position1[0], position1[1],
position2[0], position2[1])
current_slope = calculate_slope(position1[0], position1[1], position2[0],
position2[1])
current_angle = math.atan(current_slope)
print("current angle: " + str(current_angle))
radius = (euclideanDistance / (2 * math.sin(current_angle / 2)))
print("radius: " + str(radius))
distanceDriven = radius * current_angle
print("total distance driven: " + str(distanceDriven))
ratio = distanceDriven / endTicks
print("ratio between travelled distance and counted ticks: " + str(ratio))
def main():
while curX == 0.0:
pass
pos1 = (curX,curY)
time.sleep(1)
steering_cmd = NormalizedSteeringCommand()
steering_cmd.value = 1.0
pub_steering.publish(steering_cmd)
time.sleep(1)
speed_cmd = SpeedCommand()
speed_cmd.value = 0.2
pub_speed.publish(speed_cmd)
time.sleep(4)
speed_cmd.value = 0.0
pub_speed.publish(speed_cmd)
time.sleep(2)
pos2 = (curX,curY)
speed_cmd.value = 0.2
pub_speed.publish(speed_cmd)
time.sleep(4)
speed_cmd.value = 0.0
pub_speed.publish(speed_cmd)
time.sleep(2)
pos3 = (curX,curY)
print "Radius:",berechneRadius(pos1,pos2,pos3),"Meter"
print "Winkel:",berechneEinlenkWinkel(berechneRadius(pos1,pos2,pos3)),"Grad, weil niemand was mit Bogenmass anfangen kann"
def timer():
h = Header()
h.stamp = rospy.Time.now()
mensaje = NormalizedSteeringCommand()
mensaje.header = h
mensaje.value = -0.45
pub.publish(mensaje)
def driveForwardDistance():
global positions, accTickDistance, gpsDistance
i = 1
steering_cmd = NormalizedSteeringCommand()
steering_cmd.value = 0.0 #left 1.0 , right -1.0
pub_steering.publish(steering_cmd)
speed = SpeedCommand()
speed.value = 0.2 #moeglichst langsam
pub_speed.publish(speed)
time.sleep(4)
while i < len(positions):
gpsDistance += math.sqrt(
math.pow(positions[i][0] - positions[i - 1][0], 2) +
math.pow(positions[i][1] - positions[i - 1][1], 2))
accTickDistance += accTick
# print(gpsDistance)
i = i + 1
rospy.loginfo("cumulativ gpsDistance (cm): " + str(gpsDistance))
rospy.loginfo("last - first position gps distance (cm) : " + str(
math.sqrt(
math.pow(positions[len(positions) - 1][0] - positions[0][0], 2) +
math.pow(positions[len(positions) - 1][1] - positions[0][1], 2))))
rospy.loginfo("Ticks: " + str(accTickDistance))
rospy.loginfo("Distance per Tick (cumulativ gpsDistance (cm)/ ticks): " +
str(gpsDistance / accTickDistance))
speed.value = 0.0
pub_speed.publish(speed)
def PID(self):
Kp = 5
Ki = 0
Kd = 0.1
error = (self.setpoint - self.yaw)
self.acc_error = self.acc_error + error #accumulator for error
current_time = rospy.Time.now()
delta_time = (current_time - self.pre_time).to_sec()
#Kp*error Ki*area Kd*slope
PID = Kp * error + Ki * self.acc_error * delta_time + Kd * (
error - self.pre_error) / delta_time
# PID = int(round(PID))
self.pre_time = current_time
self.pre_error = error
#pid value to steering message
str_com = NormalizedSteeringCommand()
str_com.value = PID
self.str_pub.publish(str_com)
#pub speed
sp = SpeedCommand()
sp.value = 0.3
self.speed_pub.publish(sp)
def publisher():
pub_steer = rospy.Publisher('/actuators/steering_normalized',
NormalizedSteeringCommand,
queue_size=10)
pub_speed = rospy.Publisher('/actuators/speed',
SpeedCommand,
queue_size=10)
rospy.init_node('publisher', anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
'''
rospy.set_param('msg_steer', 1.0)
msg_steer = rospy.get_param("msg_steer")
rospy.loginfo(msg_steer)
pub_steer.publish(msg_steer)
rospy.set_param('msg_speed' , 0.3)
msg_speed = rospy.get_param("msg_speed")
rospy.loginfo(msg_speed)
pub_speed.publish(msg_speed)
rate.sleep()
'''
msg_steer = NormalizedSteeringCommand()
msg_steer.value = 1.0
rospy.loginfo(msg_steer)
pub_steer.publish(msg_steer)
msg_speed = SpeedCommand()
msg_speed.value = 0.3
rospy.loginfo(msg_speed)
pub_speed.publish(msg_speed)
def on_control(self, tmr):
if tmr.last_duration is None:
dt = 0.01
else:
dt = (tmr.current_expected - tmr.last_expected).to_sec()
# print dt
quat = [
self.pose.pose.pose.orientation.x,
self.pose.pose.pose.orientation.y,
self.pose.pose.pose.orientation.z,
self.pose.pose.pose.orientation.w
]
roll, pitch, yaw = tf.transformations.euler_from_quaternion(quat)
error = self.desired_angle
self.integral_error += error * dt
self.integral_error = max(self.min_i, self.integral_error)
self.integral_error = min(self.max_i, self.integral_error)
derivative_error = (error - self.last_error) / dt
self.last_error = error
pid_output = self.kp * error + self.kd * derivative_error + self.ki * self.integral_error
steering_msg = NormalizedSteeringCommand()
steering_msg.value = pid_output
self.steering_pub.publish(steering_msg)
def drive(distance, command, speed, angle):
global is_active
speed = speed * speed_mult
rospy.loginfo("speed is %f", speed)
rospy.loginfo("%s: Running %s(%f)", rospy.get_caller_id(), command, distance)
if distance <= 0:
rospy.logerr(
"%s: Error, distance argument has to be > 0! %f given",
rospy.get_caller_id(),
distance)
return
pub_info.publish("BUSY")
if is_active:
rospy.logwarn(
"%s: Warning, another command is still active! Please wait and try again.",
rospy.get_caller_id())
return
is_active = True
# stop the car and set desired steering angle + speed
speed_cmd = SpeedCommand()
speed_cmd.value = 0
pub_speed.publish(speed_cmd)
rospy.sleep(1)
steering_cmd = NormalizedSteeringCommand()
steering_cmd.value = angle
pub_steering.publish(steering_cmd)
rospy.sleep(1)
speed_cmd.value = speed
pub_speed.publish(speed_cmd)
start_pos = last_odom.pose.pose.position
current_distance = 0
while not rospy.is_shutdown() and current_distance < (distance - epsilon):
current_pos = last_odom.pose.pose.position
current_distance = sqrt(
(current_pos.x - start_pos.x)**2 + (current_pos.y - start_pos.y)**2)
# rospy.loginfo("current distance = %f", current_distance)
rospy.sleep(0.1)
speed_cmd.value = 0
pub_speed.publish(speed_cmd)
is_active = False
current_pos = last_odom.pose.pose.position
current_distance = sqrt((current_pos.x - start_pos.x)
** 2 + (current_pos.y - start_pos.y)**2)
pub_info.publish("FINISHED")
rospy.loginfo(
"%s: Finished %s(%f)\nActual travelled distance = %f",
rospy.get_caller_id(),
command,
distance,
current_distance)
def driver(steer_pub, speed_pub):
steer_msg = NormalizedSteeringCommand()
steer_msg.value = -1.0
steer_pub.publish(steer_msg)
speed_msg = SpeedCommand()
speed_msg.value = 0.3
speed_pub.publish(speed_msg)
rospy.sleep(2.5)
speed_msg.value = 0.0
speed_pub.publish(speed_msg)
def talker():
pub = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10)
pub2 = rospy.Publisher('/actuators/steering_normalized', NormalizedSteeringCommand, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
msg = SpeedCommand()
msg.value = 0.5
pub.publish(msg)
msg2 = NormalizedSteeringCommand()
msg2.value = 1.0
pub2.publish(msg2)
rate.sleep()
def talker():
steer_pub = rospy.Publisher('/actuators/steering_normalized', NormalizedSteeringCommand, queue_size=10)
speed_pub = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10)
rospy.init_node('publisher', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
steer_msg = NormalizedSteeringCommand()
steer_msg.value = 1.0
steer_pub.publish(steer_msg)
speed_msg = SpeedCommand()
speed_msg.value = 0.6
speed_pub.publish(speed_msg)
rate.sleep()
def on_localization(self, data):
current_pos = np.array(
[data.pose.pose.position.x, data.pose.pose.position.y])
look_ahead_point = self.map.lanes[self.lane].lookahead_point(
current_pos, 0.5)[0] # [0] for lookahead point inner lane
self.publish_looakhead(look_ahead_point)
car_yaw = self.get_car_yaw(data)
yaw = self.calculateYaw(current_pos, look_ahead_point,
car_yaw) # yaw difference from two points
print("I want this yaw:" + str(yaw))
steering_msg = NormalizedSteeringCommand()
steering_msg.value = yaw
self.pid_desired_angle.publish(steering_msg)
def callback(self, raw_msgs):
position = raw_msgs.pose.pose.position
x = position.x
y = position.y
current_point = [x, y]
# math transformation
orientation = raw_msgs.pose.pose.orientation
quaternion = [
orientation.x, orientation.y, orientation.z, orientation.w
]
euler = tf.transformations.euler_from_quaternion(quaternion)
self.yaw = euler[2]
def rot(yaw):
return np.array([[math.cos(yaw), -math.sin(yaw)],
[math.sin(yaw), math.cos(yaw)]])
map = myMap.Map()
lane = map.lanes[self.lane]
goal_point = lane.lookahead_point(current_point, 0.5)[0]
vector = goal_point - current_point
map_point = np.matmul(rot(-self.yaw), vector)
self.setpoint = np.arctan2(map_point[1], map_point[0])
# np.arccos(np.dot(current_point,goal_point)/(np.linalg.norm(current_point)*np.linalg.norm(goal_point)))
Kp = 2.0
Ki = 0.0
Kd = 0.2
error = self.setpoint #- self.yaw
self.acc_error = self.acc_error + error #accumulator for error
current_time = rospy.Time.now()
delta_time = (current_time - self.pre_time).to_sec()
#Kp*error Ki*area Kd*slope
PID = Kp * error + Ki * self.acc_error * delta_time + Kd * (
error - self.pre_error) / delta_time
# PID = int(round(PID))
self.pre_time = current_time
self.pre_error = error
#pid value to steering message
str_com = NormalizedSteeringCommand()
str_com.value = PID
self.str_pub.publish(str_com)
#pub speed
sp = SpeedCommand()
sp.value = 0.3
self.speed_pub.publish(sp)
def driver():
steer_pub = rospy.Publisher('/actuators/steering_normalized',
NormalizedSteeringCommand,
queue_size=10)
speed_pub = rospy.Publisher('/actuators/speed',
SpeedCommand,
queue_size=10)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
steer_msg = NormalizedSteeringCommand()
steer_msg.value = 1.0
steer_pub.publish(steer_msg)
speed_msg = SpeedCommand()
speed_msg.value = 0.3
speed_pub.publish(speed_msg)
def comienzo():
rospy.init_node('parking2', anonymous=True)
pub = rospy.Publisher('/control/command/normalized_wanted_speed', NormalizedSpeedCommand, queue_size=10)
steering = rospy.Publisher('/control/command/normalized_wanted_steering', NormalizedSteeringCommand, queue_size=10)
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
mensaje = mensaje2.value()
mensaje.header = h
mensaje.value = date.value
j = std_msgs.msg.Header()
j.stamp = rospy.Time.now()
mensaje2 = NormalizedSteeringCommand()
mensaje2.header = j
mensaje2.value = 0.5
def driver():
steer_pub = rospy.Publisher('/actuators/steering_normalized',
NormalizedSteeringCommand,
queue_size=10)
speed_pub = rospy.Publisher('/actuators/speed',
SpeedCommand,
queue_size=10)
steer_msg = NormalizedSteeringCommand()
steer_msg.value = 1.0
steer_pub.publish(steer_msg)
speed_msg = SpeedCommand()
speed_msg.value = 0.3
speed_pub.publish(speed_msg)
rospy.sleep(1)
speed_msg = SpeedCommand()
speed_msg.value = 0.0
speed_pub.publish(speed_msg)
def callbackGPS(self, gps_data):
# calculate how off steering is and give the error to PID Controler
orientation = gps_data.pose.pose.orientation
# print ("gps: " , gps_data)
current_yaw = self.get_rotation(orientation)
error = self.desired_angle - current_yaw
if (abs(error) > self.accepted_error):
update = self.PID_controller.get_PID(error)
print("err:", error)
print("yaw: ", current_yaw)
print("UPD: ", update)
steering = NormalizedSteeringCommand()
steering.value = update
self.pub_steering.publish(steering)
self.pub_speed.publish(self.speed_cmd)
rate.sleep()
def __init__(self):
rospy.init_node("basic_publisher")
self.speed_publisher = rospy.Publisher("/actuators/speed", SpeedCommand, queue_size=1)
self.steering_publisher = rospy.Publisher("/actuators/steering_normalized", NormalizedSteeringCommand,
queue_size=1)
self.r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
steer_msg = NormalizedSteeringCommand()
steer_msg.value = 1.0
self.steering_publisher.publish(steer_msg)
speed_msg = SpeedCommand()
speed_msg.value = 0.3
self.speed_publisher.publish(speed_msg)
self.r.sleep()
def __init__(self):
print("initializing Driver")
rospy.init_node("assignment_6", anonymous = True)
self.rate = rospy.Rate(10)
self.steer_pub = rospy.Publisher("/actuators/steering_normalized", NormalizedSteeringCommand, queue_size=10)
self.speed_pub = rospy.Publisher("/actuators/speed", SpeedCommand, queue_size=10)
self.steer_cmd = NormalizedSteeringCommand()
self.speed_cmd = SpeedCommand()
self.speed_sub = rospy.Subscriber("/actuators/speed", Speed, self.print_speed_given, queue_size=10)
print("initialized complete")
def backwards_right(data, date):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.value)
pub = rospy.Publisher('/control/command/normalized_wanted_speed', NormalizedSpeedCommand, queue_size=10)
steering = rospy.Publisher('/control/command/normalized_wanted_steering', NormalizedSteeringCommand, queue_size=10)
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
mensaje = NormalizedSpeedCommand()
mensaje.header = h
mensaje.value = data.value
j = std_msgs.msg.Header()
j.stamp = rospy.Time.now()
mensaje2 = NormalizedSteeringCommand()
mensaje2.header = j
mensaje2.value = date.value
if date.value > 0:
mensaje.vlaue = date.value * -1
pub.publish(mensaje)
def callback(self,raw_msgs):
#print("call")
msg=raw_msgs.pose.pose.orientation
msg_array=[msg.x,msg.y,msg.z,msg.w]
#print(str(msg))
euler=tf.transformations.euler_from_quaternion(msg_array)
#print(euler)
steering=NormalizedSteeringCommand()
steering.value=self.pid(euler[2])
speed=SpeedCommand()
speed.value=0.5
#print(str(steering.value))
#cv2.imshow('image',img)
#cv2.waitKey(3)
self.x4.publish(speed)
try:
self.x3.publish(steering)
except CvBridgeError as e:
print(e)
def publish():
pub_speed = rospy.Publisher('/actuators/speed',
SpeedCommand,
queue_size=10)
pub_steering = rospy.Publisher("/actuators/steering_normalized",
NormalizedSteeringCommand,
queue_size=10)
steering_msg = NormalizedSteeringCommand()
steering_msg.value = 1
speed_msg = SpeedCommand()
speed_msg.value = 0.3
rospy.init_node('publisher', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
pub_steering.publish(steering_msg)
pub_speed.publish(speed_msg)
rospy.loginfo('speed is ' + str(speed_msg.value))
print('speed is ' + str(speed_msg.value))
rate.sleep()
def talker():
# Publish to this existing topics
pub_steering = rospy.Publisher('/actuators/steering_normalized', NormalizedSteeringCommand, queue_size=10)
pub_speed = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10)
rospy.init_node('steering_speed_whisperer', anonymous=True)
# steering
norm_steerin_cmd = NormalizedSteeringCommand() # Command is a class create object
norm_steerin_cmd.value = 0
#speed init
speed_cmd = SpeedCommand()
speed_cmd.value = 0.3
rate = rospy.Rate(5) # 5 hz
while not rospy.is_shutdown():
info_str = "time {time} : speed: {speed} , steering: {steering}".format(time=rospy.get_time(),speed=speed_cmd.value,steering=norm_steerin_cmd.value)
rospy.loginfo(info_str)
pub_steering.publish(norm_steerin_cmd)
pub_speed.publish(speed_cmd)
rate.sleep()
def on_control(self, tmr):
if tmr.last_duration is None:
dt = 0.01
else:
dt = (tmr.current_expected - tmr.last_expected).to_sec()
if self.desired_angle is None:
return
quat = [
self.pose.pose.pose.orientation.x,
self.pose.pose.pose.orientation.y,
self.pose.pose.pose.orientation.z,
self.pose.pose.pose.orientation.w
]
roll, pitch, yaw = tf.transformations.euler_from_quaternion(quat)
diff = self.desired_angle - yaw
# normalize steering angles (keep between -pi and pi)
error = math.atan2(math.sin(diff), math.cos(diff))
self.integral_error += error * dt
self.integral_error = max(self.min_i, self.integral_error)
self.integral_error = min(self.max_i, self.integral_error)
derivative_error = (error - self.last_error) / dt
self.last_error = error
pid_output = self.kp * error + self.kd * derivative_error + self.ki * self.integral_error
steering_msg = NormalizedSteeringCommand()
steering_msg.value = pid_output
steering_msg.header.frame_id = "base_link"
steering_msg.header.stamp = rospy.Time.now()
self.steering_pub.publish(steering_msg)
def __init__(self):
rospy.init_node("initNull")
self.speed = SpeedCommand()
self.steer = NormalizedSteeringCommand()
self.position = Odometry()
self.speed_pub = rospy.Publisher("/actuators/speed", SpeedCommand, queue_size=10)
self.steer_pub = rospy.Publisher("/actuators/steering_normalized", NormalizedSteeringCommand, queue_size=10)
self.position_sub = rospy.Subscriber("/communication/gps/9", Odometry, self.on_position, queue_size=10)
self.rate = rospy.Rate(100)
while not rospy.is_shutdown():
self.speed.value = 0.0
self.speed_pub.publish(self.speed)
self.steer.value = 0.0
self.steer_pub.publish(self.steer)
self.rate.sleep()
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "I heard %f", data.value)
h = Header()
h.stamp = Rospy.Time.now()
mensaje = NormalizedSteeringCommand()
mensaje.header = h
mensaje.value = data.value
if data.value < 0:
mensaje.value = data.value * -1
if data.value > 0:
mensaje.value = data.value * -1
pub.publish(mensaje)
def __init__(self):
rospy.init_node("initSpeed", anonymous=True)
self.speed = SpeedCommand()
self.steer = NormalizedSteeringCommand()
self.position = Odometry()
self.speed_pub = rospy.Publisher("/actuators/speed",
SpeedCommand,
queue_size=10)
self.steer_pub = rospy.Publisher("/actuators/steering_normalized",
NormalizedSteeringCommand,
queue_size=10)
self.position_sub = rospy.Subscriber("/communication/gps/9",
Odometry,
self.on_position,
queue_size=10)
self.error_counter = 0
self.rate = rospy.Rate(10)
self.Kp = 0.0
self.Ki = 0.0
self.Kd = 0.0
self.current_time = time.time()
self.last_time = self.current_time
self.error_time = 0.0
self.error = 0.0
self.P = 0.0
self.I = 0.0
self.D = 0.0
self.PID = 0.0
self.max_integrator = 5.0
self.min_integrator = -5.0
self.speed_value = 0.5
self.steering_value = 0.0
while not rospy.is_shutdown():
self.drive()
pass
rospy.spin()
def __init__(self):
self.map = Map()
rospy.init_node("map_visualization")
self.steering = SteeringCommand()
self.steering_normalized = NormalizedSteeringCommand()
self.position = Odometry()
self.lane_pub = rospy.Publisher("lane", Marker, queue_size=10)
self.position_sub = rospy.Subscriber("/sensors/localization/filtered_map", Odometry, self.on_position,
queue_size=1)
self.pid_pub = rospy.Publisher("/control/steering", SteeringCommand, queue_size=1)
self.lane_switch = rospy.Subscriber("/control/lane_switch", UInt8, self.lane_switch, queue_size=1)
self.current_line = self.map.lanes[0]
self.rate = rospy.Rate(1)
while not rospy.is_shutdown():
i = 0
for lane in self.map.lanes:
msg = Marker(type=Marker.LINE_STRIP, action=Marker.ADD)
msg.header.frame_id = "map"
msg.scale.x = 0.01
msg.scale.y = 0.01
msg.color.r = 1.0
msg.color.a = 1.0
msg.id = i
for i in range(int(lane.length() * 100.0)):
inter = lane.interpolate(i / 100.0)
msg.points.append(Point(inter[0], inter[1], 0.0))
i += 1
self.lane_pub.publish(msg)
self.rate.sleep()
def simple_parking(self):
# here comes the values I will asign to the topics
# definition of message for speed
velocidad = NormalizedSpeedCommand()
# definition of message for steering
direccion = NormalizedSteeringCommand()
rospy.sleep(1.0)
#here are the values assigned to speed amd steering
# first values
velocidad.value = -0.2
direccion.value = 0.7
self.vel_pub.publish(velocidad)
self.dir_pub.publish(direccion)
# use this values for this period of time
rospy.sleep(2.0)
# second values
velocidad.value = -0.2
direccion.value = -0.7
self.vel_pub.publish(velocidad)
self.dir_pub.publish(direccion)
# use this values for this period of time
rospy.sleep(1.5)
#third values
velocidad.value = 0.2
direccion.value = 0.2
self.vel_pub.publish(velocidad)
self.dir_pub.publish(direccion)
rospy.sleep(1.0)
# for stop, in this program the stop was by using ctrl+c
velocidad.value = 0.0
direccion.value = 0.0
self.vel_pub.publish(velocidad)
self.dir_pub.publish(direccion)
rospy.sleep(10.0)
def callback(self, data):
dt = (data.header.stamp - self.last_time).to_sec()
# 25hz
if dt < 0.04:
return
self.last_time = data.header.stamp
x = data.pose.pose.position.x
y = data.pose.pose.position.y
orientation_q = data.pose.pose.orientation
orientation_list = [
orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w
]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
x_index_floor = int(math.floor(x * (100.0 / self.resolution)))
y_index_floor = int(math.floor(y * (100.0 / self.resolution)))
x_index_ceil = x_index_floor + 1
y_index_ceil = y_index_floor + 1
ceil_ratio_x = x * (100.0 / self.resolution) - x_index_floor
ceil_ratio_y = y * (100.0 / self.resolution) - y_index_floor
if x_index_floor < 0:
x_index_floor = 0
if x_index_floor > self.map_size_x / self.resolution - 1:
x_index_floor = self.map_size_x / self.resolution - 1
if y_index_floor < 0:
y_index_floor = 0
if y_index_floor > self.map_size_y / self.resolution - 1:
y_index_floor = self.map_size_y / self.resolution - 1
if x_index_ceil < 0:
x_index_ceil = 0
if x_index_ceil > self.map_size_x / self.resolution - 1:
x_index_ceil = self.map_size_x / self.resolution - 1
if y_index_ceil < 0:
y_index_ceil = 0
if y_index_ceil > self.map_size_y / self.resolution - 1:
y_index_ceil = self.map_size_y / self.resolution - 1
x3_floor, y3_floor = self.matrix[x_index_floor, y_index_floor, :]
x3_ceil, y3_ceil = self.matrix[x_index_ceil, y_index_ceil, :]
x3 = x3_floor * (1.0 - ceil_ratio_x) + x3_ceil * ceil_ratio_x
y3 = y3_floor * (1.0 - ceil_ratio_y) + y3_ceil * ceil_ratio_y
f_x = np.cos(yaw) * x3 + np.sin(yaw) * y3
f_y = -np.sin(yaw) * x3 + np.cos(yaw) * y3
angle = np.arctan2(f_y, f_x)
self.integral_error = self.integral_error + angle * dt
steering = self.Kp * angle + self.Kd * (
(angle - self.last_angle) / dt) + self.Ki * self.integral_error
self.last_angle = angle
if f_x > 0:
speed = -self.speed_value
else:
speed = self.speed_value
if steering > 1:
steering = 1
if steering < -1:
steering = -1
if f_x > 0:
speed = max(self.speed_value,
speed * ((np.pi / 3) / (abs(steering) + 1)))
steerMsg = NormalizedSteeringCommand()
steerMsg.value = steering
steerMsg.header.stamp = rospy.Time.now()
self.pub.publish(steerMsg)
if not self.shutdown_:
msg = NormalizedSpeedCommand()
msg.value = speed
msg.header.stamp = rospy.Time.now()
self.pub_speed.publish(msg)
