def setElevatorCb(self, goal):
digital_output = DigitalOutput()
digital_output.mask[0] = True
digital_output.mask[1] = True
if (goal.pos.value == SetPosition.RAISE):
self.feedback.value = "RAISING"
digital_output.values[0] = False
digital_output.values[1] = True
elif (goal.pos.value == SetPosition.LOWER):
self.feedback.value = "LOWERING"
digital_output.values[1] = False
digital_output.values[0] = True
else:
self.feedback.value = "STOPPED"
self.result.value = False
self.server.set_aborted(self.result)
self.pub.publish(digital_output)
self.server.publish_feedback(self.feedback)
d = rospy.Duration(4, 0)
rospy.sleep(d)
self.result.value = True
self.server.set_succeeded(self.result)
return True
def main():
global bumper
print("Lancement test servo")
rospy.init_node('test_servo')
rospy.sleep(0.5)
pub = rospy.Publisher("/mobile_base/commands/digital_output",
DigitalOutput)
cmd = DigitalOutput()
cmd.values = [True, True, True, True]
cmd.mask = [True, False, False, False]
while not rospy.is_shutdown():
print(cmd)
pub.publish(cmd)
cmd.values = [False, False, False, False]
rospy.sleep(5)
print(cmd)
pub.publish(cmd)
rospy.sleep(5)
cmd.values = [True, True, True, True]
print("je suis desole, je n'ai pas de servo")
rospy.spin() #boucle infinie tant qu'on quitte pas proprement
def __init__(self):
rospy.on_shutdown(self.cleanup)
self.voice = rospy.get_param("~voice", "voice_don_diphone")
self.wavepath = rospy.get_param("~wavepath", "")
# Create the sound client object
self.soundhandle = SoundClient()
rospy.sleep(1)
self.soundhandle.stopAll()
# Subscribe to the /mobile_base/events/digital_input topic to receive digital input
rospy.Subscriber('/mobile_base/events/digital_input', DigitalInputEvent, self.DigitalInputEventCallback)
# Subscribe to the /mobile_base/events/button topic to receive built-in button input
rospy.Subscriber('/mobile_base/events/button', ButtonEvent, self.ButtonEventCallback)
# Subscribe to the /mobile_base/events/bumper topic to receive bumper input
rospy.Subscriber('/mobile_base/events/bumper', BumperEvent, self.BumperEventCallback)
# publish Led messages to /mobile_base/commands/led1 & /mobile_base/commands/led2 topics to give Led signal
self.pub1 = rospy.Publisher('/mobile_base/commands/led1', Led)
self.pub2 = rospy.Publisher('/mobile_base/commands/led2', Led)
self.led1 = Led()
self.led2 = Led()
# publish Led messages to /mobile_base/commands/digital_output to give digital signal
self.pub3 = rospy.Publisher('/mobile_base/commands/digital_output', DigitalOutput)
self.digital_out = DigitalOutput()
rospy.sleep(1)
# Announce user that the robot is ready
self.soundhandle.say("Ready", self.voice)
# while loop
while not rospy.is_shutdown():
if digital_in[1]==True:
rospy.loginfo("button 1 pressed !!!")
elif digital_in[2]==True:
rospy.loginfo("button 2 pressed !!!")
elif digital_in[3]==True:
rospy.loginfo("button 3 pressed !!!")
if bumper == 0:
if bumper_state == 1:
self.led1.value = 1
elif bumper == 1:
if bumper_state == 1:
self.led1.value = 2
elif bumper == 2:
if bumper_state == 1:
self.led1.value = 3
if button == 0:
if button_state == 1:
self.led1.value = 1
self.digital_out.values = [False, False, False, False]
self.digital_out.mask = [True, True, True, True]
self.soundhandle.say("button 1 is pressed", self.voice)
rospy.sleep(1)
elif button == 1:
if button_state == 1:
self.led1.value = 2
self.digital_out.values = [True, True, True, True]
self.digital_out.mask = [True, True, True, True]
self.soundhandle.say("button 2 is pressed", self.voice)
rospy.sleep(1)
elif button == 2:
if button_state == 1:
self.led1.value = 3
self.soundhandle.say("button 3 is pressed", self.voice)
rospy.sleep(1)
self.pub1.publish(self.led1)
self.pub2.publish(self.led2)
self.pub3.publish(self.digital_out)
rospy.Rate(4).sleep()
def main():
global bumper
print("Lancement navigation aleatoire")
rospy.init_node('navigation_aleatoire')
rospy.sleep(0.5)
rospy.Subscriber("/mobile_base/events/bumper", BumperEvent, bumper)
rospy.Subscriber("/facedetector/faces", Detection, image)
pub = rospy.Publisher("/mobile_base/commands/velocity", Twist)
# pub2 = rospy.Publisher("/robotsound", SoundRequest)
pub3 = rospy.Publisher("/mobile_base/commands/sound", Sound)
pub4 = rospy.Publisher("/mobile_base/commands/digital_output",
DigitalOutput)
cmd = Twist()
# sound = SoundRequest()
sonnerie = Sound()
# sound.arg = "/opt/ros/indigo/share/sound_play/sounds/R2D2a.wav"
# sound.sound = -2
# sound.command = 1
sonnerie.value = 6
digOut = DigitalOutput()
digOut.values = [False, False, False, False]
digOut.mask = [True, False, False, False]
flag = 0
rospy.sleep(1)
indiana_jones.main()
while not rospy.is_shutdown():
duree = 1 + random.random() * 5
tempsActuel = rospy.get_time()
stop = rospy.get_time() + duree
while (rospy.get_time() < stop):
if bumper == 0:
cmd.linear.x = 0
cmd.angular.z = -2
pub.publish(cmd)
rospy.sleep(0.5)
bumper = 3
elif bumper == 1:
cmd.linear.x = -0.2
cmd.angular.z = 0
digOut.values[0] = True
pub.publish(cmd)
vador2.main()
# pub2.publish(sound)
pub4.publish(digOut)
rospy.sleep(5)
while (NbGens > 0):
rospy.sleep(2.5)
flag = 1
digOut.values[0] = False
if flag == 1:
pub3.publish(sonnerie)
rospy.sleep(0.5)
flag = 0
pub4.publish(digOut)
rospy.sleep(0.5)
cmd.linear.x = -0.2
cmd.angular.z = 2
pub.publish(cmd)
rospy.sleep(0.5)
bumper = 3
elif bumper == 2:
cmd.linear.x = 0
cmd.angular.z = 2
pub.publish(cmd)
rospy.sleep(0.5)
bumper = 3
else:
cmd.linear.x = 0.2
cmd.angular.z = 0
pub.publish(cmd)
if bumper > 2:
bumper = math.floor(random.random() * 10)
if bumper == 1:
bumper = 3
print("fin navigation autonome")
rospy.spin() #boucle infinie tant qu'on quitte pas proprement
}
settings = termios.tcgetattr(sys.stdin)
def clearing():
sys.stdout.write('\r\n')
sys.stdout.flush()
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
rospy.init_node("test_external_power")
rospy.on_shutdown(clearing)
pub = rospy.Publisher('/mobile_base/commands/external_power', DigitalOutput)
rate = rospy.Rate(10)
digital_output = DigitalOutput()
digital_output.values = [True, True, True, True]
digital_output.mask = [True, True, True, True]
print ""
#print "If you want to control the timing of publish, uses -p option."
#print ""
print "Control External Power"
print "----------------------"
print "1: Toggle the state of 3.3V"
print "2: Toggle the state of 5V"
print "3: Toggle the state of 12V5A(arm)"
print "4: Toggle the state of 12V1A(kinect)"
print ""
#print "p: publish power on/off status"
print "q: quit"
print ""
#rospy initial setup
rospy.init_node("test_output")
rospy.on_shutdown(clearing)
rate = rospy.Rate(10)
pub_ext_pwr = rospy.Publisher('/mobile_base/commands/external_power',
DigitalOutput)
pub_dgt_out = rospy.Publisher('/mobile_base/commands/digital_output',
DigitalOutput)
pub = []
pub.append(rospy.Publisher('/mobile_base/commands/led1', Led))
pub.append(rospy.Publisher('/mobile_base/commands/led2', Led))
pub_sounds = rospy.Publisher('/mobile_base/commands/sound', Sound)
# initial values
external_power = DigitalOutput()
external_power.values = [True, True, True, True]
external_power.mask = [True, True, True, True]
digital_output = DigitalOutput()
digital_output.values = [True, True, True, True]
digital_output.mask = [True, True, True, True]
leds = []
leds.append(Led())
leds.append(Led())
leds[0].value = Led.GREEN
leds[1].value = Led.GREEN
# initialize outputs
while not pub_ext_pwr.get_num_connections():
def __init__(self):
rospy.on_shutdown(self.cleanup)
# Subscribe to the /mobile_base/events/digital_input topic to receive digital input
rospy.Subscriber('/mobile_base/events/digital_input', DigitalInputEvent, self.DigitalInputEventCallback)
# Subscribe to the /mobile_base/events/button topic to receive built-in button input
rospy.Subscriber('/mobile_base/events/button', ButtonEvent, self.ButtonEventCallback)
# Subscribe to the /mobile_base/events/bumper topic to receive bumper input
rospy.Subscriber('/mobile_base/events/bumper', BumperEvent, self.BumperEventCallback)
# publish Led messages to /mobile_base/commands/led1 & /mobile_base/commands/led2 topics to give Led signal
self.pub1 = rospy.Publisher('/mobile_base/commands/led1', Led)
self.pub2 = rospy.Publisher('/mobile_base/commands/led2', Led)
self.led1 = Led()
self.led2 = Led()
# publish Led messages to /mobile_base/commands/digital_output to give digital signal
self.pub3 = rospy.Publisher('/mobile_base/commands/digital_output', DigitalOutput)
self.digital_out = DigitalOutput()
# while loop
while not rospy.is_shutdown():
if digital_in[1]==True:
rospy.loginfo("button 1 pressed !!!")
elif digital_in[2]==True:
rospy.loginfo("button 2 pressed !!!")
elif digital_in[3]==True:
rospy.loginfo("button 3 pressed !!!")
if bumper == 0:
if bumper_state == 1:
self.led1.value = 1
elif bumper == 1:
if bumper_state == 1:
self.led1.value = 2
elif bumper == 2:
if bumper_state == 1:
self.led1.value = 3
if button == 0:
if button_state == 1:
self.led1.value = 1
self.digital_out.values = [False, False, False, False]
self.digital_out.mask = [True, True, True, True]
elif button == 1:
if button_state == 1:
self.led1.value = 2
self.digital_out.values = [True, True, True, True]
self.digital_out.mask = [True, True, True, True]
elif button == 2:
if button_state == 1:
self.led1.value = 3
self.pub1.publish(self.led1)
self.pub2.publish(self.led2)
self.pub3.publish(self.digital_out)
rospy.Rate(4).sleep()
