if joy.channel(2) > 0:
cpg['direction']= cpg['forward']
elif joy.channel(2) < 0:
cpg['direction']= cpg['backward']
elif joy.channel(1) > 0:
cpg['direction']= cpg['rightturn']
elif joy.channel(1) < 0:
cpg['direction']= cpg['leftturn']
else:
cpg['move']= false
'''
#cpg['requestedLegPositions']= smk.getLegPositions(cpg['legs'])
print("I am here")
cpg['direction']= cpg['backward']
cpg = CPGgs(cpg, t, dt)
cpg['feetLog'].append(cpg['feetTemp'])
#cpg['planeLog'].append(cpg['planeTemp'])
# Command
cmd.position = cpg['legs']
pub['L'+'1'+'_'+'1'].publish(cmd.position[0][0])
pub['L'+'1'+'_'+'2'].publish(cmd.position[0][1])
pub['L'+'1'+'_'+'3'].publish(cmd.position[0][2])
pub['L'+'6'+'_'+'1'].publish(cmd.position[0][3])
pub['L'+'6'+'_'+'2'].publish(cmd.position[0][4])
pub['L'+'6'+'_'+'3'].publish(cmd.position[0][5])
def callback(self, msg):
# print(msg.linear.x)
# print(msg.linear.z)
# if(msg.linear.x > 0.1 and msg.angular.z > 0.1):
# self.cntr = self.cntr+1
# if(self.cntr%4 == 0):
# self.cpg['direction']= self.cpg['forward']
# print("forward")
# else:
# self.cpg['direction']= self.cpg['leftturn']
# print("left")
# if(msg.linear.x > 0.1 and msg.angular.z < 0.1):
# self.cntr = self.cntr+1
# if(self.cntr%4 == 0):
# self.cpg['direction']= self.cpg['forward']
# print("forward")
# else:
# self.cpg['direction']= self.cpg['rightturn']
# print("right")
# if(msg.linear.x < 0.1 and msg.angular.z > 0.1):
# self.cntr = self.cntr+1
# if(self.cntr%4 == 0):
# self.cpg['direction']= self.cpg['backward']
# print("backward")
# else:
# self.cpg['direction']= self.cpg['leftturn']
# print("left")
# if(msg.linear.x < 0.1 and msg.angular.z < 0.1):
# self.cntr = self.cntr+1
# if(self.cntr%4 == 0):
# self.cpg['direction']= self.cpg['backward']
# print("backward")
# else:
# self.cpg['direction']= self.cpg['rightturn']
# print("right")
# if(msg.linear.x > 0.1 ):
# self.cpg['direction']= self.cpg['forward']
# print("forward")
if (msg.linear.x == -2):
self.cpg['direction'] = self.cpg['rightturn']
print("backward")
if (msg.angular.z < 0.2 and msg.angular.z > -0.2):
if (msg.linear.x > 0.09):
self.cpg['direction'] = self.cpg['forward']
print("forward")
# if(msg.linear.x < 0):
# self.cpg['direction']= self.cpg['backward']
# print("backward")
if (msg.angular.z > 0.25 or msg.angular.z < -0.25):
if (msg.angular.z > 0.25):
self.cpg['direction'] = self.cpg['leftturn']
print("left")
if (msg.angular.z < -0.25):
self.cpg['direction'] = self.cpg['rightturn']
print("right")
# elif(msg.linear.x < 0.1):
# self.cpg['direction']= self.cpg['backward']
# print("backward")
# if(msg.linear.x > 0.1 ):
# self.cpg['direction']= self.cpg['forward']
# print("forward")
# elif(msg.angular.z > 0.1):
# self.cpg['direction']= self.cpg['leftturn']
# print("left")
# elif(msg.angular.z < 0.1):
# self.cpg['direction']= self.cpg['rightturn']
# print("right")
# elif(msg.linear.x < 0.1):
# self.cpg['direction']= self.cpg['backward']
# print("backward")
#self.cpg['direction']= self.cpg['forward']
self.cpg = CPGgs(self.cpg, self.t, self.dt)
self.t += 1
self.cpg['feetLog'].append(self.cpg['feetTemp'])
#cpg['planeLog'].append(cpg['planeTemp'])
# Command
self.cmd.position = self.cpg['legs']
pub['L' + '1' + '_' + '1'].publish(self.cmd.position[0][0])
pub['L' + '1' + '_' + '2'].publish(self.cmd.position[0][1])
pub['L' + '1' + '_' + '3'].publish(self.cmd.position[0][2])
pub['L' + '6' + '_' + '1'].publish(self.cmd.position[0][3])
pub['L' + '6' + '_' + '2'].publish(self.cmd.position[0][4])
pub['L' + '6' + '_' + '3'].publish(self.cmd.position[0][5])
pub['L' + '2' + '_' + '1'].publish(self.cmd.position[0][6])
pub['L' + '2' + '_' + '2'].publish(self.cmd.position[0][7])
pub['L' + '2' + '_' + '3'].publish(self.cmd.position[0][8])
pub['L' + '5' + '_' + '1'].publish(self.cmd.position[0][9])
pub['L' + '5' + '_' + '2'].publish(self.cmd.position[0][10])
pub['L' + '5' + '_' + '3'].publish(self.cmd.position[0][11])
pub['L' + '3' + '_' + '1'].publish(self.cmd.position[0][12])
pub['L' + '3' + '_' + '2'].publish(self.cmd.position[0][13])
pub['L' + '3' + '_' + '3'].publish(self.cmd.position[0][14])
pub['L' + '4' + '_' + '1'].publish(self.cmd.position[0][15])
pub['L' + '4' + '_' + '2'].publish(self.cmd.position[0][16])
pub['L' + '4' + '_' + '3'].publish(self.cmd.position[0][17])