From 8918b0fdbe3a145d2f826b8e9c42312e4bb35b8f Mon Sep 17 00:00:00 2001 From: jseawall Date: Sat, 3 Feb 2024 09:04:25 -0800 Subject: [PATCH] Cleaned Up Comments --- _drone_detect_and_follow_startup_script.py | 31 +- ...e_detect_loiter_snapshot_startup_script.py | 33 +- _drone_waypoint_inspection_startup_script.py | 33 +- ..._detect_and_3d_targeting_startup_script.py | 31 +- _object_detect_and_snapshot_startup_script.py | 32 +- _pantilt_object_tracker_startup_script.py | 33 +- ai_3d_targeting_process_script.py | 26 +- ai_detect_and_snapshot_process_script.py | 29 +- ai_detector_config_script.py | 38 +- ardupilot_rbx_driver_script.py | 44 +- ardupilot_rbx_fake_gps_config_script.py | 45 +- ardupilot_rbx_navpose_config_script.py | 31 +- ...e_detect_loiter_snapshot_mission_script.py | 41 +- drone_follow_object_mission_script.py | 38 +- drone_waypoint_demo_mission_script.py | 610 ++++++++++++++++++ drone_waypoint_inspection_mission_script.py | 61 +- navpose_get_and_publish_process_script.py | 30 +- navpose_set_fixed_config_script.py | 32 +- opencv_image_contours_process_script.py | 28 +- opencv_image_enhance_process_script.py | 29 +- opencv_image_your_custom_process_script.py | 28 +- pantilt_object_tracker_action_script.py | 38 +- pantilt_ptx_navpose_config_script.py | 32 +- pantilt_ptx_navpose_driver_script.py | 31 +- snapshot_event_save_to_disk_action_script.py | 30 +- snapshot_event_send_to_cloud_action_script.py | 26 +- snapshot_event_your_custom_action_script.py | 26 +- zed2_idx_driver_script.py | 39 +- zed2_idx_navpose_config_script.py | 32 +- 29 files changed, 1127 insertions(+), 430 deletions(-) create mode 100644 drone_waypoint_demo_mission_script.py diff --git a/_drone_detect_and_follow_startup_script.py b/_drone_detect_and_follow_startup_script.py index cb9e9ea..22b0875 100644 --- a/_drone_detect_and_follow_startup_script.py +++ b/_drone_detect_and_follow_startup_script.py @@ -24,11 +24,10 @@ # # -# Sample Solution Config Script. -# 1. launches scripts from list NEPI ROS service +# Sample Solution Startup Script. +# 1. Launches scripts from list that are not running # 2. Waits for shutdown -# 3. Stops scripts from list -# 4. Relaunchs any scripts that were running at Start +# 3. Stops scripts that were launched by this script import rospy import os @@ -40,9 +39,9 @@ from std_srvs.srv import Empty, EmptyRequest, Trigger from nepi_ros_interfaces.srv import GetScriptsQuery,GetRunningScriptsQuery ,LaunchScript, StopScript -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## +######################################### +# USER SETTINGS - Edit as Necessary +######################################### SCRIPT_LIST = ["ardupilot_rbx_driver_script.py", "zed2_idx_driver_script.py", @@ -52,6 +51,10 @@ "ai_3d_targeting_process_script.py", "drone_follow_object_mission_script.py"] # Script filenames to start/stop +######################################### +# ROS NAMESPACE SETUP +######################################### + # ROS namespace setup NEPI_BASE_NAMESPACE = "/nepi/s2x/" @@ -61,18 +64,20 @@ AUTO_LAUNCH_SCRIPT_SERVICE_NAME = NEPI_BASE_NAMESPACE + "launch_script" AUTO_STOP_SCRIPT_SERVICE_NAME = NEPI_BASE_NAMESPACE + "stop_script" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + get_installed_scripts_service = rospy.ServiceProxy(AUTO_GET_INSTALLED_SCRIPTS_SERVICE_NAME, GetScriptsQuery ) get_running_scripts_service = rospy.ServiceProxy(AUTO_GET_RUNNING_SCRIPTS_SERVICE_NAME, GetRunningScriptsQuery ) launch_script_service = rospy.ServiceProxy(AUTO_LAUNCH_SCRIPT_SERVICE_NAME, LaunchScript) stop_script_service = rospy.ServiceProxy(AUTO_STOP_SCRIPT_SERVICE_NAME, StopScript) scripts_installed_at_start = None scripts_running_at_start = None -##################################################################################### + +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -248,9 +253,9 @@ def startNode(): # Spin forever rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/_drone_detect_loiter_snapshot_startup_script.py b/_drone_detect_loiter_snapshot_startup_script.py index 27c5c7d..f6aaf7c 100644 --- a/_drone_detect_loiter_snapshot_startup_script.py +++ b/_drone_detect_loiter_snapshot_startup_script.py @@ -24,11 +24,10 @@ # # -# Sample Solution Config Script. -# 1. launches scripts from list NEPI ROS service +# Sample Solution Startup Script. +# 1. Launches scripts from list that are not running # 2. Waits for shutdown -# 3. Stops scripts from list -# 4. Relaunchs any scripts that were running at Start +# 3. Stops scripts that were launched by this script import rospy import os @@ -40,9 +39,11 @@ from std_srvs.srv import Empty, EmptyRequest, Trigger from nepi_ros_interfaces.srv import GetScriptsQuery,GetRunningScriptsQuery ,LaunchScript, StopScript -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## + +######################################### +# USER SETTINGS - Edit as Necessary +######################################### + SCRIPT_LIST = ["ardupilot_rbx_driver_script.py", "ardupilot_rbx_navpose_config_script.py", "ardupilot_rbx_fake_gps_config_script.py", @@ -52,6 +53,10 @@ "drone_detect_loiter_snapshot_mission_script.py"] # Script filenames to start/stop +######################################### +# ROS NAMESPACE SETUP +######################################### + # ROS namespace setup NEPI_BASE_NAMESPACE = "/nepi/s2x/" @@ -61,18 +66,20 @@ AUTO_LAUNCH_SCRIPT_SERVICE_NAME = NEPI_BASE_NAMESPACE + "launch_script" AUTO_STOP_SCRIPT_SERVICE_NAME = NEPI_BASE_NAMESPACE + "stop_script" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + get_installed_scripts_service = rospy.ServiceProxy(AUTO_GET_INSTALLED_SCRIPTS_SERVICE_NAME, GetScriptsQuery ) get_running_scripts_service = rospy.ServiceProxy(AUTO_GET_RUNNING_SCRIPTS_SERVICE_NAME, GetRunningScriptsQuery ) launch_script_service = rospy.ServiceProxy(AUTO_LAUNCH_SCRIPT_SERVICE_NAME, LaunchScript) stop_script_service = rospy.ServiceProxy(AUTO_STOP_SCRIPT_SERVICE_NAME, StopScript) scripts_installed_at_start = None scripts_running_at_start = None -##################################################################################### + +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -248,9 +255,9 @@ def startNode(): # Spin forever rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/_drone_waypoint_inspection_startup_script.py b/_drone_waypoint_inspection_startup_script.py index 5fe6a2b..a4a34da 100644 --- a/_drone_waypoint_inspection_startup_script.py +++ b/_drone_waypoint_inspection_startup_script.py @@ -24,11 +24,10 @@ # # -# Sample Solution Config Script. -# 1. launches scripts from list NEPI ROS service +# Sample Solution Startup Script. +# 1. Launches scripts from list that are not running # 2. Waits for shutdown -# 3. Stops scripts from list -# 4. Relaunchs any scripts that were running at Start +# 3. Stops scripts that were launched by this script import rospy import os @@ -40,15 +39,21 @@ from std_srvs.srv import Empty, EmptyRequest, Trigger from nepi_ros_interfaces.srv import GetScriptsQuery,GetRunningScriptsQuery ,LaunchScript, StopScript -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## + +######################################### +# USER SETTINGS - Edit as Necessary +######################################### + SCRIPT_LIST = ["ardupilot_rbx_driver_script.py", "ardupilot_rbx_navpose_config_script.py", "ardupilot_rbx_fake_gps_config_script.py", "snapshot_event_save_to_disk_action_script.py", "drone_waypoint_inspection_mission_script.py"] # Script filenames to start/stop +######################################### +# ROS NAMESPACE SETUP +######################################### + # ROS namespace setup NEPI_BASE_NAMESPACE = "/nepi/s2x/" @@ -58,18 +63,20 @@ AUTO_LAUNCH_SCRIPT_SERVICE_NAME = NEPI_BASE_NAMESPACE + "launch_script" AUTO_STOP_SCRIPT_SERVICE_NAME = NEPI_BASE_NAMESPACE + "stop_script" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + get_installed_scripts_service = rospy.ServiceProxy(AUTO_GET_INSTALLED_SCRIPTS_SERVICE_NAME, GetScriptsQuery ) get_running_scripts_service = rospy.ServiceProxy(AUTO_GET_RUNNING_SCRIPTS_SERVICE_NAME, GetRunningScriptsQuery ) launch_script_service = rospy.ServiceProxy(AUTO_LAUNCH_SCRIPT_SERVICE_NAME, LaunchScript) stop_script_service = rospy.ServiceProxy(AUTO_STOP_SCRIPT_SERVICE_NAME, StopScript) scripts_installed_at_start = None scripts_running_at_start = None -##################################################################################### + +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -245,9 +252,9 @@ def startNode(): # Spin forever rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/_object_detect_and_3d_targeting_startup_script.py b/_object_detect_and_3d_targeting_startup_script.py index 09e47f7..ab87b6e 100644 --- a/_object_detect_and_3d_targeting_startup_script.py +++ b/_object_detect_and_3d_targeting_startup_script.py @@ -24,11 +24,10 @@ # # -# Sample Solution Config Script. -# 1. launches scripts from list NEPI ROS service +# Sample Solution Startup Script. +# 1. Launches scripts from list that are not running # 2. Waits for shutdown -# 3. Stops scripts from list -# 4. Relaunchs any scripts that were running at Start +# 3. Stops scripts that were launched by this script import rospy import os @@ -40,9 +39,10 @@ from std_srvs.srv import Empty, EmptyRequest, Trigger from nepi_ros_interfaces.srv import GetScriptsQuery,GetRunningScriptsQuery ,LaunchScript, StopScript -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## + +######################################### +# USER SETTINGS - Edit as Necessary +######################################### SCRIPT_LIST = ["zed2_idx_driver_script.py", "navpose_set_fixed_config_script.py", @@ -51,6 +51,9 @@ "ai_3d_targeting_process_script.py"] # Script filenames to start/stop +######################################### +# ROS NAMESPACE SETUP +######################################### # ROS namespace setup NEPI_BASE_NAMESPACE = "/nepi/s2x/" @@ -61,18 +64,20 @@ AUTO_LAUNCH_SCRIPT_SERVICE_NAME = NEPI_BASE_NAMESPACE + "launch_script" AUTO_STOP_SCRIPT_SERVICE_NAME = NEPI_BASE_NAMESPACE + "stop_script" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + get_installed_scripts_service = rospy.ServiceProxy(AUTO_GET_INSTALLED_SCRIPTS_SERVICE_NAME, GetScriptsQuery ) get_running_scripts_service = rospy.ServiceProxy(AUTO_GET_RUNNING_SCRIPTS_SERVICE_NAME, GetRunningScriptsQuery ) launch_script_service = rospy.ServiceProxy(AUTO_LAUNCH_SCRIPT_SERVICE_NAME, LaunchScript) stop_script_service = rospy.ServiceProxy(AUTO_STOP_SCRIPT_SERVICE_NAME, StopScript) scripts_installed_at_start = None scripts_running_at_start = None -##################################################################################### + +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -283,9 +288,9 @@ def startNode(): # Spin forever rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/_object_detect_and_snapshot_startup_script.py b/_object_detect_and_snapshot_startup_script.py index 506a4af..1f5766a 100644 --- a/_object_detect_and_snapshot_startup_script.py +++ b/_object_detect_and_snapshot_startup_script.py @@ -24,11 +24,10 @@ # # -# Sample Solution Config Script. -# 1. launches scripts from list NEPI ROS service +# Sample Solution Startup Script. +# 1. Launches scripts from list that are not running # 2. Waits for shutdown -# 3. Stops scripts from list -# 4. Relaunchs any scripts that were running at Start +# 3. Stops scripts that were launched by this script import rospy import os @@ -40,15 +39,20 @@ from std_srvs.srv import Empty, EmptyRequest, Trigger from nepi_ros_interfaces.srv import GetScriptsQuery,GetRunningScriptsQuery ,LaunchScript, StopScript -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## + +######################################### +# USER SETTINGS - Edit as Necessary +######################################### + SCRIPT_LIST = ["ai_detector_config_script.py", "navpose_set_fixed_config_script.py", "ai_detect_and_snapshot_process_script.py", "snapshot_event_save_to_disk_action_script.py", "snapshot_event_send_to_cloud_action_script.py"] # Script filenames to start/stop +######################################### +# ROS NAMESPACE SETUP +######################################### # ROS namespace setup NEPI_BASE_NAMESPACE = "/nepi/s2x/" @@ -59,18 +63,20 @@ AUTO_LAUNCH_SCRIPT_SERVICE_NAME = NEPI_BASE_NAMESPACE + "launch_script" AUTO_STOP_SCRIPT_SERVICE_NAME = NEPI_BASE_NAMESPACE + "stop_script" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + get_installed_scripts_service = rospy.ServiceProxy(AUTO_GET_INSTALLED_SCRIPTS_SERVICE_NAME, GetScriptsQuery ) get_running_scripts_service = rospy.ServiceProxy(AUTO_GET_RUNNING_SCRIPTS_SERVICE_NAME, GetRunningScriptsQuery ) launch_script_service = rospy.ServiceProxy(AUTO_LAUNCH_SCRIPT_SERVICE_NAME, LaunchScript) stop_script_service = rospy.ServiceProxy(AUTO_STOP_SCRIPT_SERVICE_NAME, StopScript) scripts_installed_at_start = None scripts_running_at_start = None -##################################################################################### + +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -246,9 +252,9 @@ def startNode(): # Spin forever rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/_pantilt_object_tracker_startup_script.py b/_pantilt_object_tracker_startup_script.py index 82d4d01..03b1258 100644 --- a/_pantilt_object_tracker_startup_script.py +++ b/_pantilt_object_tracker_startup_script.py @@ -24,11 +24,10 @@ # # -# Sample Solution Config Script. -# 1. launches scripts from list NEPI ROS service +# Sample Solution Startup Script. +# 1. Launches scripts from list that are not running # 2. Waits for shutdown -# 3. Stops scripts from list -# 4. Relaunchs any scripts that were running at Start +# 3. Stops scripts that were launched by this script import rospy import os @@ -40,15 +39,21 @@ from std_srvs.srv import Empty, EmptyRequest, Trigger from nepi_ros_interfaces.srv import GetScriptsQuery,GetRunningScriptsQuery ,LaunchScript, StopScript -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## + +######################################### +# USER SETTINGS - Edit as Necessary +######################################### + SCRIPT_LIST = ["pantilt_ptx_navpose_driver_script.py", "navpose_set_fixed_config_script.py", "pantilt_ptx_navpose_config_script.py", "ai_detector_config_script.py", "pantilt_object_tracker_action_script.py"] # Script filenames to start/stop +######################################### +# ROS NAMESPACE SETUP +######################################### + # ROS namespace setup NEPI_BASE_NAMESPACE = "/nepi/s2x/" @@ -58,18 +63,20 @@ AUTO_LAUNCH_SCRIPT_SERVICE_NAME = NEPI_BASE_NAMESPACE + "launch_script" AUTO_STOP_SCRIPT_SERVICE_NAME = NEPI_BASE_NAMESPACE + "stop_script" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + get_installed_scripts_service = rospy.ServiceProxy(AUTO_GET_INSTALLED_SCRIPTS_SERVICE_NAME, GetScriptsQuery ) get_running_scripts_service = rospy.ServiceProxy(AUTO_GET_RUNNING_SCRIPTS_SERVICE_NAME, GetRunningScriptsQuery ) launch_script_service = rospy.ServiceProxy(AUTO_LAUNCH_SCRIPT_SERVICE_NAME, LaunchScript) stop_script_service = rospy.ServiceProxy(AUTO_STOP_SCRIPT_SERVICE_NAME, StopScript) scripts_installed_at_start = None scripts_running_at_start = None -##################################################################################### + +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -245,9 +252,9 @@ def startNode(): # Spin forever rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/ai_3d_targeting_process_script.py b/ai_3d_targeting_process_script.py index 1e38ec1..e456859 100644 --- a/ai_3d_targeting_process_script.py +++ b/ai_3d_targeting_process_script.py @@ -24,8 +24,7 @@ # # -# Sample NEPI Automation Script. -# Uses onboard ROS python library to +# Sample NEPI Process Script. # 1. Waits for ai detection topic # 2. Runs a process that convert zed stereo camera depthmap depth_array and depth_image # 3. Runs a process to calculate range and bearing of detected targets @@ -52,16 +51,18 @@ from nepi_ros_interfaces.msg import ClassifierSelection, StringArray, TargetLocalization from darknet_ros_msgs.msg import BoundingBoxes, ObjectCount -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## +######################################### +# USER SETTINGS - Edit as Necessary +######################################### #Set Runtime User Settings - TARGET_BOX_REDUCTION_PERCENT=50 # Sets the percent of target box around center to use for range calc TARGET_DEPTH_METERS=0.3 # Sets the depth filter around mean depth to use for range calc TARGET_MIN_VALUES=10 # Sets the minimum number of valid points to consider for a valid range +######################################### +# ROS NAMESPACE SETUP +######################################### # ROS namespace setup NEPI_BASE_NAMESPACE = "/nepi/s2x/" @@ -81,18 +82,19 @@ FOUND_OBJECT_TOPIC = NEPI_BASE_NAMESPACE + "classifier/found_object" AI_DETECTION_IMAGE_TOPIC = NEPI_BASE_NAMESPACE + "classifier/detection_image" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + target_data_pub = rospy.Publisher(TARGET_DATA_OUTPUT_TOPIC, TargetLocalization, queue_size=10) target_overlay_pub = rospy.Publisher(TARGET_IMAGE_OUTPUT_TOPIC, Image, queue_size=10) np_depth_array_m=None # will be replaced when depthmap is recieved and converted detect_boxes=None -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -303,9 +305,9 @@ def startNode(): rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/ai_detect_and_snapshot_process_script.py b/ai_detect_and_snapshot_process_script.py index 470b1e1..b865e31 100644 --- a/ai_detect_and_snapshot_process_script.py +++ b/ai_detect_and_snapshot_process_script.py @@ -24,10 +24,9 @@ # # -# Sample NEPI Automation Script. -# Uses onboard ROS python library to +# Sample NEPI Process Script. # 1. Waits for ai detection topic -# 2. Wait for specific object to be detected and centered +# 2. Waits for specific object to be detected and centered # 3. Publishes a snapshot event trigger out # 4. Delays trigger event for some set delay time @@ -49,15 +48,18 @@ from sensor_msgs.msg import Image from darknet_ros_msgs.msg import BoundingBoxes -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## +######################################### +# USER SETTINGS - Edit as Necessary +######################################### -###!!!!!!!! Set Automation action parameters !!!!!!!! OBJ_LABEL_OF_INTEREST = "person" OBJ_CENTERED_BUFFER_RATIO = 0.5 # acceptable band about center of image for saving purposes RESET_DELAY_S = 5 # Min delay between triggers +######################################### +# ROS NAMESPACE SETUP +######################################### + # ROS namespace setup NEPI_BASE_NAMESPACE = "/nepi/s2x/" @@ -68,16 +70,17 @@ # Snapshot Publish Topic SNAPSHOT_TRIGGER_TOPIC = NEPI_BASE_NAMESPACE + "snapshot_event" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + snapshot_trigger_pub = rospy.Publisher(SNAPSHOT_TRIGGER_TOPIC, Empty, queue_size = 1) img_width = 0 # Updated on receipt of first image img_height = 0 # Updated on receipt of first image -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -191,9 +194,9 @@ def startNode(): rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/ai_detector_config_script.py b/ai_detector_config_script.py index 18fa3bc..c67e31f 100644 --- a/ai_detector_config_script.py +++ b/ai_detector_config_script.py @@ -24,13 +24,11 @@ # # -# Sample NEPI Automation Script. -# Uses onboard ROS python library to +# Sample NEPI Config Script. # 1. Checks if AI input image topic exists -# 2. Try's to set reslution on camera -# 3. Loads selected AI model -# 4. Starts AI detection process using input image stream -# 5. Stops AI detection process on shutdown +# 2. Loads selected AI model +# 3. Starts AI detection process using input image stream +# 4. Stops AI detection process on shutdown import time import sys @@ -40,9 +38,10 @@ from std_msgs.msg import UInt8, Empty, String, Bool from nepi_ros_interfaces.msg import ClassifierSelection, StringArray -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## +######################################### +# USER SETTINGS - Edit as Necessary +######################################### + #Set AI Detector Image ROS Topic Name IMAGE_INPUT_TOPIC_NAME = "color_2d_image" @@ -51,20 +50,25 @@ DETECTION_MODEL = "common_object_detection" DETECTION_THRESHOLD = 0.5 -# NEPI ROS namespace setup +######################################### +# ROS NAMESPACE SETUP +######################################### + NEPI_BASE_NAMESPACE = "/nepi/s2x/" # AI Detector Publish Topics AI_START_TOPIC = NEPI_BASE_NAMESPACE + "start_classifier" AI_STOP_TOPIC = NEPI_BASE_NAMESPACE + "stop_classifier" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + stop_classifier_pub = rospy.Publisher(AI_STOP_TOPIC, Empty, queue_size=10) -##################################################################################### + +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -135,11 +139,9 @@ def startNode(): rospy.spin() - - -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/ardupilot_rbx_driver_script.py b/ardupilot_rbx_driver_script.py index 5b95ec0..2275c6f 100644 --- a/ardupilot_rbx_driver_script.py +++ b/ardupilot_rbx_driver_script.py @@ -24,7 +24,8 @@ # # -# Sample NEPI Concept Ardupilot RBX Driver Script. +# Sample NEPI Driver Script. +# NEPI RBX Driver for Ardupilot Autopilot Systems ### Set the namespace before importing rospy import os @@ -66,11 +67,11 @@ def ginterpolator(single_position): -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## +######################################### +# DRIVER SETTINGS +######################################### -################################################### +############################## # RBX State and Mode Dictionaries RBX_NAVPOSE = 7 # Byte Mask [GPS, Heading, Orienation] RBX_STATES = ["DISARM","ARM"] @@ -81,7 +82,7 @@ def ginterpolator(single_position): RBX_MODE_FUNCTIONS = ["stabilize","land","rtl","loiter","guided","resume"] RBX_ACTION_FUNCTIONS = ["takeoff"] -################################################### +############################## # RBX Initialization Values GOTO_TRAN_SPEED_RATIO = 0.5 GOTO_ROT_SPEED_RATIO = 0.5 @@ -91,18 +92,18 @@ def ginterpolator(single_position): CMD_TIMEOUT_SEC = 20 # Any action that changes PRINT_STATUS_1HZ = True # Print current rbx status at 1Hz - - -################################################### +############################## # ARDUPILOT Settings TAKEOFF_MIN_PITCH_DEG = 10 TAKEOFF_ALT_M = 10 +######################################### +# ROS NAMESPACE SETUP +######################################### -################################################### -# ROS namespace setup NEPI_BASE_NAMESPACE = "/nepi/s2x/" + NEPI_RBX_NAME = "ardupilot" NEPI_RBX_NAMESPACE = NEPI_BASE_NAMESPACE + NEPI_RBX_NAME + "/rbx/" NEPI_NAVPOSE_SERVICE_NAME = NEPI_BASE_NAMESPACE + "nav_pose_query" @@ -113,7 +114,6 @@ def ginterpolator(single_position): NEPI_RBX_CAPABILITIES_STATES_TOPIC = NEPI_RBX_NAMESPACE + "state_options" NEPI_RBX_CAPABILITIES_MODES_TOPIC = NEPI_RBX_NAMESPACE + "mode_options" NEPI_RBX_CAPABILITIES_ACTIONS_TOPIC = NEPI_RBX_NAMESPACE + "actions_options" - # NEPI MAVLINK RBX Driver Status Publish Topics NEPI_RBX_STATUS_RATE_HZ = 10 NEPI_RBX_STATUS_STATE_TOPIC = NEPI_RBX_NAMESPACE + "state" # Int to Defined Dictionary RBX_STATES @@ -125,12 +125,10 @@ def ginterpolator(single_position): NEPI_RBX_STATUS_GOTO_ERRORS_TOPIC = NEPI_RBX_NAMESPACE + "goto_errors" # Floats [X_Meters,Y_Meters,Z_Meters,Heading_Degrees,Roll_Degrees,Pitch_Degrees,Yaw_Degrees] NEPI_RBX_STATUS_CMD_TIMEOUT_TOPIC = NEPI_RBX_NAMESPACE + "cmd_timeout" # Int Seconds - Any command that changes ready state NEPI_RBX_STATUS_CMD_SUCCESS_TOPIC = NEPI_RBX_NAMESPACE + "cmd_success" # Bool - Any command that changes ready state - # NEPI MAVLINK RBX Driver NavPose Publish Topics NEPI_RBX_NAVPOSE_GPS_TOPIC = NEPI_RBX_NAMESPACE + "gps_fix" NEPI_RBX_NAVPOSE_ODOM_TOPIC = NEPI_RBX_NAMESPACE + "odom" NEPI_RBX_NAVPOSE_HEADING_TOPIC = NEPI_RBX_NAMESPACE + "heading" - # NEPI MAVLINK RBX Driver Settings Subscriber Topics NEPI_RBX_SET_STATE_TOPIC = NEPI_RBX_NAMESPACE + "set_state" # Int to Defined Dictionary RBX_STATES NEPI_RBX_SET_MODE_TOPIC = NEPI_RBX_NAMESPACE + "set_mode" # Int to Defined Dictionary RBX_MODES @@ -138,7 +136,6 @@ def ginterpolator(single_position): NEPI_RBX_SET_GOTO_SPEEDS_TOPIC = NEPI_RBX_NAMESPACE + "set_goto_speeds" # Float [Translation_Ratio,Rotation_Ratio] NEPI_RBX_SET_GOTO_GOALS_TOPIC = NEPI_RBX_NAMESPACE + "set_goto_goals" # Float [Max_Meters,Max_Degrees,Stabilize_Time_Sec] NEPI_RBX_SET_CMD_TIMEOUT_TOPIC = NEPI_RBX_NAMESPACE + "set_cmd_timeout" # Int Seconds - Any command that changes ready state - # NEPI MAVLINK RBX Driver Control Subscriber Topics NEPI_RBX_GO_ACTION_TOPIC = NEPI_RBX_NAMESPACE + "go_action" # Int to Defined Dictionary RBX_ACTIONS NEPI_RBX_GO_HOME_TOPIC = NEPI_RBX_NAMESPACE + "go_home" # Ignored if any active goto processes @@ -149,31 +146,28 @@ def ginterpolator(single_position): ################################################### MAVLINK_NAMESPACE = NEPI_BASE_NAMESPACE + "mavlink/" - # MAVLINK Subscriber Topics MAVLINK_STATE_TOPIC = MAVLINK_NAMESPACE + "state" MAVLINK_BATTERY_TOPIC = MAVLINK_NAMESPACE + "battery" - # MAVLINK Required Services MAVLINK_SET_HOME_SERVICE = MAVLINK_NAMESPACE + "cmd/set_home" MAVLINK_SET_MODE_SERVICE = MAVLINK_NAMESPACE + "set_mode" MAVLINK_ARMING_SERVICE = MAVLINK_NAMESPACE + "cmd/arming" MAVLINK_TAKEOFF_SERVICE = MAVLINK_NAMESPACE + "cmd/takeoff" - # MAVLINK NavPose Source Topics MAVLINK_SOURCE_GPS_TOPIC = MAVLINK_NAMESPACE + "global_position/global" MAVLINK_SOURCE_ODOM_TOPIC = MAVLINK_NAMESPACE + "global_position/local" MAVLINK_SOURCE_HEADING_TOPIC = MAVLINK_NAMESPACE + "global_position/compass_hdg" - # MAVLINK Setpoint Control Topics MAVLINK_SETPOINT_ATTITUDE_TOPIC = MAVLINK_NAMESPACE + "setpoint_raw/attitude" MAVLINK_SETPOINT_POSITION_LOCAL_TOPIC = MAVLINK_NAMESPACE + "setpoint_position/local" MAVLINK_SETPOINT_LOCATION_GLOBAL_TOPIC = MAVLINK_NAMESPACE + "setpoint_position/global" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + rbx_capabilities_navpose_pub = rospy.Publisher(NEPI_RBX_CAPABILITIES_NAVPOSE_TOPIC, UInt8, queue_size=1) rbx_capabilities_states_pub = rospy.Publisher(NEPI_RBX_CAPABILITIES_STATES_TOPIC, String, queue_size=1) rbx_capabilities_mode_pub = rospy.Publisher(NEPI_RBX_CAPABILITIES_MODES_TOPIC, String, queue_size=1) @@ -224,9 +218,9 @@ def ginterpolator(single_position): mavlink_state = None -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -1557,9 +1551,9 @@ def startNode(): rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/ardupilot_rbx_fake_gps_config_script.py b/ardupilot_rbx_fake_gps_config_script.py index e73a379..efa5374 100644 --- a/ardupilot_rbx_fake_gps_config_script.py +++ b/ardupilot_rbx_fake_gps_config_script.py @@ -25,8 +25,7 @@ # -# Sample NEPI Automation Script. -# Uses onboard ROS and MAVROS libraries to +# Sample NEPI Config Script. # 1) Publishes a fake GPS MAVLink Message # Provides two ROS control topics # a) goto_geopoint_wgs84 - Simulates move to new geopoint @@ -71,15 +70,9 @@ from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3, PoseStamped from nepi_ros_interfaces.srv import NavPoseQuery, NavPoseQueryRequest -################################################### -# SETUP - Edit as Necessary -########################################## - -################################################### -# RBX State and Mode Dictionaries -RBX_MODE_FUNCTIONS = ["stabilize","land","rtl","loiter","guided","resume"] -RBX_ACTION_FUNCTIONS = ["takeoff"] -TAKEOFF_ALT_M = 10 +######################################### +# USER SETTINGS - Edit as Necessary +######################################### #Startup Location # [Lat, Long, Altitude_WGS84] @@ -96,22 +89,23 @@ PRINT_LOCATION_1HZ = False # Print current location at 1Hz -# ROS namespace setup + + +######################################### +# ROS NAMESPACE SETUP +######################################### + NEPI_BASE_NAMESPACE = "/nepi/s2x/" NEPI_NAVPOSE_SERVICE_NAME = NEPI_BASE_NAMESPACE + "nav_pose_query" NEPI_RBX_NAMESPACE = NEPI_BASE_NAMESPACE + "ardupilot/rbx/" - MAVLINK_NAMESPACE = NEPI_BASE_NAMESPACE + "mavlink/" - # MAVLINK Fake GPS Publish Topic MAVLINK_HILGPS_TOPIC = MAVLINK_NAMESPACE + "hil/gps" - # MAVLINK Fake GPS Control Topic MAVLINK_FAKE_GPS_GOTO_GEOPOINT_TOPIC = MAVLINK_NAMESPACE + "fake_gps/goto_geopoint_wgs84" MAVLINK_FAKE_GPS_RESET_GEOPOINT_TOPIC = MAVLINK_NAMESPACE + "fake_gps/reset_geopoint_wgs84" MAVLINK_FAKE_GPS_RESET_CURRENT_TOPIC = MAVLINK_NAMESPACE + "fake_gps/reset_current" - # MAVLINK Fake GPS RBX Subscriber Topic NEPI_RBX_GOTO_POSITION_TOPIC = NEPI_RBX_NAMESPACE + "goto_position" NEPI_RBX_GOTO_LOCATION_TOPIC = NEPI_RBX_NAMESPACE + "goto_location" @@ -120,9 +114,15 @@ NEPI_RBX_SET_ACTION_TOPIC = NEPI_RBX_NAMESPACE + "set_action" # Int to Defined Dictionary RBX_MODES -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + +# RBX State and Mode Dictionaries +RBX_MODE_FUNCTIONS = ["stabilize","land","rtl","loiter","guided","resume"] +RBX_ACTION_FUNCTIONS = ["takeoff"] +TAKEOFF_ALT_M = 10 + mavlink_fake_gps_mavlink_pub = rospy.Publisher(MAVLINK_HILGPS_TOPIC, HilGPS, queue_size=1) current_location_wgs84_geo = None current_heading_deg = None @@ -130,9 +130,10 @@ navpose_update_interval = 0.1 fake_gps_enabled = True gps_publish_interval_sec=1.0/GPS_PUB_RATE_HZ -##################################################################################### + +######################################### # Methods -##########f########################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -490,9 +491,9 @@ def startNode(): # Spin forever rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/ardupilot_rbx_navpose_config_script.py b/ardupilot_rbx_navpose_config_script.py index 84b719f..87c501a 100644 --- a/ardupilot_rbx_navpose_config_script.py +++ b/ardupilot_rbx_navpose_config_script.py @@ -24,10 +24,9 @@ # # -# Sample NEPI Automation Script. -# Uses onboard ROS and MAVROS python libraries to +# Sample NEPI Config Script. # 1. Connect NEPI NavPose topics to appropriate mavros topics -# 2. Set NEPI GPS source time synchronization +# 2. Sets NEPI GPS source and time synchronization import rospy import time @@ -39,12 +38,17 @@ from nepi_ros_interfaces.srv import NavPoseQuery, NavPoseQueryRequest -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## +######################################### +# USER SETTINGS - Edit as Necessary +######################################### + SYNC_NEPI2GPS_CLOCK = True # Set to false to disable GPS clock sync -# ROS namespace setup + +######################################### +# ROS NAMESPACE SETUP +######################################### + NEPI_BASE_NAMESPACE = "/nepi/s2x/" NEPI_RBX_NAMESPACE = NEPI_BASE_NAMESPACE + "ardupilot/rbx/" @@ -60,16 +64,17 @@ NEPI_ENABLE_NAVPOSE_GPS_CLOCK_SYNC_TOPIC = NEPI_BASE_NAMESPACE + "nav_pose_mgr/enable_gps_clock_sync" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + mavros_global_msg=None mavros_heading_msg=None mavros_orientation_msg=None -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -164,9 +169,9 @@ def startNode(): -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/drone_detect_loiter_snapshot_mission_script.py b/drone_detect_loiter_snapshot_mission_script.py index 5e888f3..d35176f 100644 --- a/drone_detect_loiter_snapshot_mission_script.py +++ b/drone_detect_loiter_snapshot_mission_script.py @@ -24,8 +24,7 @@ # # -# Sample NEPI Automation Script. -# Uses onboard ROS python and mavros libraries to +# Sample NEPI Mission Script. ### Expects Classifier to be running ### # 1) Monitors AI detector output for specfic target class # 3) Changes system to Loiter mode on detection @@ -57,9 +56,17 @@ from nepi_ros_interfaces.msg import TargetLocalization from darknet_ros_msgs.msg import BoundingBoxes -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## + +################################################### +# RBX State and Mode Dictionaries +RBX_STATES = ["DISARM","ARM"] +RBX_MODES = ["STABILIZE","LAND","RTL","LOITER","GUIDED","RESUME"] +RBX_ACTIONS = ["TAKEOFF"] + + +######################################### +# USER SETTINGS - Edit as Necessary +######################################### ###!!!!!!!! Set Automation action parameters !!!!!!!! OBJ_LABEL_OF_INTEREST = "person" @@ -72,11 +79,10 @@ NEPI_NAVPOSE_SERVICE_NAME = NEPI_BASE_NAMESPACE + "nav_pose_query" NEPI_RBX_NAMESPACE = NEPI_BASE_NAMESPACE + "ardupilot/rbx/" -################################################### -# RBX State and Mode Dictionaries -RBX_STATES = ["DISARM","ARM"] -RBX_MODES = ["STABILIZE","LAND","RTL","LOITER","GUIDED","RESUME"] -RBX_ACTIONS = ["TAKEOFF"] + +######################################### +# ROS NAMESPACE SETUP +######################################### # NEPI MAVLINK RBX Driver Capabilities Publish Topics NEPI_RBX_CAPABILITIES_NAVPOSE_TOPIC = NEPI_RBX_NAMESPACE + "navpose_support" @@ -105,8 +111,6 @@ NEPI_RBX_GOTO_POSITION_TOPIC = NEPI_RBX_NAMESPACE + "goto_position" # Ignored if any active goto processes NEPI_RBX_GOTO_LOCATION_TOPIC = NEPI_RBX_NAMESPACE + "goto_location" # Ignored if any active goto processes - - # AI Detector Subscriber Topics AI_BOUNDING_BOXES_TOPIC = NEPI_BASE_NAMESPACE + "classifier/bounding_boxes" AI_DETECTION_IMAGE_TOPIC = NEPI_BASE_NAMESPACE + "classifier/detection_image" @@ -114,9 +118,10 @@ # Mission Action Topics SNAPSHOT_TRIGGER_TOPIC = NEPI_BASE_NAMESPACE + "snapshot_event" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + rbx_set_state_pub = rospy.Publisher(NEPI_RBX_SET_STATE_TOPIC, UInt8, queue_size=1) rbx_set_mode_pub = rospy.Publisher(NEPI_RBX_SET_MODE_TOPIC, UInt8, queue_size=1) rbx_set_home_current_pub = rospy.Publisher(NEPI_RBX_SET_HOME_CURRENT_TOPIC, Empty, queue_size=1) @@ -147,9 +152,9 @@ reset_delay_timer = 10000 last_reset_time = time.time() -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -670,9 +675,9 @@ def startNode(): rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/drone_follow_object_mission_script.py b/drone_follow_object_mission_script.py index 3e2ecdd..f97b071 100644 --- a/drone_follow_object_mission_script.py +++ b/drone_follow_object_mission_script.py @@ -24,8 +24,7 @@ # # -# Sample NEPI Automation Script. -# Uses onboard ROS python and mavros libraries to +# Sample NEPI Mission Script. ### Expects Classifier to be running ### # 1) Connects to Target Range and Bearing data Topic # 2) Monitors AI detector output for specfic target class @@ -61,25 +60,29 @@ from nepi_ros_interfaces.msg import TargetLocalization from darknet_ros_msgs.msg import BoundingBoxes -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## +################################################### +# RBX State and Mode Dictionaries +RBX_STATES = ["DISARM","ARM"] +RBX_MODES = ["STABILIZE","LAND","RTL","LOITER","GUIDED","RESUME"] +RBX_ACTIONS = ["TAKEOFF"] + +######################################### +# USER SETTINGS - Edit as Necessary +######################################### OBJ_LABEL_OF_INTEREST = 'chair' TARGET_OFFSET_GOAL_M = 0.5 # How close to set setpoint to target RESET_DELAY_S = 1 # Min delay between triggering a new move +######################################### +# ROS NAMESPACE SETUP +######################################### + # ROS namespace setup NEPI_BASE_NAMESPACE = "/nepi/s2x/" NEPI_NAVPOSE_SERVICE_NAME = NEPI_BASE_NAMESPACE + "nav_pose_query" NEPI_RBX_NAMESPACE = NEPI_BASE_NAMESPACE + "ardupilot/rbx/" -################################################### -# RBX State and Mode Dictionaries -RBX_STATES = ["DISARM","ARM"] -RBX_MODES = ["STABILIZE","LAND","RTL","LOITER","GUIDED","RESUME"] -RBX_ACTIONS = ["TAKEOFF"] - # NEPI MAVLINK RBX Driver Capabilities Publish Topics NEPI_RBX_CAPABILITIES_NAVPOSE_TOPIC = NEPI_RBX_NAMESPACE + "navpose_support" NEPI_RBX_CAPABILITIES_STATES_TOPIC = NEPI_RBX_NAMESPACE + "state_options" @@ -113,9 +116,10 @@ # Mission Action Topics SNAPSHOT_TRIGGER_TOPIC = NEPI_BASE_NAMESPACE + "snapshot_event" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + rbx_set_state_pub = rospy.Publisher(NEPI_RBX_SET_STATE_TOPIC, UInt8, queue_size=1) rbx_set_mode_pub = rospy.Publisher(NEPI_RBX_SET_MODE_TOPIC, UInt8, queue_size=1) rbx_set_home_current_pub = rospy.Publisher(NEPI_RBX_SET_HOME_CURRENT_TOPIC, Empty, queue_size=1) @@ -140,9 +144,9 @@ snapshot_trigger_pub = rospy.Publisher(SNAPSHOT_TRIGGER_TOPIC, Empty, queue_size = 1) -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -636,9 +640,9 @@ def startNode(): rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/drone_waypoint_demo_mission_script.py b/drone_waypoint_demo_mission_script.py new file mode 100644 index 0000000..89bd003 --- /dev/null +++ b/drone_waypoint_demo_mission_script.py @@ -0,0 +1,610 @@ +#!/usr/bin/env python +# +# NEPI Dual-Use License +# Project: nepi_sample_auto_scripts +# +# This license applies to any user of NEPI Engine software +# +# Copyright (C) 2023 Numurus, LLC +# see https://github.com/numurus-nepi/nepi_edge_sdk_base +# +# This software is dual-licensed under the terms of either a NEPI software developer license +# or a NEPI software commercial license. +# +# The terms of both the NEPI software developer and commercial licenses +# can be found at: www.numurus.com/licensing-nepi-engine +# +# Redistributions in source code must retain this top-level comment block. +# Plagiarizing this software to sidestep the license obligations is illegal. +# +# Contact Information: +# ==================== +# - https://www.numurus.com/licensing-nepi-engine +# - mailto:nepi@numurus.com +# +# + + +# Sample NEPI Mission Script. +# 1) Subscribes to NEPI nav_pose_current heading, orientation, position, location topics +# 2) Runs pre-mission processes +# 3) Runs mission goto command processes +# 4) Runs mission goto action processes +# 5) Runs post-mission processes + +# Requires the following additional scripts are running +# a) ardupilot_rbx_driver_script.py +# (Optional) Some Snapshot Action Automation Script like the following +# b)snapshot_event_save_to_disk_action_script.py +# c)snapshot_event_send_to_cloud_action_script.py +# d) (Optional) ardupilot_rbx_fake_gps_process_script.py if a real GPS fix is not available +# These scripts are available for download at: +# [link text](https://github.com/numurus-nepi/nepi_sample_auto_scripts) + +import rospy +import time +import numpy as np +import math +import tf + +from std_msgs.msg import Empty, Int8, UInt8, Bool, String, Float32, Float64, Float64MultiArray +from nav_msgs.msg import Odometry +from sensor_msgs.msg import NavSatFix +from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3, PoseStamped +from geographic_msgs.msg import GeoPoint, GeoPose, GeoPoseStamped +from mavros_msgs.msg import State, AttitudeTarget +from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest, CommandTOL, CommandHome + + +################################################### +# RBX State and Mode Dictionaries +RBX_STATES = ["DISARM","ARM"] +RBX_MODES = ["STABILIZE","LAND","RTL","LOITER","GUIDED","RESUME"] +RBX_ACTIONS = ["TAKEOFF"] + + +######################################### +# USER SETTINGS - Edit as Necessary +######################################### + +# GoTo Position Global Settings +################### +# goto_location is [LAT, LONG, ALT_WGS84, YAW_NED_DEGREES] +# Altitude is specified as meters above the WGS-84 and converted to AMSL before sending +# Yaw is specified in NED frame degrees 0-360 or +-180 +GOTO_LOCATION = [47.6541208,-122.3186620, 20, -999] # [Lat, Long, Alt WGS84, Yaw NED Frame], Enter -999 to use current value + + +######################################### +# ROS NAMESPACE SETUP +######################################### + +NEPI_BASE_NAMESPACE = "/nepi/s2x/" +NEPI_NAVPOSE_SERVICE_NAME = NEPI_BASE_NAMESPACE + "nav_pose_query" +NEPI_RBX_NAMESPACE = NEPI_BASE_NAMESPACE + "ardupilot/rbx/" + +# NEPI MAVLINK RBX Driver Capabilities Publish Topics +NEPI_RBX_CAPABILITIES_NAVPOSE_TOPIC = NEPI_RBX_NAMESPACE + "navpose_support" +NEPI_RBX_CAPABILITIES_STATES_TOPIC = NEPI_RBX_NAMESPACE + "state_options" +NEPI_RBX_CAPABILITIES_MODES_TOPIC = NEPI_RBX_NAMESPACE + "mode_options" +NEPI_RBX_CAPABILITIES_ACTIONS_TOPIC = NEPI_RBX_NAMESPACE + "actions_options" + +# NEPI MAVLINK RBX Driver Status Publish Topic +NEPI_RBX_STATUS_STATE_TOPIC = NEPI_RBX_NAMESPACE + "state" # Int to Defined Dictionary RBX_STATES +NEPI_RBX_STATUS_MODE_TOPIC = NEPI_RBX_NAMESPACE + "mode" # Int to Defined Dictionary RBX_MODES +NEPI_RBX_STATUS_READY_TOPIC = NEPI_RBX_NAMESPACE + "ready" # Bool, True if goto is complete or no active goto process +NEPI_RBX_STATUS_GOTO_ERRORS_TOPIC = NEPI_RBX_NAMESPACE + "goto_errors" # Floats [X_Meters,Y_Meters,Z_Meters,Heading_Degrees,Roll_Degrees,Pitch_Degrees,Yaw_Degrees] +NEPI_RBX_STATUS_CMD_SUCCESS_TOPIC = NEPI_RBX_NAMESPACE + "cmd_success" # Bool - Any command that changes ready state + +# NEPI MAVLINK RBX Driver Settings Subscriber Topics +NEPI_RBX_SET_TIMEOUT_SEC = 5 +NEPI_RBX_SET_STATE_TOPIC = NEPI_RBX_NAMESPACE + "set_state" # Int to Defined Dictionary RBX_STATES +NEPI_RBX_SET_MODE_TOPIC = NEPI_RBX_NAMESPACE + "set_mode" # Int to Defined Dictionary RBX_MODES +NEPI_RBX_SET_HOME_CURRENT_TOPIC = NEPI_RBX_NAMESPACE + "set_home_current" # Emplty + +# NEPI MAVLINK RBX Driver Control Subscriber Topics +NEPI_RBX_GO_ACTION_TOPIC = NEPI_RBX_NAMESPACE + "go_action" # Int to Defined Dictionary RBX_ACTIONS +NEPI_RBX_GO_HOME_TOPIC = NEPI_RBX_NAMESPACE + "go_home" # Aborts any active goto processes +NEPI_RBX_GO_STOP_TOPIC = NEPI_RBX_NAMESPACE + "go_stop" # Aborts any active goto processes +NEPI_RBX_GOTO_POSE_TOPIC = NEPI_RBX_NAMESPACE + "goto_pose" # Ignored if any active goto processes +NEPI_RBX_GOTO_POSITION_TOPIC = NEPI_RBX_NAMESPACE + "goto_position" # Ignored if any active goto processes +NEPI_RBX_GOTO_LOCATION_TOPIC = NEPI_RBX_NAMESPACE + "goto_location" # Ignored if any active goto processes + +# AI Detector Subscriber Topics +AI_BOUNDING_BOXES_TOPIC = NEPI_BASE_NAMESPACE + "classifier/bounding_boxes" +AI_DETECTION_IMAGE_TOPIC = NEPI_BASE_NAMESPACE + "classifier/detection_image" + +# Mission Action Topics +SNAPSHOT_TRIGGER_TOPIC = NEPI_BASE_NAMESPACE + "snapshot_event" + +######################################### +# Methods +######################################### + +rbx_set_state_pub = rospy.Publisher(NEPI_RBX_SET_STATE_TOPIC, UInt8, queue_size=1) +rbx_set_mode_pub = rospy.Publisher(NEPI_RBX_SET_MODE_TOPIC, UInt8, queue_size=1) +rbx_set_home_current_pub = rospy.Publisher(NEPI_RBX_SET_HOME_CURRENT_TOPIC, Empty, queue_size=1) + +rbx_go_action_pub = rospy.Publisher(NEPI_RBX_GO_ACTION_TOPIC, UInt8, queue_size=1) +rbx_go_home_pub = rospy.Publisher(NEPI_RBX_GO_HOME_TOPIC, Empty, queue_size=1) +rbx_go_stop_pub = rospy.Publisher(NEPI_RBX_GO_STOP_TOPIC, Empty, queue_size=1) +rbx_goto_pose_pub = rospy.Publisher(NEPI_RBX_GOTO_POSE_TOPIC, Float64MultiArray, queue_size=1) +rbx_goto_position_pub = rospy.Publisher(NEPI_RBX_GOTO_POSITION_TOPIC, Float64MultiArray, queue_size=1) +rbx_goto_location_pub = rospy.Publisher(NEPI_RBX_GOTO_LOCATION_TOPIC, Float64MultiArray, queue_size=1) + +rbx_cap_navpose = None +rbx_cap_states = None +rbx_cap_modes = None +rbx_cap_actions = None + +rbx_status_state = None +rbx_status_mode = None +rbx_status_ready = None +rbx_status_goto_errors = None +rbx_status_cmd_success = None + +snapshot_trigger_pub = rospy.Publisher(SNAPSHOT_TRIGGER_TOPIC, Empty, queue_size = 1) + + +##################################################################################### +# Methods +##################################################################################### + +### System Initialization processes +def initialize_actions(): + global rbx_cap_navpose + global rbx_cap_states + global rbx_cap_modes + global rbx_cap_actions + global rbx_status_state + global rbx_status_mode + global rbx_status_ready + global rbx_status_cmd_success + global img_height + global img_width + ########################################## + ### Capabilities Subscribers + # Wait for topic + print("Waiting for topic: " + NEPI_RBX_CAPABILITIES_NAVPOSE_TOPIC) + wait_for_topic(NEPI_RBX_CAPABILITIES_NAVPOSE_TOPIC) + print("Starting capabilities navpose scubscriber callback") + rospy.Subscriber(NEPI_RBX_CAPABILITIES_NAVPOSE_TOPIC, UInt8, rbx_cap_navpose_callback) + while rbx_cap_navpose is None and not rospy.is_shutdown(): + print("Waiting for capabilities navpose to publish") + time.sleep(0.1) + print(rbx_cap_navpose) + # Wait for topic + print("Waiting for topic: " + NEPI_RBX_CAPABILITIES_STATES_TOPIC) + wait_for_topic(NEPI_RBX_CAPABILITIES_STATES_TOPIC) + print("Starting state scubscriber callback") + rospy.Subscriber(NEPI_RBX_CAPABILITIES_STATES_TOPIC, String, rbx_cap_states_callback) + while rbx_cap_states is None and not rospy.is_shutdown(): + print("Waiting for capabilities states to publish") + time.sleep(0.1) + print(rbx_cap_states) + # Wait for topic + print("Waiting for topic: " + NEPI_RBX_CAPABILITIES_MODES_TOPIC) + wait_for_topic(NEPI_RBX_CAPABILITIES_MODES_TOPIC) + print("Starting modes scubscriber callback") + rospy.Subscriber(NEPI_RBX_CAPABILITIES_MODES_TOPIC, String, rbx_cap_modes_callback) + while rbx_cap_modes is None and not rospy.is_shutdown(): + print("Waiting for capabilities modes to publish") + time.sleep(0.1) + print(rbx_cap_modes) + # Wait for topic + print("Waiting for topic: " + NEPI_RBX_CAPABILITIES_ACTIONS_TOPIC) + wait_for_topic(NEPI_RBX_CAPABILITIES_ACTIONS_TOPIC) + print("Starting actions scubscriber callback") + rospy.Subscriber(NEPI_RBX_CAPABILITIES_ACTIONS_TOPIC, String, rbx_cap_actions_callback) + while rbx_cap_actions is None and not rospy.is_shutdown(): + print("Waiting for capabilities actions to publish") + time.sleep(0.1) + print(rbx_cap_actions) + ########################################## + ### Status Subscribers + # Wait for topic + print("Waiting for topic: " + NEPI_RBX_STATUS_STATE_TOPIC) + wait_for_topic(NEPI_RBX_STATUS_STATE_TOPIC) + print("Starting state scubscriber callback") + rospy.Subscriber(NEPI_RBX_STATUS_STATE_TOPIC, Int8, rbx_state_callback) + while rbx_status_state is None and not rospy.is_shutdown(): + print("Waiting for current state to publish") + time.sleep(0.1) + print(rbx_status_state) + # Wait for topic + print("Waiting for topic: " + NEPI_RBX_STATUS_MODE_TOPIC) + wait_for_topic(NEPI_RBX_STATUS_MODE_TOPIC) + print("Starting mode scubscriber callback") + rospy.Subscriber(NEPI_RBX_STATUS_MODE_TOPIC, Int8, rbx_mode_callback) + while rbx_status_mode is None and not rospy.is_shutdown(): + print("Waiting for current mode to publish") + time.sleep(0.1) + print(rbx_status_mode) + # Wait for goto controls status to publish + print("Waiting for topic: " + NEPI_RBX_STATUS_READY_TOPIC) + wait_for_topic(NEPI_RBX_STATUS_READY_TOPIC) + # Start ready status monitor + print("Starting mavros ready status subscriber") + rospy.Subscriber(NEPI_RBX_STATUS_READY_TOPIC, Bool, rbx_status_ready_callback) + while rbx_status_ready is None and not rospy.is_shutdown(): + print("Waiting for ready status to publish") + time.sleep(0.1) + # Start goto errors status monitor + print("Starting mavros goto errors subscriber") + rospy.Subscriber(NEPI_RBX_STATUS_GOTO_ERRORS_TOPIC, Float64MultiArray, rbx_status_goto_errors_callback) + while rbx_status_goto_errors is None and not rospy.is_shutdown(): + print("Waiting for goto errors status to publish") + time.sleep(0.1) + # Start cmd success status monitor + print("Starting mavros cmd success subscriber") + rospy.Subscriber(NEPI_RBX_STATUS_CMD_SUCCESS_TOPIC, Bool, rbx_status_cmd_success_callback) + while rbx_status_cmd_success is None and not rospy.is_shutdown(): + print("Waiting for cmd success status to publish") + time.sleep(0.1) + print("Initialization Complete") + +## Function for custom pre-mission actions +def pre_mission_actions(): + ########################### + # Start Your Custom Actions + ########################### + success = True + # Set Mode to Guided + success = set_rbx_mode("GUIDED") + # Arm System + success = set_rbx_state("ARM") + # Send Takeoff Command + success=go_rbx_action("TAKEOFF") + ########################### + # Stop Your Custom Actions + ########################### + print("Pre-Mission Actions Complete") + return success + +## Function for custom mission actions +def mission_actions(): + ########################### + # Start Your Custom Actions + ########################### + ## Change Vehicle Mode to Guided + success = True + print("Sending snapshot event trigger") + snapshot() + ########################### + # Stop Your Custom Actions + ########################### + print("Mission Actions Complete") + return success + +## Function for custom post-mission actions +def post_mission_actions(): + ########################### + # Start Your Custom Actions + ########################### + success = True + #success = set_rbx_mode("LAND") # Uncomment to change to Land mode + #success = set_rbx_mode("LOITER") # Uncomment to change to Loiter mode + success = set_rbx_mode("RTL") # Uncomment to change to home mode + #success = set_rbx_mode("RESUME") # Uncomment to return to last mode + time.sleep(1) + ########################### + # Stop Your Custom Actions + ########################### + print("Post-Mission Actions Complete") + return success + + +####################### +# RBX Capabilities Callbacks + +def rbx_cap_navpose_callback(cap_navpose_msg): + global rbx_cap_navpose + rbx_cap_navpose = cap_navpose_msg.data + +def rbx_cap_states_callback(cap_states_msg): + global rbx_cap_states + cap_states_str = cap_states_msg.data + rbx_cap_states = eval(cap_states_str) + +def rbx_cap_modes_callback(cap_modes_msg): + global rbx_cap_modes + cap_modes_str = cap_modes_msg.data + rbx_cap_modes = eval(cap_modes_str) + +def rbx_cap_actions_callback(cap_actions_msg): + global rbx_cap_actions + cap_actions_str = cap_actions_msg.data + rbx_cap_actions = eval(cap_actions_str) + +####################### +# RBX Status Callbacks +### Callback to update rbx current state value +def rbx_state_callback(state_msg): + global rbx_status_state + rbx_status_state = state_msg.data + +### Callback to update rbx current mode value +def rbx_mode_callback(mode_msg): + global rbx_status_mode + rbx_status_mode = mode_msg.data + +### Callback to update rbx ready status value +def rbx_status_ready_callback(ready_msg): + global rbx_status_ready + rbx_status_ready = ready_msg.data + +### Callback to update rbx goto errors status value +def rbx_status_goto_errors_callback(goto_errors_msg): + global rbx_status_goto_errors + rbx_status_goto_errors = goto_errors_msg.data + +### Callback to update rbx cmd success status value +def rbx_status_cmd_success_callback(cmd_success_msg): + global rbx_status_cmd_success + rbx_status_cmd_success = cmd_success_msg.data + +####################### +# RBX Settings Functions + +### Function to set rbx state +def set_rbx_state(state_str): + global rbx_status_state + global rbx_set_state_pub + print("*******************************") + print("Set State Request Recieved: " + state_str) + success = False + new_state_ind = -1 + for ind, state in enumerate(rbx_cap_states): + if state == state_str: + new_state_ind = ind + if new_state_ind == -1: + print("No matching state found") + else: + print("Setting state to: " + state_str) + rbx_set_state_pub.publish(new_state_ind) + timeout_timer = 0 + sleep_time_sec = 1 + while rbx_status_state != new_state_ind and timeout_timer < NEPI_RBX_SET_TIMEOUT_SEC and not rospy.is_shutdown(): + print("Waiting for rbx state " + RBX_STATES[new_state_ind] + " to set") + print("Current rbx state is " + RBX_STATES[rbx_status_state]) + time.sleep(sleep_time_sec) + timeout_timer = timeout_timer + sleep_time_sec + if rbx_status_state == new_state_ind: + success = True + print("Current rbx state is " + RBX_STATES[rbx_status_state]) + return success + +### Function to set rbx mode +def set_rbx_mode(mode_str): + global rbx_status_mode + global rbx_set_mode_pub + print("*******************************") + print("Set Mode Request Recieved: " + mode_str) + success = False + new_mode_ind = -1 + for ind, mode in enumerate(rbx_cap_modes): + if mode == mode_str: + new_mode_ind = ind + if new_mode_ind == -1: + print("No matching mode found") + else: + print("Setting mode to: " + mode_str) + rbx_set_mode_pub.publish(new_mode_ind) + timeout_timer = 0 + sleep_time_sec = 1 + while rbx_status_mode != new_mode_ind and timeout_timer < NEPI_RBX_SET_TIMEOUT_SEC and not rospy.is_shutdown(): + print("Waiting for rbx mode " + RBX_MODES[new_mode_ind] + " to set") + print("Current rbx mode is " + RBX_MODES[rbx_status_mode]) + time.sleep(sleep_time_sec) + timeout_timer = timeout_timer + sleep_time_sec + if rbx_status_mode == new_mode_ind: + success = True + print("Current rbx mode is " + RBX_MODES[rbx_status_mode]) + return success + +### Function to set home current +def set_rbx_set_home_current(): + global rbx_set_home_current_pub + print("*******************************") + print("Set Home Current Request Recieved: ") + success = False + rbx_set_home_current_pub.publish(Empty()) + success = True + return success + +####################### +# RBX Control Functions + +### Function to send rbx action control +def go_rbx_action(action_str): + global rbx_status_cmd_success + global rbx_set_action_pub + print("*******************************") + print("Go Action Request Recieved: " + action_str) + success = False + action_ind = -1 + for ind, action in enumerate(rbx_cap_actions): + if action == action_str: + action_ind = ind + if action_ind == -1: + print("No matching action found") + else: + rbx_go_action_pub.publish(action_ind) + wait_for_rbx_status_busy() + wait_for_rbx_status_ready() + success = rbx_status_cmd_success + return success + +### Function to send rbx home control +def go_rbx_home(): + global rbx_status_cmd_success + global rbx_set_home_pub + print("*******************************") + print("Go Home Request Recieved: ") + success = False + rbx_go_home_pub.publish(action_ind) + wait_for_rbx_status_busy() + wait_for_rbx_status_ready() + success = rbx_status_cmd_success + return success + +### Function to call goto Location Global control +def goto_rbx_location(goto_data): + global rbx_status_cmd_success + global rbx_goto_location_pub + # Send goto Location Command + wait_for_rbx_status_ready() + print("Starting goto Location Global Process") + goto_location_msg = create_goto_message(goto_data) + rbx_goto_location_pub.publish(goto_location_msg) + wait_for_rbx_status_busy() + wait_for_rbx_status_ready() + return rbx_status_cmd_success + +### Function to call goto Position Body control +def goto_rbx_position(goto_data): + global rbx_status_cmd_success + global rbx_goto_location_pub + # Send goto Position Command + wait_for_rbx_status_ready() + print("Starting goto Position Body Process") + goto_position_msg = create_goto_message(goto_data) + rbx_goto_position_pub.publish(goto_position_msg) + wait_for_rbx_status_busy() + wait_for_rbx_status_ready() + return rbx_status_cmd_success + +### Function to call goto Attititude NED control +def goto_rbx_pose(goto_data): + global rbx_status_cmd_success + global rbx_goto_pose_pub + # Send goto Attitude Command + wait_for_rbx_status_ready() + print("Starting goto Attitude NED Process") + goto_attitude_msg = create_goto_message(goto_data) + rbx_goto_pose_pub.publish(goto_attitude_msg) + wait_for_rbx_status_busy() + wait_for_rbx_status_ready() + return rbx_status_cmd_success + +### Function to wait for goto control process to complete +def wait_for_rbx_status_ready(): + global rbx_status_ready + global rbx_status_goto_errors + while rbx_status_ready is not True and not rospy.is_shutdown(): + print("Waiting for current cmd process to complete") + print(rbx_status_ready) + print("Current Errors") + print(rbx_status_goto_errors) + time.sleep(1) + +### Function to wait for goto control process to complete +def wait_for_rbx_status_busy(): + global rbx_status_ready + while rbx_status_ready is not False and not rospy.is_shutdown(): + print("Waiting for cmd process to start") + print(rbx_status_ready) + time.sleep(1) + +####################### +# Mission Action Functions + +### Function to send snapshot event trigger and wait for completion +def snapshot(): + global snapshot_trigger_pub + snapshot_trigger_pub.publish(Empty()) + print("Snapshot trigger sent") + +####################### +# Process Functions + +### Function for creating goto messages +def create_goto_message(goto): + print(goto) + goto_msg = Float64MultiArray() + goto_data=[] + for ind in range(len(goto)): + goto_data.append(float(goto[ind])) + print(goto_data) + goto_msg.data = goto_data + print("") + print("goto Message Created") + print(goto_msg) + return goto_msg + + +####################### +# Initialization Functions + +### Function to find a topic +def find_topic(topic_name): + topic = "" + topic_list=rospy.get_published_topics(namespace='/') + for topic_entry in topic_list: + if topic_entry[0].find(topic_name) != -1: + topic = topic_entry[0] + return topic + +### Function to check for a topic +def check_for_topic(topic_name): + topic_exists = True + topic=find_topic(topic_name) + if topic == "": + topic_exists = False + return topic_exists + +### Function to wait for a topic +def wait_for_topic(topic_name): + topic = "" + while topic == "" and not rospy.is_shutdown(): + topic=find_topic(topic_name) + time.sleep(.1) + return topic + +####################### +# StartNode and Cleanup Functions + +### Cleanup processes on node shutdown +def cleanup_actions(): + print("Shutting down: Executing script cleanup actions") + + +### Script Entrypoint +def startNode(): + rospy.loginfo("Starting Drone Waypoint Inspection Mission Script") + rospy.init_node("drone_waypoint_inspection_mission_script") + #initialize system including pan scan process + initialize_actions() + ######################################### + # Run Pre-Mission Custom Actions + print("Starting Pre-goto Actions") + success = pre_mission_actions() + ######################################### + # Start Mission + ######################################### + # Send goto Location Command + print("Starting goto Location Global Process") + success = goto_rbx_location(GOTO_LOCATION) + ######################################### + # End Mission + ######################################### + # Run Post-Mission Actions + print("Starting Post-Goto Actions") + success = post_mission_actions() + ######################################### + # Mission Complete, Shutting Down + rospy.signal_shutdown("Mission Complete, Shutting Down") + ######################################### + # Run cleanup actions on rospy shutdown + rospy.on_shutdown(cleanup_actions) + # Spin forever + rospy.spin() + + +######################################### +# Main +######################################### + +if __name__ == '__main__': + startNode() + diff --git a/drone_waypoint_inspection_mission_script.py b/drone_waypoint_inspection_mission_script.py index bf2ecaa..59e7086 100644 --- a/drone_waypoint_inspection_mission_script.py +++ b/drone_waypoint_inspection_mission_script.py @@ -25,8 +25,7 @@ # -# Sample NEPI Automation Script. -# Uses onboard ROS python and mavros libraries to +# Sample NEPI Mission Script. # 1) Subscribes to NEPI nav_pose_current heading, orientation, position, location topics # 2) Runs pre-mission processes # 3) Runs mission goto command processes @@ -56,22 +55,27 @@ from mavros_msgs.msg import State, AttitudeTarget from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest, CommandTOL, CommandHome -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## +################################################### +# RBX State and Mode Dictionaries +RBX_STATES = ["DISARM","ARM"] +RBX_MODES = ["STABILIZE","LAND","RTL","LOITER","GUIDED","RESUME"] +RBX_ACTIONS = ["TAKEOFF"] -# goto Position Global Settings -################################################### +######################################### +# USER SETTINGS - Edit as Necessary +######################################### + +# GoTo Position Global Settings +################### # goto_location is [LAT, LONG, ALT_WGS84, YAW_NED_DEGREES] # Altitude is specified as meters above the WGS-84 and converted to AMSL before sending # Yaw is specified in NED frame degrees 0-360 or +-180 -##################################################### GOTO_LOCATION = [47.6541208,-122.3186620, 10, -999] # [Lat, Long, Alt WGS84, Yaw NED Frame], Enter -999 to use current value GOTO_LOCATION_CORNERS = [[47.65412620,-122.31881480, -999, -999],[47.65402050,-122.31875320, -999, -999],[47.65391570,-122.31883630, -999, -999],[47.65397990,-122.31912330, -999, -999]] -# goto Position Local Body Settings -################################################### +# GoTo Position Local Body Settings +################### # goto_position is [X_BODY_METERS, Y_BODY_METERS, Z_BODY_METERS, YEW_BODY_DEGREES] # Local Body Position goto Function use these body relative x,y,z,yaw conventions # x+ axis is forward @@ -79,34 +83,22 @@ # z+ axis is down # Only yaw orientation updated # yaw+ clockwise, yaw- counter clockwise from x axis (0 degrees faces x+ and rotates positive using right hand rule around z+ axis down) -##################################################### GOTO_POSITION = [10,5,0,0] # [X, Y, Z, YAW] Offset in xyz meters yaw body +- 180 (+Z is Down). Use 0 value for no change -# goto Attitude NED Settings -################################################### +# GoTo Attitude NED Settings +################### # goto_attitudeInp is [ROLL_NED_DEG, PITCH_NED_DEG, YEW_NED_DEGREES] -################################################### GOTO_POSE = [-999,30,-999] # Roll, Pitch, Yaw Degrees: Enter -999 to use current value - -################################################### -# RBX State and Mode Dictionaries -RBX_STATES = ["DISARM","ARM"] -RBX_MODES = ["STABILIZE","LAND","RTL","LOITER","GUIDED","RESUME"] -RBX_ACTIONS = ["TAKEOFF"] +######################################### +# ROS NAMESPACE SETUP +######################################### # ROS namespace setup NEPI_BASE_NAMESPACE = "/nepi/s2x/" NEPI_NAVPOSE_SERVICE_NAME = NEPI_BASE_NAMESPACE + "nav_pose_query" NEPI_RBX_NAMESPACE = NEPI_BASE_NAMESPACE + "ardupilot/rbx/" - -################################################### -# RBX State and Mode Dictionaries -RBX_STATES = ["DISARM","ARM"] -RBX_MODES = ["STABILIZE","LAND","RTL","LOITER","GUIDED","RESUME"] -RBX_ACTIONS = ["TAKEOFF"] - # NEPI MAVLINK RBX Driver Capabilities Publish Topics NEPI_RBX_CAPABILITIES_NAVPOSE_TOPIC = NEPI_RBX_NAMESPACE + "navpose_support" NEPI_RBX_CAPABILITIES_STATES_TOPIC = NEPI_RBX_NAMESPACE + "state_options" @@ -134,8 +126,6 @@ NEPI_RBX_GOTO_POSITION_TOPIC = NEPI_RBX_NAMESPACE + "goto_position" # Ignored if any active goto processes NEPI_RBX_GOTO_LOCATION_TOPIC = NEPI_RBX_NAMESPACE + "goto_location" # Ignored if any active goto processes - - # AI Detector Subscriber Topics AI_BOUNDING_BOXES_TOPIC = NEPI_BASE_NAMESPACE + "classifier/bounding_boxes" AI_DETECTION_IMAGE_TOPIC = NEPI_BASE_NAMESPACE + "classifier/detection_image" @@ -143,9 +133,10 @@ # Mission Action Topics SNAPSHOT_TRIGGER_TOPIC = NEPI_BASE_NAMESPACE + "snapshot_event" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + rbx_set_state_pub = rospy.Publisher(NEPI_RBX_SET_STATE_TOPIC, UInt8, queue_size=1) rbx_set_mode_pub = rospy.Publisher(NEPI_RBX_SET_MODE_TOPIC, UInt8, queue_size=1) rbx_set_home_current_pub = rospy.Publisher(NEPI_RBX_SET_HOME_CURRENT_TOPIC, Empty, queue_size=1) @@ -171,9 +162,9 @@ snapshot_trigger_pub = rospy.Publisher(SNAPSHOT_TRIGGER_TOPIC, Empty, queue_size = 1) -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -647,9 +638,9 @@ def startNode(): rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/navpose_get_and_publish_process_script.py b/navpose_get_and_publish_process_script.py index 73abe77..7076ec7 100644 --- a/navpose_get_and_publish_process_script.py +++ b/navpose_get_and_publish_process_script.py @@ -24,10 +24,9 @@ # # -# Sample NEPI Automation Script. -# Uses onboard ROS python libraries to -# 1. call the NEPI ROS's nav_pose_mgr/query_data_products service -# 2. gets and publishes current navpose solution data at set rate to new topics: +# Sample NEPI Process Script. +# 1. Calls the NEPI ROS's nav_pose_mgr/query_data_products service +# 2. Gets and publishes current navpose solution data at set rate to new topics: # navpose = NEPI NavPose Message # heading_deg = Float64 (heading in degrees) # orientation_ned_degs = [Float64, Float64, Float64] (roll, pitch, yaw in +-180 degrees NED frame) @@ -86,12 +85,15 @@ def ginterpolator(single_position): from nepi_ros_interfaces.msg import NavPose from nepi_ros_interfaces.srv import NavPoseQuery, NavPoseQueryRequest -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## +######################################### +# USER SETTINGS - Edit as Necessary +######################################### NAVPOSE_PUB_RATE_HZ = 10 +######################################### +# ROS NAMESPACE SETUP +######################################### # ROS namespace setup NEPI_BASE_NAMESPACE = "/nepi/s2x/" @@ -110,10 +112,10 @@ def ginterpolator(single_position): NAVPOSE_PUBLISH_LOCATION_WGS84_TOPIC = NEPI_BASE_NAMESPACE + "nav_pose/location_wgs84_geo" NAVPOSE_PUBLISH_GEOID_HEIGHT_TOPIC = NEPI_BASE_NAMESPACE + "nav_pose/geoid_height_m" - -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + navpose_navpose_pub = rospy.Publisher(NAVPOSE_PUBLISH_NAVPOSE_TOPIC, NavPose, queue_size=1) navpose_heading_pub = rospy.Publisher(NAVPOSE_PUBLISH_HEADING_TOPIC, Float64, queue_size=1) navpose_orientation_ned_pub = rospy.Publisher(NAVPOSE_PUBLISH_ORIENTATION_NED_TOPIC, Float64MultiArray , queue_size=1) @@ -126,9 +128,9 @@ def ginterpolator(single_position): navpose_pub_interval_sec = float(1.0)/NAVPOSE_PUB_RATE_HZ -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -278,9 +280,9 @@ def startNode(): rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/navpose_set_fixed_config_script.py b/navpose_set_fixed_config_script.py index 4fd83c3..fa55246 100644 --- a/navpose_set_fixed_config_script.py +++ b/navpose_set_fixed_config_script.py @@ -25,13 +25,12 @@ # -# Sample NEPI Automation Script. +# Sample NEPI Config Script. # If your NEPI system does not have an attached GPS/IMU/Compass or other # NavPose source, this script can be set to run at startup setting fixed # NavPose values on your system. -# Uses onboard ROS python library to -# 1. Set a fixed NavPose Solution (Lat,Long,Alt,Heading,Roll,Pitch,Yaw) -# 2. Exit after setting +# 1. Sets a fixed NavPose Solution (Lat,Long,Alt,Heading,Roll,Pitch,Yaw) + import rospy import math @@ -42,16 +41,20 @@ from sensor_msgs.msg import NavSatFix from geometry_msgs.msg import Quaternion, QuaternionStamped -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## +######################################### +# USER SETTINGS - Edit as Necessary +######################################### # Set Start Fixed NavPose Values #Numurus Office START_GEOPOINT = [47.6540828,-122.3187578,0.0] # [Lat, Long, Altitude_AMSL_M] START_HEADING_DEG = 88.0 # Global True North, or 0 for Body Relative START_ORIENTATION_DEGS = [10.0,20.0,30.0] -# ROS namespace setup + +######################################### +# ROS NAMESPACE SETUP +######################################### + NEPI_BASE_NAMESPACE = "/nepi/s2x/" ###!!!!!!!! Set NavPose initialization values !!!!!!!! @@ -61,9 +64,14 @@ REINIT_NAVPOSE_SOLUTION_TOPIC = NEPI_BASE_NAMESPACE + "nav_pose_mgr/reinit_solution" -##################################################################################### +######################################### +# Globals +######################################### + + +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes @@ -138,9 +146,9 @@ def startNode(): # Spin forever rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/opencv_image_contours_process_script.py b/opencv_image_contours_process_script.py index 3bc2391..748b39e 100644 --- a/opencv_image_contours_process_script.py +++ b/opencv_image_contours_process_script.py @@ -25,8 +25,7 @@ # -# Sample NEPI Automation Script. -# Uses onboard ROS python library to +# Sample NEPI Process Script. # 1. Add contrours and text overlay to image and republish to a new topic # 2. Run until Stopped @@ -41,26 +40,31 @@ from cv_bridge import CvBridge from std_msgs.msg import UInt8, Empty, String, Bool -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## +######################################### +# USER SETTINGS - Edit as Necessary +######################################### -## Set Image ROS Topic Name to Use +## Set ROS Image Topic Name to Use IMAGE_INPUT_TOPIC_NAME = "color_2d_image" +######################################### +# ROS NAMESPACE SETUP +######################################### + # ROS namespace setup NEPI_BASE_NAMESPACE = "/nepi/s2x/" IMAGE_OUTPUT_TOPIC = NEPI_BASE_NAMESPACE + "image_contours" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + contour_image_pub = rospy.Publisher(IMAGE_OUTPUT_TOPIC, Image, queue_size=10) -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -162,9 +166,9 @@ def startNode(): rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/opencv_image_enhance_process_script.py b/opencv_image_enhance_process_script.py index 8e20172..0f752dc 100644 --- a/opencv_image_enhance_process_script.py +++ b/opencv_image_enhance_process_script.py @@ -25,8 +25,7 @@ # -# Sample NEPI Automation Script. -# Uses onboard ROS python library to +# Sample NEPI Process Script. # 1. run sensor imagery through an image enhancement algorithm and republish to a new topic # 2. Run until Stopped @@ -40,28 +39,32 @@ from cv_bridge import CvBridge from std_msgs.msg import UInt8, Empty, String, Bool -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## +######################################### +# USER SETTINGS - Edit as Necessary +######################################### -## Set Image ROS Topic Name to Use +## Set ROS Image Topic Name to Use IMAGE_INPUT_TOPIC_NAME = "color_2d_image" - ENHANCE_SENSITIVITY_RATIO = 0.5 +######################################### +# ROS NAMESPACE SETUP +######################################### + # ROS namespace setup NEPI_BASE_NAMESPACE = "/nepi/s2x/" IMAGE_OUTPUT_TOPIC = NEPI_BASE_NAMESPACE + "image_enhanced" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + enhanced_image_pub = rospy.Publisher(IMAGE_OUTPUT_TOPIC, Image, queue_size=10) -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -191,9 +194,9 @@ def startNode(): # Spin forever (until object is detected) rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/opencv_image_your_custom_process_script.py b/opencv_image_your_custom_process_script.py index 261c947..2a16b46 100644 --- a/opencv_image_your_custom_process_script.py +++ b/opencv_image_your_custom_process_script.py @@ -24,8 +24,7 @@ # # -# Sample NEPI Automation Script. -# Uses onboard ROS python library to +# Sample NEPI Process Script. # 1. Converts ROS image to OpenCV image # 2. Blank area for custom code # 2. Converts OpenCV image back to ROS image @@ -41,26 +40,31 @@ from cv_bridge import CvBridge from std_msgs.msg import UInt8, Empty, String, Bool -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## +######################################### +# USER SETTINGS - Edit as Necessary +######################################### -## Set Image ROS Topic Name to Use +## Set ROS Image Topic Name to Use IMAGE_INPUT_TOPIC_NAME = "color_2d_image" +######################################### +# ROS NAMESPACE SETUP +######################################### + # ROS namespace setup NEPI_BASE_NAMESPACE = "/nepi/s2x/" IMAGE_OUTPUT_TOPIC = NEPI_BASE_NAMESPACE + "image_custom" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + custom_image_pub = rospy.Publisher(IMAGE_OUTPUT_TOPIC, Image, queue_size=10) -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -149,9 +153,9 @@ def startNode(): rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/pantilt_object_tracker_action_script.py b/pantilt_object_tracker_action_script.py index aff2560..f1988d2 100644 --- a/pantilt_object_tracker_action_script.py +++ b/pantilt_object_tracker_action_script.py @@ -24,13 +24,12 @@ # # -# Sample NEPI Automation Script. -# Uses onboard ROS python library to + Sample NEPI Action Script. ### Expects Classifier to be running ### -# 1. Configure and start pan and tilt -# 2. Set and start pan and tilt search scan -# 3. Wait for specific objects to be detected and starts tracking largest detection box -# 4. Return to search/scan mode if no detected objects are being detected +# 1. Configures and starts pan and tilt +# 2. Sets and start pan and tilt search scan +# 3. Waits for specific objects to be detected and starts tracking largest detection box +# 4. Returns to search/scan mode if no detected objects are being detected # Requires the following additional scripts are running # a)ai_detector_config_script.py @@ -51,9 +50,9 @@ from darknet_ros_msgs.msg import BoundingBoxes, ObjectCount -############################################################################ -# SETUP - User Settings -############################################################################ +######################################### +# USER SETTINGS - Edit as Necessary +######################################### # Minimum detection box area as a ration of image size MIN_DETECT_BOX_AREA_RATIO = 0.15 # Filters background targets. @@ -73,10 +72,10 @@ PTX_OBJECT_TILT_OFFSET_RATIO = 0.7 # Adjust tilt tracking ratio location within box PTX_OBJ_CENTERED_BUFFER_RATIO = 0.3 # Hysteresis band about center of image for tracking purposes -############################################################################ -# SETUP - Env Settings -############################################################################ -# ROS namespace setup +######################################### +# ROS NAMESPACE SETUP +######################################### + NEPI_BASE_NAMESPACE = "/nepi/s2x/" PTX_NAMESPACE = NEPI_BASE_NAMESPACE + "iqr_pan_tilt/ptx/" @@ -96,9 +95,9 @@ AI_DETECTION_IMAGE_TOPIC = NEPI_BASE_NAMESPACE + "classifier/detection_image" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### send_pt_home_pub = rospy.Publisher(PTX_GOHOME_TOPIC, Empty, queue_size=10) set_pt_speed_ratio_pub = rospy.Publisher(PTX_SET_SPEED_RATIO_TOPIC, Float32, queue_size=10) @@ -128,9 +127,9 @@ pt_speed_now_ratio=0 -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -378,9 +377,10 @@ def startNode(): rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### + if __name__ == '__main__': startNode() #initialize system diff --git a/pantilt_ptx_navpose_config_script.py b/pantilt_ptx_navpose_config_script.py index 4acb7a1..92d5445 100644 --- a/pantilt_ptx_navpose_config_script.py +++ b/pantilt_ptx_navpose_config_script.py @@ -24,9 +24,8 @@ # # -# Sample NEPI Automation Script. -# Uses onboard ROS libraries to -# 1. Connect NEPI NavPose topics to appropriate navpose topics +# Sample NEPI Config Script. +# 1. Connects NEPI NavPose topics to appropriate navpose topics import rospy @@ -39,10 +38,15 @@ from nepi_ros_interfaces.srv import NavPoseQuery, NavPoseQueryRequest -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## -# ROS namespace setup +######################################### +# USER SETTINGS - Edit as Necessary +######################################### + + +######################################### +# ROS NAMESPACE SETUP +######################################### + NEPI_BASE_NAMESPACE = "/nepi/s2x/" PTX_NAMESPACE = NEPI_BASE_NAMESPACE + "iqr_pan_tilt/ptx/" @@ -52,16 +56,17 @@ ### Setup NEPI NavPose Settings Topic Namespaces NEPI_SET_NAVPOSE_ORIENTATION_TOPIC = NEPI_BASE_NAMESPACE + "nav_pose_mgr/set_orientation_topic" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + mavros_global_msg=None mavros_heading_msg=None mavros_orientation_msg=None -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -130,10 +135,9 @@ def startNode(): rospy.spin() - -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/pantilt_ptx_navpose_driver_script.py b/pantilt_ptx_navpose_driver_script.py index 972ad82..3d5aa2a 100644 --- a/pantilt_ptx_navpose_driver_script.py +++ b/pantilt_ptx_navpose_driver_script.py @@ -25,7 +25,8 @@ # -#Concept PTX NavPose Driver Interface for IQR PanTilt +# Sample NEPI Driver Script. +# NEPI PTX NavPose Driver Interface for IQR PanTilt Systems ################################################### # NEPI NavPose Axis Info @@ -50,10 +51,9 @@ from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3, PoseStamped, QuaternionStamped from nepi_ros_interfaces.msg import PanTiltStatus, StringArray -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## - +######################################### +# DRIVER SETTINGS +######################################### NAVPOSE_UPDATE_RATE_HZ = 10 # Pan and Tilt setup parameters @@ -62,12 +62,14 @@ # Set Start roll pitch yaw body frame values START_RPY_DEGS = [0.0,0.0,0.0]# Roll, Pitch, Yaw in body frame degs +######################################### +# ROS NAMESPACE SETUP +######################################### -### ROS namespace setup NEPI_BASE_NAMESPACE = "/nepi/s2x/" + NEPI_PTX_NAME = "iqr_pan_tilt" NEPI_PTX_NAMESPACE = NEPI_BASE_NAMESPACE + NEPI_PTX_NAME + "/ptx/" - ### PanTilt Subscribe Topics NEPI_PTX_GET_STATUS_TOPIC = NEPI_PTX_NAMESPACE + "status" ### PanTilt NavPose Publish Topic @@ -76,9 +78,10 @@ NEPI_SET_NAVPOSE_SET_ORIENTATION_TOPIC = NEPI_BASE_NAMESPACE + "nav_pose_mgr/set_orientation_topic" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + navpose_pt_orientation_pub = rospy.Publisher(NEPI_PTX_NAVPOSE_ODOM_TOPIC, Odometry , queue_size=1) navpose_update_interval_sec = float(1.0)/NAVPOSE_UPDATE_RATE_HZ current_rpy_pt_degs = START_RPY_DEGS @@ -88,10 +91,10 @@ pt_yaw_now_ratio=0 pt_pitch_now_ratio=0 pt_speed_now_ratio=0 -##################################################################################### -# Methods -##################################################################################### +######################################### +# Methods +######################################### ### System Initialization processes def initialize_actions(): @@ -214,9 +217,9 @@ def startNode(): # Spin forever rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/snapshot_event_save_to_disk_action_script.py b/snapshot_event_save_to_disk_action_script.py index 2ab486a..4f35be3 100644 --- a/snapshot_event_save_to_disk_action_script.py +++ b/snapshot_event_save_to_disk_action_script.py @@ -24,10 +24,9 @@ # # -# Sample NEPI Automation Script. -# Uses onboard ROS python library to +# Sample NEPI Action Script. # 1. Waits for snapshot_event topic message -# 2. Start saving image data to onboard storage +# 2. Starts saving image data to onboard storage # 3. Delays a specified amount of time, then stops saving # 4. Delays next trigger event action for some set delay time @@ -49,9 +48,10 @@ from nepi_ros_interfaces.srv import NavPoseQuery, NavPoseQueryRequest -########################################################################### -# SETUP - Edit as Necessary -########################################################################### +######################################### +# USER SETTINGS - Edit as Necessary +######################################### + SAVE_NAVPOSE_DATA_ENABLED = True # True-Save NavPose File with each image @@ -66,7 +66,10 @@ SAVE_FILE_PREFIX = "snapshot_event" # Use "" to ignore SAVE_IMAGE_TYPE = "jpg" -# NEPI ROS namespace setup +######################################### +# ROS NAMESPACE SETUP +######################################### + NEPI_BASE_NAMESPACE = "/nepi/s2x/" ### Snapshot Topic Name SNAPSHOT_TOPIC = NEPI_BASE_NAMESPACE + "snapshot_event" @@ -74,18 +77,19 @@ NAVPOSE_SERVICE_NAME = NEPI_BASE_NAMESPACE + "nav_pose_query" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + save_folder = "/mnt/nepi_storage/data/" + SAVE_FOLDER_NAME save_data_min_interval_s = float(1.0)/SAVE_DATA_MAX_RATE_HZ save_data_enable = False last_save_time_s = None image_topic_to_save = None -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -237,9 +241,9 @@ def startNode(): rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/snapshot_event_send_to_cloud_action_script.py b/snapshot_event_send_to_cloud_action_script.py index e93de29..6f489ff 100644 --- a/snapshot_event_send_to_cloud_action_script.py +++ b/snapshot_event_send_to_cloud_action_script.py @@ -25,7 +25,7 @@ # -# Sample NEPI Automation Script. + Sample NEPI Action Script. # Uses onboard ROS python library to # 1. Confirms IDX driver supported camera topic is publishing # 2. Waits for snapshot_event topic message @@ -40,15 +40,20 @@ from nepi_ros_interfaces.msg import IDXStatus, SaveData, SaveDataRate, StringArray -############################################################# -# SETUP - Edit as Necessary -############################################################# +######################################### +# USER SETTINGS - Edit as Necessary +######################################### #Configure Your Connect Topics to Send in NEPI RUI ###!!!!!!!! Set Automation action parameters !!!!!!!! SEND_RESET_DELAY_S = 30.0 # Seconds. Delay before starting over + +######################################### +# ROS NAMESPACE SETUP +######################################### + NEPI_BASE_NAMESPACE = "/nepi/s2x/" # NEPI Link data topics and parameters @@ -61,17 +66,18 @@ ### Snapshot Topic Name SNAPSHOT_TOPIC = NEPI_BASE_NAMESPACE + "snapshot_event" -############################################################# +######################################### # Globals -############################################################# +######################################### + nepi_link_enable_pub = rospy.Publisher(NEPI_LINK_ENABLE_TOPIC, Bool, queue_size=10) nepi_link_set_data_sources = rospy.Publisher(NEPI_LINK_SET_DATA_SOURCES_TOPIC, StringArray, queue_size=10) nepi_link_collect_data_pub = rospy.Publisher(NEPI_LINK_COLLECT_DATA_TOPIC, Empty, queue_size=10) nepi_link_connect_now_pub = rospy.Publisher(NEPI_LINK_CONNECT_TOPIC, Empty, queue_size=10) -############################################################# +######################################### # Methods -############################################################# +######################################### ### System Initialization processes def initialize_actions(): @@ -116,9 +122,9 @@ def startNode(): rospy.spin() -############################################################# +######################################### # Main -############################################################# +######################################### if __name__ == '__main__': startNode() diff --git a/snapshot_event_your_custom_action_script.py b/snapshot_event_your_custom_action_script.py index bdc294b..c55aae4 100644 --- a/snapshot_event_your_custom_action_script.py +++ b/snapshot_event_your_custom_action_script.py @@ -24,7 +24,7 @@ # # -# Sample NEPI Automation Script. + Sample NEPI Action Script. # Uses onboard ROS python library to # 1. Waits for snapshot event trigger # 2. Runs your custom snapshot event actions @@ -39,26 +39,28 @@ from std_msgs.msg import UInt8, Empty, String, Bool -########################################################################### -# SETUP - Edit as Necessary -########################################################################### +######################################### +# USER SETTINGS - Edit as Necessary +######################################### ###!!!!!!!! Set Automation action parameters !!!!!!!! TIGGER_RESET_DELAY_S = 5.0 # Seconds. Delay before starting over search/save process -# NEPI ROS namespace setup +######################################### +# ROS NAMESPACE SETUP +######################################### + NEPI_BASE_NAMESPACE = "/nepi/s2x/" ### Snapshot Topic Name SNAPSHOT_TOPIC = NEPI_BASE_NAMESPACE + "snapshot_event" -##################################################################################### +######################################### # Globals -##################################################################################### - +######################################### -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -104,9 +106,9 @@ def startNode(): rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/zed2_idx_driver_script.py b/zed2_idx_driver_script.py index 51ff192..5166dd2 100644 --- a/zed2_idx_driver_script.py +++ b/zed2_idx_driver_script.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +/usr/bin/env python # # NEPI Dual-Use License # Project: nepi_sample_auto_scripts @@ -24,8 +24,8 @@ # # - -# Concept NEPI IDX Driver Script for ZED2 Camera. +# Sample NEPI Driver Script. +# NEPI IDX Driver Script for ZED2 Camera Systems ################################################### # NEPI NavPose Axis Info @@ -65,9 +65,9 @@ -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## +######################################### +# DRIVER SETTINGS +######################################### #Define Sensor Native Parameters SENSOR_RES_OPTION_LIST = [0,1,2,3] # Maps to IDX Res Options 0-3 @@ -87,8 +87,13 @@ IDX_MIN_RANGE_RATIO=0.02 IDX_MAX_RANGE_RATIO=0.15 -# ROS namespace setup + +######################################### +# ROS NAMESPACE SETUP +######################################### + NEPI_BASE_NAMESPACE = "/nepi/s2x/" + NEPI_IDX_SENSOR_NAME = "zed2_stereo_camera" NEPI_IDX_SENSOR_NAMESPACE = NEPI_BASE_NAMESPACE + NEPI_IDX_SENSOR_NAME NEPI_IDX_NAMESPACE = NEPI_IDX_SENSOR_NAMESPACE + "/idx/" @@ -96,7 +101,6 @@ ZED_BASE_NAMESPACE = NEPI_BASE_NAMESPACE + "zed2/zed_node/" # Zed control topics ZED_PARAMETER_UPDATES_TOPIC = ZED_BASE_NAMESPACE + "parameter_updates" - # Zed data stream topics ZED_COLOR_2D_IMAGE_TOPIC = ZED_BASE_NAMESPACE + "left/image_rect_color" ZED_BW_2D_IMAGE_TOPIC = ZED_BASE_NAMESPACE + "left/image_rect_gray" @@ -106,13 +110,10 @@ ### NEPI IDX NavPose Publish Topic NEPI_IDX_NAVPOSE_ODOM_TOPIC = NEPI_IDX_NAMESPACE + "odom" - - # NEPI IDX capabilities query service NEPI_IDX_CAPABILITY_REPORT_SERVICE = NEPI_IDX_NAMESPACE + "capabilities_query" NEPI_IDX_CAPABILITY_NAVPOSE_TOPIC = NEPI_IDX_NAMESPACE + "navpose_support" NEPI_IDX_CAPABILITY_NAVPOSE = 2 # Bit Mask [GPS,ODOM,HEADING] - # NEPI IDX status and control topics NEPI_IDX_STATUS_TOPIC = NEPI_IDX_NAMESPACE + "status" NEPI_IDX_SET_BRIGHTNESS_TOPIC = NEPI_IDX_NAMESPACE + "set_brightness" @@ -121,7 +122,6 @@ NEPI_IDX_SET_RESOLUTION_MODE_TOPIC = NEPI_IDX_NAMESPACE + "set_resolution_mode" NEPI_IDX_SET_THRESHOLDING_TOPIC = NEPI_IDX_NAMESPACE + "set_thresholding" NEPI_IDX_SET_RANGE_WINDOW_TOPIC = NEPI_IDX_NAMESPACE + "set_range_window" - # NEPI IDX data stream topics NEPI_IDX_COLOR_2D_IMAGE_TOPIC = NEPI_IDX_NAMESPACE + "color_2d_image" NEPI_IDX_BW_2D_IMAGE_TOPIC = NEPI_IDX_NAMESPACE + "bw_2d_image" @@ -129,7 +129,6 @@ NEPI_IDX_DEPTH_IMAGE_TOPIC = NEPI_IDX_NAMESPACE + "depth_image" NEPI_IDX_POINTCLOUD_TOPIC = NEPI_IDX_NAMESPACE + "pointcloud" NEPI_IDX_POINTCLOUD_IMAGE_TOPIC = NEPI_IDX_NAMESPACE + "pointcloud_image" - # NEPI IDX save data subscriber topics SAVE_FOLDER = "/mnt/nepi_storage/data/" NEPI_IDX_SAVE_DATA_TOPIC = NEPI_BASE_NAMESPACE + "save_data" @@ -137,9 +136,9 @@ NEPI_IDX_SAVE_DATA_RATE_TOPIC = NEPI_BASE_NAMESPACE + "save_data_rate" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### idx_capability_navpose_pub = rospy.Publisher(NEPI_IDX_CAPABILITY_NAVPOSE_TOPIC, UInt8, queue_size=1) idx_navpose_odom_pub = rospy.Publisher(NEPI_IDX_NAVPOSE_ODOM_TOPIC, Odometry, queue_size=1) @@ -153,8 +152,6 @@ idx_pointcloud_pub = rospy.Publisher(NEPI_IDX_POINTCLOUD_TOPIC, PointCloud2, queue_size=1) idx_pointcloud_image_pub = rospy.Publisher(NEPI_IDX_POINTCLOUD_IMAGE_TOPIC, Image, queue_size=1) - - #zed_parameter_update_pub = rospy.Publisher(ZED_PARAMETER_UPDATES_TOPIC, Config, queue_size=1) zed_dynamic_reconfig_client = dynamic_reconfigure.client.Client(ZED_BASE_NAMESPACE, timeout=30) @@ -173,9 +170,9 @@ depth_image_msg = None pointcloud_msg = None -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -554,9 +551,9 @@ def startNode(): rospy.spin() -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode() diff --git a/zed2_idx_navpose_config_script.py b/zed2_idx_navpose_config_script.py index f52d9f8..0a74e65 100644 --- a/zed2_idx_navpose_config_script.py +++ b/zed2_idx_navpose_config_script.py @@ -24,9 +24,9 @@ # # -# Sample NEPI Automation Script. -# Uses onboard ROS libraries to -# 1. Connect NEPI NavPose topics to appropriate navpose topics + +# Sample NEPI Config Script. +# 1. Connects NEPI NavPose topics to appropriate navpose topics import rospy @@ -39,10 +39,15 @@ from nepi_ros_interfaces.srv import NavPoseQuery, NavPoseQueryRequest -##################################################################################### -# SETUP - Edit as Necessary ################################## -########################################## -# ROS namespace setup +######################################### +# USER SETTINGS - Edit as Necessary +######################################### + + +######################################### +# ROS NAMESPACE SETUP +######################################### + NEPI_BASE_NAMESPACE = "/nepi/s2x/" NEPI_IDX_NAMESPACE = NEPI_BASE_NAMESPACE + "zed2_stereo_camera/idx/" @@ -52,16 +57,17 @@ ### Setup NEPI NavPose Settings Topic Namespaces NEPI_SET_NAVPOSE_ORIENTATION_TOPIC = NEPI_BASE_NAMESPACE + "nav_pose_mgr/set_orientation_topic" -##################################################################################### +######################################### # Globals -##################################################################################### +######################################### + mavros_global_msg=None mavros_heading_msg=None mavros_orientation_msg=None -##################################################################################### +######################################### # Methods -##################################################################################### +######################################### ### System Initialization processes def initialize_actions(): @@ -133,9 +139,9 @@ def startNode(): -##################################################################################### +######################################### # Main -##################################################################################### +######################################### if __name__ == '__main__': startNode()