def callback(self, twist):
FLW_cmd = Command()
FRW_cmd = Command()
RLW_cmd = Command()
RRW_cmd = Command()
# linear velocity
vLinear = sqrt(twist.linear.x ** 2 + twist.linear.y ** 2)
if vLinear > self.maxLinearVelocity:
vLinear = self.maxLinearVelocity
# movement orientation
Heading = atan2(twist.linear.y, twist.linear.x)
# x axis linear velocity
xVel = vLinear * cos(Heading)
# y axis linear velocity
yVel = vLinear * sin(Heading)
# YAW axis rotational velocity
yawVel = twist.angular.z
if yawVel ** 2 > self.maxAngularVelocity ** 2:
yawVel = self.maxAngularVelocity * yawVel / abs(yawVel)
w = self.InverseKinematics(xVel, yVel, yawVel)
FLW_cmd.mode = 0
FLW_cmd.setpoint = w[0]
FRW_cmd.mode = 0
FRW_cmd.setpoint = w[1]
RLW_cmd.mode = 0
RLW_cmd.setpoint = w[2]
RRW_cmd.mode = 0
RRW_cmd.setpoint = w[3]
self.pubFLW.publish(FLW_cmd)
self.pubFRW.publish(FRW_cmd)
self.pubRLW.publish(RLW_cmd)
self.pubRRW.publish(RRW_cmd)
def callback(self, twist):
FLW_cmd = Command()
FRW_cmd = Command()
RLW_cmd = Command()
RRW_cmd = Command()
# linear velocity
vLinear = sqrt(twist.linear.x**2 + twist.linear.y**2)
if vLinear > self.maxLinearVelocity:
vLinear = self.maxLinearVelocity
# movement orientation
Heading = atan2(twist.linear.y, twist.linear.x)
# x axis linear velocity
xVel = vLinear * cos(Heading)
# y axis linear velocity
yVel = vLinear * sin(Heading)
# YAW axis rotational velocity
yawVel = twist.angular.z
if yawVel**2 > self.maxAngularVelocity**2:
yawVel = self.maxAngularVelocity * yawVel / abs(yawVel)
w = self.InverseKinematics(xVel, yVel, yawVel)
FLW_cmd.mode = 0
FLW_cmd.setpoint = w[0]
FRW_cmd.mode = 0
FRW_cmd.setpoint = w[1]
RLW_cmd.mode = 0
RLW_cmd.setpoint = w[2]
RRW_cmd.mode = 0
RRW_cmd.setpoint = w[3]
self.pubFLW.publish(FLW_cmd)
self.pubFRW.publish(FRW_cmd)
self.pubRLW.publish(RLW_cmd)
self.pubRRW.publish(RRW_cmd)
def send_command(self, setpoint):
command = Command()
command.mode = self.get_mode()
command.setpoint = setpoint
def send_command(self, setpoint):
command = Command()
command.mode = self.get_mode()
command.setpoint = setpoint