def listener(command):
# Get the ~private namespace parameters from command line or launch file.
# topic = rospy.get_param('~topic', 'chatter')
## Next structure is a pythonic "select case"
options = {"show_accel" : ('/imu', Imu),
"show_tilt" : ('/cur_tilt_angle', Float64),
"show_status" : ('/cur_tilt_status', UInt8),
"update_tilt" : ('/tilt_angle', Float64),
"image_mode" : ('/camera/driver/image_mode', int),
"depth_mode" : ('~depth_mode', int),
"depth_registration" : ('~depth_registration', int),
"data_skip" : ('data_skip', int),
}
topic, dataType = options[command]
if command == "update_tilt":
pub = rospy.Publisher(topic, dataType)
pub.publish("10")
rospy.spinOnce();
return
if command == "data_skip":
client = dynamic_reconfigure.client.Client('/camera/driver')
params = { topic : int(sys.argv[2]) }
config = client.update_configuration(params)
return
timeout = 100
# Create a subscriber with appropriate topic, custom message and name of callback function.
# rospy.Subscriber(topic, Imu, callback)
print "Reading topic %s"%topic
rcvdMsg = rospy.wait_for_message(topic, dataType, timeout)
if dataType == Imu:
callback(rcvdMsg)
else:
print rcvdMsg
def step(self, max_action):
self.performAction(max_action)
unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics',
Empty) #Unpause Gazebo here!
unpaused = unpause_physics()
while (not unpaused):
rospy.spinOnce()
rospy.sleep(
self.dt_real
) #Sleep for the step length, letting the action we sent garner a result.
pause_physics = rospy.ServiceProxy('/gazebo/pause_physics',
Empty) #Pause Gazebo here!
paused = pause_physics()
while (not paused):
rospy.spinOnce()
new_obs = self.getObs()
reward = self.getReward()
done = numpy.array([self.getDone()])
info = "Envs finished:" + str(done)
#Update previous position.
self.prevPos = self.currPos[:]
return new_obs, reward, done, info
def reset(self):
reset = rospy.ServiceProxy('/gazebo/reset_world', Empty)
resetted = reset()
while (not reset):
rospy.spinOnce()
#Reinit current positon, previous position
self.currPos = [0, 0]
self.prevPos = [-1, -1]
if self.currObs is not None:
return self.getObs()
else:
rospy.logwarn(
"In env.reset(): self.currObs is None! Waiting for observations... "
)
time_start = rospy.get_rostime()
unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics",
Empty)
unpaused = unpause_physics()
while (self.currObs is None or len(self.currObs) !=
math.floor(1081 /
(self.cellsPerDegree * self.degreesPerArc)) + 1):
rospy.sleep(1)
rospy.logwarn("Observations found {}! Resuming...".format(
len(self.currObs)))
return self.getObs()
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('subscriptor', anonymous=True)
print "Funciona"
#Obstaculos
rospy.Subscriber("/b_r1", Pose2D, callbackObs1)
rospy.Subscriber("/b_r2", Pose2D, callbackObs2)
rospy.Subscriber("/b_r3", Pose2D, callbackObs3)
rospy.Subscriber("/b_r4", Pose2D, callbackObs4)
rospy.Subscriber("/b_r0", Pose2D, callbackObs0)
#meta
rospy.Subscriber("/ball", Pose2D, callbackMeta)
#robot
rospy.Subscriber("/y_r0", Pose2D, callbackRobot)
while not rospy.is_shutdown():
if(bplan):
ruta = getRuta()
print ruta
ejecutaPlan(ruta)
# spin() simply keeps python from exiting until this node is stopped
rospy.spinOnce()
def listener(command):
# Get the ~private namespace parameters from command line or launch file.
# topic = rospy.get_param('~topic', 'chatter')
## Next structure is a pythonic "select case"
options = {
"show_accel": ('/imu', Imu),
"show_tilt": ('/cur_tilt_angle', Float64),
"show_status": ('/cur_tilt_status', UInt8),
"update_tilt": ('/tilt_angle', Float64),
"image_mode": ('/camera/driver/image_mode', int),
"depth_mode": ('~depth_mode', int),
"depth_registration": ('~depth_registration', int),
"data_skip": ('data_skip', int),
}
topic, dataType = options[command]
if command == "update_tilt":
pub = rospy.Publisher(topic, dataType)
pub.publish("10")
rospy.spinOnce()
return
if command == "data_skip":
client = dynamic_reconfigure.client.Client('/camera/driver')
params = {topic: int(sys.argv[2])}
config = client.update_configuration(params)
return
timeout = 100
# Create a subscriber with appropriate topic, custom message and name of callback function.
# rospy.Subscriber(topic, Imu, callback)
print "Reading topic %s" % topic
rcvdMsg = rospy.wait_for_message(topic, dataType, timeout)
if dataType == Imu:
callback(rcvdMsg)
else:
print rcvdMsg
def plotServer():
rospy.init_node("graph_plot_server")
server = rospy.Service("graph_plot", Empty, graphPlotCallBack)
print "Graph Plot Service ready"
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
rospy.spinOnce()
r.sleep()
def plotServer():
rospy.init_node('graph_plot_server')
server = rospy.Service('graph_plot', Empty, graphPlotCallBack)
print "Graph Plot Service ready"
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
rospy.spinOnce()
r.sleep()
def init():
rospy.init_node('create_odom', anonymous=True)
rospy.Subscriber("cmd_vel")
rospy.Publisher("odom")
rate_hz = 100
rate = rospy.Rate(rate_hz)
while not rospy.is_shutdown():
rospy.spinOnce()
calculate()
last_time = rospy.rostime.get_time()
rate.sleep()
def main():
rospy.init_node('node_choice_fonction')
pub = rospy.Publisher('fonc_to_choose', String, queue_size=10)
rospy.Subscriber("wind_angle", Quaternion, callback)
rate = rospy.Rate(100)
while not rospy.is_shutdown():
msg = String()
msg.data=0
pub.publish()
rospy.spinOnce()
rate.sleep()
def listal(): # listen-talk
rospy.init_node('listal', anonymous=True)
pub = rospy.Publisher('chatter', String, queue_size=10)
sub = rospy.Subscriber('chatter', String, callback)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
hello_str = "hello world %s" % rospy.get_time()
rospy.loginfo(hello_str)
pub.publish(hello_str)
rospy.spinOnce()
rate.sleep()
def _oneInteraction(self):
self.stepid += 1
self.agent.integrateObservation(self.task.getObservation())
self.task.performAction(self.agent.getAction())
unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) #Unpause Gazebo here!
while(not unpause):
rospy.spinOnce()
rospy.sleep(self.stepLength) #Sleep for the step length, letting the action we sent garner a result.
pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty) #Pause Gazebo here!
while(not pause):
rospy.spinOnce()
reward = self.task.getReward()
self.agent.giveReward(reward)
def gps_to_utm():
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
pub = rospy.Publisher('/ground/odom', Odometry, queue_size=1)
rospy.init_node('gps_to_utm', anonymous=True)
rate = rospy.Rate(5)
rospy.Subscriber("/at_drone_interface/gps_full_data", GpsData, callback)
# spin() simply keeps python from exiting until this node is stopped
while not rospy.is_shutdown():
pub.publish(msg)
rospy.spinOnce()
for y in range(10):
self.cf.commander.send_hover_setpoint(0, 0, 0,
alt - (y / 10 * alt))
time.sleep(0.1)
## IMAGE HANDLING / THREADS ##
# TODO: Katie figure this out -- gets images and sends it across network
# runs in parallel to main thread
def image_thread(self):
pass
DEFAULT_URI = 'radio://0/80/250K'
if __name__ == '__main__':
rospy.init_node('CFNode', anonymous=True)
if not rospy.has_param('id') or not rospy.has_param('uri'):
print("No ID or URI Specified! Abort.")
return
cf = CFNode(int(rospy.get_param('id', '0')),
rospy.get_param('uri', DEFAULT_URI))
rate = rospy.Rate(10)
while not rospy.is_shutdown():
rospy.spinOnce()
rate.sleep()
def run(self):
while (rospy.is_shutdown == False):
rospy.spinOnce()
rospy.sleep(0.1)
def __init__(self):
rospy.init_node('lezl')
self.pose = PoseStamped()
self.state_sub = rospy.Subscriber('/mavros/state',State,self.state_cb)
self.odom_sub = rospy.Subscriber('/p3at/odom',Odometry,self.odom_cb)
self.tracked_sub = rospy.Subscriber('/tracker/tracked',Bool,self.track_cb)
self.landed_sub = rospy.Subscriber('/lezl/landed',Bool,self.landed_cb)
self.track_pub = rospy.Publisher('/tracker/tracked',Bool,queue_size=10)
self.tag_track_pub = rospy.Publisher('/tag_detections/tracked',Bool,queue_size=10)
self.state_pub = rospy.Publisher('/lezl/state',TwistStamped,queue_size=10)
self.heading_sub = rospy.Subscriber('/tracker/heading',TwistStamped,self.head_cb)
self.apriltag_sub = rospy.Subscriber('/tag_detections',AprilTagDetectionArray,self.parse_tags)
local_pos_pub = rospy.Publisher('/mavros/setpoint_position/local',PoseStamped,queue_size=10)
self.cam_info_sub = rospy.Subscriber('/camera1/camera_info',CameraInfo,self.get_cam_info)
self.cam_info_pub = rospy.Publisher('/landing_pad/camera_info',CameraInfo,queue_size=10)
local_vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',TwistStamped,queue_size=10)
self.arming_client = rospy.ServiceProxy('/mavros/cmd/arming',CommandBool)
self.set_mode_client = rospy.ServiceProxy('/mavros/set_mode',SetMode)
command_client = rospy.ServiceProxy('/mavros/cmd/command',CommandLong)
self.state = State()
self.tag_track = Bool()
self.cam_info = CameraInfo()
self.tracked = False
self.landed = False
self.lezl_state = TwistStamped()
rate = rospy.Rate(20)
while self.state.connected:
rospy.spinOnce()
rate.sleep()
self.pose.pose.position.x = 0
self.pose.pose.position.y = 0
self.pose.pose.position.z = 5
wait = rospy.Time.now()
offb = True
print "Starting Mission"
while not rospy.is_shutdown():
if self.landed:
print "Landed!!!"
break
# print (rospy.Time.now() - wait)>rospy.Duration(5)
if (self.state.mode != "OFFBOARD" and
(rospy.Time.now() - wait) > rospy.Duration(5)):
print "Attempting to enter OFFBOARD"
self.mode_switch("OFFBOARD")
start = rospy.Time.now()
else:
if (not self.state.armed and
(rospy.Time.now() - wait) > rospy.Duration(5)):
print "Attempting to arm"
self.arm_cmd(1)
if not offb and self.state.mode == 'OFFBOARD':
print "Watching for landing"
self.watchdog(start)
self.pose.header.stamp = rospy.Time.now()
if not self.tracked:
local_pos_pub.publish(self.pose)
else:
pass # Controller() class should be handling this
#local_vel_pub.publish(self.heading)
rate.sleep()
print " sail out " self.sail_angle += self.incr elif key == curses.KEY_RIGHT: if self.verbose: stdscr.addstr(3, 30, "Rudder Right") print " rudder right " self.rudder_angle += self.incr if self.verbose: print " rudder %f, sail %f" %(self.rudder_angle, self.sail_angle) if self.rudder_angle < 0: self.rudder_angle = 0 if self.sail_angle < 0: self.sail_angle = 0 self.rudder_publisher.publish(self.rudder_angle) self.sail_publisher.publish(self.sail_angle) curses.endwin() return (self.rudder_angle, self.sail_angle) if __name__ == "__main__": t=teleop() t.verbose = 1 while 1: t.get_user_input() print t.rudder_angle rospy.spinOnce()
def json_grabber():
while not rospy.is_shutdown:
with urllib.request.urlopen('http://172.16.0.1:8001/FieldData/GetData') as response:
self.source = response.read()
self.data = json.loads(source.decode())
json_ball_grabber()
json_corner_grabber()
json_team_grabber()
rospy.spinOnce()
rospy.sleep()
def json_corner_grabber():
#Corners data parsing
corners = data['Corners']
cornerBL = corners[0]
cornerBR = corners[1]
cornerTL = corners[2]
cornerTR = corners[3]
corner.BotL.x = cornerBL['X']
corner.BotL.y = cornerBL['Y']
corner.BotR.x = cornerBR['X']
corner.BotR.y = cornerBR['Y']
corner.TopL.x = cornerTL['X']
corner.TopL.y = cornerTL['Y']
corner.TopR.x = cornerTR['X']
corner.TopR.y = cornerTR['Y']
def json_team_grabber():
#Red Team data parsing
Red = data['Red Team Data']
#Red Circle
rCircle = Red['Circle']
rcCenterPoint = rCircle['Object Center']
#Coordinates
rccpX = rcCenterPoint['X']
rccpY = rcCenterPoint['Y']
#Bounding Box
rcBoundinBox = rCircle['Bounding Box']
rcbbXL = rcBoundinBox['X Left']
rcbbYT = rcBoundinBox['Y Top']
rcbbXR = rcBoundinBox['X Right']
rcbbYB = rcBoundinBox['Y Bottom']
#Area
rcArea = rCircle['Area']
#Red Square
rSquare = Red['Square']
rsCenterPoint = rSquare['Object Center']
#Coordinates
rscpX = rsCenterPoint['X']
rscpY = rsCenterPoint['Y']
#Bounding Box
rsBoundinBox = rSquare['Bounding Box']
rsbbXL = rsBoundinBox['X Left']
rsbbYT = rsBoundinBox['Y Top']
rsbbXR = rsBoundinBox['X Right']
rsbbYB = rsBoundinBox['Y Bottom']
#Area
rsArea = rSquare['Area']
#Red Triangle
rTriangle = Red['Triangle']
rtCenterPoint = rTriangle['Object Center']
#Coordinates
rtcpX = rtCenterPoint['X']
rtcpY = rtCenterPoint['Y']
#Bounding Btox
rtBoundinBox = rTriangle['Bounding Box']
rtbbXL = rtBoundinBox['X Left']
rtbbYT = rtBoundinBox['Y Top']
rtbbXR = rtBoundinBox['X Right']
rtbbYB = rtBoundinBox['Y Bottom']
#Area
rtArea = rTriangle['Area']
#Write Red team data to ROS message (.msg)
#Blue Team data parsing
Blue = data['Blue Team Data']
#Blue Circle
bCircle = Blue['Circle']
bcCenterPoint = bCircle['Object Center']
#Coordinates
bccpX = bcCenterPoint['X']
bccpY = bcCenterPoint['Y']
#Bounding Box
bcBoundinBox = bCircle['Bounding Box']
bcbbXL = bcBoundinBox['X Left']
bcbbYT = bcBoundinBox['Y Top']
bcbbXR = bcBoundinBox['X Right']
bcbbYB = bcBoundinBox['Y Bottom']
#Area
bcArea = bCircle['Area']
#Blue Square
bSquare = Blue['Square']
bsCenterPoint = bSquare['Object Center']
#Coordinates
bscpX = bsCenterPoint['X']
bscpY = bsCenterPoint['Y']
#Bounding Box
bsBoundinBox = bSquare['Bounding Box']
bsbbXL = bsBoundinBox['X Left']
bsbbYT = bsBoundinBox['Y Top']
bsbbXR = bsBoundinBox['X Right']
bsbbYB = bsBoundinBox['Y Bottom']
#Area
bsArea = bSquare['Area']
#Blue Triangle
bTriangle = Blue['Triangle']
btCenterPoint = bTriangle['Object Center']
#Coordinates
btcpX = btCenterPoint['X']
btcpY = btCenterPoint['Y']
#Bounding Box
btBoundinBox = bSquare['Bounding Box']
btbbXL = btBoundinBox['X Left']
btbbYT = btBoundinBox['Y Top']
btbbXR = btBoundinBox['X Right']
btbbYB = btBoundinBox['Y Bottom']
#Area
btArea = bTriangle['Area']
def __init__(self):
init_pubs()
self.corner = Map()
with urllib.request.urlopen('http://172.16.0.1:8001/FieldData/GetData') as response:
self.source = response.read()
self.data = json.loads(source.decode())
while not rospy.is_shutdown():
# self.json_grabber()
# pub_corner.publish(corners)
# pub_ball.publish(ball_coordinates)
# pub_red_triangle.publish(rtCenterPoint)
# pub_red_square.publish(rsCenterPoint)
# pub_red_circle.publish(rcCenterPoint)
# pub_blue_triangle.publish(btCenterPoint)
# pub_blue_square.publish(bsCenterPoint)
# pub_blue_circle.publish(bcCenterPoint)
rospy.spinOnce()
rospy.sleep()
# Initializes publishers
def init_pubs():
rospy.loginfo('Initializing publisher...')
pub_corner = rospy.Publisher('mapper/raw_data/corners',Map,queue_size=10)
pub_ball = rospy.Publisher('mapper/raw_data/ball',Map,queue_size=10)
pub_red_triangle = rospy.Publisher('mapper/raw_data/red/triangle',Map,queue_size=10)
pub_red_square = rospy.Publisher('mapper/raw_data/red/square',Map,queue_size=10)
pub_red_circle = rospy.Publisher('mapper/raw_data/red/circle',Map,queue_size=10)
pub_blue_triangle = rospy.Publisher('mapper/raw_data/blue/triangle',Map,queue_size=10)
pub_blue_square = rospy.Publisher('mapper/raw_data/blue/square',Map,queue_size=10)
pub_blue_circle = rospy.Publisher('mapper/raw_data/blue/circle',Map,queue_size=10)
rospy.loginfo('Done.')
rate = rospy.Rate(10) # 10 Hz
def json_grabber():
while not rospy.is_shutdown:
with urllib.request.urlopen('http://172.16.0.1:8001/FieldData/GetData') as response:
self.source = response.read()
self.data = json.loads(source.decode())
json_ball_grabber()
json_corner_grabber()
json_team_grabber()
rospy.spinOnce()
rospy.sleep()
def json_corner_grabber():
#Corners data parsing
corners = data['Corners']
cornerBL = corners[0]
cornerBR = corners[1]
cornerTL = corners[2]
cornerTR = corners[3]
corner.BotL.x = cornerBL['X']
corner.BotL.y = cornerBL['Y']
corner.BotR.x = cornerBR['X']
corner.BotR.y = cornerBR['Y']
corner.TopL.x = cornerTL['X']
corner.TopL.y = cornerTL['Y']
corner.TopR.x = cornerTR['X']
corner.TopR.y = cornerTR['Y']
def json_team_grabber():
#Red Team data parsing
Red = data['Red Team Data']
#Red Circle
rCircle = Red['Circle']
rcCenterPoint = rCircle['Object Center']
#Coordinates
rccpX = rcCenterPoint['X']
rccpY = rcCenterPoint['Y']
#Bounding Box
rcBoundinBox = rCircle['Bounding Box']
rcbbXL = rcBoundinBox['X Left']
rcbbYT = rcBoundinBox['Y Top']
rcbbXR = rcBoundinBox['X Right']
rcbbYB = rcBoundinBox['Y Bottom']
#Area
rcArea = rCircle['Area']
#Red Square
rSquare = Red['Square']
rsCenterPoint = rSquare['Object Center']
#Coordinates
rscpX = rsCenterPoint['X']
rscpY = rsCenterPoint['Y']
#Bounding Box
rsBoundinBox = rSquare['Bounding Box']
rsbbXL = rsBoundinBox['X Left']
rsbbYT = rsBoundinBox['Y Top']
rsbbXR = rsBoundinBox['X Right']
rsbbYB = rsBoundinBox['Y Bottom']
#Area
rsArea = rSquare['Area']
#Red Triangle
rTriangle = Red['Triangle']
rtCenterPoint = rTriangle['Object Center']
#Coordinates
rtcpX = rtCenterPoint['X']
rtcpY = rtCenterPoint['Y']
#Bounding Btox
rtBoundinBox = rTriangle['Bounding Box']
rtbbXL = rtBoundinBox['X Left']
rtbbYT = rtBoundinBox['Y Top']
rtbbXR = rtBoundinBox['X Right']
rtbbYB = rtBoundinBox['Y Bottom']
#Area
rtArea = rTriangle['Area']
#Write Red team data to ROS message (.msg)
#Blue Team data parsing
Blue = data['Blue Team Data']
#Blue Circle
bCircle = Blue['Circle']
bcCenterPoint = bCircle['Object Center']
#Coordinates
bccpX = bcCenterPoint['X']
bccpY = bcCenterPoint['Y']
#Bounding Box
bcBoundinBox = bCircle['Bounding Box']
bcbbXL = bcBoundinBox['X Left']
bcbbYT = bcBoundinBox['Y Top']
bcbbXR = bcBoundinBox['X Right']
bcbbYB = bcBoundinBox['Y Bottom']
#Area
bcArea = bCircle['Area']
#Blue Square
bSquare = Blue['Square']
bsCenterPoint = bSquare['Object Center']
#Coordinates
bscpX = bsCenterPoint['X']
bscpY = bsCenterPoint['Y']
#Bounding Box
bsBoundinBox = bSquare['Bounding Box']
bsbbXL = bsBoundinBox['X Left']
bsbbYT = bsBoundinBox['Y Top']
bsbbXR = bsBoundinBox['X Right']
bsbbYB = bsBoundinBox['Y Bottom']
#Area
bsArea = bSquare['Area']
#Blue Triangle
bTriangle = Blue['Triangle']
btCenterPoint = bTriangle['Object Center']
#Coordinates
btcpX = btCenterPoint['X']
btcpY = btCenterPoint['Y']
#Bounding Box
btBoundinBox = bSquare['Bounding Box']
btbbXL = btBoundinBox['X Left']
btbbYT = btBoundinBox['Y Top']
btbbXR = btBoundinBox['X Right']
btbbYB = btBoundinBox['Y Bottom']
#Area
btArea = bTriangle['Area']
def main():
rospy.init_node('data_record',anonymous=True)
rospy.Subscriber('/test_topic',Obu_connect,recordCallback)
rospy.spinOnce()
rospy.loginfo('Digital Compass Setup Complete!')
else:
#sned error no data in buffer
rospy.logerr('[Sparton][2]Received No data from DigitalCompass. Shutdown!')
rospy.signal_shutdown('Received No data from DigitalCompass')
#### loop-back test only
#Testdata='P:,%i,ap,-6.34,-22.46,1011.71,gp,0.00,0.00,-0.00,yt,342.53,q,0.98,-0.01,0.01,-0.15\n'
# 0 1 2 3 4 5 6 7 8 9 10 11 1213 14 15 16
#Testdata='P:,%i,ap,-6.34,-22.46,1011.71,gp,0.00,0.00,-0.00,q,0.98,-0.01,0.01,-0.15\n'
#Testdata='P:,%i,ap,-6.34,-22.46,1011.71,gp,0.00,0.00,-0.00,q,1.,0.,0.,0.\n'
# 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14
#i=0
### end of loop-back test
while not rospy.is_shutdown():
rospy.spinOnce();
### loop-back test only
#i+=1
#D_Compass.write(Testdata % i) # send testdata and do loop-back in RS232 for debug
### end of loop-back test
data = D_Compass.readline()
#rospy.loginfo("Received a sentence: %s" % data)
#if not check_checksum(data):
# rospy.logerr("Received a sentence with an invalid checksum. Sentence was: %s" % data)
# continue
#DatatimeNow = rospy.get_rostime()
DataTimeSec=rospy.get_time()
fields = data.split(',')
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('motor_GPIO', anonymous=False)
rospy.Subscriber("motor_commands", String, callback)
x_dirPin = gpiozero.LED(2)
y_dirPin = gpiozero.LED(4)
z_dirPin = gpiozero.LED(6)
x_speedPin = gpiozero.LED(3)
y_speedPin = gpiozero.LED(5)
z_speedPin = gpiozero.LED(7)
while not rospy.is_shutdown():
#directional pins
if (commands[0] == "left"):
x_dirPin.off()
else:
x_dirPin.on()
if (commands[2] == "back"):
y_dirPin.off()
else:
y_dirPin.on()
if (commands[4] == "down"):
z_dirPin.off()
else:
z_dirPin.on()
#update speeds
x_speed = int(commands[1])
y_speed = int(commands[3])
z_speed = int(commands[5])
#if you're not holding down the button, set speed to zero
if (commands[0] == "stop"):
x_speed = 0
if (commands[2] == "stop"):
y_speed = 0
if (commands[4] == "stop"):
z_speed = 0
#finds periods at which to pulse with
x_period = 1/x_speed*1000000
y_period = 1/y_speed*1000000
z_period = 1/z_speed*1000000
#use mod of timer by period to see if it's time to pulse on or off
if((time.time*1000000)%x_period < x_period//2):
x_speedPin.on()
else:
x_speedPin.off()
if((time.time*1000000)%y_period < y_period//2):
y_speedPin.on()
else:
x_speedPin.off()
if((time.time*1000000)%z_period < z_period//2):
z_speedPin.on()
else:
z_speedPin.off()
rospy.spinOnce() # spinOnce() checks for new messages once
