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)
import time import roboclaw_driver.roboclaw_driver as rc #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
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)