class NavSpeak:
def __init__(self):
self.move_base_goal_sub = rospy.Subscriber(
"/move_base/goal",
MoveBaseActionGoal,
self.move_base_goal_callback,
queue_size=1)
self.move_base_result_sub = rospy.Subscriber(
"/move_base/result",
MoveBaseActionResult,
self.move_base_result_callback,
queue_size=1)
self.sound = SoundClient()
def move_base_goal_callback(self, msg):
self.sound.play(2)
def move_base_result_callback(self, msg):
text = "{}: {}".format(goal_status(msg.status.status), msg.status.text)
rospy.loginfo(text)
if msg.status.status == GoalStatus.SUCCEEDED:
self.sound.play(1)
elif msg.status.status == GoalStatus.PREEMPTED:
self.sound.play(2)
elif msg.status.status == GoalStatus.ABORTED:
self.sound.play(3)
else:
self.sound.play(4)
time.sleep(1)
self.sound.say(text)
def callback(self, signs):
soundhandle = SoundClient()
soundhandle.play(2)
rospy.sleep(1)
def play_nonblocking():
"""
Play the same sounds with manual pauses between them.
"""
rospy.loginfo('Example: Playing sounds in *non-blocking* mode.')
# NOTE: you must sleep at the beginning to let the SoundClient publisher
# establish a connection to the soundplay_node.
soundhandle = SoundClient(blocking=False)
rospy.sleep(1)
# In the non-blocking version you need to sleep between calls.
rospy.loginfo('Playing say-beep at full volume.')
soundhandle.playWave('say-beep.wav')
rospy.sleep(1)
rospy.loginfo('Playing say-beep at volume 0.3.')
soundhandle.playWave('say-beep.wav', volume=0.3)
rospy.sleep(1)
rospy.loginfo('Playing sound for NEEDS_PLUGGING.')
soundhandle.play(SoundRequest.NEEDS_PLUGGING)
rospy.sleep(1)
rospy.loginfo('Speaking some long string.')
soundhandle.say('It was the best of times, it was the worst of times.')
def play_nonblocking():
"""
Play the same sounds with manual pauses between them.
"""
rospy.loginfo('Example: Playing sounds in *non-blocking* mode.')
# NOTE: you must sleep at the beginning to let the SoundClient publisher
# establish a connection to the soundplay_node.
soundhandle = SoundClient(blocking=False)
rospy.sleep(1)
# In the non-blocking version you need to sleep between calls.
rospy.loginfo('Playing say-beep at full volume.')
soundhandle.playWave('say-beep.wav')
rospy.sleep(1)
rospy.loginfo('Playing say-beep at volume 0.3.')
soundhandle.playWave('say-beep.wav', volume=0.3)
rospy.sleep(1)
rospy.loginfo('Playing sound for NEEDS_PLUGGING.')
soundhandle.play(SoundRequest.NEEDS_PLUGGING)
rospy.sleep(1)
rospy.loginfo('Speaking some long string.')
soundhandle.say('It was the best of times, it was the worst of times.')
def callback(data):
if str(data) == "data: Bark":
soundhandle = SoundClient()
rospy.sleep(1)
soundhandle.play(1)
pubshoot = rospy.Publisher('bark', String)
pubshoot.publish(String("Fire"))
def mover():
global laser_range
rospy.init_node('mover', anonymous=True)
# subscribe to odometry data
rospy.Subscriber('odom', Odometry, get_odom_dir)
# subscribe to LaserScan data
rospy.Subscriber('scan', LaserScan, get_laserscan)
# subscribe to map occupancy data
rospy.Subscriber('map', OccupancyGrid, get_occupancy)
rospy.on_shutdown(stopbot)
rate = rospy.Rate(5) # 5 Hz
# save start time to file
start_time = time.time()
# initialize variable to write elapsed time to file
timeWritten = 0
# find direction with the largest distance from the Lidar,
# rotate to that direction, and start moving
pick_direction()
while not rospy.is_shutdown():
if laser_range.size != 0:
# check distances in front of TurtleBot and find values less
# than stop_distance
lri = (laser_range[front_angles] < float(stop_distance)).nonzero()
rospy.loginfo('Distances: %s', str(lri))
else:
lri[0] = []
# if the list is not empty
if (len(lri[0]) > 0):
rospy.loginfo(['Stop!'])
# find direction with the largest distance from the Lidar
# rotate to that direction
# start moving
pick_direction()
# check if SLAM map is complete
if timeWritten:
if closure(occdata):
# map is complete, so save current time into file
with open("maptime.txt", "w") as f:
f.write("Elapsed Time: " + str(time.time() - start_time))
timeWritten = 1
# play a sound
soundhandle = SoundClient()
rospy.sleep(1)
soundhandle.stopAll()
soundhandle.play(SoundRequest.NEEDS_UNPLUGGING)
rospy.sleep(2)
# save the map
cv2.imwrite('mazemap.png', occdata)
rate.sleep()
class NavSpeak:
def __init__(self):
self.move_base_goal_sub = rospy.Subscriber("/move_base/goal", MoveBaseActionGoal, self.move_base_goal_callback, queue_size = 1)
self.move_base_result_sub = rospy.Subscriber("/move_base/result", MoveBaseActionResult, self.move_base_result_callback, queue_size = 1)
self.sound = SoundClient()
self.lang = "japanese" # speak japanese by default
if rospy.has_param("/nav_speak/lang"):
self.lang = rospy.get_param("/nav_speak/lang")
self.pub = rospy.Publisher('/robotsound_jp/goal', SoundRequestActionGoal, queue_size=1)
def move_base_goal_callback(self, msg):
self.sound.play(2)
def move_base_result_callback(self, msg):
text = "{}: {}".format(goal_status(msg.status.status), msg.status.text)
rospy.loginfo(text)
if self.lang == "japanese": # speak japanese
sound_goal = SoundRequestActionGoal()
sound_goal.goal_id.stamp = rospy.Time.now()
sound_goal.goal.sound_request.sound = -3
sound_goal.goal.sound_request.command = 1
sound_goal.goal.sound_request.volume = 1.0
sound_goal.goal.sound_request.arg2 = "jp"
if msg.status.status == GoalStatus.SUCCEEDED:
self.sound.play(1)
time.sleep(1)
if self.lang == "japanese":
sound_goal.goal.sound_request.arg = "到着しました"
self.pub.publish(sound_goal)
else:
self.sound.say(text)
elif msg.status.status == GoalStatus.PREEMPTED:
self.sound.play(2)
time.sleep(1)
if self.lang == "japanese":
sound_goal.goal.sound_request.arg = "別のゴールがセットされました"
self.pub.publish(sound_goal)
else:
self.sound.say(text)
elif msg.status.status == GoalStatus.ABORTED:
self.sound.play(3)
time.sleep(1)
if self.lang == "japanese":
sound_goal.goal.sound_request.arg = "中断しました"
self.pub.publish(sound_goal)
else:
self.sound.say(text)
else:
self.sound.play(4)
time.sleep(1)
self.sound.say(text)
def play_blocking():
"""
Play various sounds, blocking until each is completed before going to the
next.
"""
rospy.loginfo('Example: Playing sounds in *blocking* mode.')
soundhandle = SoundClient(blocking=True)
rospy.loginfo('Playing say-beep at full volume.')
soundhandle.playWave('say-beep.wav')
rospy.loginfo('Playing say-beep at volume 0.3.')
soundhandle.playWave('say-beep.wav', volume=0.3)
rospy.loginfo('Playing sound for NEEDS_PLUGGING.')
soundhandle.play(SoundRequest.NEEDS_PLUGGING)
rospy.loginfo('Speaking some long string.')
soundhandle.say('It was the best of times, it was the worst of times.')
def play_blocking():
"""
Play various sounds, blocking until each is completed before going to the
next.
"""
rospy.loginfo('Example: Playing sounds in *blocking* mode.')
soundhandle = SoundClient(blocking=True)
rospy.loginfo('Playing say-beep at full volume.')
soundhandle.playWave('say-beep.wav')
rospy.loginfo('Playing say-beep at volume 0.3.')
soundhandle.playWave('say-beep.wav', volume=0.3)
rospy.loginfo('Playing sound for NEEDS_PLUGGING.')
soundhandle.play(SoundRequest.NEEDS_PLUGGING)
rospy.loginfo('Speaking some long string.')
soundhandle.say('It was the best of times, it was the worst of times.')
class Talker:
def __init__(self):
self.soundhandle = SoundClient()
rospy.sleep(1)
self.voice = 'voice_kal_diphone'
self.volume = 1.0
def say(self, txt):
print('Saying: %s' % txt)
#print 'Voice: %s' % voice
#print 'Volume: %s' % volume
self.soundhandle.say(txt, self.voice, self.volume)
rospy.sleep(1)
def play(self, num):
print('Playing sound nr.: %s' % num)
self.soundhandle.play(num, self.volume)
rospy.sleep(1)
def play_explicit():
rospy.loginfo('Example: SoundClient play methods can take in an explicit'
' blocking parameter')
soundhandle = SoundClient() # blocking = False by default
rospy.sleep(0.5) # Ensure publisher connection is successful.
sound_beep = soundhandle.waveSound("say-beep.wav", volume=0.5)
# Play the same sound twice, once blocking and once not. The first call is
# blocking (explicitly specified).
sound_beep.play(blocking=True)
# This call is not blocking (uses the SoundClient's setting).
sound_beep.play()
rospy.sleep(0.5) # Let sound complete.
# Play a blocking sound.
soundhandle.play(SoundRequest.NEEDS_UNPLUGGING, blocking=True)
# Create a new SoundClient where the default behavior *is* to block.
soundhandle = SoundClient(blocking=True)
soundhandle.say('Say-ing stuff while block-ing')
soundhandle.say('Say-ing stuff without block-ing', blocking=False)
rospy.sleep(1)
class NavSpeak:
def __init__(self):
self.move_base_goal_sub = rospy.Subscriber("/move_base/goal", MoveBaseActionGoal, self.move_base_goal_callback, queue_size = 1)
self.move_base_result_sub = rospy.Subscriber("/move_base/result", MoveBaseActionResult, self.move_base_result_callback, queue_size = 1)
self.sound = SoundClient()
def move_base_goal_callback(self, msg):
self.sound.play(2)
def move_base_result_callback(self, msg):
text = "{}: {}".format(goal_status(msg.status.status), msg.status.text)
rospy.loginfo(text)
if msg.status.status == GoalStatus.SUCCEEDED:
self.sound.play(1)
elif msg.status.status == GoalStatus.PREEMPTED:
self.sound.play(2)
elif msg.status.status == GoalStatus.ABORTED:
self.sound.play(3)
else:
self.sound.play(4)
time.sleep(1)
self.sound.say(text)
def play_explicit():
rospy.loginfo('Example: SoundClient play methods can take in an explicit'
' blocking parameter')
soundhandle = SoundClient() # blocking = False by default
rospy.sleep(0.5) # Ensure publisher connection is successful.
sound_beep = soundhandle.waveSound("say-beep.wav", volume=0.5)
# Play the same sound twice, once blocking and once not. The first call is
# blocking (explicitly specified).
sound_beep.play(blocking=True)
# This call is not blocking (uses the SoundClient's setting).
sound_beep.play()
rospy.sleep(0.5) # Let sound complete.
# Play a blocking sound.
soundhandle.play(SoundRequest.NEEDS_UNPLUGGING, blocking=True)
# Create a new SoundClient where the default behavior *is* to block.
soundhandle = SoundClient(blocking=True)
soundhandle.say('Say-ing stuff while block-ing')
soundhandle.say('Say-ing stuff without block-ing', blocking=False)
rospy.sleep(1)
class RunStopServer(object):
def __init__(self):
"""Provide dead-man-switch like server for handling wouse run-stops."""
rospy.Service("wouse_run_stop", WouseRunStop, self.service_cb)
self.run_stop = RunStop()
if DEAD_MAN_CONFIGURATION:
self.sound_client = SoundClient()
self.timeout = rospy.Duration(rospy.get_param('wouse_timeout', 1.5))
self.tone_period = rospy.Duration(10)
self.last_active_time = rospy.Time.now()
self.last_sound = rospy.Time.now()
rospy.Timer(self.timeout, self.check_receiving)
def check_receiving(self, event):
"""After timeout, check to ensure that activity is seen from wouse."""
silence = rospy.Time.now() - self.last_active_time
#print silence, " / ", self.timeout
if silence < self.timeout:
# print "Receiving"
return
#else:
# print "NOT receiving"
if (silence > self.timeout and (rospy.Time.now() - self.last_sound) > self.tone_period):
rospy.logwarn("RunStopServer has not heard from wouse recently.")
self.sound_client.play(3)#1
self.last_sound = rospy.Time.now()
def service_cb(self, req):
"""Handle service requests to start/stop run-stop. Used to reset."""
#print "Separation: ", req.time-self.last_active_time
self.last_active_time = req.time
#print "Delay: ", rospy.Time.now() - self.last_active_time
if req.stop:
return self.run_stop.stop()
elif req.start:
return self.run_stop.start()
else:
return True #only update timestamp
def get_occupancy(
msg
): #This function gets the occupancy data and pumps it to closure function to check if the map is complete
global occdata
global cnt
# create numpy array
msgdata = numpy.array(msg.data)
# compute histogram to identify percent of bins with -1
occ_counts = numpy.histogram(msgdata, occ_bins)
# calculate total number of bins
total_bins = msg.info.width * msg.info.height
# log the info
# rospy.loginfo('Unmapped: %i Unoccupied: %i Occupied: %i Total: %i', occ_counts[0][0], occ_counts[0][1], occ_counts[0][2], total_bins)
# make msgdata go from 0 instead of -1, reshape into 2D
oc2 = msgdata + 1
# reshape to 2D array using column order
occdata = numpy.uint8(
oc2.reshape(msg.info.height, msg.info.width, order='F'))
contourCheck = 1
if contourCheck:
if closure(occdata):
# map is complete, so save current time into file
with open("maptime.txt", "w") as f:
f.write("done")
contourCheck = 0
# play a sound
soundhandle = SoundClient()
rospy.sleep(1)
soundhandle.stopAll()
soundhandle.play(SoundRequest.NEEDS_UNPLUGGING)
rospy.sleep(2)
# save the map
cv2.imwrite('mazemap.png', occdata)
cnt = 2
print("Map finished, Check your map at 'mapzemap.png'.")
class CarSound(object):
"""
car sound class
"""
def __init__(self):
self.entity = car_sound_pb2.SoundRequest()
self.soundhandle = SoundClient()
self.voice = 'voice_kal_diphone'
self.lasttime = rospy.get_time()
def callback_sound(self, data):
"""
new sound request
"""
print "New Sound Msg"
self.entity.ParseFromString(data.data)
print self.entity
if self.entity.mode == car_sound_pb2.SoundRequest.SAY:
self.soundhandle.say(self.entity.words, self.voice)
elif self.entity.mode == car_sound_pb2.SoundRequest.BEEP:
self.soundhandle.play(SoundRequest.NEEDS_PLUGGING)
self.lasttime = rospy.get_time()
class CarSound(object):
"""
car sound class
"""
def __init__(self):
self.entity = car_sound_pb2.SoundRequest()
self.soundhandle = SoundClient()
self.voice = 'voice_kal_diphone'
self.lasttime = rospy.get_time()
def callback_sound(self, data):
"""
new sound request
"""
print "New Sound Msg"
self.entity.ParseFromString(data.data)
print self.entity
if self.entity.mode == car_sound_pb2.SoundRequest.SAY:
self.soundhandle.say(self.entity.words, self.voice)
elif self.entity.mode == car_sound_pb2.SoundRequest.BEEP:
self.soundhandle.play(SoundRequest.NEEDS_PLUGGING)
self.lasttime = rospy.get_time()
from sound_play.msg import SoundRequest
from sound_play.libsoundplay import SoundClient
if __name__ == '__main__':
rospy.init_node('battery_guard')
soundclient = SoundClient()
last_complain = rospy.Time()
r = rospy.Rate(.1)
while not rospy.is_shutdown():
with open('/sys/class/power_supply/BAT0/capacity') as f:
capacity = int(f.readline().rstrip())
with open('/sys/class/power_supply/BAT0/status') as f:
status = f.readline().rstrip()
if status != "Charging" and rospy.Time.now(
) - last_complain > rospy.Duration(60):
if capacity < 10:
rospy.logerr(
"No battery power remaining. Connect the robot laptop to power immediately."
)
soundclient.play(SoundRequest.NEEDS_PLUGGING_BADLY)
last_complain = rospy.Time.now()
elif capacity < 15:
rospy.logwarn(
"Only little battery power remaining. Please connect the robot laptop to power."
)
soundclient.play(SoundRequest.NEEDS_PLUGGING)
last_complain = rospy.Time.now()
r.sleep()
#soundhandle.playWave('dummy')
# print 'say'
# soundhandle.say('Hello world!')
# sleep(3)
rospy.loginfo('wave')
soundhandle.playWave('say-beep.wav')
sleep(2)
rospy.loginfo('quiet wave')
soundhandle.playWave('say-beep.wav', 0.3)
sleep(2)
rospy.loginfo('plugging')
soundhandle.play(SoundRequest.NEEDS_PLUGGING)
sleep(2)
rospy.loginfo('quiet plugging')
soundhandle.play(SoundRequest.NEEDS_PLUGGING, 0.3)
sleep(2)
rospy.loginfo('unplugging')
soundhandle.play(SoundRequest.NEEDS_UNPLUGGING)
sleep(2)
rospy.loginfo('plugging badly')
soundhandle.play(SoundRequest.NEEDS_PLUGGING_BADLY)
sleep(2)
rospy.loginfo('unplugging badly')
# Author: Blaise Gassend
import sys
if __name__ == '__main__':
if len(sys.argv) != 2 or sys.argv[1] == '--help':
print 'Usage: %s <sound_id>'%sys.argv[0]
print
print 'Plays one of the built-in sounds based on its integer ID. Look at the sound_play/SoundRequest message definition for IDs.'
exit(1)
# Import here so that usage is fast.
import rospy
from sound_play.msg import SoundRequest
from sound_play.libsoundplay import SoundClient
rospy.init_node('play', anonymous = True)
soundhandle = SoundClient()
rospy.sleep(1)
num = int(sys.argv[1])
print 'Playing sound %i.'%num
soundhandle.play(num)
rospy.sleep(1)
import sys
import rospy
from sound_play.msg import SoundRequest
from sound_play.libsoundplay import SoundClient
if __name__ == "__main__":
rospy.init_node('beep_node', anonymous = True)
soundhandle = SoundClient()
rospy.sleep(1)
soundhandle.play(2, 1.0)
rospy.sleep(1)
class anomaly_detector:
def __init__(self, task_name, feature_list, save_data_path, training_data_pkl, verbose=False, \
online_raw_viz=False):
rospy.init_node(task_name)
rospy.loginfo('Initializing anomaly detector')
self.task_name = task_name
self.feature_list = feature_list
self.save_data_path = save_data_path
self.training_data_pkl = os.path.join(save_data_path, training_data_pkl)
self.online_raw_viz = online_raw_viz
self.count = 0
self.enable_detector = False
self.soundHandle = SoundClient()
# visualization
if self.online_raw_viz:
## plt.ion()
self.fig = plt.figure(1)
## pylab.hold(False)
self.plot_data = {}
self.plot_len = 22000
self.initParams()
self.initComms()
self.initDetector()
self.reset()
def initParams(self):
'''
Load feature list
'''
self.rf_radius = rospy.get_param('hrl_manipulation_task/receptive_field_radius')
self.nState = 15
self.cov_mult = 1.0
self.threshold = -200.0
def initComms(self):
'''
Subscribe raw data
'''
# Publisher
self.action_interruption_pub = rospy.Publisher('InterruptAction', String)
# Subscriber
rospy.Subscriber('/hrl_manipulation_task/raw_data', MultiModality, self.rawDataCallback)
# Service
self.detection_service = rospy.Service('anomaly_detector_enable/'+self.task_name, Bool_None, \
self.enablerCallback)
def initDetector(self):
if os.path.isfile(self.training_data_pkl) is not True:
print "There is no saved data"
sys.exit()
data_dict = ut.load_pickle(self.training_data_pkl)
self.trainingData, self.param_dict = extractLocalFeature(data_dict['trainData'], self.feature_list, \
self.rf_radius)
# training hmm
self.nEmissionDim = len(self.trainingData)
detection_param_pkl = os.path.join(self.save_data_path, 'hmm_'+self.task_name+'.pkl')
self.ml = hmm.learning_hmm_multi_n(self.nState, self.nEmissionDim, check_method='progress', verbose=False)
ret = self.ml.fit(self.trainingData, cov_mult=[self.cov_mult]*self.nEmissionDim**2, ml_pkl=detection_param_pkl, use_pkl=True)
if ret == 'Failure':
print "-------------------------"
print "HMM returned failure!! "
print "-------------------------"
sys.exit()
def enablerCallback(self, msg):
if msg.data is True:
self.enable_detector = True
else:
# Reset detector
self.enable_detector = False
self.reset()
return Bool_NoneResponse()
def rawDataCallback(self, msg):
self.audio_feature = msg.audio_feature
self.audio_power = msg.audio_power
self.audio_azimuth = msg.audio_azimuth
self.audio_head_joints = msg.audio_head_joints
self.audio_cmd = msg.audio_cmd
self.kinematics_ee_pos = msg.kinematics_ee_pos
self.kinematics_ee_quat = msg.kinematics_ee_quat
self.kinematics_jnt_pos = msg.kinematics_jnt_pos
self.kinematics_jnt_vel = msg.kinematics_jnt_vel
self.kinematics_jnt_eff = msg.kinematics_jnt_eff
self.kinematics_target_pos = msg.kinematics_target_pos
self.kinematics_target_quat = msg.kinematics_target_quat
self.ft_force = msg.ft_force
self.ft_torque = msg.ft_torque
self.vision_pos = msg.vision_pos
self.vision_quat = msg.vision_quat
self.pps_skin_left = msg.pps_skin_left
self.pps_skin_right = msg.pps_skin_right
def extractLocalFeature(self):
dataSample = []
# Unimoda feature - Audio --------------------------------------------
if 'unimodal_audioPower' in self.feature_list:
ang_max, ang_min = getAngularSpatialRF(self.kinematics_ee_pos, self.rf_radius)
audio_power_min = self.param_dict['unimodal_audioPower_power_min']
if self.audio_azimuth > ang_min and self.audio_azimuth < ang_max:
unimodal_audioPower = self.audio_power
else:
unimodal_audioPower = audio_power_min
dataSample.append(unimodal_audioPower)
# Unimodal feature - Kinematics --------------------------------------
if 'unimodal_kinVel' in self.feature_list:
print "not implemented"
# Unimodal feature - Force -------------------------------------------
if 'unimodal_ftForce' in self.feature_list:
ftForce_pca = self.param_dict['unimodal_ftForce_pca']
unimodal_ftForce = ftForce_pca.transform(self.ft_force)
if len(np.array(unimodal_ftForce).flatten()) > 1:
dataSample += list(np.squeeze(unimodal_ftForce))
else:
dataSample.append( np.squeeze(unimodal_ftForce) )
# Crossmodal feature - relative dist --------------------------
if 'crossmodal_targetRelativeDist' in feature_list:
crossmodal_targetRelativeDist = np.linalg.norm(np.array(self.kinematics_target_pos) - \
np.array(self.kinematics_ee_pos))
dataSample.append( crossmodal_targetRelativeDist )
# Crossmodal feature - relative angle --------------------------
if 'crossmodal_targetRelativeAng' in feature_list:
diff_ang = qt.quat_angle(self.kinematics_ee_quat, self.kinematics_target_quat)
crossmodal_targetRelativeAng = abs(diff_ang)
dataSample.append( crossmodal_targetRelativeAng )
# Scaling ------------------------------------------------------------
scaled_features = (np.array(dataSample) - np.array(self.param_dict['feature_min']) )\
/( np.array(self.param_dict['feature_max']) - np.array(self.param_dict['feature_min']) )
return scaled_features
def reset(self):
'''
Reset parameters
'''
self.dataList = []
def run(self):
'''
Run detector
'''
rospy.loginfo("Start to run anomaly detection: "+self.task_name)
self.count = 0
## self.enable_detector = True
rate = rospy.Rate(20) # 25Hz, nominally.
while not rospy.is_shutdown():
if self.enable_detector is False: continue
self.count += 1
# extract feature
self.dataList.append( self.extractLocalFeature() )
if len(np.shape(self.dataList)) == 1: continue
if np.shape(self.dataList)[0] < 10: continue
# visualization
if self.online_raw_viz:
for i in xrange(len(self.dataList[-1])):
self.plot_data.setdefault(i, [])
self.plot_data[i].append(self.dataList[-1][i])
if self.count % 50 == 0:
for i in xrange(len(self.dataList[-1])):
ax = self.fig.add_subplot( len(self.plot_data.keys())*100+10+(i+1) )
ax.plot(self.plot_data[i], 'r')
if self.count > 280:
for i, feature_name in enumerate(feature_list):
ax = self.fig.add_subplot( len(self.plot_data.keys())*100+10+(i+1) )
ax.plot(np.array(self.trainingData[i]).T, 'b')
plt.show()
# Run anomaly checker
anomaly, error = self.ml.anomaly_check(np.array(self.dataList).T, self.threshold)
print "anomaly check : ", anomaly, " " , error, " dat shape: ", np.array(self.dataList).T.shape
# anomaly decision
## if np.isnan(error): continue #print "anomaly check returned nan"
if anomaly or np.isnan(error) or np.isinf(error):
print "anoooooooooooooooooooooomaly"
self.action_interruption_pub.publish(self.task_name+'_anomaly')
self.soundHandle.play(2)
self.enable_detector = False
self.reset()
rate.sleep()
# Author: Blaise Gassend
import sys
if __name__ == '__main__':
if len(sys.argv) < 2 or len(sys.argv) > 3 or sys.argv[1] == '--help':
print 'Usage: %s <sound_id> [volume]'%sys.argv[0]
print
print 'Plays one of the built-in sounds based on its integer ID. Look at the sound_play/SoundRequest message definition for IDs.\n The (optional) volume parameter sets the volume for the sound as a value between 0 and 1.0, where 0 is mute.'
exit(1)
# Import here so that usage is fast.
import rospy
from sound_play.msg import SoundRequest
from sound_play.libsoundplay import SoundClient
rospy.init_node('play', anonymous = True)
soundhandle = SoundClient()
rospy.sleep(1)
num = int(sys.argv[1])
volume = float(sys.argv[2]) if len(sys.argv) == 3 else 1.0
print 'Playing sound %i.'%num
soundhandle.play(num, volume)
rospy.sleep(1)
import sys
if __name__ == '__main__':
import rospy
argv = rospy.myargv()
if len(argv) < 2 or len(argv) > 3 or argv[1] == '--help':
print('Usage: %s <sound_id> [volume]' % argv[0])
print()
print(
'Plays one of the built-in sounds based on its integer ID. Look at the sound_play/SoundRequest message definition for IDs.\n The (optional) volume parameter sets the volume for the sound as a value between 0 and 1.0, where 0 is mute.'
)
exit(1)
# Import here so that usage is fast.
from sound_play.msg import SoundRequest
from sound_play.libsoundplay import SoundClient
rospy.init_node('play', anonymous=True)
soundhandle = SoundClient()
rospy.sleep(1)
num = int(argv[1])
volume = float(argv[2]) if len(argv) == 3 else 1.0
rospy.loginfo('Playing sound %i.' % num)
soundhandle.play(num, volume)
rospy.sleep(1)
class onlineAnomalyDetection(Thread):
MAX_INT = 32768.0
CHUNK = 1024 # frame per buffer
RATE = 48000 # sampling rate
UNIT_SAMPLE_TIME = 1.0 / float(RATE)
CHANNEL = 2 # number of channels
FORMAT = pyaudio.paInt16
def __init__(self,
subject='s1',
task='s',
targetFrame=None,
tfListener=None,
isScooping=True,
audioTool=None):
super(onlineAnomalyDetection, self).__init__()
self.daemon = True
self.cancelled = False
self.isRunning = False
# Predefined settings
self.downSampleSize = 100 #200
self.cov_mult = 5.0
self.isScooping = isScooping
self.subject = subject
self.task = task
if self.isScooping:
self.nState = 10
self.cutting_ratio = [0.0, 0.9] #[0.0, 0.7]
self.anomaly_offset = -15.0
self.scale = [1.0, 1.0, 1.0, 0.7] #10
self.ml_thres_pkl = 'ml_scooping_thres.pkl'
else:
self.nState = 15
self.cutting_ratio = [0.0, 0.7]
self.anomaly_offset = -20
self.scale = [1.0, 1.0, 0.7, 1.0] #10
self.ml_thres_pkl = 'ml_feeding_thres.pkl'
print 'is scooping:', self.isScooping
self.publisher = rospy.Publisher('visualization_marker', Marker)
self.interruptPublisher = rospy.Publisher('InterruptAction', String)
self.targetFrame = targetFrame
# Data logging
self.updateNumber = 0
self.lastUpdateNumber = 0
self.init_time = rospy.get_time()
if tfListener is None:
self.transformer = tf.TransformListener()
else:
self.transformer = tfListener
# Gripper
self.lGripperPosition = None
self.lGripperRotation = None
self.mic = None
self.grips = []
# Spoon
self.spoon = None
# FT sensor
self.force = None
self.torque = None
# Audio
if audioTool is None:
self.audioTool = tool_audio_slim()
self.audioTool.start()
else:
self.audioTool = audioTool
# Kinematics
self.jointAngles = None
self.jointVelocities = None
self.soundHandle = SoundClient()
saveDataPath = '/home/dpark/git/hrl-assistive/hrl_multimodal_anomaly_detection/src/hrl_multimodal_anomaly_detection/hmm/batchDataFiles/%s_%d_%d_%d_%d.pkl'
# Setup HMM to perform online anomaly detection
self.hmm, self.minVals, self.maxVals, self.minThresholds \
= onlineHMM.iteration(downSampleSize=self.downSampleSize,
scale=self.scale, nState=self.nState,
cov_mult=self.cov_mult, anomaly_offset=self.anomaly_offset, verbose=False,
isScooping=self.isScooping, use_pkl=False,
train_cutting_ratio=self.cutting_ratio,
findThresholds=True, ml_pkl=self.ml_thres_pkl,
savedDataFile=saveDataPath % (('scooping' if self.isScooping else 'feeding'),
self.downSampleSize, self.scale[0], self.nState, int(self.cov_mult)))
print 'Threshold:', self.minThresholds
self.forces = []
self.distances = []
self.angles = []
self.audios = []
self.forcesRaw = []
self.distancesRaw = []
self.anglesRaw = []
self.audiosRaw = []
self.times = []
self.likelihoods = []
self.anomalyOccured = False
# self.avgAngle = onlineHMM.trainData[2]
self.forceSub = rospy.Subscriber('/netft_data', WrenchStamped,
self.forceCallback)
print 'Connected to FT sensor'
self.objectCenter = None
self.objectCenterSub = rospy.Subscriber(
'/ar_track_alvar/bowl_cen_pose'
if isScooping else '/ar_track_alvar/mouth_pose', PoseStamped,
self.objectCenterCallback)
print 'Connected to center of object publisher'
groups = rospy.get_param('/haptic_mpc/groups')
for group in groups:
if group['name'] == 'left_arm_joints':
self.joint_names_list = group['joints']
self.jstate_lock = threading.RLock()
self.jointSub = rospy.Subscriber("/joint_states", JointState,
self.jointStatesCallback)
print 'Connected to robot kinematics'
def reset(self):
self.isRunning = True
self.forces = []
self.distances = []
self.angles = []
self.audios = []
self.forcesRaw = []
self.distancesRaw = []
self.anglesRaw = []
self.audiosRaw = []
self.times = []
self.anomalyOccured = False
self.updateNumber = 0
self.lastUpdateNumber = 0
self.init_time = rospy.get_time()
self.lGripperPosition = None
self.lGripperRotation = None
self.mic = None
self.grips = []
self.spoon = None
self.force = None
self.torque = None
self.jointAngles = None
self.jointVelocities = None
self.objectCenter = None
self.likelihoods = []
self.audioTool.reset(self.init_time)
def run(self):
"""Overloaded Thread.run, runs the update
method once per every xx milliseconds."""
# rate = rospy.Rate(1000) # 25Hz, nominally.
while not self.cancelled:
if self.isRunning and self.updateNumber > self.lastUpdateNumber and self.objectCenter is not None:
self.lastUpdateNumber = self.updateNumber
if not self.processData(): continue
if not self.anomalyOccured and len(self.forces) > 15:
# Perform anomaly detection
(anomaly, error) = self.hmm.anomaly_check(
self.forces, self.distances, self.angles, self.audios,
self.minThresholds * 1.5) #temp
print 'Anomaly error:', error
if anomaly:
if self.isScooping:
self.interruptPublisher.publish('Interrupt')
else:
self.interruptPublisher.publish('InterruptHead')
self.anomalyOccured = True
self.soundHandle.play(2)
print 'AHH!! There is an anomaly at time stamp', rospy.get_time(
) - self.init_time, (anomaly, error)
fig = plt.figure()
for i, modality in enumerate(
[[self.forces] + onlineHMM.trainData[0][:13],
[self.distances] + onlineHMM.trainData[1][:13],
[self.angles] + onlineHMM.trainData[2][:13],
[self.audios] + onlineHMM.trainData[3][:13]]):
ax = plt.subplot(int('41' + str(i + 1)))
for index, (modal, times) in enumerate(
zip(modality, [self.times] +
onlineHMM.trainTimeList[:3])):
ax.plot(times, modal, label='%d' % index)
ax.legend()
fig.savefig(
'/home/dpark/git/hrl-assistive/hrl_multimodal_anomaly_detection/src/hrl_multimodal_anomaly_detection/fooboohooyou.pdf'
)
print "saved pdf file"
rospy.sleep(2.0)
# plt.show()
# rate.sleep()
print 'Online anomaly thread cancelled'
def cancel(self, cancelAudio=True):
self.isRunning = False
if cancelAudio:
self.audioTool.cancel()
self.saveData()
rospy.sleep(1.0)
def saveData(self):
# TODO Save data (Check with daehyung if any more data should be added)
data = dict()
data['forces'] = self.forces
data['distances'] = self.distances
data['angles'] = self.angles
data['audios'] = self.audios
data['forcesRaw'] = self.forcesRaw
data['distancesRaw'] = self.distancesRaw
data['anglesRaw'] = self.anglesRaw
data['audioRaw'] = self.audiosRaw
data['times'] = self.times
data['anomalyOccured'] = self.anomalyOccured
data['minThreshold'] = self.minThresholds
data['likelihoods'] = self.likelihoods
data['jointAngles'] = self.jointAngles
data['jointVelocities'] = self.jointVelocities
directory = os.path.join(os.path.dirname(__file__),
'onlineDataRecordings/')
if not os.path.exists(directory):
os.makedirs(directory)
fileName = os.path.join(
directory, self.subject + '_' + self.task + '_' +
time.strftime('%m-%d-%Y_%H-%M-%S.pkl'))
with open(fileName, 'wb') as f:
pickle.dump(data, f, protocol=pickle.HIGHEST_PROTOCOL)
print 'Online data saved to file.'
def processData(self):
# Find nearest time stamp from training data
# timeStamp = rospy.get_time() - self.init_time
# index = np.abs(self.times - timeStamp).argmin()
self.transposeGripper()
# Use magnitude of forces
force = np.linalg.norm(self.force)
# Determine distance between mic and center of object
distance = np.linalg.norm(self.mic - self.objectCenter)
# Find angle between gripper-object vector and gripper-spoon vector
micSpoonVector = self.spoon - self.mic
micObjectVector = self.objectCenter - self.mic
angle = np.arccos(
np.dot(micSpoonVector, micObjectVector) /
(np.linalg.norm(micSpoonVector) * np.linalg.norm(micObjectVector)))
# Process either visual or audio data depending on which we're using
if len(self.audioTool.audio_data_raw) > 0:
audio = self.audioTool.audio_data_raw[-1]
else:
return False
if audio is None:
print 'Audio is None'
return False
audio = get_rms(audio)
# print 'Audio:', audio
self.forcesRaw.append(force)
self.distancesRaw.append(distance)
self.anglesRaw.append(angle)
self.audiosRaw.append(audio)
# Scale data
force = self.scaling(force,
minVal=self.minVals[0],
maxVal=self.maxVals[0],
scale=self.scale[0])
distance = self.scaling(distance,
minVal=self.minVals[1],
maxVal=self.maxVals[1],
scale=self.scale[1])
angle = self.scaling(angle,
minVal=self.minVals[2],
maxVal=self.maxVals[2],
scale=self.scale[2])
audio = self.scaling(audio,
minVal=self.minVals[3],
maxVal=self.maxVals[3],
scale=self.scale[3])
# Find nearest time stamp from training data
timeStamp = rospy.get_time() - self.init_time
index = np.abs(np.array(onlineHMM.trainTimeList[0]) -
timeStamp).argmin()
self.forces.append(force)
self.distances.append(distance)
self.angles.append(angle)
#self.angles.append(onlineHMM.trainData[2][0][index])
self.audios.append(audio)
self.times.append(rospy.get_time() - self.init_time)
if len(self.forces) > 1:
self.likelihoods.append(
self.hmm.likelihoods(self.forces, self.distances, self.angles,
self.audios))
return True
@staticmethod
def scaling(x, minVal, maxVal, scale=1.0):
return (x - minVal) / (maxVal - minVal) * scale
def forceCallback(self, msg):
self.force = np.array(
[msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
self.torque = np.array(
[msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z])
self.updateNumber += 1
def objectCenterCallback(self, msg):
self.objectCenter = np.array(
[msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
def jointStatesCallback(self, data):
joint_angles = []
## joint_efforts = []
joint_vel = []
jt_idx_list = [0] * len(self.joint_names_list)
for i, jt_nm in enumerate(self.joint_names_list):
jt_idx_list[i] = data.name.index(jt_nm)
for i, idx in enumerate(jt_idx_list):
if data.name[idx] != self.joint_names_list[i]:
raise RuntimeError('joint angle name does not match.')
joint_angles.append(data.position[idx])
## joint_efforts.append(data.effort[idx])
joint_vel.append(data.velocity[idx])
with self.jstate_lock:
self.jointAngles = joint_angles
## self.joint_efforts = joint_efforts
self.jointVelocities = joint_vel
def transposeGripper(self):
# Transpose gripper position to camera frame
self.transformer.waitForTransform(self.targetFrame,
'/l_gripper_tool_frame',
rospy.Time(0), rospy.Duration(5))
try:
self.lGripperPosition, self.lGripperRotation = self.transformer.lookupTransform(
self.targetFrame, '/l_gripper_tool_frame', rospy.Time(0))
transMatrix = np.dot(
tf.transformations.translation_matrix(self.lGripperPosition),
tf.transformations.quaternion_matrix(self.lGripperRotation))
except tf.ExtrapolationException:
print 'Transpose of gripper failed!'
return
# Use a buffer of gripper positions
if len(self.grips) >= 2:
lGripperTransposeMatrix = self.grips[-2]
else:
lGripperTransposeMatrix = transMatrix
self.grips.append(transMatrix)
# Determine location of mic
mic = [0.12, -0.02, 0]
# print 'Mic before', mic
self.mic = np.dot(lGripperTransposeMatrix,
np.array([mic[0], mic[1], mic[2], 1.0]))[:3]
# print 'Mic after', self.mic
# Determine location of spoon
spoon3D = [0.22, -0.050, 0]
self.spoon = np.dot(
lGripperTransposeMatrix,
np.array([spoon3D[0], spoon3D[1], spoon3D[2], 1.0]))[:3]
def find_input_device(self):
device_index = None
for i in range(self.p.get_device_count()):
devinfo = self.p.get_device_info_by_index(i)
print('Device %d: %s' % (i, devinfo['name']))
for keyword in ['mic', 'input', 'icicle']:
if keyword in devinfo['name'].lower():
print('Found an input: device %d - %s' %
(i, devinfo['name']))
device_index = i
return device_index
if device_index is None:
print('No preferred input found; using default input device.')
return device_index
def publishPoints(self,
name,
points,
size=0.01,
r=0.0,
g=0.0,
b=0.0,
a=1.0):
marker = Marker()
marker.header.frame_id = '/torso_lift_link'
marker.ns = name
marker.type = marker.POINTS
marker.action = marker.ADD
marker.scale.x = size
marker.scale.y = size
marker.color.a = a
marker.color.r = r
marker.color.g = g
marker.color.b = b
for point in points:
p = Point()
# print point
p.x, p.y, p.z = point
marker.points.append(p)
self.publisher.publish(marker)
class NavSpeak(object):
def __init__(self):
self.is_speaking = False
self.move_base_goal_sub = rospy.Subscriber(
"/move_base/goal",
MoveBaseActionGoal,
self.move_base_goal_callback,
queue_size=1)
self.move_base_result_sub = rospy.Subscriber(
"/move_base/result",
MoveBaseActionResult,
self.move_base_result_callback,
queue_size=1)
self.robotsound_jp_status_sub = rospy.Subscriber(
"/robotsound_jp/status",
GoalStatusArray,
self.robotsound_jp_status_callback,
queue_size=1)
self.sound = SoundClient(blocking=True)
self.lang = rospy.get_param("~lang", "japanese")
self.volume = rospy.get_param("~volume", 1.0)
self.client = actionlib.SimpleActionClient('robotsound_jp',
SoundRequestAction)
self.client.wait_for_server()
def move_base_goal_callback(self, msg):
self.sound.play(2, volume=self.volume)
def move_base_result_callback(self, msg):
# Wait if other node is speaking
while self.is_speaking is True:
time.sleep(1)
text = "{}: {}".format(goal_status(msg.status.status), msg.status.text)
rospy.loginfo(text)
if self.lang == "japanese": # speak japanese
sound_goal = SoundRequestGoal()
sound_goal.sound_request.sound = -3
sound_goal.sound_request.command = 1
sound_goal.sound_request.volume = self.volume
sound_goal.sound_request.arg2 = "jp"
if msg.status.status == GoalStatus.SUCCEEDED:
self.sound.play(1, volume=self.volume)
time.sleep(1)
if self.lang == "japanese":
sound_goal.sound_request.arg = "到着しました"
self.client.send_goal(sound_goal)
else:
self.sound.say(text, volume=self.volume)
elif msg.status.status == GoalStatus.PREEMPTED:
self.sound.play(2, volume=self.volume)
time.sleep(1)
if self.lang == "japanese":
sound_goal.sound_request.arg = "別のゴールがセットされました"
self.client.send_goal(sound_goal)
else:
self.sound.say(text, volume=self.volume)
elif msg.status.status == GoalStatus.ABORTED:
self.sound.play(3, volume=self.volume)
time.sleep(1)
if self.lang == "japanese":
sound_goal.sound_request.arg = "中断しました"
self.client.send_goal(sound_goal)
else:
self.sound.say(text, volume=self.volume)
else:
self.sound.play(4, volume=self.volume)
time.sleep(1)
self.sound.say(text, volume=self.volume)
def robotsound_jp_status_callback(self, msg):
if len(msg.status_list) == 0:
self.is_speaking = False
else:
self.is_speaking = True
class NavSpeak(object):
def __init__(self):
self.move_base_goal_sub = rospy.Subscriber(
"/move_base/goal",
MoveBaseActionGoal,
self.move_base_goal_callback,
queue_size=1)
self.move_base_result_sub = rospy.Subscriber(
"/move_base/result",
MoveBaseActionResult,
self.move_base_result_callback,
queue_size=1)
self.sound = SoundClient()
self.lang = "japanese" # speak japanese by default
if rospy.has_param("/nav_speak/lang"):
self.lang = rospy.get_param("/nav_speak/lang")
self.pub = rospy.Publisher('/robotsound_jp/goal',
SoundRequestActionGoal,
queue_size=1)
def move_base_goal_callback(self, msg):
self.sound.play(2)
def move_base_result_callback(self, msg):
text = "{}: {}".format(goal_status(msg.status.status), msg.status.text)
rospy.loginfo(text)
if self.lang == "japanese": # speak japanese
sound_goal = SoundRequestActionGoal()
sound_goal.goal_id.stamp = rospy.Time.now()
sound_goal.goal.sound_request.sound = -3
sound_goal.goal.sound_request.command = 1
sound_goal.goal.sound_request.volume = 1.0
sound_goal.goal.sound_request.arg2 = "jp"
if msg.status.status == GoalStatus.SUCCEEDED:
self.sound.play(1)
time.sleep(1)
if self.lang == "japanese":
sound_goal.goal.sound_request.arg = "到着しました"
self.pub.publish(sound_goal)
else:
self.sound.say(text)
elif msg.status.status == GoalStatus.PREEMPTED:
self.sound.play(2)
time.sleep(1)
if self.lang == "japanese":
sound_goal.goal.sound_request.arg = "別のゴールがセットされました"
self.pub.publish(sound_goal)
else:
self.sound.say(text)
elif msg.status.status == GoalStatus.ABORTED:
self.sound.play(3)
time.sleep(1)
if self.lang == "japanese":
sound_goal.goal.sound_request.arg = "中断しました"
self.pub.publish(sound_goal)
else:
self.sound.say(text)
else:
self.sound.play(4)
time.sleep(1)
self.sound.say(text)
class voice_cmd_vel:
def __init__(self):
rospy.on_shutdown(self.cleanup)
self.speed = 0.1
self.buildmap = False
self.follower = False
self.navigation = False
self.msg = Twist()
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 10)
# .txt file with name and x, y coordinates for location
self.map_locations = rospy.get_param("~map_locations")
# odometry topic name
self.odometry_topic = rospy.get_param("~odometry_topic", "odom")
# cmd_vel topic name
self.cmd_vel_topic = rospy.get_param("~cmd_vel_topic", "cmd_vel")
self.locations = dict()
fh=open(self.map_locations)
for line in fh:
name = line.rstrip().split(":")
temp = str(line.rstrip().rsplit(":", 1)[1])
coordinates = temp.split()
self.locations[name[0]] = Pose(Point(float(coordinates[0]), float(coordinates[1]), 0.000), Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0)))
# Create the sound client object
self.soundhandle = SoundClient()
rospy.sleep(1)
self.soundhandle.stopAll()
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# Announce that we are ready for input
rospy.sleep(1)
self.soundhandle.say('Hi, my name is Petrois')
rospy.sleep(2)
self.soundhandle.say("Say one of the navigation commands")
# publish to cmd_vel, subscribe to speech output
self.pub = rospy.Publisher(self.cmd_vel_topic, Twist, queue_size=2)
rospy.Subscriber('recognizer/output', String, self.speechCb)
r = rospy.Rate(10.0)
while not rospy.is_shutdown():
self.pub.publish(self.msg)
r.sleep()
def speechCb(self, msg):
rospy.loginfo(msg.data)
if msg.data.find("fast") > -1:
if self.speed != 0.3:
self.soundhandle.say('Speeding up')
self.set_speed(0.3)
else:
self.soundhandle.say('Already at full speed')
if msg.data.find("half") > -1:
if self.speed != 0.2:
self.soundhandle.say('Going at half speed')
self.set_speed(0.2)
else:
self.soundhandle.say('Already at half speed')
if msg.data.find("slow") > -1:
if self.speed != 0.1:
self.soundhandle.say('Slowing down')
self.set_speed(0.1)
else:
self.soundhandle.say('Already at slow speed')
if msg.data.find("forward") > -1:
self.forward()
elif msg.data.find("right") > -1:
self.right()
elif msg.data.find("left") > -1:
self.left()
elif msg.data.find("back") > -1:
self.backward()
elif msg.data.find("stop") > -1 or msg.data.find("halt") > -1:
self.stop()
################################# follower commands
if msg.data.find("follow me") > -1:
self.run_follower(True)
elif msg.data.find("stop follower") > -1:
self.run_follower(False)
################################ map commands
if msg.data.find("build map") > -1:
self.build_map(True)
elif msg.data.find("save map") > -1:
self.save_map()
elif msg.data.find("stop map") > -1:
self.build_map(False)
################################ navigation commands
if msg.data.find("kk") > -1: #OJOOOOOOOOOOOOOOOOOOO cambiar
if self.navigation == False:
self.soundhandle.say('Starting navigation stack')
rospy.sleep(2)
self.soundhandle.say('Visualizing costmaps')
self.msg = Twist()
self.proc5 = subprocess.Popen(['roslaunch', 'pioneer_utils', 'navigation_p3at.launch'])
self.proc6 = subprocess.Popen(['roslaunch', 'pioneer_utils', 'rviz-navigation.launch'])
self.navigation = True
else:
self.soundhandle.say('Already in navigation mode')
elif msg.data.find("kk") > -1: #OJOOOOOOOOOOOOO cambiar
if self.navigation == True:
self.msg = Twist()
print 'proc5 = ', self.proc5.pid
self.proc5.terminate()
kill(self.proc5.pid)
self.proc5.kill()
print 'proc6 = ', self.proc6.pid
self.proc6.terminate()
kill(self.proc6.pid)
self.proc6.kill()
self.navigation = False
self.soundhandle.say('Navigation stopped')
else:
self.soundhandle.say('I am not in navigation mode')
elif msg.data.find("navigate to") > -1:
if len(msg.data.split()) > 2:
if msg.data.rsplit("navigate to ", 1)[1] in self.locations.keys():
self.send_goal(msg.data.rsplit("navigate to ", 1)[1])
self.pub.publish(self.msg)
def send_goal(self, location_name):
location = self.locations.get(location_name)
# Set up goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = location
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
rospy.loginfo(self.goal)
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location_name))
self.soundhandle.say("Going to " + str(location_name))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
def run_follower(self, on):
if on and self.follower == False:
self.msg = Twist()
subprocess.Popen(['roslaunch', 'pioneer_utils', 'simple-follower.launch'])
self.soundhandle.say('Okay. Show me the way')
self.follower = True
elif on and self.follower:
self.soundhandle.say('Already in follower mode')
elif on == False and self.follower:
self.msg = Twist()
subprocess.Popen(['rosnode', 'kill', 'turtlebot_follower'])
self.follower = False
self.soundhandle.say('Follower mode disabled')
def forward(self):
self.soundhandle.play(1)
self.msg.linear.x = self.speed
self.msg.angular.z = 0
def backward(self):
self.soundhandle.play(1)
self.msg.linear.x = -self.speed
self.msg.angular.z = 0
def left(self):
self.soundhandle.play(1)
self.msg.linear.x = 0
self.msg.angular.z = self.speed*2
def right(self):
self.soundhandle.play(1)
self.msg.linear.x = 0
self.msg.angular.z = -self.speed*2
def stop(self):
self.move_base.cancel_goal()
self.run_follower(False)
self.msg = Twist()
self.soundhandle.play(1)
def set_speed(self, vel):
if self.msg.linear.x > 0:
self.msg.linear.x = vel
elif self.msg.linear.x < 0:
self.msg.linear.x = -vel
if self.msg.angular.z >0:
self.msg.angular.z = vel
elif self.msg.angular.z < 0:
self.msg.angular.z = -vel
self.speed = vel
def build_map(self, on):
if on and self.buildmap == False:
self.stop()
self.soundhandle.say('Building map with slam gmapping')
subprocess.Popen(['rosnode', 'kill', 'amcl'])
subprocess.Popen(['rosnode', 'kill', 'map_server'])
subprocess.Popen(['roslaunch', 'p2os_launch', 'gmapping.launch'])
self.buildmap = True
elif on and self.buildmap:
self.soundhandle.say('Already building a map')
elif on == False and self.buildmap:
self.stop()
subprocess.Popen(['rosnode', 'kill', 'slam_gmapping'])
self.buildmap = False
subprocess.Popen(['roslaunch', 'pioneer_utils', 'floor_zero-map.launch'])
subprocess.Popen(['roslaunch', 'p2os_launch', 'amcl.launch'])
self.soundhandle.say('Building map stopped')
else:
self.soundhandle.say('I am not building any map')
def save_map(self):
if self.buildmap == True:
self.msg = Twist()
subprocess.Popen(['rosrun', 'map_server', 'map_saver', '-f', 'new_map'])
rospy.sleep(4)
print 'map saved at ~/.ros directory as new_map.pgm new_map.yaml'
self.soundhandle.say('Map saved successfully')
else:
self.soundhandle.say('I am not building any map so there is no map to save')
def cleanup(self):
# stop the robot!
self.stop()
self.pub.publish(twist)
class onlineAnomalyDetection(Thread):
MAX_INT = 32768.0
CHUNK = 1024 # frame per buffer
RATE = 48000 # sampling rate
UNIT_SAMPLE_TIME = 1.0 / float(RATE)
CHANNEL = 2 # number of channels
FORMAT = pyaudio.paInt16
def __init__(self, subject='s1', task='s', targetFrame=None, tfListener=None, isScooping=True, audioTool=None):
super(onlineAnomalyDetection, self).__init__()
self.daemon = True
self.cancelled = False
self.isRunning = False
# Predefined settings
self.downSampleSize = 100 #200
self.cov_mult = 5.0
self.isScooping = isScooping
self.subject = subject
self.task = task
if self.isScooping:
self.nState = 10
self.cutting_ratio = [0.0, 0.9] #[0.0, 0.7]
self.anomaly_offset = -15.0
self.scale = [1.0,1.0,1.0,0.7] #10
self.ml_thres_pkl='ml_scooping_thres.pkl'
else:
self.nState = 15
self.cutting_ratio = [0.0, 0.7]
self.anomaly_offset = -20
self.scale = [1.0,1.0,0.7,1.0] #10
self.ml_thres_pkl='ml_feeding_thres.pkl'
print 'is scooping:', self.isScooping
self.publisher = rospy.Publisher('visualization_marker', Marker)
self.interruptPublisher = rospy.Publisher('InterruptAction', String)
self.targetFrame = targetFrame
# Data logging
self.updateNumber = 0
self.lastUpdateNumber = 0
self.init_time = rospy.get_time()
if tfListener is None:
self.transformer = tf.TransformListener()
else:
self.transformer = tfListener
# Gripper
self.lGripperPosition = None
self.lGripperRotation = None
self.mic = None
self.grips = []
# Spoon
self.spoon = None
# FT sensor
self.force = None
self.torque = None
# Audio
if audioTool is None:
self.audioTool = tool_audio_slim()
self.audioTool.start()
else:
self.audioTool = audioTool
# Kinematics
self.jointAngles = None
self.jointVelocities = None
self.soundHandle = SoundClient()
saveDataPath = '/home/dpark/git/hrl-assistive/hrl_multimodal_anomaly_detection/src/hrl_multimodal_anomaly_detection/hmm/batchDataFiles/%s_%d_%d_%d_%d.pkl'
# Setup HMM to perform online anomaly detection
self.hmm, self.minVals, self.maxVals, self.minThresholds \
= onlineHMM.iteration(downSampleSize=self.downSampleSize,
scale=self.scale, nState=self.nState,
cov_mult=self.cov_mult, anomaly_offset=self.anomaly_offset, verbose=False,
isScooping=self.isScooping, use_pkl=False,
train_cutting_ratio=self.cutting_ratio,
findThresholds=True, ml_pkl=self.ml_thres_pkl,
savedDataFile=saveDataPath % (('scooping' if self.isScooping else 'feeding'),
self.downSampleSize, self.scale[0], self.nState, int(self.cov_mult)))
print 'Threshold:', self.minThresholds
self.forces = []
self.distances = []
self.angles = []
self.audios = []
self.forcesRaw = []
self.distancesRaw = []
self.anglesRaw = []
self.audiosRaw = []
self.times = []
self.likelihoods = []
self.anomalyOccured = False
# self.avgAngle = onlineHMM.trainData[2]
self.forceSub = rospy.Subscriber('/netft_data', WrenchStamped, self.forceCallback)
print 'Connected to FT sensor'
self.objectCenter = None
self.objectCenterSub = rospy.Subscriber('/ar_track_alvar/bowl_cen_pose' if isScooping else '/ar_track_alvar/mouth_pose', PoseStamped, self.objectCenterCallback)
print 'Connected to center of object publisher'
groups = rospy.get_param('/haptic_mpc/groups' )
for group in groups:
if group['name'] == 'left_arm_joints':
self.joint_names_list = group['joints']
self.jstate_lock = threading.RLock()
self.jointSub = rospy.Subscriber("/joint_states", JointState, self.jointStatesCallback)
print 'Connected to robot kinematics'
def reset(self):
self.isRunning = True
self.forces = []
self.distances = []
self.angles = []
self.audios = []
self.forcesRaw = []
self.distancesRaw = []
self.anglesRaw = []
self.audiosRaw = []
self.times = []
self.anomalyOccured = False
self.updateNumber = 0
self.lastUpdateNumber = 0
self.init_time = rospy.get_time()
self.lGripperPosition = None
self.lGripperRotation = None
self.mic = None
self.grips = []
self.spoon = None
self.force = None
self.torque = None
self.jointAngles = None
self.jointVelocities = None
self.objectCenter = None
self.likelihoods = []
self.audioTool.reset(self.init_time)
def run(self):
"""Overloaded Thread.run, runs the update
method once per every xx milliseconds."""
# rate = rospy.Rate(1000) # 25Hz, nominally.
while not self.cancelled:
if self.isRunning and self.updateNumber > self.lastUpdateNumber and self.objectCenter is not None:
self.lastUpdateNumber = self.updateNumber
if not self.processData(): continue
if not self.anomalyOccured and len(self.forces) > 15:
# Perform anomaly detection
(anomaly, error) = self.hmm.anomaly_check(self.forces, self.distances, self.angles, self.audios, self.minThresholds*1.5) #temp
print 'Anomaly error:', error
if anomaly:
if self.isScooping:
self.interruptPublisher.publish('Interrupt')
else:
self.interruptPublisher.publish('InterruptHead')
self.anomalyOccured = True
self.soundHandle.play(2)
print 'AHH!! There is an anomaly at time stamp', rospy.get_time() - self.init_time, (anomaly, error)
fig = plt.figure()
for i, modality in enumerate([[self.forces] + onlineHMM.trainData[0][:13], [self.distances] + onlineHMM.trainData[1][:13], [self.angles] + onlineHMM.trainData[2][:13], [self.audios] + onlineHMM.trainData[3][:13]]):
ax = plt.subplot(int('41' + str(i+1)))
for index, (modal, times) in enumerate(zip(modality, [self.times] + onlineHMM.trainTimeList[:3])):
ax.plot(times, modal, label='%d' % index)
ax.legend()
fig.savefig('/home/dpark/git/hrl-assistive/hrl_multimodal_anomaly_detection/src/hrl_multimodal_anomaly_detection/fooboohooyou.pdf')
print "saved pdf file"
rospy.sleep(2.0)
# plt.show()
# rate.sleep()
print 'Online anomaly thread cancelled'
def cancel(self, cancelAudio=True):
self.isRunning = False
if cancelAudio:
self.audioTool.cancel()
self.saveData()
rospy.sleep(1.0)
def saveData(self):
# TODO Save data (Check with daehyung if any more data should be added)
data = dict()
data['forces'] = self.forces
data['distances'] = self.distances
data['angles'] = self.angles
data['audios'] = self.audios
data['forcesRaw'] = self.forcesRaw
data['distancesRaw'] = self.distancesRaw
data['anglesRaw'] = self.anglesRaw
data['audioRaw'] = self.audiosRaw
data['times'] = self.times
data['anomalyOccured'] = self.anomalyOccured
data['minThreshold'] = self.minThresholds
data['likelihoods'] = self.likelihoods
data['jointAngles'] = self.jointAngles
data['jointVelocities'] = self.jointVelocities
directory = os.path.join(os.path.dirname(__file__), 'onlineDataRecordings/')
if not os.path.exists(directory):
os.makedirs(directory)
fileName = os.path.join(directory, self.subject + '_' + self.task + '_' + time.strftime('%m-%d-%Y_%H-%M-%S.pkl'))
with open(fileName, 'wb') as f:
pickle.dump(data, f, protocol=pickle.HIGHEST_PROTOCOL)
print 'Online data saved to file.'
def processData(self):
# Find nearest time stamp from training data
# timeStamp = rospy.get_time() - self.init_time
# index = np.abs(self.times - timeStamp).argmin()
self.transposeGripper()
# Use magnitude of forces
force = np.linalg.norm(self.force)
# Determine distance between mic and center of object
distance = np.linalg.norm(self.mic - self.objectCenter)
# Find angle between gripper-object vector and gripper-spoon vector
micSpoonVector = self.spoon - self.mic
micObjectVector = self.objectCenter - self.mic
angle = np.arccos(np.dot(micSpoonVector, micObjectVector) / (np.linalg.norm(micSpoonVector) * np.linalg.norm(micObjectVector)))
# Process either visual or audio data depending on which we're using
if len(self.audioTool.audio_data_raw) > 0:
audio = self.audioTool.audio_data_raw[-1]
else:
return False
if audio is None:
print 'Audio is None'
return False
audio = get_rms(audio)
# print 'Audio:', audio
self.forcesRaw.append(force)
self.distancesRaw.append(distance)
self.anglesRaw.append(angle)
self.audiosRaw.append(audio)
# Scale data
force = self.scaling(force, minVal=self.minVals[0], maxVal=self.maxVals[0], scale=self.scale[0])
distance = self.scaling(distance, minVal=self.minVals[1], maxVal=self.maxVals[1], scale=self.scale[1])
angle = self.scaling(angle, minVal=self.minVals[2], maxVal=self.maxVals[2], scale=self.scale[2])
audio = self.scaling(audio, minVal=self.minVals[3], maxVal=self.maxVals[3], scale=self.scale[3])
# Find nearest time stamp from training data
timeStamp = rospy.get_time() - self.init_time
index = np.abs(np.array(onlineHMM.trainTimeList[0]) - timeStamp).argmin()
self.forces.append(force)
self.distances.append(distance)
self.angles.append(angle)
#self.angles.append(onlineHMM.trainData[2][0][index])
self.audios.append(audio)
self.times.append(rospy.get_time() - self.init_time)
if len(self.forces) > 1:
self.likelihoods.append(self.hmm.likelihoods(self.forces, self.distances, self.angles, self.audios))
return True
@staticmethod
def scaling(x, minVal, maxVal, scale=1.0):
return (x - minVal) / (maxVal - minVal) * scale
def forceCallback(self, msg):
self.force = np.array([msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
self.torque = np.array([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z])
self.updateNumber += 1
def objectCenterCallback(self, msg):
self.objectCenter = np.array([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
def jointStatesCallback(self, data):
joint_angles = []
## joint_efforts = []
joint_vel = []
jt_idx_list = [0]*len(self.joint_names_list)
for i, jt_nm in enumerate(self.joint_names_list):
jt_idx_list[i] = data.name.index(jt_nm)
for i, idx in enumerate(jt_idx_list):
if data.name[idx] != self.joint_names_list[i]:
raise RuntimeError('joint angle name does not match.')
joint_angles.append(data.position[idx])
## joint_efforts.append(data.effort[idx])
joint_vel.append(data.velocity[idx])
with self.jstate_lock:
self.jointAngles = joint_angles
## self.joint_efforts = joint_efforts
self.jointVelocities = joint_vel
def transposeGripper(self):
# Transpose gripper position to camera frame
self.transformer.waitForTransform(self.targetFrame, '/l_gripper_tool_frame', rospy.Time(0), rospy.Duration(5))
try :
self.lGripperPosition, self.lGripperRotation = self.transformer.lookupTransform(self.targetFrame, '/l_gripper_tool_frame', rospy.Time(0))
transMatrix = np.dot(tf.transformations.translation_matrix(self.lGripperPosition), tf.transformations.quaternion_matrix(self.lGripperRotation))
except tf.ExtrapolationException:
print 'Transpose of gripper failed!'
return
# Use a buffer of gripper positions
if len(self.grips) >= 2:
lGripperTransposeMatrix = self.grips[-2]
else:
lGripperTransposeMatrix = transMatrix
self.grips.append(transMatrix)
# Determine location of mic
mic = [0.12, -0.02, 0]
# print 'Mic before', mic
self.mic = np.dot(lGripperTransposeMatrix, np.array([mic[0], mic[1], mic[2], 1.0]))[:3]
# print 'Mic after', self.mic
# Determine location of spoon
spoon3D = [0.22, -0.050, 0]
self.spoon = np.dot(lGripperTransposeMatrix, np.array([spoon3D[0], spoon3D[1], spoon3D[2], 1.0]))[:3]
def find_input_device(self):
device_index = None
for i in range(self.p.get_device_count()):
devinfo = self.p.get_device_info_by_index(i)
print('Device %d: %s'%(i, devinfo['name']))
for keyword in ['mic', 'input', 'icicle']:
if keyword in devinfo['name'].lower():
print('Found an input: device %d - %s'%(i, devinfo['name']))
device_index = i
return device_index
if device_index is None:
print('No preferred input found; using default input device.')
return device_index
def publishPoints(self, name, points, size=0.01, r=0.0, g=0.0, b=0.0, a=1.0):
marker = Marker()
marker.header.frame_id = '/torso_lift_link'
marker.ns = name
marker.type = marker.POINTS
marker.action = marker.ADD
marker.scale.x = size
marker.scale.y = size
marker.color.a = a
marker.color.r = r
marker.color.g = g
marker.color.b = b
for point in points:
p = Point()
# print point
p.x, p.y, p.z = point
marker.points.append(p)
self.publisher.publish(marker)
def main():
global target
global odata
global cmd_rotate
global quats
global cur_pose
rospy.init_node('nav_test', anonymous=False, disable_signals=True)
tfBuffer = tf2_ros.Buffer()
tfListener = tf2_ros.TransformListener(tfBuffer)
rospy.sleep(1)
rospy.Subscriber('map', OccupancyGrid, get_occupancy, tfBuffer)
rospy.sleep(3.0)
# save start time
start_time = time.time()
# initialize variable to write elapsed time to file
contourCheck = 1
navigator = GoToPose()
position = {'x': target[0], 'y': target[1]}
quaternion = {'r1': 0.000, 'r2': 0.000, 'r3': 0.000, 'r4': 1.000}
rospy.loginfo("Go to (%s, %s) pose", position['x'], position['y'])
navigator.goto(position, quaternion)
rospy.on_shutdown(stopbot)
rate = rospy.Rate(10)
shutdown = False
while not shutdown:
while not cmd_rotate:
if contourCheck == 1:
if closure(odata):
# map is complete, so save current time into file
with open("maptime.txt", "w") as f:
f.write("Elapsed Time: " +
str(time.time() - start_time))
contourCheck = 5
# play a sound
soundhandle = SoundClient()
rospy.sleep(1)
soundhandle.stopAll()
soundhandle.play(SoundRequest.NEEDS_UNPLUGGING)
rospy.sleep(2)
# save the map
cv2.imwrite('mazemap.png', occdata)
pub = rospy.Publisher('mapdone', String)
pub.publish('Done!')
rospy.sleep(1)
toggle = False
if contourCheck == 5 and not stored_quat and not stored_pose:
position = {'x': stored_pose[0], 'y': stored_pose[1]}
quaternion = {
'r1': stored_quat[0],
'r2': stored_quat[1],
'r3': stored_quat[2],
'r4': stored_quat[3]
}
while distance(cur_pose, stored_pose) > 3:
rospy.loginfo("Go to (%s, %s) pose", position['x'],
position['y'])
navigator.goto(position, quaternion)
rospy.loginfo('Shooting Loc Reached')
pub2 = rospy.Publisher('alldone', String, queue_size=10)
pub2.publish('Done')
time.sleep(1)
shutdown = True
if toggle or stored_quat or stored_pose:
position = {'x': target[0], 'y': target[1]}
quaternion = {
'r1': 0.000,
'r2': 0.000,
'r3': 0.000,
'r4': 1.000
}
rospy.loginfo("Go to (%s, %s) pose", position['x'],
position['y'])
navigator.goto(position, quaternion)
rate.sleep()
rotatebot(cmd_rotate)
cmd_rotate = ''
stored_pose = cur_pose
stored_quat = quats
# Author: Blaise Gassend
import sys
if __name__ == '__main__':
if len(sys.argv) != 2 or sys.argv[1] == '--help':
print 'Usage: %s <sound_id>' % sys.argv[0]
print
print 'Plays one of the built-in sounds based on its integer ID. Look at the sound_play/SoundRequest message definition for IDs.'
exit(1)
# Import here so that usage is fast.
import rospy
from sound_play.msg import SoundRequest
from sound_play.libsoundplay import SoundClient
rospy.init_node('play', anonymous=True)
soundhandle = SoundClient()
rospy.sleep(1)
num = int(sys.argv[1])
print 'Playing sound %i.' % num
soundhandle.play(num)
rospy.sleep(1)
class voice_cmd_vel:
def __init__(self):
rospy.on_shutdown(self.cleanup)
self.speed = 0.1
self.buildmap = False
self.follower = False
self.navigation = False
self.msg = Twist()
# Create the sound client object
self.soundhandle = SoundClient()
rospy.sleep(1)
self.soundhandle.stopAll()
# Announce that we are ready for input
rospy.sleep(1)
self.soundhandle.say('Hi, my name is Petrois')
rospy.sleep(3)
self.soundhandle.say("Say one of the navigation commands")
# publish to cmd_vel, subscribe to speech output
self.pub_ = rospy.Publisher("cmd_vel", Twist, queue_size=2)
rospy.Subscriber('recognizer/output', String, self.speechCb)
r = rospy.Rate(10.0)
while not rospy.is_shutdown():
self.pub_.publish(self.msg)
r.sleep()
def speechCb(self, msg):
rospy.loginfo(msg.data)
if msg.data.find("fast") > -1:
if self.speed != 0.3:
self.soundhandle.say('Speeding up')
if self.msg.linear.x > 0:
self.msg.linear.x = 0.3
elif self.msg.linear.x < 0:
self.msg.linear.x = -0.3
if self.msg.angular.z >0:
self.msg.angular.z = 0.3
elif self.msg.angular.z < 0:
self.msg.angular.z = -0.3
self.speed = 0.3
else:
self.soundhandle.say('Already at full speed')
if msg.data.find("half") > -1:
if self.speed != 0.2:
self.soundhandle.say('Going at half speed')
if self.msg.linear.x > 0:
self.msg.linear.x = 0.2
elif self.msg.linear.x < 0:
self.msg.linear.x = -0.2
if self.msg.angular.z >0:
self.msg.angular.z = 0.2
elif self.msg.angular.z < 0:
self.msg.angular.z = -0.2
self.speed = 0.2
else:
self.soundhandle.say('Already at half speed')
if msg.data.find("slow") > -1:
if self.speed != 0.1:
self.soundhandle.say('Slowing down')
if self.msg.linear.x > 0:
self.msg.linear.x = 0.1
elif self.msg.linear.x < 0:
self.msg.linear.x = -0.1
if self.msg.angular.z >0:
self.msg.angular.z = 0.1
elif self.msg.angular.z < 0:
self.msg.angular.z = -0.1
self.speed = 0.1
else:
self.soundhandle.say('Already at slow speed')
if msg.data.find("forward") > -1:
self.soundhandle.play(1)
self.msg.linear.x = self.speed
self.msg.angular.z = 0
elif msg.data.find("left") > -1:
self.soundhandle.play(1)
if self.msg.linear.x != 0:
if self.msg.angular.z < self.speed:
self.msg.angular.z += 0.05
else:
self.msg.angular.z = self.speed*2
elif msg.data.find("right") > -1:
self.soundhandle.play(1)
if self.msg.linear.x != 0:
if self.msg.angular.z > -self.speed:
self.msg.angular.z -= 0.05
else:
self.msg.angular.z = -self.speed*2
elif msg.data.find("back") > -1:
self.soundhandle.play(1)
self.msg.linear.x = -self.speed
self.msg.angular.z = 0
elif msg.data.find("stop") > -1 or msg.data.find("halt") > -1:
self.soundhandle.play(1)
self.msg = Twist()
################################# follower commands
if msg.data.find("follow me") > -1:
if self.follower == False:
self.msg = Twist()
self.proc1 = subprocess.Popen(['roslaunch', 'pioneer_utils', 'simple-follower.launch'])
self.soundhandle.say('Okay. Show me the way')
self.follower = True
else:
self.soundhandle.say('Already in follower mode')
elif msg.data.find("stop follower") > -1:
if self.follower == True:
self.msg = Twist()
print 'proc1 = ', self.proc1.pid
self.proc1.terminate()
kill(self.proc1.pid)
self.proc1.kill()
self.follower = False
self.soundhandle.say('Follower mode disabled')
else:
self.soundhandle.say('Hey, I wasnt following you')
################################ map commands
if msg.data.find("build map") > -1:
if self.buildmap == False:
self.soundhandle.say('Building map with slam gmapping')
rospy.sleep(2)
self.soundhandle.say('Visualizing map')
self.msg = Twist()
self.proc2 = subprocess.Popen(['roslaunch', 'p2os_launch', 'gmapping.launch'])
self.proc3 = subprocess.Popen(['roslaunch', 'pioneer_utils', 'rviz-gmapping.launch'])
self.buildmap = True
else:
self.soundhandle.say('Already building a map')
elif msg.data.find("save map") > -1:
if self.buildmap == True:
self.msg = Twist()
self.proc4 = subprocess.Popen(['rosrun', 'map_server', 'map_saver', '-f', 'new_map'])
rospy.sleep(6)
print 'map saved at ~/.ros directory as new_map.pgm new_map.yaml'
self.soundhandle.say('Map saved successfully')
else:
self.soundhandle.say('I am not building any map so there is no map to save')
elif msg.data.find("stop map") > -1:
if self.buildmap == True:
self.msg = Twist()
print 'proc2 = ', self.proc2.pid
self.proc2.terminate()
kill(self.proc2.pid)
self.proc2.kill()
print 'proc3 = ', self.proc3.pid
self.proc3.terminate()
kill(self.proc3.pid)
self.proc3.kill()
print 'proc4 = ', self.proc4.pid
self.proc4.terminate()
kill(self.proc4.pid)
self.proc4.kill()
self.buildmap = False
self.soundhandle.say('Building map stopped')
else:
self.soundhandle.say('I am not building any map')
################################ navigation commands
if msg.data.find("navigate") > -1:
if self.navigation == False:
self.soundhandle.say('Starting navigation stack')
rospy.sleep(2)
self.soundhandle.say('Visualizing costmaps')
self.msg = Twist()
self.proc5 = subprocess.Popen(['roslaunch', 'pioneer_utils', 'navigation_p3at.launch'])
self.proc6 = subprocess.Popen(['roslaunch', 'pioneer_utils', 'rviz-navigation.launch'])
self.navigation = True
else:
self.soundhandle.say('Already in navigation mode')
elif msg.data.find("stop navigation") > -1:
if self.navigation == True:
self.msg = Twist()
print 'proc5 = ', self.proc5.pid
self.proc5.terminate()
kill(self.proc5.pid)
self.proc5.kill()
print 'proc6 = ', self.proc6.pid
self.proc6.terminate()
kill(self.proc6.pid)
self.proc6.kill()
self.navigation = False
self.soundhandle.say('Navigation stopped')
else:
self.soundhandle.say('I am not in navigation mode')
self.pub_.publish(self.msg)
def cleanup(self):
# stop the robot!
twist = Twist()
self.pub_.publish(twist)
soundhandle = 0
def sleep(t):
try:
rospy.sleep(t)
except:
pass
def voice_play_callback(data):
global soundhandle
s = soundhandle.voiceSound(data.data)
s.play()
sleep(2)
if __name__ == '__main__':
rospy.init_node('robot_voice_node', anonymous=True)
rospy.Subscriber("robot_voice", String, voice_play_callback)
global soundhandle
soundhandle = SoundClient()
rospy.sleep(1)
soundhandle.stopAll()
soundhandle.play(SoundRequest.NEEDS_PLUGGING)
sleep(1)
global soundhandle
s = soundhandle.voiceSound('Ass slam aly kum sir. How can I be of assistance')
s.play()
sleep(3)
rospy.spin()
print
#print 'Try to play wave files that do not exist.'
#soundhandle.playWave('17')
#soundhandle.playWave('dummy')
#print 'say'
#soundhandle.say('Hello world!')
#sleep(3)
#
print 'wave'
soundhandle.playWave('say-beep.wav')
sleep(2)
print 'plugging'
soundhandle.play(SoundRequest.NEEDS_PLUGGING)
sleep(2)
print 'unplugging'
soundhandle.play(SoundRequest.NEEDS_UNPLUGGING)
sleep(2)
print 'plugging badly'
soundhandle.play(SoundRequest.NEEDS_PLUGGING_BADLY)
sleep(2)
print 'unplugging badly'
soundhandle.play(SoundRequest.NEEDS_UNPLUGGING_BADLY)
sleep(2)
s1 = soundhandle.builtinSound(SoundRequest.NEEDS_UNPLUGGING_BADLY)
rospy.loginfo(['Stop!'])
# find direction with the largest distance from the Lidar
# rotate to that direction
# start moving
pick_direction()
# check if SLAM map is complete
if contourCheck :
if closure(occdata) :
# map is complete, so save current time into file
with open("maptime.txt", "w") as f:
f.write("Elapsed Time: " + str(time.time() - start_time))
contourCheck = 0
# play a sound
soundhandle = SoundClient()
rospy.sleep(1)
soundhandle.stopAll()
soundhandle.play(SoundRequest.NEEDS_UNPLUGGING)
rospy.sleep(2)
# save the map
cv2.imwrite('mazemap.png',occdata)
rate.sleep()
if __name__ == '__main__':
try:
mover()
except rospy.ROSInterruptException:
pass
