NETWORK: Added message handlers for SSC-LANT msgs (fm TTECG_itx_2 branch)

ap_network_bridge/src/network.py

@@ -417,6 +417,25 @@ def net_copter_takeoff(message, bridge):
 
     bridge.publish('recv_takeoff', msg, latched=True)
 
+def net_camera_control(message, bridge):
+    msg = std_msgs.msg.Empty()
+    bridge.publish('recv_cam_control', msg, latched=True)
+
+def net_tactic_camera_control(message, bridge):
+    msg = std_msgs.msg.Empty()
+    bridge.publish('recv_tactic_trigger', msg, latched=True)
+
+def net_copter_yaw(message, bridge):
+    msg = pilot_msg.Yaw()
+    msg.yaw_degrees = message.report.yaw_degrees
+
+    bridge.publish('recv_yaw', msg, latched=True)
+
+def net_copter_land(message, bridge):
+    msg = std_msgs.msg.UInt8()
+    msg.data = enums.LAND
+    bridge.publish('mode_num', msg, latched=True)    
+
 def send_waypoints(points):
     response = messages.WaypointMsg()
         
@@ -805,6 +824,82 @@ def net_mission_config(message, bridge):
     bridge.doInThread(main, error)
 net_mission_config.active = False
 
+def net_generic_wp_config(message, bridge):
+    def writeWPLine(index, cmd_type, lon=None, lat=None, alt=None, cmd_args=None):
+        if index is None:
+            index = 0
+        if cmd_type is None:
+            print("You must supply a valid command type!")
+            return None
+        if lon is None:
+            lon = 0
+        if lat is None:
+            lat = 0
+        if alt is None:
+            alt = 0
+        if cmd_args is None:
+            cmd_args = [0, 0, 0, 0]
+
+        # file format
+        # <INDEX> <CURRENT WP> <COORD> <CMD> <P1> <P2> <P3> <P4> <LAT> <LON> <ALT> <CONTINUE>
+        wp_line = "{index}\t{cur_wp}\t{coord}\t{cmd}\t{p1}\t{p2}\t{p3}\t{p4}\t{lat}\t{lon}\t{alt}\t{cont}\n".format(
+            index=index,
+            cur_wp=1,
+            coord=0,
+            cmd=cmd_type,
+            p1=cmd_args[0], p2=cmd_args[1], p3=cmd_args[2], p4=cmd_args[3],
+            lat=lat, lon=lon, alt=alt,
+            cont=1)
+        return wp_line
+
+    def main():
+        # Reset OK flags so users know work is in progress
+        rospy.set_param('ok_wp', False)
+        base_file = os.path.expanduser("~/blessed/generic-{}.wp".format(message.msg_dst))
+        temp_file = base_file + ".tmp"
+
+        with open(temp_file, 'w+') as wp_file:
+            # file format version
+            version = 110
+            wp_file.write("QGC WPL {}\n".format(version))
+
+            wp_file.write(writeWPLine(0, enums.WP_TYPE_NORMAL))
+            wp_file.write(writeWPLine(1, enums.WP_TYPE_TAKEOFF, alt=message.alt))
+            wp_file.write(writeWPLine(2, enums.WP_TYPE_NORMAL))
+            wp_file.write(writeWPLine(3, enums.WP_TYPE_NORMAL))
+            wp_file.write(writeWPLine(4, enums.WP_TYPE_NORMAL))
+            wp_file.write(writeWPLine(5, enums.WP_TYPE_LOITER))
+            wp_file.write(writeWPLine(6, enums.WP_TYPE_LAND_SEQUENCE))
+            wp_file.write(writeWPLine(7, enums.WP_TYPE_LAND, lat=message.lat, lon=message.lon))
+            wp_file.write(writeWPLine(8, enums.WP_TYPE_JUMP, cmd_args=[7, -1, 0, 0]))
+            wp_file.write(writeWPLine(9, enums.WP_TYPE_LOITER))
+
+        # Call load service
+        res = bridge.callService('load_wp', pilot_srv.FileLoad, name=temp_file)
+
+        # Update OK flag
+        rospy.set_param('ok_wp', res.ok)
+
+        # Clean up temp file
+        # os.remove(temp_file)
+
+        # Make sure waypoint 1 is reloaded on autopilot
+        msg = std_msgs.msg.UInt16()
+        msg.data = 1
+        bridge.publish('recv_waypoint_goto', msg, latched=True)
+
+        # Reset active flag
+        net_generic_wp_config.active = False
+
+    def error():
+        net_generic_wp_config.active = False
+
+    if net_generic_wp_config.active:
+        raise Exception("configuration currently in progress")
+        net_generic_wp_config.active = True
+    bridge.doInThread(main, error)
+net_generic_wp_config.active = False
+
 # Highest-numbered are administrative/debug messages
 
 def net_ap_reboot(message, bridge):
@@ -871,6 +966,15 @@ if __name__ == '__main__':
         bridge.addSubHandler('firing_reports', \
                              ap_msg.FiringReportStamped, \
                              sub_firing_report)
+        bridge.addSubHandler('camera_control_reports', \
+                             std_msgs.msg.Empty, \
+                             net_camera_control)
+        bridge.addSubHandler('yaw_control', \
+                             ap_msg.YawReportStamped, \
+                             net_copter_yaw)
+        bridge.addSubHandler('land_control', \
+                             std_msgs.msg.Empty, \
+                             net_copter_land)
         bridge.addSubHandler('attack_reports', \
                              ap_msg.AttackReportStamped, \
                              sub_attack_report)
@@ -899,6 +1003,7 @@ if __name__ == '__main__':
         bridge.addNetHandler(messages.Calibrate, net_calibrate)
         bridge.addNetHandler(messages.Demo, net_demo)
         bridge.addNetHandler(messages.MissionConfig, net_mission_config)
+        bridge.addNetHandler(messages.GenericWPConfig, net_generic_wp_config)
         bridge.addNetHandler(messages.AutopilotReboot, net_ap_reboot)
         bridge.addNetHandler(messages.PayloadHeartbeat, net_health_state)
         bridge.addNetHandler(messages.PayloadShutdown, net_shutdown)
@@ -918,6 +1023,8 @@ if __name__ == '__main__':
                              log_success=False)
         bridge.addNetHandler(messages.WinchesterReport, net_winchester_report)
         bridge.addNetHandler(messages.Takeoff, net_copter_takeoff)
+        bridge.addNetHandler(messages.CameraControl, net_camera_control)
+        bridge.addNetHandler(messages.TacticCameraControl, net_tactic_camera_control)
 
         # Run the loop (shouldn't stop until node is shut down)
         print "\nStarting network bridge loop...\n"