def torque_callback(self, message):
pwm_command = Pwm()
pwm_command.pwm = [1500] * 8
force = np.array([
message.linear.x, message.linear.y, message.linear.z,
message.angular.x, message.angular.y, message.angular.z
])
print '==================== PWM ===================='
print("%+5.4f %+5.4f %+5.4f" %
(message.linear.x, message.linear.y, message.linear.z))
print("%+5.4f %+5.4f %+5.4f" %
(message.angular.x, message.angular.y, message.angular.z))
torque = np.matmul(self.direction_inverse.T, force.T)
for run in range(0, 8):
if (torque[run] < 0):
if (torque[run] > self.min_force[run][0] / 2):
torque[run] = 0
elif (torque[run] > self.min_force[run][0]):
torque[run] = self.min_force[run][0]
else:
if (torque[run] < self.min_force[run][1] / 2):
torque[run] = 0
elif (torque[run] < self.min_force[run][1]):
torque[run] = self.min_force[run][1]
cmd = []
for i in range(0, 4):
cmd.append(lup.lookup_pwm_02(torque[i]))
if (torque[i] == 0):
cmd[i] = 1500
for i in range(4, 8):
cmd.append(lup.lookup_pwm_01(torque[i]))
if (torque[i] == 0):
cmd[i] = 1500
for i in range(0, 8):
pwm_command.pwm[i] = cmd[i] # thrust i
pwm = pwm_command.pwm
print '==================== PWM ===================='
print("%+5.4f %+5.4f %+5.4f %+5.4f" % (pwm[0], pwm[1], pwm[2], pwm[3]))
print("%+5.4f %+5.4f %+5.4f %+5.4f" % (pwm[4], pwm[5], pwm[6], pwm[7]))
# print(str(pwm[0]) + "\t" + str(pwm[1]) + "\t"
# + str(pwm[2]) + "\t" + str(pwm[3]) + "\n"
# + str(pwm[4]) + "\t" + str(pwm[5]) + "\t"
# + str(pwm[6]) + "\t" + str(pwm[7]))
self.pwm_publisher.publish(pwm_command)
def joy_callback(self, message):
#print '** input ** '
#print message
pwm_command = Pwm()
pwm_command.pwm = [1500] * 8
#compute thrust for each thruster based on joy stick command
#trusts_scale = [3,3,4.1,2.4,2.4,2.4]
#F = array([message.linear.x*3, message.linear.y*3, message.linear.z*4.1,
# message.angular.x*2.4, message.angular.y*2.4, message.angular.z*1.6])
#F = array([message.linear.x*3, message.linear.y*3, message.linear.z*4.1,
# message.angular.x*2.4, message.angular.y*2.4, message.angular.z*2.4])
F = array([
message.linear.x, message.linear.y, message.linear.z,
message.angular.x, message.angular.y, message.angular.z
])
if (self.print_data):
print '======= M ========'
print self.M
print '======= F ========'
print F
print F.T
t = dot(self.M, F.T)
if (self.print_data):
print '=================** thrust **=============='
print t
cmd = []
for i in range(0, 4):
cmd.append(lup.lookup_pwm_02(t[i]))
if (cmd[i] > 1477 and cmd[i] < 1525):
cmd[i] = 1500
# cmd.append(lup.lookup_pwm_02(-1*t[3]))
for i in range(4, 8):
cmd.append(lup.lookup_pwm_01(t[i]))
if (cmd[i] > 1447 and cmd[i] < 1552):
cmd[i] = 1500
# print '=========== cmd ==========='
# print cmd
#tmp = [i-1500 for i in cmd]
#green robot
force = 1
pwm_command.pwm[0] = cmd[0] * force #500*t[0]; #thrust 1
pwm_command.pwm[1] = cmd[1] * force #500*t[1]; #thrust 2
pwm_command.pwm[2] = cmd[2] * force #500*t[2]; #thrust 3
pwm_command.pwm[3] = cmd[3] * force #500*t[3]; #thrust 4
pwm_command.pwm[4] = cmd[4] * force #500*t[4]; #thrust 5
pwm_command.pwm[5] = cmd[5] * force #500*t[5]; #thrust 6
pwm_command.pwm[6] = cmd[6] * force #500*t[6]; #thrust 7
pwm_command.pwm[7] = cmd[7] * force #500*t[7]; #thrust 8
pwm = pwm_command.pwm
print '=========== PWM ==========='
print(
str(pwm[0]) + "\t" + str(pwm[1]) + "\t" + str(pwm[2]) + "\t" +
str(pwm[3]) + "\n" + str(pwm[4]) + "\t" + str(pwm[5]) + "\t" +
str(pwm[6]) + "\t" + str(pwm[7]))
# if(self.print_data):
# print '\n============PWM before bound============='
# print pwm_command
# for i in range(8) :
# if pwm_command.pwm[i] > 1900 : #at first 1800 2200 2000
# pwm_command.pwm[i] = 1900
# elif pwm_command.pwm[i] < 1100 :
# pwm_command.pwm[i] = 1100 #at first 1200 800 900
# if(message.linear.x == 0 and message.linear.y == 0 and message.angular.z == 0):
# pwm_command.pwm[4] = 1500
# pwm_command.pwm[5] = 1500
# pwm_command.pwm[6] = 1500
# pwm_command.pwm[7] = 1500
# if(self.print_data):
# print '\n============PWM============='
# print pwm_command
# pwm_command.header.stamp = rospy.Time.now()
self.pwm_publisher.publish(pwm_command)
def joy_callback(self , message): #print '** input ** ' #print message pwm_command = Pwm() pwm_command.pwm = [1500]*8 #compute thrust for each thruster based on joy stick command #trusts_scale = [3,3,4.1,2.4,2.4,2.4] F = array([message.linear.x*3, message.linear.y*3, message.linear.z*4.1, message.angular.x*2.4, message.angular.y*2.4, message.angular.z*2.4]) print '======= F ========' print F print F.T t = dot(self.M, F.T) print '=================** thrust **==============' print t cmd = lup.lookup_pwm_array(t) print '=========== cmd ===========' print cmd #tmp = [i-1500 for i in cmd] #tul #pwm_command.pwm[0] += tmp[0]#500*t[0]; #thrust0 4/ \5 #pwm_command.pwm[1] -= tmp[1]#500*t[1]; #thrust1 / \ #pwm_command.pwm[2] += tmp[2]#500*t[2]; #thrust2 0 | | 1 #pwm_command.pwm[3] += tmp[3]#500*t[3]; #thrust3 | | #pwm_command.pwm[4] -= tmp[4]#500*t[4]; #thrust4 | | #pwm_command.pwm[5] += tmp[5]#500*t[5]; #thrust5 2 | | 3 #pwm_command.pwm[6] += tmp[6]#500*t[6]; #thrust6 old + \ / #pwm_command.pwm[7] -= tmp[7]#500*t[7]; #thrust7 old - 6\ /7 #ek #pwm_command.pwm[0] += tmp[0]#500*t[0]; #thrust0 4/ \5 #pwm_command.pwm[1] -= tmp[1]#500*t[1]; #thrust1 / \ #pwm_command.pwm[2] += tmp[2]#500*t[2]; #thrust2 0 | | 1 #pwm_command.pwm[3] += tmp[3]#500*t[3]; #thrust3 | | #pwm_command.pwm[4] -= tmp[4]#500*t[4]; #thrust4 | | #pwm_command.pwm[5] -= tmp[5]#500*t[5]; #thrust5 2 | | 3 #pwm_command.pwm[6] -= tmp[6]#500*t[6]; #thrust6 \ / #pwm_command.pwm[7] -= tmp[7]#500*t[7]; #thrust7 #ek+control force = 1 pwm_command.pwm[0] = cmd[0]*force#500*t[0]; #thrust0 pwm_command.pwm[1] = cmd[1]*force#500*t[1]; #thrust1 pwm_command.pwm[2] = cmd[2]*force#500*t[2]; #thrust2 pwm_command.pwm[3] = cmd[3]*force#500*t[3]; #thrust3 pwm_command.pwm[4] = cmd[4]*force#500*t[4]; #thrust4 pwm_command.pwm[5] = cmd[5]*force#500*t[5]; #thrust5 pwm_command.pwm[6] = cmd[6]*force#500*t[6]; #thrust6 pwm_command.pwm[7] = cmd[7]*force#500*t[7]; #thrust7 print '\n============PWM before bound=============' print pwm_command for i in range(8) : if pwm_command.pwm[i] > 1700 : #at first 1800 pwm_command.pwm[i] = 1700 elif pwm_command.pwm[i] < 1300 : pwm_command.pwm[i] = 1300 #at first 1200 print '\n============PWM=============' print pwm_command pwm_command.header.stamp = rospy.Time.now() self.pwm_publisher.publish(pwm_command)
def joy_callback(self, message):
#print '** input ** '
#print message
pwm_command = Pwm()
pwm_command.pwm = [1500] * 8
#compute thrust for each thruster based on joy stick command
#trusts_scale = [3,3,4.1,2.4,2.4,2.4]
#F = array([message.linear.x*3, message.linear.y*3, message.linear.z*4.1,
# message.angular.x*2.4, message.angular.y*2.4, message.angular.z*1.6])
#F = array([message.linear.x*3, message.linear.y*3, message.linear.z*4.1,
# message.angular.x*2.4, message.angular.y*2.4, message.angular.z*2.4])
F = array([
message.linear.x * 3, message.linear.y * 3,
message.linear.z * 20.5, message.angular.x * 4.8,
message.angular.y * 4.8, message.angular.z * 6.4
])
print '======= M ========'
print self.M
print '======= F ========'
print F
print F.T
t = dot(self.M, F.T)
print '=================** thrust **=============='
print t
cmd = lup.lookup_pwm_array_01(t)
print '=========== cmd ==========='
print cmd
#tmp = [i-1500 for i in cmd]
#green robot
force = 1
pwm_command.pwm[0] = cmd[0] * force #500*t[0]; #thrust 1
pwm_command.pwm[1] = cmd[1] * force #500*t[1]; #thrust 2
pwm_command.pwm[2] = cmd[2] * force #500*t[2]; #thrust 3
pwm_command.pwm[3] = cmd[3] * force #500*t[3]; #thrust 4
pwm_command.pwm[4] = cmd[4] * force #500*t[4]; #thrust 5
pwm_command.pwm[5] = cmd[5] * force #500*t[5]; #thrust 6
pwm_command.pwm[6] = cmd[6] * force #500*t[6]; #thrust 7
pwm_command.pwm[7] = cmd[7] * force #500*t[7]; #thrust 8
print '\n============PWM before bound============='
print pwm_command
for i in range(8):
if pwm_command.pwm[i] > 1900: #at first 1800 2200 2000
pwm_command.pwm[i] = 1900
elif pwm_command.pwm[i] < 1100:
pwm_command.pwm[i] = 1100 #at first 1200 800 900
print '\n============PWM============='
print pwm_command
pwm_command.header.stamp = rospy.Time.now()
self.pwm_publisher.publish(pwm_command)
from hg_ros_pololu.msg import Pwm
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import rospy
import numpy as np
import matplotlib.pyplot as plt
import rospkg
queue = []
# pub = rospy.Publisher("/gx4_45_imu/data", Imu, queue_size=1)
# pub = rospy.Publisher("/pwm_auv_state", Float64[], queue_size=1)
count = 0
# r = 0.05
path = rospkg.RosPack().get_path('zeabus_sensor') + '/scripts'
rate = 1 / 20.
f = open(path + '/auv_state_pwm_00.csv', 'w+')
pwm = Pwm()
pwm.pwm = [1500] * 8
auv_state = Odometry()
imu = Imu()
get_data = True
pub_pwm = rospy.Publisher("/pwm/sub_sampling", Pwm, queue_size=1)
pub_imu = rospy.Publisher("/imu/sub_sampling", Imu, queue_size=1)
pub_state = rospy.Publisher("/state/sub_sampling", Odometry, queue_size=1)
def change_rate():
global rate, auv_state, pwm, count, f, get_data, imu
pub_imu.publish(imu)
pub_pwm.publish(pwm)
pub_state.publish(auv_state)