maxX = 320
minY = 140
maxY = 470
#280
#360
#170
#470
difference = maxX - minX
differenceY = maxY - minY
prevAngle = 0
seeingOrange = 0
# Create a session to send and receive messages from a running OD4Session;
# Replay mode: CID = 253
# Live mode: CID = 112
# TODO: Change to CID 112 when this program is used on Kiwi.
session = OD4Session.OD4Session(cid=112)
# Register a handler for a message; the following example is listening
# for messageID 1039 which represents opendlv.proxy.DistanceReading.
# Cf. here: https://github.com/chalmers-revere/opendlv.standard-message-set/blob/master/opendlv.odvd#L113-L115
messageIDDistanceReading = 1039
session.registerMessageCallback(
messageIDDistanceReading, onDistance,
opendlv_standard_message_set_v0_9_6_pb2.opendlv_proxy_DistanceReading)
# Connect to the network session.
session.connect()
################################################################################
# The following lines connect to the camera frame that resides in shared memory.
# This name must match with the name used in the h264-decoder-viewer.yml file.
name = "/tmp/img.argb"
# Obtain the keys for the shared memory and semaphores.
#!/usr/bin/env python2
# Copyright (C) 2018 Christian Berger
#
# This Source Code Form is subject to the terms of the Mozilla Public
# License, v. 2.0. If a copy of the MPL was not distributed with this
# file, You can obtain one at http://mozilla.org/MPL/2.0/.
# This is the Python version of libcluon's OD4Session:
import OD4Session
# This is our example message specification.
import MyExampleMessageSpec_pb2
# "Main" part.
session = OD4Session.OD4Session(
cid=253) # Connect to running OD4Session at CID 253.
session.connect()
# Create test message.
testMessage = MyExampleMessageSpec_pb2.my_TestMessage()
testMessage.attribute11 = "Hello Python World!"
session.send(30005, testMessage.SerializeToString())
session.run()
def start_microservice():
global local_pos_h, local_pos_x, local_pos_y
verbose = True
# create a default connection id for libcluon
cid = 112
# Create a session to send and receive messages from a running OD4Session;
session = OD4Session.OD4Session(cid=112)
# recive
session.registerMessageCallback(
5321, control_signals, porpoise_message_set.opendlv_proxy_ControlBoat)
# Connect to the network session.
session.connect()
if verbose:
print("Simulator")
# define messages
msg_waypoints = porpoise_message_set.opendlv_proxy_Waypoints()
msg_pose_data = porpoise_message_set.opendlv_proxy_Pose()
msg_mission = porpoise_message_set.opendlv_proxy_Mission()
# send mission
msg_mission.target_speed = target_speed
msg_mission.mission_type_controller = mission_type_controller
msg_mission.mission_type_pathplanner = mission_type_pathplanner
session.send(5325, msg_mission.SerializeToString())
# turn waypoints to local
rot_m = np.array([[np.cos(-boat_heading), -np.sin(-boat_heading)],
[np.sin(-boat_heading),
np.cos(-boat_heading)]])
waypoints_local = waypoints_global - np.array([[boat_pos_x], [boat_pos_y]])
waypoints_local = rot_m.dot(waypoints_local)
# send first batch of pose data
init_boat_pos_y = boat_pos_y
init_boat_pos_x = boat_pos_x
init_boat_heading = boat_heading
#msg_pose_data.init_pos_x = init_boat_pos_x
#msg_pose_data.init_pos_y = init_boat_pos_y
#msg_pose_data.init_yaw = init_boat_heading
#msg_pose_data.init_roll = 0
#msg_pose_data.init_pitch = 0
msg_pose_data.easting = boat_pos_x
msg_pose_data.northing = boat_pos_y
msg_pose_data.yaw = boat_heading
msg_pose_data.pitch = 0
msg_pose_data.roll = 0
msg_pose_data.velocity = 0
# Send message on id
session.send(5324, msg_pose_data.SerializeToString())
# send waypoints
for i in range(num_waypoints):
msg_waypoints.list_index = i
msg_waypoints.coord_x = waypoints_local[0, i]
msg_waypoints.coord_y = waypoints_local[1, i]
msg_waypoints.is_last_message = 0
if i == num_waypoints - 1:
msg_waypoints.is_last_message = 1
session.send(5323, msg_waypoints.SerializeToString())
time.sleep(delta_T)
fig, ax = plt.subplots()
ax.axis([x_min, x_max, y_min, y_max])
iter = 0
old_path = np.array([[init_boat_pos_x], [init_boat_pos_y]])
# sim loop
while True:
iter += 1
# take sim step
local_speed_y = sim_step()
print("sim running: speed forward", local_speed_y)
#if verbose:
#msg_pose_data.init_pos_x = init_boat_pos_x
#msg_pose_data.init_pos_y = init_boat_pos_y
#msg_pose_data.init_yaw = init_boat_heading
#msg_pose_data.init_roll = 0
#msg_pose_data.init_pitch = 0
msg_pose_data.easting = boat_pos_x
msg_pose_data.northing = boat_pos_y
msg_pose_data.yaw = boat_heading
msg_pose_data.pitch = 0
msg_pose_data.roll = 0
msg_pose_data.velocity = local_speed_y
# Send message on id
session.send(5324, msg_pose_data.SerializeToString())
if iter % (5 / delta_T) == 0:
# turn waypoints to local
rot_m_ = np.array([[np.cos(-boat_heading), -np.sin(-boat_heading)],
[np.sin(-boat_heading),
np.cos(-boat_heading)]])
waypoints_local = waypoints_global - np.array([[boat_pos_x],
[boat_pos_y]])
waypoints_local = rot_m_.dot(waypoints_local)
local_pos_x = boat_pos_x
local_pos_y = boat_pos_y
local_pos_h = boat_heading
# send waypoints
for i in range(num_waypoints):
msg_waypoints.list_index = i
msg_waypoints.coord_x = waypoints_local[0, i]
msg_waypoints.coord_y = waypoints_local[1, i]
msg_waypoints.is_last_message = 0
if i == num_waypoints - 1:
msg_waypoints.is_last_message = 1
session.send(5323, msg_waypoints.SerializeToString())
# plot
if iter % (0.1 / delta_T) == 0:
ax.plot(waypoints_global[0, :], waypoints_global[1, :], '-*k')
plt.xlim(x_min, x_max)
plt.ylim(y_min, y_max)
#ax.plot(waypoints_local[0,:], waypoints_local[1,:], '-*k')
ax.axis('equal')
ax.plot(boat_pos_x, boat_pos_y, '*b')
rot_m_boat = np.array(
[[np.cos(boat_heading), -np.sin(boat_heading)],
[np.sin(boat_heading),
np.cos(boat_heading)]])
new_boat = rot_m_boat.dot(boat_points) + np.array([[boat_pos_x],
[boat_pos_y]])
#rot_m_boat = np.array([[np.cos(boat_heading - local_pos_h), - np.sin(boat_heading-local_pos_h)],[np.sin(boat_heading-local_pos_h), np.cos(boat_heading-local_pos_h)]])
#rot_m_boat_ = np.array([[np.cos(-local_pos_h), - np.sin(-local_pos_h)],[np.sin(-local_pos_h), np.cos(-local_pos_h)]])
#new_boat = rot_m_boat.dot(boat_points) + rot_m_boat_.dot(np.array([[boat_pos_x-local_pos_x],[boat_pos_y-local_pos_y]]))
ax.axis('equal')
ax.plot(new_boat[0, :], new_boat[1, :], '-r')
# Wait delta_T seconds before next reading
new_pos = np.array([[boat_pos_x], [boat_pos_y]])
old_path = np.append(old_path, new_pos, axis=1)
ax.plot(old_path[0, :], old_path[1, :], '*b')
plt.pause(delta_T)
ax.clear()
time.sleep(delta_T)
else:
detector = None
C_angle = angle_controller(640.0 / 2.0, 30.0 / 320.0)
C_speed = speed_controller(0.35, 0.15 / 0.30, 0.1, 10, 0.05)
Lp_dist = lowpass(0.2)
if local:
if mode == 'calibrate':
hsv_cal = hsv_calibrate('src/hsv_thresh.npz')
CID = 253
else:
CID = 112
# TODO: Change to CID 112 when this program is used on Kiwi.
session = OD4Session.OD4Session(CID)
# Register a handler for a message; the following example is listening
# for messageID 1039 which represents opendlv.proxy.DistanceReading.
# Cf. here: https://github.com/chalmers-revere/opendlv.standard-message-set/blob/master/opendlv.odvd#L113-L115
messageIDDistanceReading = 1039
session.registerMessageCallback(
messageIDDistanceReading, onDistance,
opendlv_standard_message_set_v0_9_6_pb2.opendlv_proxy_DistanceReading)
# Connect to the network session.
session.connect()
################################################################################
# The following lines connect to the camera frame that resides in shared memory.
# This name must match with the name used in the h264-decoder-viewer.yml file.
name = "/tmp/img.argb"
# Obtain the keys for the shared memory and semaphores.
:return: nothing, writes in message_info dictionary
"""
logger.debug(f"Received distance; senderStamp= {senderStamp}")
logger.debug(
f"sent: {timeStamps[0]}, received: {timeStamps[1]}, sample time stamps: {timeStamps[2]}"
)
logger.debug(msg)
if senderStamp == 0:
message_info["some"] = msg.distance
# distance is the name of a entry in the msg
if senderStamp == 1:
message_info["date"] = msg.distance
# setup UPD multicast
session = OD4Session.OD4Session(cid=own_cid)
session.registerMessageCallback(incoming_id_1, process_message,
incoming_type_1)
session.connect()
# setup shared memory ###########################################################
keySharedMemory = sysv_ipc.ftok(file_name, 1, True)
keySemMutex = sysv_ipc.ftok(file_name, 2, True)
keySemCondition = sysv_ipc.ftok(file_name, 3, True)
shm = sysv_ipc.SharedMemory(keySharedMemory)
mutex = sysv_ipc.Semaphore(keySemCondition)
cond = sysv_ipc.Semaphore(keySemCondition)
################################################################################
# initializations
cX = yellow_hits[i][0]
cY = yellow_hits[i][1]
cv2.circle(image, (cX, cY), 3, (0, 255, 0), -1)
cv2.putText(image, "center", (cX - 10, cY - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
print("Number of blue: " + str(len(blue_hits)))
print("Number of yellow: " + str(len(yellow_hits)))
return blue_hits, yellow_hits, image
# Create a session to send and receive messages from a running OD4Session;
# Replay mode: CID = 253
# Live mode: CID = 112
# TODO: Change to CID 112 when this program is used on Kiwi.
cid = 253
session = OD4Session.OD4Session(cid)
# Register a handler for a message; the following example is listening
# for messageID 1039 which represents opendlv.proxy.DistanceReading.
# Cf. here: https://github.com/chalmers-revere/opendlv.standard-message-set/blob/master/opendlv.odvd#L113-L115
messageIDDistanceReading = 1039
session.registerMessageCallback(
messageIDDistanceReading, onDistance,
opendlv_standard_message_set_v0_9_6_pb2.opendlv_proxy_DistanceReading)
# Connect to the network session.
session.connect()
################################################################################
# The following lines connect to the camera frame that resides in shared memory.
# This name must match with the name used in the h264-decoder-viewer.yml file.
name = "/tmp/img.argb"
# Obtain the keys for the shared memory and semaphores.
# Parsing options
for opt, arg in opts:
if opt in ("--cid"):
cid = arg
elif opt in ("-v", "--verbose"):
verbose = True
elif opt in ("-d", "--debug"):
verbose = True
debug = True
if verbose:
print "\n--- Microservices Monitor ---\n"
# Create a session to send and receive messages from a running OD4Session;
session = OD4Session.OD4Session(int(cid))
# Connect to the network session.
session.connect()
# Create a docker client from environmental variables
dockerClient = docker.from_env()
################################################################################
while True:
timeBegin = time.time()
if verbose:
print "\n[" + datetime.datetime.fromtimestamp(
time.time()).strftime('%H:%M:%S') + "]"
microserviceStats = opendlv_messages.opendlv_system_MicroserviceStatistics(
print msg
if senderStamp == 0:
distances["front"] = msg.distance
if senderStamp == 1:
distances["left"] = msg.distance
if senderStamp == 2:
distances["rear"] = msg.distance
if senderStamp == 3:
distances["right"] = msg.distance
# Create a session to send and receive messages from a running OD4Session;
# Replay mode: CID = 253
# Live mode: CID = 112
# TODO: Change to CID 112 when this program is used on Kiwi.
session = OD4Session.OD4Session(cid=253)
# Register a handler for a message; the following example is listening
# for messageID 1039 which represents opendlv.proxy.DistanceReading.
# Cf. here: https://github.com/chalmers-revere/opendlv.standard-message-set/blob/master/opendlv.odvd#L113-L115
messageIDDistanceReading = 1039
session.registerMessageCallback(
messageIDDistanceReading, onDistance,
opendlv_standard_message_set_v0_9_6_pb2.opendlv_proxy_DistanceReading)
# Connect to the network session.
session.connect()
################################################################################
# The following lines connect to the camera frame that resides in shared memory.
# This name must match with the name used in the h264-decoder-viewer.yml file.
name = "/tmp/img.argb"
# Obtain the keys for the shared memory and semaphores.
def getDistance(height):
k1 = 62287
k2 = -0.973
return k1 * math.pow(height, k2)
# Create a session to send and receive messages from a running OD4Session;
# Replay mode: CID = 253
# Live mode: CID = 112
# TODO: Change to CID 112 when this program is used on Kiwi.
cid_value = 112
if car == 0:
cid_value = 112
elif car == 1:
cid_value = 111
session = OD4Session.OD4Session(cid=cid_value)
# Register a handler for a message; the following example is listening
# for messageID 1039 which represents opendlv.proxy.DistanceReading.
# Cf. here: https://github.com/chalmers-revere/opendlv.standard-message-set/blob/master/opendlv.odvd#L113-L115
#not needed for big car
if car == 0:
messageIDDistanceReadingUltraSonic = 1039
session.registerMessageCallback(
messageIDDistanceReadingUltraSonic, ultraSonicRead,
opendlv_standard_message_set_v0_9_6_pb2.opendlv_proxy_DistanceReading)
messageIDDistanceReadingInfraRed = 1037
session.registerMessageCallback(
messageIDDistanceReadingInfraRed, infraRedRead,
opendlv_standard_message_set_v0_9_6_pb2.opendlv_proxy_VoltageReading)
def start_microservice():
global boat_pos_y, boat_pos_x, boat_heading, boat_ang_vel, boat_speed_y, boat_speed_x
verbose = True
# create a default connection id for libcluon
cid = 112
# Create a session to send and receive messages from a running OD4Session;
session = OD4Session.OD4Session(cid=112)
# recive
session.registerMessageCallback(
5321, control_signals, porpoise_message_set.opendlv_proxy_ControlBoat)
# Connect to the network session.
session.connect()
if verbose:
print("Simulator")
# define messages
msg_waypoints = porpoise_message_set.opendlv_proxy_Waypoints()
msg_pose_data = porpoise_message_set.opendlv_proxy_Pose()
msg_mission = porpoise_message_set.opendlv_proxy_Mission()
# send mission
msg_mission.target_speed = target_speed
msg_mission.mission_type_controller = 0
msg_mission.mission_type_pathplanner = 3
session.send(5325, msg_mission.SerializeToString())
msg_mission.target_speed = target_speed
msg_mission.mission_type_controller = mission_type_controller
msg_mission.mission_type_pathplanner = mission_type_pathplanner
session.send(5325, msg_mission.SerializeToString())
# send first batch of pose data
msg_pose_data.easting = boat_pos_x + np.random.normal(0, gps_est_std)
msg_pose_data.northing = boat_pos_y + np.random.normal(0, gps_est_std)
msg_pose_data.yaw = boat_heading + np.random.normal(0, heading_std)
msg_pose_data.pitch = 0
msg_pose_data.roll = 0
msg_pose_data.velocity = 0
# Send message on id
session.send(5324, msg_pose_data.SerializeToString())
# plot
fig1, ax1 = plt.subplots()
x_L, y_L, gps_zone_num, gps_zone_letter = utm.from_latlon(
57.698356, 11.932705)
y_L += 50
x_L -= 20
scale_img = 2.53
imgplot = ax1.imshow(
img,
aspect='equal',
origin='upper',
extent=[x_L, x_L + 915 * scale_img, y_L, y_L + 539 * scale_img])
ax1.plot(waypoints_global[0, :], waypoints_global[1, :], '-*k')
plt.pause(delta_T)
iter = 0
#path_save = np.array([[boat_pos_x],[boat_pos_y], [boat_heading], [0]])
error_dist = np.array([])
# sim loop
while True:
start = time.clock()
# take sim step
local_speed_y = sim_step()
print("sim running: speed forward", local_speed_y)
# calc noisy poistion from true position + noise
boat_pos_x_noise = boat_pos_x + np.random.normal(0, gps_est_std)
boat_pos_y_noise = boat_pos_y + np.random.normal(0, gps_north_std)
boat_heading_noise = boat_heading + np.random.normal(0, heading_std)
#pose msg
msg_pose_data.easting = boat_pos_x_noise
msg_pose_data.northing = boat_pos_y_noise
msg_pose_data.yaw = boat_heading_noise
msg_pose_data.pitch = 0
msg_pose_data.roll = 0
msg_pose_data.velocity = local_speed_y + np.random.normal(0, speed_std)
# Send message on id
session.send(5324, msg_pose_data.SerializeToString())
new_pose = np.array([[boat_pos_x], [boat_pos_y], [boat_heading],
[local_speed_y]])
#path_save = np.append(path_save,new_pose, axis=1)
if iter % (5 / delta_T) == 0:
# turn waypoints to local
rot_m_ = np.array(
[[np.cos(-boat_heading_noise), -np.sin(-boat_heading_noise)],
[np.sin(-boat_heading_noise),
np.cos(-boat_heading_noise)]])
waypoints_local = waypoints_global - np.array([[boat_pos_x_noise],
[boat_pos_y_noise]])
waypoints_local = rot_m_.dot(waypoints_local)
# send waypoints
for i in range(num_waypoints):
msg_waypoints.list_index = i
msg_waypoints.coord_x = waypoints_local[0, i]
msg_waypoints.coord_y = waypoints_local[1, i]
msg_waypoints.is_last_message = 0
if i == num_waypoints - 1:
msg_waypoints.is_last_message = 1
if mission_type_pathplanner == 1 or mission_type_pathplanner == 0:
session.send(5323, msg_waypoints.SerializeToString())
# plot
if iter % (1 / delta_T) == 0:
rot_m_boat = np.array(
[[np.cos(boat_heading), -np.sin(boat_heading)],
[np.sin(boat_heading),
np.cos(boat_heading)]])
new_boat = rot_m_boat.dot(boat_points) + np.array([[boat_pos_x],
[boat_pos_y]])
boat_plt = ax1.plot(new_boat[0, :], new_boat[1, :], '-r')
# save path into array if needed
new_pos = np.array([[boat_pos_x], [boat_pos_y]])
#old_path = np.append(old_path,new_pos, axis=1)
if iter % (10 / delta_T) == 0:
ax1.plot(boat_pos_x, boat_pos_y, '*b')
plt.pause(delta_T / 10)
for i in range(len(boat_plt)):
boat_plt[i].remove()
# save error
distance_ = closet_dist_to_lines(waypoints_global, new_pos)
error_dist = np.append(error_dist, distance_)
time_diff = time.clock() - start
sleep_t = delta_T - time_diff
if sleep_t > 0:
time.sleep(sleep_t)
iter += 1
if np.sqrt((boat_pos_x - waypoints_global[0, num_waypoints - 1])**2 +
(boat_pos_y - waypoints_global[1, num_waypoints - 1])**2
) < target_treashold:
boat_plt = ax1.plot(new_boat[0, :], new_boat[1, :], '-r')
print('Mission done')
print('mean error', np.mean(error_dist))
print('max error', np.max(error_dist))
#np.save('Path_data', path_save)
plt.show()
break