def onMessage(self, payload, isBinary):
global trajectory_publisher, mode_publisher
sendtraj = trajectory()
sendmode = mode()
try:
datadict = json.loads(payload.decode('utf-8'))
sendtraj.orientation.roll = float(datadict['r'])
sendtraj.orientation.pitch = float(datadict['p'])
sendtraj.orientation.yaw = float(datadict['c'])
sendtraj.translation.x = float(datadict['x'])
sendtraj.translation.y = float(datadict['y'])
sendtraj.translation.z = float(datadict['z'])
trajectory_publisher.publish(sendtraj)
self.sendMessage(b"Joystick executor message consumed!")
except Exception as e:
# If anything messes up, make sure the thrusters aren't spinning anymore
sendtraj.orientation.roll = 0
sendtraj.orientation.pitch = 0
sendtraj.orientation.yaw = 0
sendtraj.translation.x = 0
sendtraj.translation.y = 0
sendtraj.translation.z = 0
sendmode.auvmode = True
sendmode.rovmode = False
trajectory_publisher.publish(sendtraj)
mode_publisher.publish(sendmode)
def publisher():
pub = rospy.Publisher('trajectory_commands', trajectory, queue_size=3)
rospy.init_node('trajectory_talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
msg = trajectory()
msg.orientation.roll = .1
msg.orientation.pitch = -.2
msg.orientation.yaw = .3
msg.translation.x = -.4
msg.translation.y = .5
msg.translation.z = -.6
pub.publish(msg)
rate.sleep()
def handle_peripherals(joystick, msg):
global already_sent_zero
global last_sent
global servo_in_position
global stepper_already_moving
global thruster_already_killed
global thruster_already_unkilled
hat = joystick.get_hat(0)
hat = hat_to_val(hat[0], hat[1])
io_request_ = io_request()
if hat is None:
if not already_sent_zero:
io_request_.executor = "individual_thruster_control"
io_request_.string = last_sent
already_sent_zero = True
msg.io_requests += (io_request_, )
else:
io_request_.executor = "individual_thruster_control"
io_request_.string = hat
io_request_.float = 0.75
last_sent = hat
already_sent_zero = False
msg.io_requests += (io_request_, )
if joystick.get_button(
0
): # Safety trigger: Do not Send trajectory data if this trigger is held.
msg.desired_trajectory = trajectory()
if not joystick.get_button(
1
): # 'Boost mode': If this button is pressed, multiply trajectory by 2
# We implement this by always cutting by 2, and then when the button is pressed, not cutting in half.
msg.desired_trajectory.translation.x = msg.desired_trajectory.translation.x / 2
msg.desired_trajectory.translation.y = msg.desired_trajectory.translation.y / 2
msg.desired_trajectory.translation.z = msg.desired_trajectory.translation.z / 2
msg.desired_trajectory.orientation.roll = msg.desired_trajectory.orientation.roll / 2
msg.desired_trajectory.orientation.pitch = msg.desired_trajectory.orientation.pitch / 2
msg.desired_trajectory.orientation.yaw = msg.desired_trajectory.orientation.yaw / 2
if joystick.get_button(6): # Kill thrusters button
if not thruster_already_killed:
io_request_ = io_request()
io_request_.executor = "kill_thruster"
io_request_.string = "front_left"
msg.io_requests += (io_request_, )
io_request_ = io_request()
io_request_.executor = "kill_thruster"
io_request_.string = "front_right"
msg.io_requests += (io_request_, )
io_request_ = io_request()
io_request_.executor = "kill_thruster"
io_request_.string = "top_front"
msg.io_requests += (io_request_, )
thruster_already_killed = True
elif thruster_already_killed: # Make sure the button is released before we send another stream of kill stuff
thruster_already_killed = False
if joystick.get_button(7): # Un-kill thrusters button
if not thruster_already_unkilled:
io_request_ = io_request()
io_request_.executor = "unkill_thruster"
io_request_.string = "front_left"
msg.io_requests += (io_request_, )
io_request_ = io_request()
io_request_.executor = "unkill_thruster"
io_request_.string = "front_right"
msg.io_requests += (io_request_, )
io_request_ = io_request()
io_request_.executor = "unkill_thruster"
io_request_.string = "top_front"
msg.io_requests += (io_request_, )
thruster_already_unkilled = True
elif thruster_already_unkilled: # Make sure the button is released before we send another stream of un-kill stuff
thruster_already_unkilled = False
if joystick.get_button(10): # Servo button
io_request_ = io_request() # Clean message
io_request_.executor = "servo"
io_request_.float = 0.5
servo_in_position = True
msg.io_requests += (io_request_, )
elif servo_in_position: # If the button is not pressed and the servo is currently in position:
# This way, when the button is released, the servo channel should return to 0.
io_request_ = io_request() # Clean message
io_request_.executor = "servo"
io_request_.float = 0
servo_in_position = False
msg.io_requests += (io_request_, )
if joystick.get_button(11): # Stepper button
if not stepper_already_moving:
io_request_ = io_request()
io_request_.executor = "stepper"
io_request_.int32 = 800 # At 400 hz, this should be noticable on a oscilloscope
msg.io_requests += (io_request_, )
stepper_already_moving = True
elif stepper_already_moving: # Make sure the button is released before we send another stream of stepper stuff
stepper_already_moving = False
return msg # If we wanted to do something with button presses, we could mess around with that sort of thing here.
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Enbarr. If not, see <https://www.gnu.org/licenses/>.
"""
import rospy
from auv.msg import trajectory, io_request, surface_command
trajectory_requester = rospy.Publisher('trajectory_request',
trajectory,
queue_size=3)
io_requester = rospy.Publisher('io_request', io_request, queue_size=3)
last_trajectory = trajectory()
last_io_requests = surface_command().io_requests
def trajectory_content_match(msg1, msg2):
return msg1.orientation == msg2.orientation and msg1.translation == msg2.translation
def io_content_match(msg1, msg_list):
for msg in msg_list:
if msg1.executor == msg.executor and \
msg1.float == msg.float and \
msg1.boolean == msg.boolean and \
msg1.int32 == msg.int32 and \
msg1.string == msg.string:
return True
def trajectory_callback(data):
pubmsg = trajectory()
pubmsg.translation = data.translation
pubmsg.orientation = data.orientation
if AUVMODE:
pub.publish(data)
def joystick_callback(data):
pubmsg = trajectory()
pubmsg.translation = data.translation
pubmsg.orientation = data.orientation
if ROVMODE:
pub.publish(data)
def callback_trajectory(data):
pubmsg = trajectory()
# Get the roll/pitch/yaw, x/y/z values scaled up by P
p_times_proportional_orientation = [
value_p * data.orientation.roll, value_p * data.orientation.pitch,
value_p * data.orientation.yaw
]
p_times_proportional_translation = [
value_p * data.translation.x, value_p * data.translation.y,
value_p * data.translation.z
]
# Integrate orientation and translation values
integrated_orientation = [0, 0, 0]
integrated_translation = [0, 0, 0]
for point in history:
integrated_orientation = [
integrated_orientation[0] + point.orientation.roll,
integrated_orientation[1] + point.orientation.pitch,
integrated_orientation[2] + point.orientation.yaw
]
integrated_translation = [
integrated_translation[0] + point.translation.x,
integrated_translation[1] + point.translation.y,
integrated_translation[2] + point.translation.z
]
integrated_orientation = [
integrated_orientation[0] // integrated_orientation.__len__(),
integrated_orientation[1] // integrated_orientation.__len__(),
integrated_orientation[2] // integrated_orientation.__len__()
]
integrated_translation = [
integrated_translation[0] // integrated_translation.__len__(),
integrated_translation[1] // integrated_translation.__len__(),
integrated_translation[2] // integrated_translation.__len__()
]
# Now that things are integrated, scale by I
i_times_integrated_orientation = [
value_i * integrated_orientation[0],
value_i * integrated_orientation[1],
value_i * integrated_orientation[2]
]
i_times_integrated_translation = [
value_i * integrated_translation[0],
value_i * integrated_translation[1],
value_i * integrated_translation[2]
]
# Sum up the scaled P and I values and assign to a message
pubmsg.orientation.roll = p_times_proportional_orientation[
0] + i_times_integrated_orientation[0]
pubmsg.orientation.pitch = p_times_proportional_orientation[
1] + i_times_integrated_orientation[1]
pubmsg.orientation.yaw = p_times_proportional_orientation[
2] + i_times_integrated_orientation[2]
pubmsg.translation.x = p_times_proportional_translation[
0] + i_times_integrated_translation[0]
pubmsg.translation.y = p_times_proportional_translation[
1] + i_times_integrated_translation[1]
pubmsg.translation.z = p_times_proportional_translation[
2] + i_times_integrated_translation[2]
global Publisher
Publisher.publish(pubmsg)