def set_bg_pose(self, pose_data): ##---Translate bg pose from (lat, lon, alt), (quat) to -> H_s = [ R, (lat, lon, alt ] print "set genius geopose" #--- swm.run('set genius geopose %.6f %.6f %.2f %.2f %.2f %.2f %.2f' % (pose_data.position.x, pose_data.position.y, pose_data.position.z, pose_data.orientation.w, pose_data.orientation.x, pose_data.orientation.y, pose_data.orientation.z))
currentLon = 0 currentAlt = 0 def updateAttitude(data): global currentRoll, currentPitch, currentYaw currentRoll = data.roll * math.pi/180.0 currentPitch = data.pitch * math.pi/180.0 currentYaw = data.yaw * math.pi/180.0 def updatePosition(data): global currentLat, currentLon, currentAlt currentLat = data.latitude currentLon = data.longitude currentAlt = data.altitude if __name__ == '__main__': try: rospy.init_node('dcm_com') print "[dcm_com:] initialising %s data on DCM database ..." % (agentName) swm.run('set donkey geopose 0 0 0 0 0 0 1') rospy.Subscriber("mti/filter/orientation",orientationEstimate,updateAttitude) rospy.Subscriber("mti/filter/position",positionEstimate,updatePosition) rate = rospy.Rate(0.2) while not rospy.is_shutdown(): currentQuaternion = tf.transformations.quaternion_from_euler(currentRoll, currentPitch, currentYaw, "sxyz") swm.run('set donkey geopose %.6f %.6f %.2f %.2f %.2f %.2f %.2f' % (currentLat, currentLon, currentAlt, currentQuaternion[0], currentQuaternion[1], currentQuaternion[2], currentQuaternion[3])) print "[dcm_com:] the current geopose of the donkey in the DCM is %.6f,%.6f %.2f %.2f %.2f %.2f %.2f" % (currentLat, currentLon, currentAlt, currentQuaternion[0], currentQuaternion[1], currentQuaternion[2], currentQuaternion[3]) rate.sleep() except rospy.ROSInterruptException: pass
# version with ACK maessage # swm.updateSWM(transforUpdateMsg) print "[{}]: position".format(self.name) print "[{},{},{}]".format(self.current_pose[0][3],self.current_pose[1][3],self.current_pose[2][3]) print "battery level: [{} V]".format(self.battery_voltage) print "artva signal: [{} ]".format(self.artva_signal) print "#####" time.sleep(1.0/self.send_freq) if __name__ == '__main__': # 1.) python swm.py intialize swm.run("initialize") # 2.) load_map # Get correct ids 15142 swm.initCom() objectAttribute = "gis:origin" objectValue = "wgs84" getOrigin = { "@worldmodeltype": "RSGQuery", "query": "GET_NODES", "attributes": [ {"key": objectAttribute, "value": objectValue }, ] }