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)