コード例 #1
0
    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)) 
コード例 #2
0
    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,
        ],