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
Example #2
0
    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
Example #3
0
    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()
Example #4
0
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
Example #6
0
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()
Example #7
0
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()
Example #8
0
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()
Example #10
0
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()
Example #11
0
    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)
Example #12
0
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()
Example #13
0
        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)
Example #15
0
    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()
Example #16
0
					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']
Example #19
0
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