def cmd_vel_callback(self, twist):
self.last_set_speed_time = rospy.get_rostime()
linear_x = twist.linear.x
if linear_x > self.MAX_SPEED:
linear_x = self.MAX_SPEED
if linear_x < -self.MAX_SPEED:
linear_x = -self.MAX_SPEED
vr = linear_x + twist.angular.z * self.BASE_WIDTH / 2.0 # m/s
vl = linear_x - twist.angular.z * self.BASE_WIDTH / 2.0
vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s
vl_ticks = int(vl * self.TICKS_PER_METER)
rospy.loginfo("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)
try:
if vr_ticks is 0 and vl_ticks is 0:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
else:
if 0 <= vr_ticks:
roboclaw.ForwardM1(self.address, vr_ticks)
else:
roboclaw.BackwardM1(self.address, -vr_ticks)
if 0 <= vl_ticks:
roboclaw.ForwardM2(self.address, vl_ticks)
else:
roboclaw.BackwardM2(self.address, -vl_ticks)
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
def cmd_vel_callback(self, twist):
if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 0.1:
self.last_set_speed_time = rospy.get_rostime()
linear_x = twist.linear.x
if linear_x > self.MAX_SPEED:
linear_x = self.MAX_SPEED
if linear_x < -self.MAX_SPEED:
linear_x = -self.MAX_SPEED
vr = linear_x + twist.angular.z * (
self.BASE_WIDTH + self.BASE_LENGTH) # m/s
vl = linear_x - twist.angular.z * (self.BASE_WIDTH +
self.BASE_LENGTH)
vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s
vl_ticks = int(vl * self.TICKS_PER_METER)
# rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)
try:
# This is a hack way to keep a poorly tuned PID from making noise at speed 0
if vr_ticks is 0 and vl_ticks is 0:
roboclaw.ForwardM1(self.rear_address, 0) # left
roboclaw.ForwardM2(self.rear_address, 0) # right
roboclaw.ForwardM1(self.front_address, 0) # left
roboclaw.ForwardM2(self.front_address, 0) # right
else:
roboclaw.SpeedM1M2(self.rear_address, vl_ticks, vr_ticks)
roboclaw.SpeedM1M2(self.front_address, vl_ticks, vr_ticks)
except OSError as e:
rospy.logwarn("Rear SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
def cmd_vel_callback(self, twist):
self.last_set_speed_time = rospy.get_rostime()
linear_x = twist.linear.x
if linear_x > self.MAX_SPEED:
linear_x = self.MAX_SPEED
if linear_x < -self.MAX_SPEED:
linear_x = -self.MAX_SPEED
vr = linear_x + twist.angular.z * self.BASE_WIDTH / 2.0 # m/s
vl = linear_x - twist.angular.z * self.BASE_WIDTH / 2.0
vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s
vl_ticks = int(vl * self.TICKS_PER_METER)
rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)
try:
# This is a hack way to keep a poorly tuned PID from making noise at speed 0
if vr_ticks is 0 and vl_ticks is 0:
roboclaw.ForwardM1(self.addressBackMotors, 0)
roboclaw.ForwardM2(self.addressBackMotors, 0)
roboclaw.ForwardM1(self.addressFrontMotors, 0)
roboclaw.ForwardM2(self.addressFrontMotors, 0)
else:
roboclaw.SpeedM1M2(self.addressBackMotors, vr_ticks, vl_ticks)
roboclaw.SpeedM1M2(self.addressFrontMotors, vr_ticks, vl_ticks)
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
def cmd_pwm_callback(self, point):
self.last_set_speed_time = rospy.get_rostime()
motor1 = int(point.x)
motor2 = int(point.y)
# rospy.logdebug("motor1:%d motor2: %d", motor1, motor2)
try:
# This is a hack way to keep a poorly tuned PID from making noise at speed 0
if motor1 is 0 and motor2 is 0:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
else:
if (motor1 >= 0):
roboclaw.ForwardM1(self.address, motor1)
else:
roboclaw.BackwardM1(self.address, -motor1)
if (motor2 >= 0):
roboclaw.ForwardM2(self.address, motor2)
else:
roboclaw.BackwardM2(self.address, -motor2)
except OSError as e:
rospy.logwarn("Motor OSError: %d", e.errno)
rospy.logdebug(e)
def run(self):
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(10)
while not rospy.is_shutdown():
if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 1:
rospy.loginfo("Did not get command for 1 second, stopping")
try:
roboclaw.ForwardM1(self.address_front, 0)
roboclaw.ForwardM2(self.address_front, 0)
roboclaw.ForwardM1(self.address_back, 0)
roboclaw.ForwardM2(self.address_back, 0)
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
# TODO need find solution to the OSError11 looks like sync problem with serial
try:
status1, enc1, crc1 = roboclaw.ReadEncM1(self.address_front)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM1 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status2, enc2, crc2 = roboclaw.ReadEncM2(self.address_front)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status3, enc3, crc3 = roboclaw.ReadEncM1(self.address_back)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM3 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status4, enc4, crc4 = roboclaw.ReadEncM2(self.address_back)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM4 OSError: %d", e.errno)
rospy.logdebug(e)
if ('enc1' in vars()) and ('enc2' in vars()) and (
'enc3' in vars()) and ('enc4' in vars()):
rospy.logdebug(" Encoders %d %d %d %d" %
(enc1, enc2, enc3, enc4))
self.encodm.update_publish(enc1, enc2, enc3, enc4)
self.updater.update()
r_time.sleep()
def shutdown(self):
rospy.loginfo("Shutting down")
try:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
except OSError:
rospy.logerr("Shutdown did not work trying again")
try:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
except OSError as e:
rospy.logerr("Could not shutdown motors!!!!")
rospy.logdebug(e)
def go_twist(self, twist):
linear_x = twist.linear.x
if linear_x > self.MAX_SPEED:
linear_x = self.MAX_SPEED
if linear_x < -self.MAX_SPEED:
linear_x = -self.MAX_SPEED
vr = linear_x + twist.angular.z * self.BASE_WIDTH / 2.0 # m/s
vl = linear_x - twist.angular.z * self.BASE_WIDTH / 2.0
vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s
vl_ticks = int(vl * self.TICKS_PER_METER)
rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)
try:
# This is a hack way to keep a poorly tuned PID from making noise at speed 0
if False and vr_ticks is 0 and vl_ticks is 0:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
else:
if(self.LEFT_MOTOR_NUMBER == 1):
m1_ticks = vl_ticks
m2_ticks = vr_ticks
else:
m1_ticks = vr_ticks
m2_ticks = vl_ticks
roboclaw.SpeedAccelM1M2_2(self.address, 0, m1_ticks, 0, m2_ticks)
except OSError as e:
rospy.logwarn("SpeedAccelM1M2_2 OSError: %d", e.errno)
rospy.logdebug(e)
def cmd(self, arr):
self.last_set_speed_time = rospy.get_rostime()
if len(arr) != len(self.addresses) * 2:
rospy.logwarn(
"node configured to control %d motors but %d speeds received" %
(len(self.addresses), len(arr)))
return
i = 0
for addr in self.addresses:
# convert radians per sec to ticks per sec
v1 = arr[i] * self.TICKS_PER_RADIAN
v2 = arr[i + 1] * self.TICKS_PER_RADIAN
# clamp
if self.MAX_SPEED != 0:
if abs(v1) > self.MAX_SPEED: v1 = copysign(self.MAX_SPEED, v1)
if abs(v2) > self.MAX_SPEED: v2 = copysign(self.MAX_SPEED, v2)
v1, v2 = int(v1), int(v2)
# now send
try:
if v1 == 0 and v2 == 0:
roboclaw.ForwardM1(addr, 0)
roboclaw.ForwardM2(addr, 0)
else:
roboclaw.SpeedAccelM1M2(addr, 10000, v1, v2)
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
i += 2
def run(self):
r_time = rospy.Rate(10)
while not rospy.is_shutdown():
if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 1:
rospy.logdebug("Did not get comand for 1 second, stopping")
try:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
# TODO need find solution to the OSError11 looks like sync problem with serial
status1, enc1, crc1 = None, None, None
status2, enc2, crc2 = None, None, None
try:
status1, enc1, crc1 = roboclaw.ReadEncM1(self.address)
except ValueError:
rospy.logwarn("Value Error passed M1")
pass
except OSError as e:
rospy.logwarn("ReadEncM1 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status2, enc2, crc2 = roboclaw.ReadEncM2(self.address)
except ValueError:
rospy.logwarn("Value Error passed M2")
pass
except OSError as e:
rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
#if (enc1 in locals()) and (enc2 in locals()):
try:
if (g_invert_motor_axes):
enc1 = -enc1
enc2 = -enc2
if (g_flip_left_right_motors):
enc1_t = enc1
enc2_t = enc2
enc2 = enc1_t
enc1 = enc2_t
rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
self.encodm.update_publish(
enc2, enc1) #update_publish expects enc_left enc_right
#self.updater.update()
except:
print("problems reading encoders")
r_time.sleep()
def cmd_vel_callback(self, twist):
self.last_set_speed_time = rospy.get_rostime()
linear_x = twist.linear.x
if linear_x > self.MAX_SPEED:
linear_x = self.MAX_SPEED
if linear_x < -self.MAX_SPEED:
linear_x = -self.MAX_SPEED
rospy.logdebug("linear_x before:%f, after:%f", twist.linear.x,
linear_x)
# velocity in m/s
v_l = linear_x - twist.angular.z * self.BASE_WIDTH / 2.0
v_r = linear_x + twist.angular.z * self.BASE_WIDTH / 2.0
rospy.logdebug("angular.z:%f, base_width:%f, v_l:%f, v_r:%f",
twist.angular.z, self.BASE_WIDTH, v_l, v_r)
# roboclaw duty values
rc_val_l = int(self.MAX_COMMAND_VALUE / self.MAX_SPEED * v_l)
rc_val_r = int(self.MAX_COMMAND_VALUE / self.MAX_SPEED * v_r)
rospy.logdebug("rc_val_l before:%f, rc_val_r before:%f", rc_val_l,
rc_val_r)
if rc_val_l > self.MAX_COMMAND_VALUE:
rc_val_l = self.MAX_COMMAND_VALUE
if rc_val_l < -self.MAX_COMMAND_VALUE:
rc_val_l = -self.MAX_COMMAND_VALUE
if rc_val_r > self.MAX_COMMAND_VALUE:
rc_val_r = self.MAX_COMMAND_VALUE
if rc_val_r < -self.MAX_COMMAND_VALUE:
rc_val_r = -self.MAX_COMMAND_VALUE
rospy.logdebug("rc_val_l after:%f, rc_val_r after:%f", rc_val_l,
rc_val_r)
try:
if rc_val_r >= 0:
roboclaw.ForwardM1(self.address, rc_val_r)
else:
roboclaw.BackwardM1(self.address, rc_val_r * -1)
if rc_val_l >= 0:
roboclaw.ForwardM2(self.address, rc_val_l)
else:
roboclaw.BackwardM2(self.address, rc_val_l * -1)
except OSError as e:
rospy.logwarn("ForwardM1/M2 OSError: %d", e.errno)
rospy.logdebug(e)
def run(self):
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(30)
while not rospy.is_shutdown():
with self.lock:
if (rospy.get_rostime() -
self.last_set_speed_time).to_sec() > 1:
rospy.loginfo("Did not get comand for 1 second, stopping")
try:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
# TODO need find solution to the OSError11 looks like sync problem with serial
status1, enc1, crc1 = None, None, None
status2, enc2, crc2 = None, None, None
statusC, amp1, amp2 = None, None, None
try:
status1, enc1, crc1 = roboclaw.ReadEncM1(self.address)
except ValueError:
pass
try:
status2, enc2, crc2 = roboclaw.ReadEncM2(self.address)
except ValueError:
pass
try:
status1c, amp1, amp2 = roboclaw.ReadCurrents(self.address)
except ValueError:
pass
if (enc1 != None) & (enc2 != None):
rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
self.encodm.update_publish(enc1, enc2)
self.updater.update()
else:
rospy.logdebug("Error Reading enc")
if (amp1 != None) & (amp2 != None):
rospy.logdebug(" Currents %d %d" % (amp1, amp2))
amps = Motors_currents()
amps.motor1 = float(amp1) / 100
amps.motor2 = float(amp2) / 100
self.motors_currents_pub.publish(amps)
else:
rospy.logdebug("Error Reading Currents")
r_time.sleep()
def run(self):
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(10)
while not rospy.is_shutdown():
if not self.stopped and (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 1:
rospy.loginfo("Did not get command for 1 second, stopping")
try:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
self.stopped = True
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
else:
if self.twist != None:
self.go_twist(self.twist)
# TODO need find solution to the OSError11 looks like sync problem with serial
status1, enc1, crc1 = None, None, None
status2, enc2, crc2 = None, None, None
try:
status1, enc1, crc1 = roboclaw.ReadEncM1(self.address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM1 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status2, enc2, crc2 = roboclaw.ReadEncM2(self.address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
if (enc1 != None and enc2 != None):
rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
if(self.LEFT_MOTOR_NUMBER == 1):
enc_left = enc1
enc_right = enc2
else:
enc_left = enc2
enc_right = enc1
self.encodm.update_publish(enc_left, enc_right)
self.updater.update()
r_time.sleep()
def run(self):
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(10)
while not rospy.is_shutdown():
if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 1:
rospy.loginfo("Did not get command for 1 second, stopping")
try:
# roboclaw.ForwardM1(self.address, 0)
# roboclaw.ForwardM2(self.address, 0)
with self.mutex:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
# TODO need find solution to the OSError11 looks like sync problem with serial
status1, enc1, crc1 = None, None, None
status2, enc2, crc2 = None, None, None
try:
# status1, enc1, crc1 = roboclaw.ReadEncM1(self.address)
with self.mutex:
status1, enc1, crc1 = roboclaw.ReadEncM1(self.address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM1 OSError: %d", e.errno)
rospy.logdebug(e)
try:
# status2, enc2, crc2 = roboclaw.ReadEncM2(self.address)
with self.mutex:
status2, enc2, crc2 = roboclaw.ReadEncM2(self.address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
#if ('enc1' in vars()) and ('enc2' in vars()):
if (isinstance(enc1, Number) and isinstance(enc2, Number)):
rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
# self.encodm.update_publish(enc1, enc2)
self.encodm.update_publish(enc2, enc1)
self.updater.update()
r_time.sleep()
def run(self):
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(10)
while not rospy.is_shutdown():
if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 1:
rospy.loginfo("Did not get command for 1 second, stopping")
try:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
r_time.sleep()
def run(self):
if self.twist is None:
return
# self.last_set_speed_time = rospy.get_rostime()
if self.twist.linear.x != 0 or self.twist.angular.z != 0:
self.stopped = False
linear_x = self.twist.linear.x
if linear_x > self.MAX_SPEED:
linear_x = self.MAX_SPEED
if linear_x < -self.MAX_SPEED:
linear_x = -self.MAX_SPEED
vr = linear_x + self.twist.angular.z * self.BASE_WIDTH # m/s
vl = linear_x - self.twist.angular.z * self.BASE_WIDTH
self.twist = None
vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s
vl_ticks = int(vl * self.TICKS_PER_METER)
#print("-- vr_ticks:{} vl_ticks:{}".format(vr_ticks, vl_ticks))
rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)
try:
# This is a hack way to keep a poorly tuned PID from making noise at speed 0
# if vr_ticks is 0 and vl_ticks is 0:
# roboclaw.ForwardM1(self.address, 0)
# roboclaw.ForwardM2(self.address, 0)
# else:
# roboclaw.SpeedM1M2(self.address, vr_ticks, vl_ticks)
if vr_ticks is 0 and vl_ticks is 0:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
self.vr_ticks = 0
self.vl_ticks = 0
else:
gain = 0.5
self.vr_ticks = gain * vr_ticks + (1 - gain) * self.vr_ticks
self.vl_ticks = gain * vl_ticks + (1 - gain) * self.vl_ticks
roboclaw.SpeedM1M2(self.address, int(self.vr_ticks),
int(self.vl_ticks))
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
def cmd_vel_callback(self, twist):
"""Command the motors based on the incoming twist message"""
self.last_set_speed_time = rospy.get_rostime()
rospy.logdebug("Twist: -Linear X: %d -Angular Z: %d",twist.linear.x,twist.angular.z)
linear_x = twist.linear.x
angular_z = twist.angular.z
if linear_x > self.LINEAR_MAX_SPEED:
linear_x = self.LINEAR_MAX_SPEED
elif linear_x < -self.LINEAR_MAX_SPEED:
linear_x = -self.LINEAR_MAX_SPEED
# Take linear x and angular z values and compute command
motor1_command = linear_x/self.LINEAR_MAX_SPEED + angular_z/self.ANGULAR_MAX_SPEED
motor2_command = linear_x/self.LINEAR_MAX_SPEED - angular_z/self.ANGULAR_MAX_SPEED
# Scale to motor pwm
motor1_command = int(motor1_command * 127)
motor2_command = int(motor2_command * 127)
# Clip commands to within bounds (-127,127)
motor1_command = min(127, motor1_command)
motor1_command = max(-127, motor1_command)
motor2_command = min(127, motor2_command)
motor2_command = max(-127, motor2_command)
rospy.logdebug("motor1 command = %d",int(motor1_command))
rospy.logdebug("motor2 command = %d",int(motor2_command))
try:
if motor1_command >= 0:
roboclaw.ForwardM1(self.address, motor1_command)
else:
roboclaw.BackwardM1(self.address, -motor1_command)
if motor2_command >= 0:
roboclaw.ForwardM2(self.address, motor2_command)
else:
roboclaw.BackwardM2(self.address, -motor2_command)
except OSError as e:
rospy.logwarn("Roboclaw OSError: %d", e.errno)
rospy.logdebug(e)
def run(self):
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(30)
while not rospy.is_shutdown():
# stop movement if robot doesn't recieve commands for 1 sec
if (self.STOP_MOVEMENT and not self.movement.stopped
and rospy.get_rostime().to_sec() -
self.movement.last_set_speed_time.to_sec() > 1):
rospy.loginfo("Did not get command for 1 second, stopping")
try:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
self.movement.stopped = True
# TODO need find solution to the OSError11 looks like sync problem with serial
status1, enc1, crc1 = None, None, None
status2, enc2, crc2 = None, None, None
try:
status1, enc1, crc1 = roboclaw.ReadEncM1(self.address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM1 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status2, enc2, crc2 = roboclaw.ReadEncM2(self.address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
if ('enc1' in vars()) and ('enc2' in vars() and enc1 and enc2):
rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
if (self.encodm):
self.encodm.update_publish(enc1, enc2)
self.updater.update()
self.movement.run()
r_time.sleep()
def run(self):
"""Run the main ros loop"""
rospy.loginfo("Starting motor drive")
roboclaw.Flush()
r_time = rospy.Rate(10)
while not rospy.is_shutdown():
if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > self.TIMEOUT:
rospy.loginfo("Did not get command for 1 second, stopping")
try:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
roboclaw.SpeedM1M2(self.address, 0, 0)
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
# TODO need find solution to the OSError11 looks like sync problem with serial status1, enc1, crc1 = None, None, None
status2, enc2, crc2 = None, None, None
try:
status1, enc1, crc1 = roboclaw.ReadEncM1(self.address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM1 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status2, enc2, crc2 = roboclaw.ReadEncM2(self.address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
if ((enc1 is not None) and (enc2 is not None)):
rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
self.encodm.update_publish(enc1, enc2)
self.updater.update()
r_time.sleep()
def cmd_vel_callback(self, twist):
self.last_set_speed_time = rospy.get_rostime()
linear_x = -twist.linear.x
angular_z = twist.angular.z
# Set speed limits
if abs(linear_x) > self.MAX_SPEED_LINEAR:
linear_x = np.sign(linear_x) * self.MAX_SPEED_LINEAR
if abs(angular_z) > self.MAX_SPEED_ANGULAR:
angular_z = np.sign(angular_z) * self.MAX_SPEED_ANGULAR
vel_new = np.array([linear_x, angular_z])
self.vel_old = self.a * vel_new + (1.0 - self.a) * self.vel_old
linear_x, angular_z = self.vel_old[0], self.vel_old[1]
print("==== cmd_callback ====")
print("> linear: ", linear_x, " | ", self.MAX_SPEED_LINEAR)
print("> angular: ", angular_z, " | ", self.MAX_SPEED_ANGULAR)
vr = linear_x + twist.angular.z * self.BASE_WIDTH / 2.0 # m/s
vl = linear_x - twist.angular.z * self.BASE_WIDTH / 2.0
print("--------------------")
print("> vr: ", vr)
print("> vl: ", vl)
print("=======================")
vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s
vl_ticks = int(vl * self.TICKS_PER_METER)
rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)
try:
# This is a hack way to keep a poorly tuned PID from making noise at speed 0
if abs(vr_ticks) <= 1e-5 and abs(vl_ticks) <= 1e-5:
with self.mutex:
self.vel_old = np.array([0.0, 0.0])
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
else:
with self.mutex:
roboclaw.SpeedM1M2(self.address, vr_ticks, vl_ticks)
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
def shutdown(self):
"""Handle shutting down the node"""
rospy.loginfo("Shutting down")
if hasattr(self, "sub"):
self.sub.unregister() # so it doesn't get called after we're dead
try:
roboclaw.Open(self.dev_name, self.baud_rate)
roboclaw.SpeedM1M2(self.address, 0, 0)
roboclaw.Close()
rospy.loginfo("Closed Roboclaw serial connection")
#roboclaw.ForwardM1(self.address, 0)
#roboclaw.ForwardM2(self.address, 0)
except OSError:
rospy.logerr("Shutdown did not work trying again")
try:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
except OSError as e:
rospy.logerr("Could not shutdown motors!!!!")
rospy.logdebug(e)
quit()
def cmd_vel_callback(self, twist):
self.last_set_speed_time = rospy.get_rostime()
max_linear_speed = 1.7
max_angular_speed = 1.7
lx = self.BASE_LENGTH
ly = self.BASE_WIDTH
r = self.WHEEL_RADIUS
linear_x = twist.linear.x
linear_y = twist.linear.y
angular_z = twist.angular.z
if linear_x > max_linear_speed:
linear_x = max_linear_speed
if linear_x < -max_linear_speed:
linear_x = -max_linear_speed
if linear_y > max_linear_speed:
linear_y = max_linear_speed
if linear_y < -max_linear_speed:
linear_y = -max_linear_speed
if angular_z > max_angular_speed:
angular_z = max_angular_speed
if angular_z < -max_angular_speed:
angular_z = -max_angular_speed
#######inverse kinematic Equations
B_inv = np.array([[1, -1, -(lx + ly) / 2], [1, 1, (lx + ly) / 2],
[1, 1, -(lx + ly) / 2], [1, -1, (lx + ly) / 2]])
X[0] = linear_x
X[1] = linear_y
X[2] = angular_z
U_inv = X
#wheel velocities
U = np.dot(B_inv, U_inv)
try:
if U[0] is 0 and U[1] is 0:
roboclaw.ForwardM1(self.address_front, 0)
roboclaw.ForwardM2(self.address_front, 0)
roboclaw.ForwardM1(self.address_back, 0)
roboclaw.ForwardM2(self.address_back, 0)
else:
real_sp_m1 = ((U[0] / 1.7) * 63) + 64
real_sp_m2 = ((U[1] / 1.7) * 63) + 64
real_sp_m3 = ((U[2] / 1.7) * 63) + 64
real_sp_m4 = ((U[3] / 1.7) * 63) + 64
roboclaw.ForwardBackwardM1(self.address_front, int(real_sp_m1))
roboclaw.ForwardBackwardM2(self.address_front, int(real_sp_m2))
roboclaw.ForwardBackwardM1(self.address_back, int(real_sp_m3))
roboclaw.ForwardBackwardM2(self.address_back, int(real_sp_m4))
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
import signal
import sys
import roboclaw_driver.roboclaw_driver as rc
rc.Open('/dev/ttyACM0', 115200)
address = 0x80
rc.ForwardMixed(address, 0)
rc.TurnRightMixed(address, 0)
print("run speed command")
try:
rc.SpeedM1(address, 300)
time.sleep(2)
rc.SpeedM2(address, 300)
time.sleep(2)
rc.SpeedM1M2(address, 150, 150)
time.sleep(2)
rc.SpeedM1(address, -300)
time.sleep(2)
rc.SpeedM2(address, -300)
time.sleep(2)
rc.SpeedM1M2(address, -150, -150)
time.sleep(2)
rc.SpeedM1M2(address, 0, 0)
except Exception as e:
print(e)
finally:
print("Finish")
rc.ForwardM1(address, 0)
rc.ForwardM2(address, 0)
#Windows comport name
#rc = Roboclaw("COM9",115200)
#Linux comport name
#rc = Roboclaw("/dev/ttyACM0",115200)
rc.Open("/dev/ttyACM0", 115200)
address = 0x80
while(1):
rc.ForwardM1(address,32) #1/4 power forward
rc.BackwardM2(address,32) #1/4 power backward
time.sleep(2)
rc.BackwardM1(address,32) #1/4 power backward
rc.ForwardM2(address,32) #1/4 power forward
time.sleep(2)
rc.BackwardM1(address,0) #Stopped
rc.ForwardM2(address,0) #Stopped
time.sleep(5)
m1duty = 16
m2duty = -16
rc.ForwardBackwardM1(address,64+m1duty) #1/4 power forward
rc.ForwardBackwardM2(address,64+m2duty) #1/4 power backward
time.sleep(2)
m1duty = -16
m2duty = 16
rc.ForwardBackwardM1(address,64+m1duty) #1/4 power backward
def run(self):
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(10)
while not rospy.is_shutdown():
if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 1:
rospy.loginfo("Did not get command for 1 second, stopping")
try:
roboclaw.ForwardM1(self.rear_address, 0)
roboclaw.ForwardM2(self.rear_address, 0)
roboclaw.ForwardM1(self.front_address, 0)
roboclaw.ForwardM2(self.front_address, 0)
self.last_set_speed_time = rospy.get_rostime()
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
# TODO need find solution to the OSError11 looks like sync problem with serial
rear_status1, rear_enc1, rear_crc1 = None, None, None
rear_status2, rear_enc2, rear_crc2 = None, None, None
front_status1, front_enc1, front_crc1 = None, None, None
front_status2, front_enc2, front_crc2 = None, None, None
# rear roboclaw
try:
rear_status1, rear_enc1, rear_crc1 = roboclaw.ReadEncM1(
self.rear_address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("Rear ReadEncM1 OSError: %d", e.errno)
rospy.logdebug(e)
try:
rear_status2, rear_enc2, rear_crc2 = roboclaw.ReadEncM2(
self.rear_address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("Rear ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
# front roboclaw
try:
front_status1, front_enc1, front_crc1 = roboclaw.ReadEncM1(
self.front_address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("Front ReadEncM1 OSError: %d", e.errno)
rospy.logdebug(e)
try:
front_status2, front_enc2, front_crc2 = roboclaw.ReadEncM2(
self.front_address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("Front ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
if (rear_enc1 is not None and rear_enc2 is not None
and front_enc1 is not None and front_enc2 is not None):
rospy.logdebug(" Encoders rear: %s %s front: %s %s",
rear_enc1, rear_enc2, front_enc1, front_enc2)
self.encodm.update_publish(rear_enc1, rear_enc2, front_enc1,
front_enc2)
if (self.DIAGNOSTIC):
self.updater.update()
r_time.sleep()
def __init__(self):
self.MAX_COMMAND_VALUE = 127
self.ERRORS = {
0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"),
0x0001:
(diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"),
0x0002:
(diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"),
0x0004:
(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"),
0x0008:
(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"),
0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Temperature2"),
0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Main batt voltage high"),
0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Logic batt voltage high"),
0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Logic batt voltage low"),
0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"M1 driver fault"),
0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"M2 driver fault"),
0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Main batt voltage high"),
0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Main batt voltage low"),
0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Temperature1"),
0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Temperature2"),
0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"),
0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")
}
rospy.init_node("roboclaw_node")
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
dev_name = rospy.get_param("~dev", "/dev/ttyACM0")
baud_rate = int(rospy.get_param("~baud", "115200"))
self.address = int(rospy.get_param("~address", "128"))
if self.address > 0x87 or self.address < 0x80:
rospy.logfatal("Address out of range")
rospy.signal_shutdown("Address out of range")
# TODO need someway to check if address is correct
try:
roboclaw.Open(dev_name, baud_rate)
except Exception as e:
rospy.logfatal("Could not connect to Roboclaw")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to Roboclaw")
self.updater = diagnostic_updater.Updater()
self.updater.setHardwareID("Roboclaw")
self.updater.add(
diagnostic_updater.FunctionDiagnosticTask("Vitals",
self.check_vitals))
try:
version = roboclaw.ReadVersion(self.address)
except Exception as e:
rospy.logwarn("Problem getting roboclaw version")
rospy.logdebug(e)
pass
if not version[0]:
rospy.logwarn("Could not get version from roboclaw")
else:
rospy.logdebug(repr(version[1]))
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
self.MAX_SPEED = float(rospy.get_param("~max_speed", "2.0"))
self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.315"))
self.last_set_speed_time = rospy.get_rostime()
rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback)
rospy.sleep(1)
rospy.logdebug("dev %s", dev_name)
rospy.logdebug("baud %d", baud_rate)
rospy.logdebug("address %d", self.address)
rospy.logdebug("max_speed %f", self.MAX_SPEED)
rospy.logdebug("base_width %f", self.BASE_WIDTH)
