def sendLEDs():
led = Led()
if (stopMode):
led.value = 3
else:
led.value = 1
pubLED.publish(led)
def mitsos():
pub = rospy.Publisher('/mobile_base/commands/led1', Led, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
rate1 = rospy.Rate(1)
while not rospy.is_shutdown():
z=0
while z < 5:
ledstate = Led()
ledstate.value = 1
pub.publish(ledstate)
z += 0.1
print(z)
rate.sleep()
y=0
while y < 5:
ledstate = Led()
ledstate.value = 0
pub.publish(ledstate)
y += 0.5
print(y)
rate1.sleep()
def light(self):
"""
Flashes the light on the robot (useful for checking connection or for raves)
"""
#r: var used to control the flashing frequenzy in hz
# cmd: data structure for the led() information, for more information refer to msg docs on Led()
#create a publisher which talks to the lights
#rate at chich to publish
r = rospy.Rate(10);
#create the message to the node
cmd = Led()
#set value
cmd.value = 1
t = 10
while not rospy.is_shutdown() and t > 0:
cmd.value = 1
#publish the message
self.led1.publish(cmd)
rospy.loginfo("published the info to the node")
r.sleep()
cmd.value = 3
self.led1.publish(cmd)
r.sleep()
t = t-1
rospy.sleep(1)
def ButtonEventCallback(self,data):
pub = []
pub.append(rospy.Publisher('/mobile_base/commands/led1',Led))
pub.append(rospy.Publisher('/mobile_base/commands/led2',Led))
rate = rospy.Rate(1)
leds = []
leds.append(Led())
leds.append(Led())
#LED STUFF
if ( data.state == ButtonEvent.RELEASED ):
state = "released"
leds[0].value = Led.BLACK
leds[1].value = Led.BLACK
else:
state = "pressed"
if ( data.button == ButtonEvent.Button0 ):
button = "B0"
leds[0].value = Led.GREEN
leds[1].value = Led.ORANGE
elif ( data.button == ButtonEvent.Button1 ):
button = "B1"
leds[0].value = Led.ORANGE
leds[1].value = Led.RED
if (self.flag == 1):
os.system("rosrun turtlesim turtlesim_node &");
self.flag = 0;
else:
button = "B2"
leds[0].value = Led.BLACK
leds[1].value = Led.BLACK
os.system("rosnode kill /turtlesim");
pub[0].publish(leds[0])
pub[1].publish(leds[1])
rospy.loginfo("Button %s was %s."%(button, state))
def __init__(self):
rospy.init_node('Ubuntu_Demo', anonymous=True)
# How to stop Turtlebot
rospy.loginfo("To stop Turtlebot CTRL + c")
# What to do on CTRL + C
rospy.on_shutdown(self.shutdown)
self.cmd_vel = rospy.Publisher('/cmd_vel_mux/input/navi',
Twist,
queue_size=10)
self.pub1 = rospy.Publisher('/mobile_base/commands/led1',
Led,
queue_size=1)
self.pub2 = rospy.Publisher('/mobile_base/commands/led2',
Led,
queue_size=1)
self.led1 = Led()
self.led2 = Led()
rospy.Subscriber('/mobile_base/events/bumper', BumperEvent,
self.processBumperEvent)
rospy.Subscriber('/mobile_base/events/button', ButtonEvent,
self.processButtonEvent)
rospy.Subscriber('/mobile_base/sensors/core', SensorState,
self.processSensorState)
def cleanUp():
global currentCommand
currentCommand.linear.x = 0.0
currentCommand.angular.z = 0.0
led = Led()
led.value = 0
pubLED.publish(led)
pub.publish(currentCommand)
rospy.sleep(1)
def sendLED():
rospy.init_node('led_sender', anonymous=True)
pub = rospy.Publisher('/mobile_base/commands/led1', Led, queue_size=10)
while pub.get_num_connections() == 0:
pass
led = Led()
x = 3
while x != -1:
led.value = x
pub.publish(led)
x = x - 1
rospy.sleep(1)
def countCallback(self, msg):
print("GOT COUNT", msg)
if msg.data == 1:
self.led1_pub.publish(Led(1))
elif msg.data == 2:
self.led2_pub.publish(Led(1))
elif msg.data == 3:
self.led1_pub.publish(Led(1))
self.led2_pub.publish(Led(1))
for _ in range(msg.data):
self.sound_pub.publish(Sound(0))
rospy.sleep(rospy.Duration(0.5))
self.done_count = True
def set_led(self, led, color):
"""Set the color of an LED
You can set LED 1 or LED 2 to any of these colors:
- 'off'/'black'
- 'green'
- 'orange'
- 'red'
Example:
robot.set_led(1, 'green')
robot.wait(1)
robot.set_led(1, 'off')
"""
if str(led) not in self.__led_pubs:
self.say(
"!! Invalid led '{0}', must be either '1' or '2'".format(led))
return
if color not in self.led_colors:
self.say("!! Invalid led color '{0}', must be one of: {1}".format(
color, self.led_colors))
return
self.__led_pubs[str(led)].publish(Led(self.led_colors[color]))
def execute(self, userdata):
global START, SHAPE
self.led1_pub.publish(Led(0))
self.led2_pub.publish(Led(0))
self.count2_pub.publish(Bool(data=True))
print("published")
while not rospy.is_shutdown():
if self.done_shape and self.done_count:
break
rospy.Rate(10).sleep()
if not START:
return "exit"
return "success"
def toogle_led(self):
""" Toggle the LED on the robot
"""
self.led_state = not self.led_state
rospy.loginfo(rospy.get_caller_id() + "toggle led: %s ",
self.led_state)
i = 1 if self.led_state else 0
self.led_pub.publish(Led(i))
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.linear)
pub = rospy.Publisher('/mobile_base/commands/led1', Led, queue_size=10)
print "L I N E A R"
print data.linear.x
if data.linear.x == 0.1:
ledstate = Led()
ledstate.value = 1
ledstate.value = 2
pub.publish(ledstate)
else:
ledstate = Led()
ledstate.value = 0
pub.publish(ledstate)
def execute(self, userdata):
global START
print userdata.object_count
if START and not rospy.is_shutdown():
if userdata.object_count == 1:
self.led1_pub.publish(Led(1))
elif userdata.object_count == 2:
self.led2_pub.publish(Led(1))
elif userdata.object_count == 3:
self.led1_pub.publish(Led(1))
self.led2_pub.publish(Led(1))
for _ in range(userdata.object_count):
self.sound_pub.publish(Sound(0))
rospy.sleep(rospy.Duration(0.5))
return "success"
if not START:
return "exit"
def update_state(self, state):
super(LedWidget, self).update_state(state)
self._pub.publish(Led(state))
if state is 1:
self.setToolTip("LED: Green")
elif state is 2:
self.setToolTip("LED: Orange")
elif state is 3:
self.setToolTip("LED: Red")
else:
self.setToolTip("LED: Off")
def display_led(count):
global led_pub1, led_pub2
if count == 0:
led_pub1.publish(Led(Led.BLACK))
led_pub2.publish(Led(Led.BLACK))
elif count == 1:
led_pub1.publish(Led(Led.BLACK))
led_pub2.publish(Led(Led.ORANGE))
elif count == 2:
led_pub1.publish(Led(Led.ORANGE))
led_pub2.publish(Led(Led.BLACK))
elif count == 3:
led_pub1.publish(Led(Led.ORANGE))
led_pub2.publish(Led(Led.ORANGE))
def leds(self):
while self.is_disabled() and not rospy.is_shutdown():
self.pub_led1.publish(Led(value=2))
self.pub_led2.publish(Led(value=3))
rospy.sleep(0.5)
self.pub_led1.publish(Led(value=3))
self.pub_led2.publish(Led(value=2))
rospy.sleep(0.5)
self.pub_led1.publish(Led(value=0))
self.pub_led2.publish(Led(value=0))
def __Set_led(self, color):
# Set the color of an LED
# You can set LED to any of these colors:
# - 'off'/'black'
# - 'green'
# - 'orange'
# - 'red'
# Example:
# robot.set_led('green')
# robot.set_led('off')
if color not in self.__led_colors:
print("!! Invalid led color " + color)
rospy.signal_shutdown("!! Invalid led color " + color)
return
if self.__color_status != color:
#print ("Led" + str(self.__led_num) + ": " + color)
self.__led_pub.publish(Led(self.__led_colors[color]))
self.__color_status = color
return
#!/usr/bin/env python
import rospy
from kobuki_msgs.msg import Led
from kobuki_msgs.msg import ButtonEvent
from kobuki_msgs.msg import BumperEvent
from std_msgs.msg import Empty
import math
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
pub = rospy.Publisher("/mobile_base/commands/velocity", Twist, queue_size=10)
pub1 = rospy.Publisher('/mobile_base/commands/led1', Led, queue_size=10)
pub2 = rospy.Publisher('/mobile_base/commands/led2', Led, queue_size=10)
led = Led()
buttonGreen = True
currentCommand = Twist()
currentCommand.linear.x = 0.0
currentCommand.angular.z = 0.0
targetCommand = Twist()
targetCommand.linear.x = 0.0
targetCommand.linear.y = 0.0
targetCommand.angular.z = 0.0
x = 0.0
y = 0.0
def resetter():
pub = rospy.Publisher('/mobile_base/commands/reset_odometry', Empty,
queue_size=10)
while pub.get_num_connections() == 0:
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 shutdown(self):
# stop turtlebot
rospy.loginfo("Stop TurtleBot")
self.led1.publish(Led())
def __init__(self):
rospy.on_shutdown(self.cleanup)
# 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/led1 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 command message to joints/servos of arm
self.joint4 = rospy.Publisher('/arm_wrist_flex_joint/command',
Float64) #4
self.joint1 = rospy.Publisher('/arm_shoulder_pan_joint/command',
Float64) #1
self.joint2 = rospy.Publisher('/arm_shoulder_lift_joint/command',
Float64) #2
self.joint3 = rospy.Publisher('/arm_elbow_flex_joint/command',
Float64) #3
self.joint5 = rospy.Publisher('/gripper_joint/command', Float64)
self.pos1 = Float64()
self.pos2 = Float64()
self.pos3 = Float64()
self.pos4 = Float64()
self.pos5 = Float64()
#subscribe to the robot sensor state
rospy.Subscriber('/blobs', Blobs, self.callback)
self.message = Twist()
# publish twist messages to mobile base velocity
self.pub = rospy.Publisher('/mobile_base/commands/velocity', Twist)
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 recognizer output
rospy.Subscriber('/recognizer/output', String, self.identify)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait for the action server to become available
self.move_base.wait_for_server(rospy.Duration(120))
rospy.loginfo("Connected to move base server")
# Announce that we are ready for input
self.soundhandle.playWave(self.wavepath + "/R2D2a.wav")
rospy.sleep(1)
self.soundhandle.say("initiated", self.voice)
rospy.sleep(1)
# A variable to hold the initial pose of the robot to be set by the user in RViz
initial_pose = PoseWithCovarianceStamped()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped,
self.update_initial_pose)
# Get the initial pose from the user
self.soundhandle.say("Where am I", self.voice)
rospy.loginfo(
"*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..."
)
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
self.soundhandle.say("Where to find bottle", self.voice)
# Arm in "home" position
self.pos1 = 0.000
self.pos2 = -1.658
self.pos3 = -2.88
self.pos4 = -1.01
self.pos5 = 0.000
self.joint1.publish(self.pos1)
self.joint2.publish(self.pos2)
self.joint3.publish(self.pos3)
self.joint4.publish(self.pos4)
self.joint5.publish(self.pos5)
rospy.sleep(3)
locations = dict()
quaternion = quaternion_from_euler(0.0, 0.0, 1.5708)
locations['A'] = Pose(
Point(0.5, 0.5, 0.000),
Quaternion(quaternion[0], quaternion[1], quaternion[2],
quaternion[3]))
quaternion = quaternion_from_euler(0.0, 0.0, 3.1412)
locations['B'] = Pose(
Point(0.5, -0.5, 0.000),
Quaternion(quaternion[0], quaternion[1], quaternion[2],
quaternion[3]))
quaternion = quaternion_from_euler(0.0, 0.0, -1.5708)
locations['C'] = Pose(
Point(1.0, 1.0, 0.000),
Quaternion(quaternion[0], quaternion[1], quaternion[2],
quaternion[3]))
quaternion = quaternion_from_euler(0.0, 0.0, 0.0)
locations['D'] = Pose(
Point(1.0, -1.0, 0.000),
Quaternion(quaternion[0], quaternion[1], quaternion[2],
quaternion[3]))
self.goal = MoveBaseGoal()
rospy.loginfo("Starting navigation test")
while not rospy.is_shutdown():
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
if hand == 1:
self.pos1 = 0.000
self.pos2 = 0.902
self.pos3 = -1.892
self.pos4 = -0.436
self.pos5 = -0.523
self.joint1.publish(self.pos1)
self.joint2.publish(self.pos2)
self.joint3.publish(self.pos3)
self.joint4.publish(self.pos4)
self.joint5.publish(self.pos5)
rospy.sleep(1)
self.pos4 = 1.57
self.joint4.publish(self.pos4)
rospy.sleep(1)
self.pos1 = 0.000
self.pos2 = 1.745
self.pos3 = -1.57
self.joint1.publish(self.pos1)
self.joint2.publish(self.pos2)
self.joint3.publish(self.pos3)
rospy.sleep(2)
global start
global hand
start = 3
hand = 0
if hand == 2:
self.pos1 = 0.000
self.pos2 = 1.745
self.pos3 = -1.05
self.pos4 = 1.178
self.pos5 = -0.523
self.joint1.publish(self.pos1)
self.joint2.publish(self.pos2)
self.joint3.publish(self.pos3)
self.joint4.publish(self.pos4)
self.joint5.publish(self.pos5)
rospy.sleep(1)
self.pos5 = 0.48
self.joint5.publish(self.pos5)
rospy.sleep(2)
self.pos1 = 0.000
self.pos2 = -0.9265
self.pos3 = -2.387
self.pos5 = 0.48
self.joint1.publish(self.pos1)
self.joint2.publish(self.pos2)
self.joint3.publish(self.pos3)
self.joint5.publish(self.pos5)
rospy.sleep(1)
self.pos4 = -0.30
self.joint4.publish(self.pos4)
rospy.sleep(2)
global hand
global start
hand = 0
start = 4
if start == 1:
if message == "point A":
self.soundhandle.say("Going to Argentina", self.voice)
rospy.sleep(2)
self.goal.target_pose.pose = locations['A']
self.move_base.send_goal(self.goal)
waiting = self.move_base.wait_for_result(
rospy.Duration(300))
if waiting == 1:
self.soundhandle.say("Reached Argentina", self.voice)
rospy.sleep(2)
self.soundhandle.say("Ready for next location",
self.voice)
rospy.sleep(2)
global start
start = 0
if message == "point B":
self.soundhandle.say("Going to Brazil", self.voice)
rospy.sleep(2)
self.goal.target_pose.pose = locations['B']
self.move_base.send_goal(self.goal)
waiting = self.move_base.wait_for_result(
rospy.Duration(300))
if waiting == 1:
self.soundhandle.say("Reached Brazil", self.voice)
rospy.sleep(2)
self.soundhandle.say("Ready to go", self.voice)
rospy.sleep(2)
global start
start = 0
if message == "point C":
self.soundhandle.say("Going to China", self.voice)
rospy.sleep(2)
self.goal.target_pose.pose = locations['C']
self.move_base.send_goal(self.goal)
waiting = self.move_base.wait_for_result(
rospy.Duration(300))
if waiting == 1:
self.soundhandle.say("Reached China", self.voice)
rospy.sleep(2)
self.soundhandle.say("Ready for next place",
self.voice)
rospy.sleep(2)
global start
start = 0
if message == "point D":
self.soundhandle.say("Going to Japan", self.voice)
rospy.sleep(2)
self.goal.target_pose.pose = locations['D']
self.move_base.send_goal(self.goal)
waiting = self.move_base.wait_for_result(
rospy.Duration(300))
if waiting == 1:
self.soundhandle.say("Reached Japan", self.voice)
rospy.sleep(2)
self.soundhandle.say("Finding bottle", self.voice)
rospy.sleep(2)
global start
start = 2
if message == "origin":
self.soundhandle.say("Going back home", self.voice)
rospy.sleep(2)
self.goal.target_pose.pose = self.origin
self.move_base.send_goal(self.goal)
waiting = self.move_base.wait_for_result(
rospy.Duration(300))
if waiting == 1:
#self.soundhandle.say("Reached Malaysia", self.voice)
#rospy.sleep(2)
if again == 3:
self.soundhandle.say("Here is your bottle",
self.voice)
rospy.sleep(2)
global start
start = 10
else:
self.soundhandle.say("Home sweet home", self.voice)
rospy.sleep(2)
global start
start = 0
#self.move_base.wait_for_result(rospy.Duration(5))
#rospy.Rate(2).sleep()
if start == 2:
self.message.linear.x = 0.0
self.message.linear.y = 0
self.message.linear.z = 0
self.message.angular.x = 0.0
self.message.angular.y = 0
self.message.angular.z = turn
self.pub.publish(self.message)
rospy.sleep(0.3)
self.message.linear.x = 0.0
self.message.linear.y = 0
self.message.linear.z = 0
self.message.angular.x = 0.0
self.message.angular.y = 0
self.message.angular.z = 0
self.pub.publish(self.message)
rospy.sleep(2.5)
if start == 3:
if bumper == 0 or bumper == 1 or bumper == 2:
if bumper_state == 1:
rospy.loginfo("bumper hit!")
self.led1.value = 3
global start
global hand
self.message.linear.x = 0.0
self.message.linear.y = 0
self.message.linear.z = 0
self.message.angular.x = 0.0
self.message.angular.y = 0
self.message.angular.z = 0
self.pub.publish(self.message)
#self.cmd_vel_pub.publish(self.message)
rospy.sleep(1)
#self.digital_out.values = [True, True, True, True]
#self.digital_out.mask = [True, True, True, True]
#self.pub3.publish(self.digital_out)
start = 10
hand = 2
if start != 10:
self.message.linear.x = 0.10
self.message.linear.y = 0
self.message.linear.z = 0
self.message.angular.x = 0.0
self.message.angular.y = 0
self.message.angular.z = 0
self.pub.publish(self.message)
#rospy.Rate(5).sleep()
if start == 4:
self.message.linear.x = -0.4
self.message.linear.y = 0
self.message.linear.z = 0
self.message.angular.x = 0.0
self.message.angular.y = 0
self.message.angular.z = 0
self.pub.publish(self.message)
#self.cmd_vel_pub.publish(self.message)
rospy.sleep(2)
self.message.linear.x = 0.0
self.message.linear.y = 0
self.message.linear.z = 0
self.message.angular.x = 0.0
self.message.angular.y = 0
self.message.angular.z = 0
self.pub.publish(self.message)
#self.cmd_vel_pub.publish.publish(self.message)
global message
message = "origin"
rospy.sleep(1)
global start
start = 1
# send the message and delay
self.pub1.publish(self.led1)
self.pub2.publish(self.led2)
rospy.Rate(50).sleep()
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()
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():
rate.sleep()
pub_ext_pwr.publish(external_power)
while not pub_dgt_out.get_num_connections():
rate.sleep()
pub_dgt_out.publish(digital_output)
while not pub[0].get_num_connections():
rate.sleep()
#!/usr/bin/env python
import rospy
from kobuki_msgs.msg import Led
if __name__ == '__main__':
rospy.init_node("checkpoint1_3_2_led_python", anonymous=True)
pub = rospy.Publisher("/mobile_base/commands/led1", Led, queue_size=1)
rospy.sleep(1)
msg = Led()
for i in range(3):
msg.value = i + 1
rospy.loginfo("publish msg [value: %d]" % (msg.value))
pub.publish(msg)
rospy.sleep(1)
msg.value = 0
rospy.loginfo("publish msg [value: %d]" % (msg.value))
pub.publish(msg)
goal = getFrontierCentroid(frontier)
print "goal"
print goal
rospy.wait_for_service('goal_nav_service')
driveToGoal = rospy.ServiceProxy('goal_nav_service', GoToGoal)
response = driveToGoal(goal[0], goal[1])
if (response.status == 0):
break
print "done with current frontier"
preexplorationMove()
print "identifying new frontiers"
frontiersList = identifyFrontiers()
#call("spd-say 'I'm done.'");
i = 1
ledmsg = Led()
while (not rospy.is_shutdown()):
print "DONE EXPLORING"
if ((i%2) == 0):
ledmsg.value = 0
else:
ledmsg.value = (i+1)/2
i += 1
i = i%6
led1Pub.publish(ledmsg)
led2Pub.publish(ledmsg)
time.sleep(0.5)
ledmsg.value = 0
led1Pub.publish(ledmsg)
def enable_LEDs(self): msg = Led() msg.value = Led.GREEN for pub in self.led_publishers: pub.publish(msg)
def enable_LEDs(self):
msg = Led()
msg.value = Led.GREEN
for pub in self.led_publishers:
pub.publish(msg)
def disable_LEDs(self): msg = Led() msg.value = Led.BLACK for pub in self.led_publishers: pub.publish(msg)
def disable_LEDs(self):
msg = Led()
msg.value = Led.BLACK
for pub in self.led_publishers:
pub.publish(msg)
def ledUpdate(value):
global pub_led1
led = Led()
led.value = value
pub_led1.publish(value)
goal = getFrontierCentroid(frontier)
print "goal"
print goal
rospy.wait_for_service('goal_nav_service')
driveToGoal = rospy.ServiceProxy('goal_nav_service', GoToGoal)
response = driveToGoal(goal[0], goal[1])
if (response.status == 0):
break
print "done with current frontier"
preexplorationMove()
print "identifying new frontiers"
frontiersList = identifyFrontiers()
#call("spd-say 'I'm done.'");
i = 1
ledmsg = Led()
while (not rospy.is_shutdown()):
print "DONE EXPLORING"
if ((i % 2) == 0):
ledmsg.value = 0
else:
ledmsg.value = (i + 1) / 2
i += 1
i = i % 6
led1Pub.publish(ledmsg)
led2Pub.publish(ledmsg)
time.sleep(0.5)
ledmsg.value = 0
led1Pub.publish(ledmsg)
