def callback_right(message):
utime = long(rospy.get_time()) * 1000000
# starboard_channel = "WAMV.STBD_MOTOR_CONTROL" # This controls the motors directly
starboard_channel = "WAMV.RIGHT_ROS_CMD"
MULTIPLIER = 100.
LIMIT = 1000.
starboard = message.data * MULTIPLIER
if starboard > LIMIT:
starboard = LIMIT
elif starboard < -LIMIT:
starboard = -LIMIT
starboard_msg = asv_torqeedo_motor_command_t()
starboard_msg.command_speed = int(starboard)
starboard_msg.enabled = True
starboard_msg.utime = utime
rospy.logdebug("Publising starboard")
lc.publish(starboard_channel, starboard_msg.encode())
def callback_left(message):
# port_channel = "WAMV.PORT_MOTOR_CONTROL"
port_channel = "WAMV.LEFT_ROS_CMD"
LIMIT = 1000.
MULTIPLIER = 100.
port = message.data * MULTIPLIER
if port > LIMIT:
port = LIMIT
elif port < -LIMIT:
port = -LIMIT
port_msg = asv_torqeedo_motor_command_t()
utime = long(rospy.get_time()) * 1000000
port_msg.command_speed = int(port)
port_msg.enabled = True
port_msg.utime = utime
rospy.logdebug("Publishing port")
lc.publish(port_channel, port_msg.encode())
def Main():
global f_speed
global r_speed
channel = 'WAMV.PORT_MOTOR_CONTROL'
if len(sys.argv) > 1:
channel = sys.argv[1] # e.g. 'STARBOARD_MOTOR_CONTROL'
print('ACFR USYD Torqeedo Motor TESTER started for driver LCM channel: {}'.
format(channel))
print('Usage: python torqeedo_tester.py <motor_lcm_channel>')
print(' e.g. python torqeedo_tester.py PORT_MOTOR_CONTROL\n')
print('Motor Control: 8 + \n ')
print(' 5 ')
print(' idle ')
print(' 2 - ')
# Start with both neg -> idle
cmd_speed = 0
# Setup LCM
lc = lcm.LCM()
msg = asv_torqeedo_motor_command_t()
while True:
time.sleep(0.1)
choice = int((readchar.readkey()))
if choice == 8:
# increase speed ^
cmd_speed = (cmd_speed + 100) #%(650)
elif choice == 2:
cmd_speed = (cmd_speed - 100)
elif choice == 5:
# reverse <
cmd_speed = 0
msg.command_speed = cmd_speed
msg.enabled = True
msg.utime = long(time.time()) * 1000000
lc.publish(channel, msg.encode())
def publishZero(channel):
msg = asv_torqeedo_motor_command_t()
msg.command_speed = 0
msg.enabled = False
msg.utime = long(rospy.get_time()) * 1000000
lc.publish(channel, msg.encode())
import lcm
# name = left_motor
from acfrlcm import asv_torqeedo_motor_command_t
# type = acfrlcm_asv_torqeedo_motor_command_t
import time
while (True):
time.sleep(1)
msg = asv_torqeedo_motor_command_t()