def callback(data):
SampleWin = 0.25
SignalMinL = 1.0
SignalMaxL = 0.0
SignalMinR = 1.0
SignalMaxR = 0.0
threshold = 0.7
msg = vect_msg()
pub = rospy.Publisher('mic_vect', vect_msg, queue_size=10)
start = time.time()
while (time.time() - start) < SampleWin:
valL = float(data.data[0] / 1024.0) # Read left mic
valR = float(data.data[1] / 1024.0) # Read left mic
if (valL) == None:
rospy.logwarn("Left Mic acquisition failed")
valL = 0
if (valR) == None:
rospy.logwarn("Right Mic acquisition failed")
valR = 0
# Processing left mic
if (valL > SignalMaxL):
SignalMaxL = valL
elif valL < SignalMinL:
SignalMinL = valL
# Processing right mic
if (valR > SignalMaxR):
SignalMaxR = valR
elif valR < SignalMinR:
SignalMinR = valR
pktopkL = valL
pktopkR = valR
# rospy.loginfo(pktopkL)
if (SignalMaxL > threshold or SignalMaxR > threshold):
value = 1
else:
value = 0
if pktopkL > 0.65:
rospy.loginfo('Left Calmp')
SignalMinL = 1.0
SignalMaxL = 0.0
if pktopkR > 0.65:
rospy.loginfo('Right Calmp')
SignalMinR = 1.0
SignalMaxR = 0.0
angle = (pktopkL - pktopkR) * 180 / np.pi
msg.angle = angle
msg.value = value
msg.header.stamp = rospy.Time.now()
pub.publish(msg)
rospy.loginfo('Microphone vector sent')
def callback(data):
data_light = [data.data[0], data.data[1]]
lux = compute_lux(data_light)
msg = vect_msg()
#TODO define here a behavior according to the lux value
angle = 0
# Fill vector message
msg.angle = angle
msg.value = lux
msg.header.stamp = rospy.Time.now()
pub = rospy.Publisher('light_vect', vect_msg, queue_size=10)
pub.publish(msg)
rospy.loginfo('Light vector sent')
def navigator():
# Intialize node and topic
pub = rospy.Publisher('navigator_vect', vect_msg, queue_size=10)
rospy.init_node('navigator_behaviour', anonymous=True)
rate = rospy.Rate(10) # 10hz
msg = vect_msg()
while not rospy.is_shutdown():
angle = 0
value = 0.8
vect = [angle,value]
msg.header.stamp = rospy.Time.now()
msg.angle = angle
msg.value = value
rospy.loginfo('Navigator vector data sent')
pub.publish(msg)
rate.sleep()
def callback(data):
angle = 0.
force = 0.
msg = vect_msg()
forceL = data.data[0] / 1024.0
forceR = data.data[1] / 1024.0
#TODO define here a behavior according to the lux value
angle = (forceL - forceR) * 180 / 1023
force = (forceL + forceR) / 2
# Fill vector message
msg.angle = angle
msg.value = force
msg.header.stamp = rospy.Time.now()
pub = rospy.Publisher('force_vect', vect_msg, queue_size=10)
pub.publish(msg)
rospy.loginfo('Force vector sent')
import rospy
from lattepanda_behaviours.msg import vect_msg
import numpy
import message_filters
import vector_fcn as vectfcn
import os
os.environ['MAVLINK20']='1' #set mavlink2 for odometry message
from pymavlink import mavutil
# from drawnow import drawnow, figure
from nav_msgs.msg import Odometry
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import numpy as np
connection = mavutil.mavlink_connection("udp:192.168.1.10:8150",input=False)
msg = vect_msg()
# TODO define mavlink message to send a vector instead of Odometry Msg
def callback(nav_sub,mic_sub,rs_sub,force_sub,light_sub,pub):
# Collecting vectors
vect_a = [mic_sub.angle, mic_sub.value]
vect_b = [nav_sub.angle, nav_sub.value]
vect_c = [rs_sub.angle, rs_sub.value]
vect_d = [force_sub.angle, force_sub.value]
vect_e = [light_sub.angle, light_sub.value]
# Sum vectors
bb_vect = vectfcn.sum_vect(vectfcn.sum_vect(vectfcn.sum_vect(vectfcn.sum_vect(vect_a,vect_b),vect_c),vect_d),vect_e)
rospy.loginfo(bb_vect)