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 _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 ))