def run(self): """Blocking loop which runs after initialization has completed""" rate = rospy.Rate(5) mutex_rate = rospy.Rate(10) status = Status() counter = 0 while not rospy.is_shutdown(): while self.mutex and not rospy.is_shutdown(): mutex_rate.sleep() self.mutex = True # read from roboclaws and publish try: self.read_encoder_values() self.enc_pub.publish(self.current_enc_vals) except AssertionError as read_exc: rospy.logwarn( "Failed to read encoder values") if (counter >= 10): status.battery = self.getBattery() status.temp = self.getTemp() status.current = self.getCurrents() status.error_status = self.getErrors() self.status_pub.publish(status) counter = 0 self.mutex = False counter += 1 rate.sleep()
def run(self): """Blocking loop which runs after initialization has completed""" rate = rospy.Rate(8) status = Status() counter = 0 while not rospy.is_shutdown(): # Check to see if there are commands in the buffer to send to the motor controller if self.drive_cmd_buffer: drive_fcn = self.send_drive_buffer_velocity drive_fcn(self.drive_cmd_buffer) self.drive_cmd_buffer = None if self.corner_cmd_buffer: self.send_corner_buffer(self.corner_cmd_buffer) self.corner_cmd_buffer = None # read from roboclaws and publish try: self.read_encoder_values() self.enc_pub.publish(self.current_enc_vals) except AssertionError as read_exc: rospy.logwarn("Failed to read encoder values") # Downsample the rate of less important data if (counter >= 5): status.battery = self.read_battery() status.temp = self.read_temperatures() status.current = self.read_currents() status.error_status = self.read_errors() counter = 0 # stop the motors if we haven't received a command in a while now = rospy.Time.now() if now - self.time_last_cmd > self.velocity_timeout: # rather than a hard stop, send a ramped velocity command self.drive_cmd_buffer = CommandDrive() self.send_drive_buffer_velocity(self.drive_cmd_buffer) self.time_last_cmd = now # so this doesn't get called all the time self.status_pub.publish(status) counter += 1 rate.sleep()
def run(self): """Blocking loop which runs after initialization has completed""" rate = rospy.Rate(8) status = Status() counter = 0 while not rospy.is_shutdown(): # Check to see if there are commands in the buffer to send to the motor controller if self.drive_cmd_buffer: drive_fcn = self.send_drive_buffer_velocity drive_fcn(self.drive_cmd_buffer) self.drive_cmd_buffer = None if self.corner_cmd_buffer: self.send_corner_buffer(self.corner_cmd_buffer) self.corner_cmd_buffer = None # read from roboclaws and publish try: self.read_encoder_values() self.enc_pub.publish(self.current_enc_vals) except AssertionError as read_exc: rospy.logwarn("Failed to read encoder values") # Downsample the rate of less important data if (counter >= 5): status.battery = self.getBattery() status.temp = self.getTemp() status.current = self.getCurrents() status.error_status = self.getErrors() counter = 0 self.status_pub.publish(status) counter += 1 rate.sleep()
rospy.on_shutdown(shutdown) sub = rospy.Subscriber("/robot_cmds", Commands, callback) enc_pub = rospy.Publisher("/encoder", Encoder, queue_size=1) status_pub = rospy.Publisher("/status", Status, queue_size=1) rate = rospy.Rate(5) status = Status() enc = Encoder() enc.abs_enc = [1000] * 4 enc.abs_enc_angles = [-100] * 4 status.battery = 0 status.temp = [0] * 5 status.current = [0] * 10 status.error_status = [0] * 5 counter = 0 while not rospy.is_shutdown(): while mutex: time.sleep(0.001) mutex = True enc.abs_enc = motorcontrollers.getCornerEnc() #mc_data.abs_enc_angles = motorcontrollers.getCornerEncAngle() if (counter >= 10): status.battery = motorcontrollers.getBattery() status.temp = motorcontrollers.getTemp() status.current = motorcontrollers.getCurrents() status.error_status = motorcontrollers.getErrors()
def run(self): """Blocking loop which runs after initialization has completed""" rate = rospy.Rate(8) status = Status() # time last command was executed in the run loop time_last_cmd = rospy.Time.now() # true if we're idling and started rapping down velocity to # bring the motors to full stop idle_ramp = False # if we're idled idle = False counter = 0 while not rospy.is_shutdown(): now = rospy.Time.now() # Check to see if there are commands in the buffer to send to the motor controller if self.drive_cmd_buffer: drive_fcn = self.send_drive_buffer_velocity drive_fcn(self.drive_cmd_buffer) self.drive_cmd_buffer = None time_last_cmd = now idle_ramp = False idle = False if self.corner_cmd_buffer: self.send_corner_buffer(self.corner_cmd_buffer) self.corner_cmd_buffer = None time_last_cmd = now idle_ramp = False idle = False # read from roboclaws and publish try: self.read_encoder_values() self.enc_pub.publish(self.current_enc_vals) except AssertionError as read_exc: rospy.logwarn("Failed to read encoder values") # Downsample the rate of less important data if (counter >= 5): status.battery = self.read_battery() status.temp = self.read_temperatures() status.current = self.read_currents() status.error_status = self.read_errors() counter = 0 # stop the motors if we haven't received a command in a while if not idle and (now - time_last_cmd > self.velocity_timeout): # rather than a hard stop, send a ramped velocity command to 0 if not idle_ramp: rospy.loginfo("Idling: ramping down velocity to zero") idle_ramp = True drive_cmd_buffer = CommandDrive() self.send_drive_buffer_velocity(drive_cmd_buffer) # if we've already ramped down, send a full stop to minimize # idle power consumption else: rospy.loginfo("Idling: full stopping motors") self.stop_motors() idle = True # so that's there's a delay between ramping and full stop time_last_cmd = now self.status_pub.publish(status) counter += 1 rate.sleep()