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 },
]
}
