def set_kill(self):
self.killed = True
# Zero thrust
self.thrust_pub.publish(thrusterNewtons(
id = 3,
thrust = 0
))
self.thrust_pub.publish(thrusterNewtons(
id = 2,
thrust = 0
))
rospy.logwarn('Tank steer PD Killed because: %s' % self.kill_listener.get_kills())
def send_thrust(self, force, thruster):
'''Publish thrust for a particular thruster'''
self.thrust_pub.publish(
thrusterNewtons(
id=thruster,
thrust=force,
)
)
def reset_thrust():
for thruster in [2, 3]:
thrust_pub.publish(
thrusterNewtons(
id=thruster,
thrust=0.0,
)
)
def pubStatus(event):
thruster = thrusterNewtons(STARBOARD_THRUSTER, starboard_current)
newton_pub.publish(thruster)
thruster = thrusterNewtons(PORT_THRUSTER, port_current)
newton_pub.publish(thruster)
def send_thrust(self, force, thruster):
'''Publish thrust for a particular thruster'''
self.thrust_pub.publish(thrusterNewtons(
id=thruster,
thrust=force,
))
def pubStatus(event):
thruster = thrusterNewtons(STARBOARD_THRUSTER, starboard_current)
newton_pub.publish(thruster)
thruster = thrusterNewtons(PORT_THRUSTER, port_current)
newton_pub.publish(thruster)
def _wrench_cb(self, msg):
# Since it is impossible to move in the y direction with this thruster config this direction is completly ignored
# Torque is equal to the z component of the wrench.torque
desired = np.matrix([[msg.wrench.force.x], [msg.wrench.torque.z]])
# Multiple by precalculated wrench
thrust = self.wrench_tf * desired
# Check limits
# TODO: get actual limits
# Preserve torque but adjust force if outside of limits
diff = abs(thrust[0, 0] - thrust[1,0])
max_diff = self.max_thrust - self.min_thrust
if diff > max_diff:
# Can't satisfy torque
rospy.logwarn("Torque request can not be satisfied!")
if thrust[0,0] > self.max_thrust:
thrust[0,0] = self.max_thrust
thrust[1,0] = self.min_thrust
elif thrust[0,0] < self.min_thrust:
thrust[0,0] = self.min_thrust
thrust[1,0] = self.max_thrust
elif thrust[1,0] > self.max_thrust:
thrust[1,0] = self.max_thrust
thrust[0,0] = self.min_thrust
elif thrust[1,0] < self.min_thrust:
thrust[1,0] = self.min_thrust
thrust[0,0] = self.max_thrust
else:
if thrust[0,0] > self.max_thrust:
rospy.logwarn("Port thrust above max, preserving torque and reducing force")
offset = thrust[0,0] - self.max_thrust
thrust[0,0] = self.max_thrust
thrust[1,0] -= offset
elif thrust[0,0] < self.min_thrust:
rospy.logwarn("Port thrust below minimum, preserving torque and reducing force")
offset = self.min_thrust - thrust[0,0]
thrust[0,0] = self.min_thrust
thrust[1,0] += offset
elif thrust[1,0] > self.max_thrust:
rospy.logwarn("Starboard thrust above max, preserving torque and reducing force")
offset = thrust[1,0] - self.max_thrust
thrust[1,0] = self.max_thrust
thrust[0,0] -= offset
elif thrust[1,0] < self.min_thrust:
rospy.logwarn("Starboard thrust below minimum, preserving torque and reducing force")
offset = self.min_thrust - thrust[1,0]
thrust[1,0] = self.min_thrust
thrust[0,0] += offset
# Zero servos
for x in range(2,4):
self.servo_pub.publish(DynamixelFullConfig(
id= x,
goal_position= math.pi,
moving_speed= 12, # near maximum, not actually achievable ...
torque_limit= 1023,
goal_acceleration= 38,
control_mode= DynamixelFullConfig.JOINT,
goal_velocity= 10,
))
if not self.killed:
# Output thrust
self.thrust_pub.publish(thrusterNewtons(
id = 3,
thrust = thrust[0,0]
))
self.thrust_pub.publish(thrusterNewtons(
id = 2,
thrust = thrust[1,0]
))
else:
# Output thrust
self.thrust_pub.publish(thrusterNewtons(
id = 3,
thrust = 0
))
self.thrust_pub.publish(thrusterNewtons(
id = 2,
thrust = 0
))
