def command_cb(self, msg):
"""Convert thrust to angular velocity commands for uuv_thruster_ros_plugin"""
for i in range(len(self._thrusters)):
cmd = FloatStamped()
force = eval("msg.force."+self._thrusters[i])
# Compute angular velocity from force
sign = 1 if force >= 0 else -1
ang_vel = sign * math.sqrt(abs(force / self._coeff))
cmd.header = msg.header
cmd.data = ang_vel
self._pubs[i].publish(cmd)
def command_cb(self, msg):
"""Read in thruster commands from main vehicle, then convert to angular velocity commands for uuv_thruster_ros_plugin"""
for thruster in self._thrusters:
cmd = FloatStamped()
force = 0
code = "force = msg.force." + thruster
exec(code) # Run the command, get thruster force
ang_vel = self.get_angular_velocity(force)
cmd.header = msg.header
code = "cmd.data = " + str(ang_vel)
exec(code) # Run the command, populate message
self._pubs[thruster].publish(cmd)