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 calculate_drive_velocities(self, speed, current_radius):
"""
Calculate target velocities for the drive motors based on desired speed and current turning radius
:param speed: Drive speed command range from -max_vel to max_vel, with max vel depending on the turning radius
:param radius: Current turning radius in m
"""
# clip the value to the maximum allowed velocity
speed = max(-self.max_vel, min(self.max_vel, speed))
cmd_msg = CommandDrive()
if speed == 0:
return cmd_msg
elif abs(
current_radius
) >= self.max_radius: # Very large turning radius, all wheels same speed
angular_vel = speed / self.wheel_radius
cmd_msg.left_front_vel = angular_vel
cmd_msg.left_middle_vel = angular_vel
cmd_msg.left_back_vel = angular_vel
cmd_msg.right_back_vel = angular_vel
cmd_msg.right_middle_vel = angular_vel
cmd_msg.right_front_vel = angular_vel
return cmd_msg
else:
# for the calculations, we assume positive radius (turn left) and adjust later
radius = abs(current_radius)
# the entire vehicle moves with the same angular velocity dictated by the desired speed,
# around the radius of the turn. v = r * omega
angular_velocity_center = float(speed) / radius
# calculate desired velocities of all centers of wheels. Corner wheels on the same side
# move with the same velocity. v = r * omega again
vel_middle_closest = (radius - self.d4) * angular_velocity_center
vel_corner_closest = math.hypot(radius - self.d1,
self.d3) * angular_velocity_center
vel_corner_farthest = math.hypot(radius + self.d1,
self.d3) * angular_velocity_center
vel_middle_farthest = (radius + self.d4) * angular_velocity_center
# now from these desired velocities, calculate the desired angular velocity of each wheel
# v = r * omega again
ang_vel_middle_closest = vel_middle_closest / self.wheel_radius
ang_vel_corner_closest = vel_corner_closest / self.wheel_radius
ang_vel_corner_farthest = vel_corner_farthest / self.wheel_radius
ang_vel_middle_farthest = vel_middle_farthest / self.wheel_radius
if current_radius > 0: # turning left
cmd_msg.left_front_vel = ang_vel_corner_closest
cmd_msg.left_back_vel = ang_vel_corner_closest
cmd_msg.left_middle_vel = ang_vel_middle_closest
cmd_msg.right_back_vel = ang_vel_corner_farthest
cmd_msg.right_front_vel = ang_vel_corner_farthest
cmd_msg.right_middle_vel = ang_vel_middle_farthest
else: # turning right
cmd_msg.left_front_vel = ang_vel_corner_farthest
cmd_msg.left_back_vel = ang_vel_corner_farthest
cmd_msg.left_middle_vel = ang_vel_middle_farthest
cmd_msg.right_back_vel = ang_vel_corner_closest
cmd_msg.right_front_vel = ang_vel_corner_closest
cmd_msg.right_middle_vel = ang_vel_middle_closest
return cmd_msg
def calculate_drive_ratios(self, speed, current_radius):
"""
Calculate target speed ratios for the drive motors based on desired speed and current turning radius.
:param speed: Drive speed command range from -max_vel to max_vel, with max vel depending on the turning radius.
:param radius: Current turning radius in meters.
"""
# clip the value to the maximum allowed velocity
cmd_msg = CommandDrive()
if speed == 0:
return cmd_msg
elif abs(current_radius) >= self.max_radius: # Very large turning radius, all wheels same speed
cmd_msg.left_front_vel = speed
cmd_msg.left_middle_vel = speed
cmd_msg.left_back_vel = speed
cmd_msg.right_back_vel = speed
cmd_msg.right_middle_vel = speed
cmd_msg.right_front_vel = speed
return cmd_msg
else:
# for the calculations, we assume positive radius (turn left) and adjust later
radius = abs(current_radius)
# the entire vehicle moves with the same angular velocity dictated by the desired speed,
# around the radius of the turn. v = r * omega
angular_velocity_center = float(speed) / radius
# calculate desired velocities of all centers of wheels. Corner wheels on the same side
# move with the same velocity. v = r * omega again
r1 = (radius - self.d4)
r2 = ((self.d3**2 + (radius - self.d1)**2)**0.5)
r3 = ((self.d3**2 + (radius + self.d1)**2)**0.5)
r4 = (radius + self.d4)
vel_middle_closest = (r1/r4) * speed
vel_corner_closest = (r2/r4) * speed
vel_corner_farthest = (r3/r4) * speed
vel_middle_farthest = speed
if current_radius > 0: # turning left
cmd_msg.left_front_vel = vel_corner_closest
cmd_msg.left_back_vel = vel_corner_closest
cmd_msg.left_middle_vel = vel_middle_closest
cmd_msg.right_back_vel = vel_corner_farthest
cmd_msg.right_front_vel = vel_corner_farthest
cmd_msg.right_middle_vel = vel_middle_farthest
else: # turning right
cmd_msg.left_front_vel = vel_corner_farthest
cmd_msg.left_back_vel = vel_corner_farthest
cmd_msg.left_middle_vel = vel_middle_farthest
cmd_msg.right_back_vel = vel_corner_closest
cmd_msg.right_front_vel = vel_corner_closest
cmd_msg.right_middle_vel = vel_middle_closest
return cmd_msg
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()