Commit d2944ce7 authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

changed datatypes to arrays, added publishers, publishers untested

parent 9faf465f
......@@ -32,6 +32,7 @@ import rosbag
from rospkg import RosPack
from std_msgs.msg import Float32
from std_msgs.msg import String
from std_msgs.msg import Float32MultiArray
# Add library
#sys.path.append("lib")
......@@ -154,43 +155,24 @@ class PPSRadioClient:
self.change_status_to(DISCONNECTED)
def _anchorDist_received_callback(self, timestamp, data, anchorDist):
distance1 = Float32()
distance2 = Float32()
distance3 = Float32()
distance4 = Float32()
distance5 = Float32()
distance6 = Float32()
distance1.data = data["ranging.distance1"]
distance2.data = data["ranging.distance2"]
distance3.data = data["ranging.distance3"]
distance4.data = data["ranging.distance4"]
distance5.data = data["ranging.distance5"]
distance6.data = data["ranging.distance6"]
#TODO: Add Publisher
distances = Float32MultiArray()
distances.data = [data["ranging.distance1"],data["ranging.distance2"],data["ranging.distance3"],data["ranging.distance4"],data["ranging.distance5"],data["ranging.distance6"]]
distances_pub.publish(distances)
def _xyzrpy_received_callback(self, timestamp, data, xyzrpy):
#coordX = Float32()
#coordY = Float32()
#coordZ = Float32()
#arrXYZ = Float32MultiArray()
#arrXYZ.data = [data["xxx.x"], fdata["xxx.y"],data["xxx.z"]]
#xyz_pub.publish(arrXYZ)
stabilizerYaw = Float32()
stabilizerPitch = Float32()
stabilizerRoll = Float32()
arrRPY = Float32MultiArray()
arrRPY.data = [data["stabilizer.roll"],data["stabilizer.pitch"],data["stabilizer.yaw"]]
#coordX.data = data["xxx.x"]
#coordY.data = data["xxx.y"]
#coordY.data = data["xxx.y"]
stabilizerYaw.data = data["stabilizer.yaw"]
stabilizerPitch.data = data["stabilizer.pitch"]
stabilizerRoll.data = data["stabilizer.roll"]
rpy_pub.publish(arrRPY)
#TODO: Add Publisher
#print arrRPY
#print "roll: %s" % stabilizerRoll
#print "pitch: %s" % stabilizerPitch
#print "yaw: %s" %stabilizerYaw
#def _data_received_callback(self, timestamp, data, logconf):
def _vbat_received_callback(self, timestamp, data, batLog):
......@@ -418,6 +400,15 @@ if __name__ == '__main__':
global cfbattery_pub
cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
global rpy_pub
rpy_pub = rospy.Publisher(node_name + '/RPY', Float32MultiArray, queue_size=10)
#global xyz_pub
#xyz_pub = rospy.Publisher(node_name + '/XYZ', Float32MultiArray, queue_size=10)
#global distances_pub
#distances_pub = rospy.Publisher(node_name + '/distances', Float32MultiArray, queue_size=10)
global cf_client
cf_client = PPSRadioClient()
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment