def handleMessage(self, twist):
print(twist)
motorSpeeds = balboaMotorSpeeds()
motorSpeeds.left = 0
motorSpeeds.right = 0
# rate = rospy.Rate(10); # go through the loop 10 times per second
rospy.loginfo("Start Running")
if (twist.linear.x != 0):
if (twist.linear.x > 0):
motorSpeeds.left = 15
motorSpeeds.right = 15
if (twist.linear.x < 0):
motorSpeeds.left = -15
motorSpeeds.right = -15
if (twist.angular.z != 0):
if (twist.angular.z > 0):
motorSpeeds.right += 7.5
if (twist.angular.z < 0):
motorSpeeds.left += 7.5
self.pub.publish(motorSpeeds)
time.sleep(
0.1) # for how much it travel before reseting speed stopping
motorSpeeds.left = 0
motorSpeeds.right = 0
self.pub.publish(motorSpeeds)
def handleKeyPress(self, msg):
#Interpret message from keyboard, flag new message received
self.msg_received = True
linear = msg.linear.x
angular = msg.angular.z
#Translate into motor speeds
self.message = balboaMotorSpeeds()
self.message.left = (linear * 7.5) - (angular * 2.5)
self.message.right = (linear * 7.5) + (angular * 2.5)
def __init__(self):
#Initialize the node
rospy.init_node('teleop_robot', anonymous=True)
#Message to send to motorSpeeds topic and flag for sending
self.message = balboaMotorSpeeds()
self.msg_received = False
#Start listening and set up subscriber
rospy.Subscriber("/turtle1/cmd_vel", Twist, self.handleKeyPress)
self.publisher = rospy.Publisher("motorSpeeds",
balboaMotorSpeeds,
queue_size=1)
def __init__(self):
self.motorPub = rospy.Publisher("motorSpeeds",
balboaMotorSpeeds,
queue_size=5)
self.turnPub = rospy.Publisher("turnSignal", Float64, queue_size=5)
rospy.init_node('BalboaMove')
self.movement = True
self.sub = rospy.Subscriber('movementCommand',
Float64,
self.setMovement,
queue_size=1)
self.motorSpeeds = balboaMotorSpeeds()
self.motorSpeeds.left = 0
self.motorSpeeds.right = 0
self.turnDirection = True #true for left, false for right
rospy.spin()
def main_loop(self):
#Create rate object for main loop
rate = rospy.Rate(20) #20 Hz
while not rospy.is_shutdown():
#If keyboard message present, push through, otherwise, stop
if self.msg_received:
self.publisher.publish(self.message)
self.msg_received = False
else:
self.message = balboaMotorSpeeds()
self.message.left = 0
self.message.right = 0
self.publisher.publish(self.message)
#Sleep until next loop
rate.sleep()
def calc_motor_speeds(self):
message = balboaMotorSpeeds()
message.left = (self.dist_pid * LINEAR_SPEED_SCALE) - (
self.angle_pid * ANGULAR_SPEED_SCALE)
if message.left > MOTOR_SPEED_CAP:
message.left = MOTOR_SPEED_CAP
elif message.left < -1 * MOTOR_SPEED_CAP:
message.left = -1 * MOTOR_SPEED_CAP
message.right = (self.dist_pid * LINEAR_SPEED_SCALE) + (
self.angle_pid * ANGULAR_SPEED_SCALE)
if message.right > MOTOR_SPEED_CAP:
message.right = MOTOR_SPEED_CAP
elif message.right < -1 * MOTOR_SPEED_CAP:
message.right = -1 * MOTOR_SPEED_CAP
self.publisher.publish(message)
def getLinearPIDOutputAndCalculateSpeed(self, output):
self.linearPIDValue = output.data
motorSpeed = balboaMotorSpeeds()
motorSpeed.left = self.linearPIDValue
motorSpeed.right = self.linearPIDValue
motorSpeed.left = motorSpeed.left - self.angularPIDValue * .25
motorSpeed.right = motorSpeed.right + self.angularPIDValue * .25
if (motorSpeed.left > 25):
motorSpeed.left = 25
elif (motorSpeed.left < -25):
motorSpeed.left = -25
if (motorSpeed.right > 25):
motorSpeed.right = 25
elif (motorSpeed.right < -25):
motorSpeed.right = -25
self.motorSpeedPublisher.publish(motorSpeed)
def talker(data):
#initializes the publisher
pub = rospy.Publisher('/motorSpeeds', balboaMotorSpeeds, queue_size=0)
cmd = balboaMotorSpeeds()
rate = rospy.Rate(100) #High rate to increase responsiveness
pub.publish(cmd) #publish a blank command once to increase reliability
rate.sleep()
#this is for linear movement
cmd.left += int(data.linear.x * 10.0)
cmd.right += int(data.linear.x * 10.0)
#this is for rotational movement
cmd.left += int(data.angular.z * -5.0)
cmd.right += int(data.angular.z * 5.0)
maxSpeed = 50
#This sets a maximum speed that the left wheel can turn
if cmd.left > maxSpeed:
cmd.left = maxSpeed
elif cmd.left < -maxSpeed:
cmd.left = -maxSpeed
#This sets a maximum speed that the left right can turn
if cmd.right > maxSpeed:
cmd.right = maxSpeed
elif cmd.right < -maxSpeed:
cmd.right = -maxSpeed
#Publish 5 times to increase reliability
for x in range(0, 5):
pub.publish(cmd)
#Sets speed to 0 for when buttons arent pressed so that the robot stops
cmd.left = 0.0
cmd.right = 0.0
pub.publish(cmd)
rate.sleep()
