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 setupInboundMessageCBs(self):
self.ibC = []
self.ibC.append(
(type(RunStop()), partial(self.ctrlFrame.onRunStopMsg)))
self.ibC.append(
(type(Status()), partial(self.statusFrame.onStatusMsg)))
self.ibC.append(
(type(Encoder()), partial(self.roverFrame.onEncoderMsg)))
self.ibC.append(
(type(Odometry()), partial(self.odomFrame.onOCSOdomMsg)))
self.ibC.append(
(type(Tracker()), partial(self.statusFrame.onTrackerMsg)))
self.ibC.append(
(type(CompStatus()), partial(self.roverFrame.onCompStatsMsg)))
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()
print "killing motors"
motorcontrollers.killMotors()
if __name__ == "__main__":
rospy.init_node("motor_controller")
rospy.loginfo("Starting the motor_controller node")
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
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()