def __init__(self, scene_setup_is_done = False, port = "22422", root_uuid = None, name = "robot", send_freq = 10, max_vel = 0.001, curr_pose = None, goal_pose = None, origin_uuid = None , rob_uuid = None, tranform_origin_rob_uuid = None, detected_victim_group_uuid = None):
Thread.__init__(self)
self.active = True
self.scene_setup_is_done = scene_setup_is_done
self.port = port #"22422"
if root_uuid:
self.root_uuid = root_uuid #"853cb0f0-e587-4880-affe-90001da1262d"
else:
print "create random root_uuid"
self.root_uuid = str(uuid.uuid4())
self.rob_name = name #"robot"
self.send_freq = send_freq #10 #in Hz
self.max_vel = max_vel # max velocity factor of robot
# for battery simulation
self.battery_uuid = "052d2a58-9566-46d7-adef-336f280baf2d" #str(uuid.uuid4())
# A wasp has a voltage range of 13.5V 16.8V
self.battery_min = 13.5 # [V]
self.battery_max = 16.8 # [V]
self.battery_discharge_per_second = 0.1 # [V/s]
self.battery_voltage = self.battery_max
# artva simulation
self.artva_uuid = "9218643f-a71d-48bc-8cf1-8a701bd5a5fd" #str(uuid.uuid4())
self.artva_signal = 0 # range [0-100] ?!?
self.artva_range = 0.0001 # sort of lat lon distance ....
if curr_pose:
self.current_pose = curr_pose
else:
print "set current pose to review setting at Champoluc"
self.current_pose = [
[1,0,0,45.84561555807046],
[0,1,0,7.72886713924574],
[0,0,1,3.0],
[0,0,0,1]
]
if goal_pose:
self.goal_pose = goal_pose
else:
print "setting goal_pose to current_pose"
self.goal_pose = self.current_pose
if origin_uuid:
self.origin_uuid = origin_uuid
else:
print "create random origin_uuid"
self.origin_uuid = str(uuid.uuid4())
if rob_uuid:
self.rob_uuid = rob_uuid
else:
print "create random rob_uuid"
self.rob_uuid = str(uuid.uuid4())
if tranform_origin_rob_uuid:
self.tranform_origin_rob_uuid = tranform_origin_rob_uuid
else:
print "create random tranform_origin_rob_uuid"
self.tranform_origin_rob_uuid = str(uuid.uuid4())
if detected_victim_group_uuid:
self.detected_victim_group_uuid = detected_victim_group_uuid
else:
print "create random detected_victim_group_uuid"
self.detected_victim_group_uuid = str(uuid.uuid4())
self.victim_pose = self.goal_pose
# # Set up the ZMQ PUB-SUB communication layer.
# self.context = zmq.Context()
# self.socket = self.context.socket(zmq.PUB)
# self.socket.bind("tcp://*:%s" % self.port)
# time.sleep(1)
if(self.scene_setup_is_done == False):
# create origin node as subgroup of root
newOriginMsg = {
"@worldmodeltype": "RSGUpdate",
"operation": "CREATE",
"node": {
"@graphtype": "Node",
"id": self.origin_uuid,
"attributes": [
{"key": "gis:origin", "value": "wgs84"},
{"key": "comment", "value": "Reference frame for geo poses. Use this ID for Transform queries."},
],
},
"parentId": self.root_uuid,
}
# self.socket.send_string(json.dumps(newOriginMsg))
print (json.dumps(newOriginMsg))
swm.updateSWM(newOriginMsg)
# create robot node
newNodeMsg = {
"@worldmodeltype": "RSGUpdate",
"operation": "CREATE",
"node": {
"@graphtype": "Group",
"id": self.rob_uuid,
"attributes": [
{"key": "shepa:agent_name", "value": self.rob_name},
],
},
"parentId": self.root_uuid,
}
# self.socket.send_string(json.dumps(newNodeMsg))
print (json.dumps(newNodeMsg))
swm.updateSWM(newNodeMsg)
# create transform for initial condition of robot
newTransformMsg = {
"@worldmodeltype": "RSGUpdate",
"operation": "CREATE",
"node": {
"@graphtype": "Connection",
"@semanticContext":"Transform",
"id": self.tranform_origin_rob_uuid,
"attributes": [
{"key": "name", "value": self.rob_name + "_geopose"},
{"key": "tf:type", "value": "wgs84"}
],
"sourceIds": [
self.origin_uuid,
],
"targetIds": [
self.rob_uuid,
],
"history" : [
{
"stamp": {
"@stamptype": "TimeStampDate",
"stamp": datetime.datetime.now().strftime('%Y-%m-%dT%H:%M:%S') + "Z"
},
"transform": {
"type": "HomogeneousMatrix44",
"matrix": self.current_pose,
"unit": "latlon"
}
}
],
},
"parentId": self.root_uuid,
}
# self.socket.send_string(json.dumps(newTransformMsg))
swm.updateSWM(newTransformMsg)
print (json.dumps(newTransformMsg))
# create node to represent the battery state
newBatteryMsg = {
"@worldmodeltype": "RSGUpdate",
"operation": "CREATE",
"node": {
"@graphtype": "Node",
"id": self.battery_uuid,
"attributes": [
{"key": "sensor:battery", "value": self.battery_voltage},
],
},
"parentId": self.rob_uuid,
}
swm.updateSWM(newBatteryMsg)
print (json.dumps(newBatteryMsg))
# sensor node for ARTVA?
newArtvaMsg = {
"@worldmodeltype": "RSGUpdate",
"operation": "CREATE",
"node": {
"@graphtype": "Node",
"id": self.artva_uuid,
"attributes": [
{"key": "sherpa:artva_signal", "value": self.artva_signal},
],
},
"parentId": self.rob_uuid,
}
swm.updateSWM(newArtvaMsg)
print (json.dumps(newArtvaMsg))
missionOriginId = "e3c33a96-9923-4eeb-9a9b-e1b57ce67f86"
newMissionOriginMsg = {
"@worldmodeltype": "RSGUpdate",
"operation": "CREATE",
"node": {
"@graphtype": "Node",
"id": missionOriginId,
"attributes": [
{"key": "sherpa:origin", "value": "initial"},
{"key": "comment", "value": "Reference frame for the mission. Typcally an initial odometry frame of one of the animals."},
],
},
"parentId": swm_mission_uuid,
}
print (json.dumps(newMissionOriginMsg))
swm.updateSWM(newMissionOriginMsg)
missionOriginGeoposeId = "86f210f0-f52a-40d6-95d0-d5f9254fb476"
newMissionOriginMsg = {
"@worldmodeltype": "RSGUpdate",
"operation": "CREATE",
"node": {
"@graphtype": "Connection",
"@semanticContext":"Transform",
"id": str(uuid.uuid4()),
"attributes": [
{"key": "tf:type", "value": "wgs84"}
],
"sourceIds": [
swm_origin_uuid,
],