From 67f9c50f7df027a9fc7e0ee44c15990c646c3840 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Thu, 22 Jun 2023 14:45:17 -0700 Subject: [PATCH 01/44] making spawn changes and adding double handed cargo --- .../isaac_gazebo/launch/spawn_object.launch | 25 +- .../launch/start_simulation.launch | 5 +- ...{cargo_bag.urdf.xacro => cargo.urdf.xacro} | 4 +- description/urdf/cargo_double.urdf.xacro | 62 +++ .../{socks.urdf.xacro => sock.urdf.xacro} | 0 isaac/launch/controller/sim_start.launch | 2 + isaac/launch/sim.launch | 4 + isaac/resources/rviz/iss.rviz | 402 +++++++++--------- scripts/setup/install_desktop_packages.sh | 40 +- 9 files changed, 308 insertions(+), 236 deletions(-) rename description/urdf/{cargo_bag.urdf.xacro => cargo.urdf.xacro} (97%) create mode 100755 description/urdf/cargo_double.urdf.xacro rename description/urdf/{socks.urdf.xacro => sock.urdf.xacro} (100%) diff --git a/astrobee/simulation/isaac_gazebo/launch/spawn_object.launch b/astrobee/simulation/isaac_gazebo/launch/spawn_object.launch index d487526a..bfe7a520 100644 --- a/astrobee/simulation/isaac_gazebo/launch/spawn_object.launch +++ b/astrobee/simulation/isaac_gazebo/launch/spawn_object.launch @@ -21,26 +21,11 @@ - - - - - - - - - - - - - - + + + + + diff --git a/astrobee/simulation/isaac_gazebo/launch/start_simulation.launch b/astrobee/simulation/isaac_gazebo/launch/start_simulation.launch index 81070506..3192d523 100644 --- a/astrobee/simulation/isaac_gazebo/launch/start_simulation.launch +++ b/astrobee/simulation/isaac_gazebo/launch/start_simulation.launch @@ -22,6 +22,7 @@ + @@ -31,12 +32,12 @@ + args="-e $(arg physics) $(arg world)" /> + args="-e $(arg physics) $(arg world)" /> diff --git a/description/urdf/cargo_bag.urdf.xacro b/description/urdf/cargo.urdf.xacro similarity index 97% rename from description/urdf/cargo_bag.urdf.xacro rename to description/urdf/cargo.urdf.xacro index e5e9225d..27a8def8 100755 --- a/description/urdf/cargo_bag.urdf.xacro +++ b/description/urdf/cargo.urdf.xacro @@ -2,12 +2,12 @@ - + diff --git a/description/urdf/cargo_double.urdf.xacro b/description/urdf/cargo_double.urdf.xacro new file mode 100755 index 00000000..64f3b41e --- /dev/null +++ b/description/urdf/cargo_double.urdf.xacro @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 62.5 + world + body + true + false + false + false + + + diff --git a/description/urdf/socks.urdf.xacro b/description/urdf/sock.urdf.xacro similarity index 100% rename from description/urdf/socks.urdf.xacro rename to description/urdf/sock.urdf.xacro diff --git a/isaac/launch/controller/sim_start.launch b/isaac/launch/controller/sim_start.launch index be4c26a5..3317f759 100644 --- a/isaac/launch/controller/sim_start.launch +++ b/isaac/launch/controller/sim_start.launch @@ -22,6 +22,7 @@ + @@ -30,5 +31,6 @@ + diff --git a/isaac/launch/sim.launch b/isaac/launch/sim.launch index a9b9b42c..15c790d5 100644 --- a/isaac/launch/sim.launch +++ b/isaac/launch/sim.launch @@ -29,6 +29,8 @@ + + @@ -166,6 +168,7 @@ + @@ -175,6 +178,7 @@ + diff --git a/isaac/resources/rviz/iss.rviz b/isaac/resources/rviz/iss.rviz index d75f7162..a0681156 100644 --- a/isaac/resources/rviz/iss.rviz +++ b/isaac/resources/rviz/iss.rviz @@ -5,12 +5,11 @@ Panels: Property Tree Widget: Expanded: - /Debug1 - - /Debug1/Sensors1 - /Debug1/Mobility1/Mapper1 - /ISAAC1 - /Cargo Bags1/Cargo Estimate1 - Splitter Ratio: 0.596541762 - Tree Height: 250 + Splitter Ratio: 0.5965417623519897 + Tree Height: 234 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -19,17 +18,18 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: "" +Preferences: + PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: @@ -37,17 +37,11 @@ Visualization Manager: Displays: - Class: rviz/TF Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: false - CTB_05_1050/body: - Value: true - CTB_05_1060/body: - Value: true - CTB_05_1070/body: - Value: true - CTB_05_1080/body: - Value: true RFID1: Value: true RFID1/body: @@ -62,97 +56,103 @@ Visualization Manager: Value: true body: Value: true - cargo/approach: + bumble/ar_tag: Value: true - cargo/berth: + bumble/body: Value: true - cargo/body: + bumble/cam: Value: true - cargo/complete: + bumble/dock/body: Value: true - cargo_goal/approach: + bumble/dock_cam: Value: true - cargo_goal/berth: + bumble/flashlight_aft: Value: true - cargo_goal/body: + bumble/flashlight_front: Value: true - cargo_goal/complete: + bumble/handrail/approach: Value: true - dock/berth1: + bumble/handrail/body: Value: true - dock/berth1/approach: + bumble/handrail/complete: Value: true - dock/berth1/complete: + bumble/haz_cam: Value: true - dock/berth2: + bumble/imu: Value: true - dock/berth2/approach: + bumble/inertial_link: Value: true - dock/berth2/complete: + bumble/laser: Value: true - dock/body: + bumble/nav_cam: Value: true - granite/body: + bumble/payload/bottom_aft: Value: true - handrail/approach: + bumble/payload/bottom_front: Value: true - handrail/body: + bumble/payload/top_aft: Value: true - handrail/complete: + bumble/payload/top_front: Value: true - iss/body: + bumble/perch_cam: Value: true - perch/body: + bumble/sci_cam: + Value: true + bumble/soundsee: + Value: true + bumble/target: Value: true - queen/ar_tag: + bumble/top_aft: Value: true - queen/body: + bumble/top_aft_arm_distal_link: Value: true - queen/dock/body: + bumble/top_aft_arm_proximal_link: Value: true - queen/dock_cam: + bumble/top_aft_gripper_left_distal_link: Value: true - queen/flashlight_aft: + bumble/top_aft_gripper_left_proximal_link: Value: true - queen/flashlight_front: + bumble/top_aft_gripper_right_distal_link: Value: true - queen/haz_cam: + bumble/top_aft_gripper_right_proximal_link: Value: true - queen/imu: + bumble/truth: Value: true - queen/inertial_link: + cargo/approach: + Value: true + cargo/berth: Value: true - queen/laser: + cargo/body: Value: true - queen/nav_cam: + cargo/complete: Value: true - queen/payload/bottom_aft: + cargo_goal/approach: Value: true - queen/payload/bottom_front: + cargo_goal/berth: Value: true - queen/payload/top_aft: + cargo_goal/body: Value: true - queen/payload/top_front: + cargo_goal/complete: Value: true - queen/perch_cam: + dock/berth1: Value: true - queen/sci_cam: + dock/berth1/approach: Value: true - queen/top_aft: + dock/berth1/complete: Value: true - queen/top_aft_arm_distal_link: + dock/berth2: Value: true - queen/top_aft_arm_proximal_link: + dock/berth2/approach: Value: true - queen/top_aft_gripper_left_distal_link: + dock/berth2/complete: Value: true - queen/top_aft_gripper_left_proximal_link: + dock/body: Value: true - queen/top_aft_gripper_right_distal_link: + granite/body: Value: true - queen/top_aft_gripper_right_proximal_link: + iss/body: Value: true - queen/truth: + perch/body: Value: true rviz: Value: true @@ -166,21 +166,37 @@ Visualization Manager: Value: true world: Value: true - Marker Scale: 0.300000012 + Marker Alpha: 1 + Marker Scale: 0.30000001192092896 Name: TF Show Arrows: false Show Axes: true Show Names: true Tree: - world: - CTB_05_1050/body: + body: + wifi_receiver: {} - CTB_05_1060/body: + bumble/cam: + bumble/target: {} - CTB_05_1070/body: + bumble/handrail/body: + bumble/handrail/approach: {} - CTB_05_1080/body: + bumble/handrail/complete: {} + cargo/berth: + cargo/body: + cargo/approach: + {} + cargo/complete: + {} + cargo_goal/berth: + cargo_goal/body: + cargo_goal/approach: + {} + cargo_goal/complete: + {} + world: RFID1: {} RFID1/body: @@ -193,66 +209,68 @@ Visualization Manager: {} RFID3/body: {} - dock/body: - dock/berth1: - dock/berth1/approach: - {} - dock/berth1/complete: - {} - dock/berth2: - dock/berth2/approach: - {} - dock/berth2/complete: - {} - granite/body: - {} - iss/body: - {} - perch/body: - {} - queen/body: - queen/ar_tag: + bumble/body: + bumble/ar_tag: + {} + bumble/dock_cam: {} - queen/dock_cam: + bumble/flashlight_aft: {} - queen/flashlight_aft: + bumble/flashlight_front: {} - queen/flashlight_front: + bumble/haz_cam: {} - queen/haz_cam: + bumble/imu: {} - queen/imu: + bumble/inertial_link: {} - queen/inertial_link: + bumble/laser: {} - queen/laser: + bumble/nav_cam: {} - queen/nav_cam: + bumble/payload/bottom_aft: {} - queen/payload/bottom_aft: + bumble/payload/bottom_front: {} - queen/payload/bottom_front: + bumble/payload/top_aft: {} - queen/payload/top_aft: + bumble/payload/top_front: {} - queen/payload/top_front: + bumble/perch_cam: {} - queen/perch_cam: + bumble/sci_cam: {} - queen/sci_cam: + bumble/soundsee: {} - queen/top_aft: - queen/top_aft_arm_proximal_link: - queen/top_aft_arm_distal_link: - queen/top_aft_gripper_left_proximal_link: - queen/top_aft_gripper_left_distal_link: + bumble/top_aft: + bumble/top_aft_arm_proximal_link: + bumble/top_aft_arm_distal_link: + bumble/top_aft_gripper_left_proximal_link: + bumble/top_aft_gripper_left_distal_link: {} - queen/top_aft_gripper_right_proximal_link: - queen/top_aft_gripper_right_distal_link: + bumble/top_aft_gripper_right_proximal_link: + bumble/top_aft_gripper_right_distal_link: {} - queen/dock/body: + bumble/dock/body: + {} + bumble/truth: + {} + dock/body: + dock/berth1: + dock/berth1/approach: + {} + dock/berth1/complete: + {} + dock/berth2: + dock/berth2/approach: + {} + dock/berth2/complete: + {} + granite/body: {} - queen/truth: + iss/body: + {} + perch/body: {} rviz: {} @@ -266,45 +284,47 @@ Visualization Manager: Value: true - Class: rviz/Group Displays: - - Class: rviz/Axes + - Alpha: 1 + Class: rviz/Axes Enabled: true Length: 3 Name: Axes - Radius: 0.00999999978 + Radius: 0.009999999776482582 Reference Frame: world + Show Trail: false Value: true - - Alpha: 0.100000001 + - Alpha: 0.10000000149011612 Cell Size: 1 Class: rviz/Grid Color: 255; 255; 127 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Billboards Name: Grid 1m Normal Cell Count: 0 Offset: X: 0 Y: 0 - Z: 4.8499999 + Z: 4.849999904632568 Plane: XY Plane Cell Count: 100 Reference Frame: world Value: true - - Alpha: 0.100000001 - Cell Size: 0.100000001 + - Alpha: 0.10000000149011612 + Cell Size: 0.10000000149011612 Class: rviz/Grid Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid 0.1m Normal Cell Count: 0 Offset: X: 0 Y: 0 - Z: 4.8499999 + Z: 4.849999904632568 Plane: XY Plane Cell Count: 1000 Reference Frame: world @@ -315,14 +335,14 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid 10m Normal Cell Count: 0 Offset: X: 0 Y: 0 - Z: 4.8499999 + Z: 4.849999904632568 Plane: XY Plane Cell Count: 10 Reference Frame: world @@ -399,23 +419,7 @@ Visualization Manager: Link Tree Style: Links in Alphabetic Order Name: Honey Robot Description: /honey/robot_description - TF Prefix: honey - Update Interval: 0 - Value: true - Visual Enabled: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - Name: Bumble - Robot Description: /bumble/robot_description - TF Prefix: bumble + TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true @@ -429,50 +433,66 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - queen/body: + bumble/body: Alpha: 1 Show Axes: false Show Trail: false Value: true - queen/inertial_link: + bumble/inertial_link: Alpha: 1 Show Axes: false Show Trail: false - queen/top_aft: + bumble/top_aft: Alpha: 1 Show Axes: false Show Trail: false Value: true - queen/top_aft_arm_distal_link: + bumble/top_aft_arm_distal_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - queen/top_aft_arm_proximal_link: + bumble/top_aft_arm_proximal_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - queen/top_aft_gripper_left_distal_link: + bumble/top_aft_gripper_left_distal_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - queen/top_aft_gripper_left_proximal_link: + bumble/top_aft_gripper_left_proximal_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - queen/top_aft_gripper_right_distal_link: + bumble/top_aft_gripper_right_distal_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - queen/top_aft_gripper_right_proximal_link: + bumble/top_aft_gripper_right_proximal_link: Alpha: 1 Show Axes: false Show Trail: false Value: true + Name: Bumble + Robot Description: /bumble/robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order Name: Queen Robot Description: /queen/robot_description TF Prefix: "" @@ -582,6 +602,7 @@ Visualization Manager: - Class: rviz/Image Enabled: true Image Topic: /hw/depth_haz/extended/amplitude_int + Max Value: 1 Median window: 5 Min Value: 0 Name: "DEBUG: HazCam intensity" @@ -593,8 +614,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.541443229 - Min Value: -0.164811939 + Max Value: 0.5414432287216187 + Min Value: -0.16481193900108337 Value: true Axis: Y Channel Name: intensity @@ -605,15 +626,13 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: HazCam Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1 - Size (m): 0.00999999978 + Size (m): 0.009999999776482582 Style: Points Topic: /hw/depth_haz/points Unreliable: false @@ -635,15 +654,13 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: PerchCam Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1 - Size (m): 0.00999999978 + Size (m): 0.009999999776482582 Style: Points Topic: /hw/depth_perch/points Unreliable: false @@ -669,15 +686,13 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: Features Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.0500000007 + Size (m): 0.05000000074505806 Style: Spheres Topic: /gnc/ekf/features Unreliable: false @@ -686,15 +701,16 @@ Visualization Manager: Value: false - Alpha: 1 Axes Length: 1 - Axes Radius: 0.100000001 + Axes Radius: 0.10000000149011612 Class: rviz/Pose Color: 255; 255; 0 Enabled: true - Head Length: 0.300000012 - Head Radius: 0.100000001 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 Name: Ground Truth + Queue Size: 10 Shaft Length: 0.5 - Shaft Radius: 0.00999999978 + Shaft Radius: 0.009999999776482582 Shape: Arrow Topic: /loc/truth Unreliable: false @@ -724,11 +740,11 @@ Visualization Manager: Class: rviz/Path Color: 25; 255; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Name: CurrentPlan Offset: X: 0 @@ -736,9 +752,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /mob/choreographer/segment Unreliable: false Value: true @@ -758,10 +775,11 @@ Visualization Manager: Color: 204; 51; 204 Enabled: true History Length: 1 - Line Thickness: 0.100000001 + Line Thickness: 0.10000000149011612 Name: Trajectory Plot Acceleration: false Plot Velocity: false + Queue Size: 10 Style: Mike Tangent Samples: 25 Topic: /mob/planner_qp/trajectory @@ -775,10 +793,11 @@ Visualization Manager: Color: 255; 0; 0 Enabled: false History Length: 1 - Line Thickness: 0.100000001 + Line Thickness: 0.10000000149011612 Name: TrajectoryDebug Plot Acceleration: false Plot Velocity: false + Queue Size: 10 Style: Mike Tangent Samples: 10 Topic: /mob/planner_qp/trajectory_debug @@ -814,7 +833,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Alpha: 0.100000001 + - Alpha: 0.10000000149011612 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 @@ -829,15 +848,13 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: FreeSpace Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.100000001 + Size (m): 0.10000000149011612 Style: Boxes Topic: /mob/mapper/free_space_cloud Unreliable: false @@ -847,8 +864,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -3.6400001 - Min Value: -5.23999977 + Max Value: -3.640000104904175 + Min Value: -5.239999771118164 Value: true Axis: Z Channel Name: intensity @@ -859,15 +876,13 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: Map Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.100000001 + Size (m): 0.10000000149011612 Style: Boxes Topic: /mob/mapper/obstacle_cloud Unreliable: false @@ -1015,7 +1030,7 @@ Visualization Manager: Name: Objects - Class: rviz/Group Displays: - - Alpha: 0.699999988 + - Alpha: 0.699999988079071 Class: rviz/RobotModel Collision Enabled: false Enabled: true @@ -1137,7 +1152,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -1148,41 +1166,41 @@ Visualization Manager: Current: Class: rviz/FPS Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.634796858 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7547968029975891 Position: - X: 7.19384241 - Y: 7.89102077 - Z: -1.62936008 + X: 8.946130752563477 + Y: 8.705817222595215 + Z: -3.052593469619751 + Roll: 0 Target Frame: rviz - Value: FPS (rviz) - Yaw: 0.435249835 + Yaw: 0.74524986743927 Saved: ~ Window Geometry: "DEBUG: AcousticsCam": collapsed: false "DEBUG: DockCam": collapsed: false + "DEBUG: HazCam intensity": + collapsed: false "DEBUG: HeatCam": collapsed: false "DEBUG: NavCam": collapsed: false "DEBUG: SciCam": collapsed: false - "DEBUG: HazCam": - collapsed: false Displays: collapsed: false - Height: 788 + Height: 1018 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -1191,6 +1209,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1219 + Width: 1443 X: 62 - Y: 32 + Y: 27 diff --git a/scripts/setup/install_desktop_packages.sh b/scripts/setup/install_desktop_packages.sh index 21cef37a..09bf8d70 100755 --- a/scripts/setup/install_desktop_packages.sh +++ b/scripts/setup/install_desktop_packages.sh @@ -40,30 +40,30 @@ for i in $pkg_files; do pkgs="$pkgs $(<"$i")" done -# if this file exists, we are using the astrobee repository -# need to set up tunnel through m (NASA users only) -if [ -e $arssrc ]; -then - username=${NDC_USERNAME:+${NDC_USERNAME}@} +# # if this file exists, we are using the astrobee repository +# # need to set up tunnel through m (NASA users only) +# if [ -e $arssrc ]; +# then +# username=${NDC_USERNAME:+${NDC_USERNAME}@} - # Add these packages to the apt sources (we remove them later, so apt update succeeds) +# # Add these packages to the apt sources (we remove them later, so apt update succeeds) - NO_TUNNEL=${NO_TUNNEL:-0} # Override this with 1 before invoking if the tunnel is not desired +# NO_TUNNEL=${NO_TUNNEL:-0} # Override this with 1 before invoking if the tunnel is not desired - if [ "${NO_TUNNEL}" -eq 1 ]; then - echo "Getting the custom Debian without tunnel" - sudo /bin/bash -c "echo \"deb [arch=amd64] http://astrobee.ndc.nasa.gov/software_new ${DIST} main\" > $arssrc" || exit 1 - sudo /bin/bash -c "echo \"deb-src http://astrobee.ndc.nasa.gov/software_new ${DIST} main\" >> $arssrc" || exit 1 - else - echo "Tunnelling to get the custom Debian" - sudo /bin/bash -c "echo \"deb [arch=amd64] http://127.0.0.1:8765/software_new ${DIST} main\" > $arssrc" || exit 1 - sudo /bin/bash -c "echo \"deb-src http://127.0.0.1:8765/software_new ${DIST} main\" >> $arssrc" || exit 1 - ssh -N -L 8765:astrobee.ndc.nasa.gov:80 ${username}m.ndc.nasa.gov & - fi +# if [ "${NO_TUNNEL}" -eq 1 ]; then +# echo "Getting the custom Debian without tunnel" +# sudo /bin/bash -c "echo \"deb [arch=amd64] http://astrobee.ndc.nasa.gov/software_new ${DIST} main\" > $arssrc" || exit 1 +# sudo /bin/bash -c "echo \"deb-src http://astrobee.ndc.nasa.gov/software_new ${DIST} main\" >> $arssrc" || exit 1 +# else +# echo "Tunnelling to get the custom Debian" +# sudo /bin/bash -c "echo \"deb [arch=amd64] http://127.0.0.1:8765/software_new ${DIST} main\" > $arssrc" || exit 1 +# sudo /bin/bash -c "echo \"deb-src http://127.0.0.1:8765/software_new ${DIST} main\" >> $arssrc" || exit 1 +# ssh -N -L 8765:astrobee.ndc.nasa.gov:80 ${username}m.ndc.nasa.gov & +# fi - trap "kill $! 2> /dev/null; sudo truncate -s 0 $arssrc; wait $!" 0 HUP QUIT ILL ABRT FPE SEGV PIPE TERM INT - sleep 1 -fi +# trap "kill $! 2> /dev/null; sudo truncate -s 0 $arssrc; wait $!" 0 HUP QUIT ILL ABRT FPE SEGV PIPE TERM INT +# sleep 1 +# fi sudo apt-get update || exit 1 From dfb9ad74a88eee3454e5cb83946e6cad78877e06 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Wed, 8 Nov 2023 18:07:03 -0800 Subject: [PATCH 02/44] adding a proof of concept example of the executor --- .../inspection/tools/inspection_tool.cc | 4 +- astrobee/survey_manager/CMakeLists.txt | 56 +++++++ astrobee/survey_manager/package.xml | 21 +++ .../survey_manager/scripts/command_astrobee | 146 ++++++++++++++++++ 4 files changed, 225 insertions(+), 2 deletions(-) create mode 100644 astrobee/survey_manager/CMakeLists.txt create mode 100644 astrobee/survey_manager/package.xml create mode 100755 astrobee/survey_manager/scripts/command_astrobee diff --git a/astrobee/behaviors/inspection/tools/inspection_tool.cc b/astrobee/behaviors/inspection/tools/inspection_tool.cc index a131ef1a..ea7d22a7 100644 --- a/astrobee/behaviors/inspection/tools/inspection_tool.cc +++ b/astrobee/behaviors/inspection/tools/inspection_tool.cc @@ -185,7 +185,7 @@ geometry_msgs::PoseArray ReadPosesFile(std::string file) { quat_robot.setRPY(euler_roll * DEG2RAD, euler_pitch * DEG2RAD, euler_yaw * DEG2RAD); } else { - std::cout << "Ignoring invalid line: " << line << std::endl; + // std::cout << "Ignoring invalid line: " << line << std::endl; continue; } } @@ -390,7 +390,7 @@ void ConnectedCallback( if (!client->IsConnected()) return; // Print out a status message std::cout << "\r " - << "\rState: CONNECTED" << std::flush; + << "\rState: CONNECTED\n" << std::flush; SendGoal(client); } diff --git a/astrobee/survey_manager/CMakeLists.txt b/astrobee/survey_manager/CMakeLists.txt new file mode 100644 index 00000000..93daf02c --- /dev/null +++ b/astrobee/survey_manager/CMakeLists.txt @@ -0,0 +1,56 @@ +# Copyright (c) 2021, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking +# platform" software is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +cmake_minimum_required(VERSION 3.0) +project(survey_manager) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +SET(catkin2_DIR "${CMAKE_SOURCE_DIR}/../../../cmake") +find_package(catkin2 REQUIRED COMPONENTS +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( +) + +########### +## Build ## +########### + +# Specify additional locations of header files +# Your package locations should be listed before other locations +include_directories( + ${catkin_INCLUDE_DIRS} +) + + +############# +## Install ## +############# + +catkin_install_python(PROGRAMS scripts/command_astrobee + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) \ No newline at end of file diff --git a/astrobee/survey_manager/package.xml b/astrobee/survey_manager/package.xml new file mode 100644 index 00000000..ecb3d537 --- /dev/null +++ b/astrobee/survey_manager/package.xml @@ -0,0 +1,21 @@ + + + survey_manager + 0.0.0 + The survey_manager package + + Apache License, Version 2.0 + + + ISAAC Flight Software + + + ISAAC Flight Software + + + catkin + + + + + diff --git a/astrobee/survey_manager/scripts/command_astrobee b/astrobee/survey_manager/scripts/command_astrobee new file mode 100755 index 00000000..0fc40659 --- /dev/null +++ b/astrobee/survey_manager/scripts/command_astrobee @@ -0,0 +1,146 @@ +#!/usr/bin/env python +# +# Copyright (c) 2021, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking +# platform" software is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +import argparse +import subprocess +import sys + + + +command = 'rosrun inspection inspection_tool -panorama -panorama_poses /resources/panorama_iss.txt -panorama_mode "5_mapper_and_hugin"' + + +def send_command_with_input(command): + print(command) + + # Start the process + process = subprocess.Popen(command, shell=True, stdin=subprocess.PIPE, stdout=sys.stdout, stderr=subprocess.PIPE, text=True) + + try: + while process.poll() is None: + + # Get user input dynamically + user_input = input() + + # Check if the user wants to exit + if user_input.lower().strip() == 'exit': + break + + # Send the user input to the process + process.stdin.write(user_input + "\n") + process.stdin.flush() + + # Close stdin to indicate no more input will be sent + process.stdin.close() + + # Read output and error (if any) + output, error = process.communicate() + + # Get the final exit code + return process.returncode + + except: + return -1 + + +def send_command(command): + print(command) + process = subprocess.Popen(command, shell=True, stdout=sys.stdout, stderr=sys.stdout, text=True) + + # Get the output and error (if any) + return process.wait() + +def get_position(bay): + + if bay == "jem_bay1": + return "'11, -4, 4.8'" + if bay == "jem_bay2": + return "'11, -5, 4.8'" + if bay == "jem_bay3": + return "'11, -6, 4.8'" + if bay == "jem_bay4": + return "'11, -7, 4.8'" + if bay == "jem_bay5": + return "'11, -8, 4.8'" + if bay == "jem_bay6": + return "'11, -9, 4.8'" + if bay == "jem_bay7": + return "'11, -9.7, 4.8'" + + +def repeat_inspection(): + while True: + user_input = input("Do you want to continue? (y/n): ").lower().strip() + if user_input == 'y': + if send_command_with_input("rosrun inspection inspection_tool -resume") != 0: + repeat_inspection() # Continue recursively + break + elif user_input == 'n': + print("Exiting.") + break + else: + print("Invalid input. Please enter 'y' or 'n'.") + + +class CustomFormatter(argparse.ArgumentDefaultsHelpFormatter): + pass + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=CustomFormatter + ) + + parser.add_argument( + "command_name", + help="Prefix for bagfiles to merge. Bags should all be in the current working directory.", + ) + parser.add_argument( + "arg1", + default="", + help="Prefix for bagfiles to merge. Bags should all be in the current working directory.", + ) + parser.add_argument( + "arg2", + default="", + help="Prefix for bagfiles to merge. Bags should all be in the current working directory.", + ) + args = parser.parse_args() + + + if args.command_name == "dock": + send_command("rosrun executive teleop -dock") + + elif args.command_name == "dock": + send_command("rosrun executive teleop -undock") + + elif args.command_name == "move": + send_command("rosrun executive teleop -move -pos " + args.arg1) + + elif args.command_name == "panorama": + send_command("python gds_helper_batch.py -i cmd -- bagger -start pano_" + args.arg1 + "_" + args.arg2) + if send_command_with_input("rosrun inspection inspection_tool -panorama -panorama_mode '5_mapper_and_hugin' -pos " + str(get_position(args.arg1))) != 0: + repeat_inspection() + send_command("python gds_helper_batch.py -i cmd -- bagger -stop") + + elif args.command_name == "stereo": + send_command("python gds_helper_batch.py -i cmd -- bagger -start stereo_" + args.arg1 + "_" + args.arg2) + send_command("rosrun executive teleop -undock") + send_command("python gds_helper_batch.py -i cmd -- bagger -stop") From 796695bade7aa089af3e85b0f345a105ed6be243 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Wed, 8 Nov 2023 18:16:30 -0800 Subject: [PATCH 03/44] running through isort --- astrobee/survey_manager/scripts/command_astrobee | 2 -- 1 file changed, 2 deletions(-) diff --git a/astrobee/survey_manager/scripts/command_astrobee b/astrobee/survey_manager/scripts/command_astrobee index 0fc40659..73410f17 100755 --- a/astrobee/survey_manager/scripts/command_astrobee +++ b/astrobee/survey_manager/scripts/command_astrobee @@ -22,8 +22,6 @@ import argparse import subprocess import sys - - command = 'rosrun inspection inspection_tool -panorama -panorama_poses /resources/panorama_iss.txt -panorama_mode "5_mapper_and_hugin"' From 8456b900a960a6f95d78e18f353647cee5ff6046 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Tue, 14 Nov 2023 18:11:24 -0800 Subject: [PATCH 04/44] committing before changing things around --- .../survey_manager/scripts/command_astrobee | 58 +++++++++++-------- 1 file changed, 33 insertions(+), 25 deletions(-) diff --git a/astrobee/survey_manager/scripts/command_astrobee b/astrobee/survey_manager/scripts/command_astrobee index 73410f17..585c3e97 100755 --- a/astrobee/survey_manager/scripts/command_astrobee +++ b/astrobee/survey_manager/scripts/command_astrobee @@ -22,7 +22,9 @@ import argparse import subprocess import sys -command = 'rosrun inspection inspection_tool -panorama -panorama_poses /resources/panorama_iss.txt -panorama_mode "5_mapper_and_hugin"' + + +# command = 'rosrun inspection inspection_tool -panorama -panorama_poses /resources/panorama_iss.txt -panorama_mode "5_mapper_and_hugin"' def send_command_with_input(command): @@ -97,6 +99,30 @@ def repeat_inspection(): print("Invalid input. Please enter 'y' or 'n'.") +def survey_manager_executer(goal, bay, run): + + if goal == "dock": + send_command("rosrun executive teleop -dock") + + elif goal == "dock": + send_command("rosrun executive teleop -undock") + + elif goal == "move": + send_command("rosrun executive teleop -move -pos " + bay) + + elif goal == "panorama": + send_command("python gds_helper_batch.py -i cmd -- bagger -start pano_" + bay + "_" + run) + if send_command_with_input("rosrun inspection inspection_tool -panorama -panorama_mode '5_mapper_and_hugin' -pos " + str(get_position(bay))) != 0: + repeat_inspection() + send_command("python gds_helper_batch.py -i cmd -- bagger -stop") + + elif goal == "stereo": + send_command("python gds_helper_batch.py -i cmd -- bagger -start stereo_" + bay + "_" + run) + send_command("python gds_helper_batch.py -i cmd -- plan -load plans/ISAAC/" + bay + "_stereo_mapping.fplan") + send_command("python gds_helper_batch.py -i cmd -- plan -run") + send_command("python gds_helper_batch.py -i cmd -- bagger -stop") + + class CustomFormatter(argparse.ArgumentDefaultsHelpFormatter): pass @@ -111,34 +137,16 @@ if __name__ == "__main__": help="Prefix for bagfiles to merge. Bags should all be in the current working directory.", ) parser.add_argument( - "arg1", + "bay", default="", - help="Prefix for bagfiles to merge. Bags should all be in the current working directory.", + help="Target bay start.", ) + parser.add_argument( - "arg2", + "run", default="", - help="Prefix for bagfiles to merge. Bags should all be in the current working directory.", + help="Run number, increases as we add attempts.", ) args = parser.parse_args() - - if args.command_name == "dock": - send_command("rosrun executive teleop -dock") - - elif args.command_name == "dock": - send_command("rosrun executive teleop -undock") - - elif args.command_name == "move": - send_command("rosrun executive teleop -move -pos " + args.arg1) - - elif args.command_name == "panorama": - send_command("python gds_helper_batch.py -i cmd -- bagger -start pano_" + args.arg1 + "_" + args.arg2) - if send_command_with_input("rosrun inspection inspection_tool -panorama -panorama_mode '5_mapper_and_hugin' -pos " + str(get_position(args.arg1))) != 0: - repeat_inspection() - send_command("python gds_helper_batch.py -i cmd -- bagger -stop") - - elif args.command_name == "stereo": - send_command("python gds_helper_batch.py -i cmd -- bagger -start stereo_" + args.arg1 + "_" + args.arg2) - send_command("rosrun executive teleop -undock") - send_command("python gds_helper_batch.py -i cmd -- bagger -stop") + survey_manager_executer(args.command_name, args.bay, args.run) From 255143dac97647100547bf3ba180ec530f56ae13 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Mon, 27 Nov 2023 08:25:08 -0800 Subject: [PATCH 05/44] sockets working --- astrobee/survey_manager/CMakeLists.txt | 2 +- .../survey_manager/scripts/command_astrobee | 304 +++++++++++++----- .../survey_manager/scripts/monitor_astrobee | 97 ++++++ 3 files changed, 328 insertions(+), 75 deletions(-) create mode 100755 astrobee/survey_manager/scripts/monitor_astrobee diff --git a/astrobee/survey_manager/CMakeLists.txt b/astrobee/survey_manager/CMakeLists.txt index 93daf02c..c83f14e4 100644 --- a/astrobee/survey_manager/CMakeLists.txt +++ b/astrobee/survey_manager/CMakeLists.txt @@ -52,5 +52,5 @@ include_directories( ## Install ## ############# -catkin_install_python(PROGRAMS scripts/command_astrobee +catkin_install_python(PROGRAMS scripts/command_astrobee scripts/monitor_astrobee DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) \ No newline at end of file diff --git a/astrobee/survey_manager/scripts/command_astrobee b/astrobee/survey_manager/scripts/command_astrobee index 585c3e97..577c61c2 100755 --- a/astrobee/survey_manager/scripts/command_astrobee +++ b/astrobee/survey_manager/scripts/command_astrobee @@ -21,51 +21,11 @@ import argparse import subprocess import sys +import os +import threading - - -# command = 'rosrun inspection inspection_tool -panorama -panorama_poses /resources/panorama_iss.txt -panorama_mode "5_mapper_and_hugin"' - - -def send_command_with_input(command): - print(command) - - # Start the process - process = subprocess.Popen(command, shell=True, stdin=subprocess.PIPE, stdout=sys.stdout, stderr=subprocess.PIPE, text=True) - - try: - while process.poll() is None: - - # Get user input dynamically - user_input = input() - - # Check if the user wants to exit - if user_input.lower().strip() == 'exit': - break - - # Send the user input to the process - process.stdin.write(user_input + "\n") - process.stdin.flush() - - # Close stdin to indicate no more input will be sent - process.stdin.close() - - # Read output and error (if any) - output, error = process.communicate() - - # Get the final exit code - return process.returncode - - except: - return -1 - - -def send_command(command): - print(command) - process = subprocess.Popen(command, shell=True, stdout=sys.stdout, stderr=sys.stdout, text=True) - - # Get the output and error (if any) - return process.wait() +import socket +import select def get_position(bay): @@ -84,43 +44,234 @@ def get_position(bay): if bay == "jem_bay7": return "'11, -9.7, 4.8'" +class SurveyExecutor: + + def __init__(self, robot_name, goal, bay, run): + self.robot = robot_name + self.input_path = '/tmp/input_' + robot_name + self.output_path = '/tmp/output_' + robot_name + + # Check if the file exists + if os.path.exists(self.input_path): + os.remove(self.input_path) + if os.path.exists(self.output_path): + os.remove(self.output_path) + + # Declare socket for process input + self.sock_input = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) + self.sock_input.settimeout(1) # Set a timeout for socket operations + self.sock_input.bind(self.input_path) + self.sock_input.listen(1) # Listen for one connection + # self.sock_input_connected = False + # self.sock_input.accept(connect_input_callback) + + # Declare socket for process output + self.sock_output = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) + self.sock_output.settimeout(1) # Set a timeout for socket operations + self.sock_output.bind(self.output_path) + self.sock_output.listen(1) # Listen for one connection + # self.sock_output_connected = False + # self.sock_output.accept(connect_output_callback) + + # Open the input FIFO file descriptor with non-blocking flag + # self.input_fifo_fd = os.open(self.input_fifo_path, os.O_RDONLY | os.O_NONBLOCK) + # self.input_fifo = os.fdopen(self.input_fifo_fd) + + # Open the output FIFO file descriptor with non-blocking flag + # self.output_fifo_fd = os.open(self.output_fifo_path, os.O_WRONLY) + # self.output_fifo = os.fdopen(self.output_fifo_fd, 'w') + + + self._stop_event = threading.Event() + + + self.survey_manager_executor(goal, bay, run) + + self.sock_input.close() + self.sock_output.close() + + # def connect_input_callback(sock, addr): + # """Callback function that is called when a connection is made to the socket.""" + # print("Input connection made from", addr) + # sock_input_connected = False + + # def connect_output_callback(sock, addr): + # """Callback function that is called when a connection is made to the socket.""" + # print("Output connection made from", addr) + # sock_output_connected = False + + def thread_write_output(self, process): + print("starting thread_write_output...") + # Store commulative output + output_total = "" + connected = False + try: + while True: + # Get output from process + output = process.stdout.readline() + if (output == '' and process.poll() is not None) or self._stop_event.is_set(): + break + if output: + output_total += output + + # If socket is not connected try to connect + try: + if not connected: + print("trying to connect") + conn, addr = self.sock_output.accept() + conn.setblocking(False) + + connected = True + conn.send(output_total.encode("utf-8")[:1024]) + except socket.timeout: + continue + except (socket.error, BrokenPipeError): + print("Error sending data. Receiver may have disconnected.") + connected = False + + # If socket is already connected, send output + if connected: + try: + conn.send(output.encode("utf-8")[:1024]) + except (socket.error, BrokenPipeError): + print("Error sending data. Receiver may have disconnected.") + connected = False + + # print(output.strip()) + # self.sock_output.send(output.encode("utf-8")[:1024]) + except Exception as e: + print("exit output:") + print(e) + # finally: + # # Save total output into a log + # print(output_total) + + def thread_read_input(self, process): + print("starting thread_read_input...") + try: + while True: + while not self._stop_event.is_set(): + print("waiting for connection") + try: + client_socket, client_address = self.sock_input.accept() + break + except socket.timeout: + continue + if self._stop_event.is_set(): + break + client_socket.settimeout(1) # Set a timeout for socket operations + + + while True: + print("accepted connection:") + print(client_address) + + while not self._stop_event.is_set(): + print("waiting to receive") + try: + request = client_socket.recv(1024).decode("utf-8") + break + except socket.timeout: + continue + if self._stop_event.is_set(): + break + + if not request: + break + print("got: " + request) + + print(request) + process.stdin.write(request + "\n") + process.stdin.flush() + except Exception as e: + print("exit input:") + print(e) + + + + def send_command_with_input(self, command): + print(command) + return_code = -1 + + try: + # Start the process + process = subprocess.Popen(command, shell=True, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True) + + # Start input and output threads + input_thread = threading.Thread(target=self.thread_read_input, args=(process,)) + input_thread.start() + output_thread = threading.Thread(target=self.thread_write_output, args=(process,)) + output_thread.start() + + output_thread.join() + # Get the return code of the process + return_code = process.poll() + + except Exception as e: + print("exit main:") + print(e) + # Get the return code of the process + process.kill() + finally: + # Forcefully stop the thread (not recommended) + print("Killing input thread...") + self._stop_event.set() + input_thread.join() + + # Get the final exit code + return return_code + + def send_command(self, command): + print(command) + process = subprocess.Popen(command, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True) + + # Get the output and error (if any) + return process.wait() + + + def repeat_inspection(self): + while True: + user_input = input("Do you want to continue? (y/n): ").lower().strip() + if user_input == 'y': + if send_command_with_input("rosrun inspection inspection_tool -resume") != 0: + repeat_inspection() # Continue recursively + break + elif user_input == 'n': + print("Exiting.") + break + else: + print("Invalid input. Please enter 'y' or 'n'.") + + + def survey_manager_executor(self, goal, bay, run): -def repeat_inspection(): - while True: - user_input = input("Do you want to continue? (y/n): ").lower().strip() - if user_input == 'y': - if send_command_with_input("rosrun inspection inspection_tool -resume") != 0: - repeat_inspection() # Continue recursively - break - elif user_input == 'n': - print("Exiting.") - break - else: - print("Invalid input. Please enter 'y' or 'n'.") + if goal == "dock": + self.send_command("rosrun executive teleop -dock") + elif goal == "dock": + self.send_command("rosrun executive teleop -undock") -def survey_manager_executer(goal, bay, run): + elif goal == "move": + self.send_command("rosrun executive teleop -move -pos " + bay) - if goal == "dock": - send_command("rosrun executive teleop -dock") + # Change exposure if needed - elif goal == "dock": - send_command("rosrun executive teleop -undock") + # Change map if needed - elif goal == "move": - send_command("rosrun executive teleop -move -pos " + bay) + elif goal == "panorama": - elif goal == "panorama": - send_command("python gds_helper_batch.py -i cmd -- bagger -start pano_" + bay + "_" + run) - if send_command_with_input("rosrun inspection inspection_tool -panorama -panorama_mode '5_mapper_and_hugin' -pos " + str(get_position(bay))) != 0: - repeat_inspection() - send_command("python gds_helper_batch.py -i cmd -- bagger -stop") + self.send_command("python gds_helper_batch.py -i cmd -- bagger -start pano_" + bay + "_" + run) + if self.send_command_with_input("rosrun inspection inspection_tool -panorama -panorama_mode '5_mapper_and_hugin' -pos " + str(get_position(bay))) != 0: + self.repeat_inspection() + self.send_command("python gds_helper_batch.py -i cmd -- bagger -stop") - elif goal == "stereo": - send_command("python gds_helper_batch.py -i cmd -- bagger -start stereo_" + bay + "_" + run) - send_command("python gds_helper_batch.py -i cmd -- plan -load plans/ISAAC/" + bay + "_stereo_mapping.fplan") - send_command("python gds_helper_batch.py -i cmd -- plan -run") - send_command("python gds_helper_batch.py -i cmd -- bagger -stop") + + + elif goal == "stereo": + self.send_command("python gds_helper_batch.py -i cmd -- bagger -start stereo_" + bay + "_" + run) + self.send_command("python gds_helper_batch.py -i cmd -- plan -load plans/ISAAC/" + bay + "_stereo_mapping.fplan") + self.send_command("python gds_helper_batch.py -i cmd -- plan -run") + self.send_command("python gds_helper_batch.py -i cmd -- bagger -stop") class CustomFormatter(argparse.ArgumentDefaultsHelpFormatter): @@ -132,6 +283,11 @@ if __name__ == "__main__": description=__doc__, formatter_class=CustomFormatter ) + parser.add_argument( + "robot_name", + default="", + help="Robot name executing the command.", + ) parser.add_argument( "command_name", help="Prefix for bagfiles to merge. Bags should all be in the current working directory.", @@ -149,4 +305,4 @@ if __name__ == "__main__": ) args = parser.parse_args() - survey_manager_executer(args.command_name, args.bay, args.run) + e = SurveyExecutor(args.robot_name, args.command_name, args.bay, args.run) diff --git a/astrobee/survey_manager/scripts/monitor_astrobee b/astrobee/survey_manager/scripts/monitor_astrobee new file mode 100755 index 00000000..9776ed49 --- /dev/null +++ b/astrobee/survey_manager/scripts/monitor_astrobee @@ -0,0 +1,97 @@ +#!/usr/bin/env python +# +# Copyright (c) 2021, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking +# platform" software is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +import argparse +import sys +import os +import threading +import socket + +def thread_write_input(sock_input): + print("starting thread_write_input...") + # Store commulative output + output_total = "" + try: + while True: + # Get user input dynamically + print("get input") + user_input = input() + print("user input: " + user_input) + # Check if the user wants to exit + if user_input.lower().strip() == 'exit': + break + sock_input.send(user_input.encode("utf-8")[:1024]) + + except: + print("exit output") + finally: + # Save total output into a log + print(output_total) + +def thread_read_output(sock_output): + print("starting thread_read_output...") + try: + while True: + request = sock_output.recv(1024) + request = request.decode("utf-8") # convert bytes tro str + + print(request) + except: + print("exit input") + +def survey_monitor(robot_name): + input_path = '/tmp/input_' + robot_name + output_path = '/tmp/output_' + robot_name + + sock_client_input = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) + sock_client_input.connect(input_path) + sock_client_output = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) + sock_client_output.connect(output_path) + + # Start input and output threads + input_thread = threading.Thread(target=thread_write_input, args=(sock_client_input,)) + input_thread.start() + output_thread = threading.Thread(target=thread_read_output, args=(sock_client_output,)) + output_thread.start() + + # Wait for the thread + input_thread.join() + output_thread.join() + + # Close the sockets + sock_client_input.close() + sock_client_output.close() + +class CustomFormatter(argparse.ArgumentDefaultsHelpFormatter): + pass + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=CustomFormatter + ) + + parser.add_argument( + "robot_name", + help="Name of the robot that needs monitoring.", + ) + args = parser.parse_args() + + survey_monitor(args.robot_name) From a7c09ca452533b79619b758486e008242840044f Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Mon, 27 Nov 2023 08:30:32 -0800 Subject: [PATCH 06/44] fix lint --- astrobee/survey_manager/scripts/command_astrobee | 6 +++--- astrobee/survey_manager/scripts/monitor_astrobee | 5 +++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/astrobee/survey_manager/scripts/command_astrobee b/astrobee/survey_manager/scripts/command_astrobee index 577c61c2..b1224590 100755 --- a/astrobee/survey_manager/scripts/command_astrobee +++ b/astrobee/survey_manager/scripts/command_astrobee @@ -19,13 +19,13 @@ # under the License. import argparse +import os +import select +import socket import subprocess import sys -import os import threading -import socket -import select def get_position(bay): diff --git a/astrobee/survey_manager/scripts/monitor_astrobee b/astrobee/survey_manager/scripts/monitor_astrobee index 9776ed49..2a9d691c 100755 --- a/astrobee/survey_manager/scripts/monitor_astrobee +++ b/astrobee/survey_manager/scripts/monitor_astrobee @@ -19,10 +19,11 @@ # under the License. import argparse -import sys import os -import threading import socket +import sys +import threading + def thread_write_input(sock_input): print("starting thread_write_input...") From e63ff36d4ce8f81d6b738a2e5409c2b41fa8fbe6 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Thu, 7 Dec 2023 14:59:15 -0800 Subject: [PATCH 07/44] adding example action node --- astrobee/survey_manager/CMakeLists.txt | 5 ++ .../survey_manager/src/move_action_node.cpp | 59 +++++++++++++++++++ 2 files changed, 64 insertions(+) create mode 100644 astrobee/survey_manager/src/move_action_node.cpp diff --git a/astrobee/survey_manager/CMakeLists.txt b/astrobee/survey_manager/CMakeLists.txt index c83f14e4..181faab7 100644 --- a/astrobee/survey_manager/CMakeLists.txt +++ b/astrobee/survey_manager/CMakeLists.txt @@ -48,6 +48,11 @@ include_directories( ) +# Uncomment this when plansys merged +# # Action for move_inspect +# add_executable(move_action_node src/move_action_node.cpp) +# target_link_libraries(move_action_node ${catkin_LIBRARIES} ) + ############# ## Install ## ############# diff --git a/astrobee/survey_manager/src/move_action_node.cpp b/astrobee/survey_manager/src/move_action_node.cpp new file mode 100644 index 00000000..ecba8813 --- /dev/null +++ b/astrobee/survey_manager/src/move_action_node.cpp @@ -0,0 +1,59 @@ +/* Copyright (c) 2021, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking + * platform" software is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +#include + +#include + +namespace plansys2_actions { + +class MoveAction : public plansys2::ActionExecutorClient { + public: + MoveAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate) + : ActionExecutorClient(nh, action, rate) {} + + protected: + void do_work() { + ROS_ERROR_STREAM("Executing [MOVE]"); + // std::string from, towards; + // from = get_arguments()[1]; + // towards = get_arguments()[2]; + } +}; +} // namespace plansys2_actions + + +// Main entry point for application +int main(int argc, char *argv[]) { + // Initialize a ros node + ros::init(argc, argv, "move_action"); + ros::NodeHandle nh(name); + + // Start action node + // We could actually add multiple action nodes here being aware that we might need a ros::AsyncSpinner + // (https://github.com/Bckempa/ros2_planning_system/blob/noetic-devel/plansys2_bt_actions/src/bt_action_node.cpp#L41) + auto action_node = std::make_shared(nh, name, std::chrono_literals::1s); + action_node->trigger_transition(ros::lifecycle::CONFIGURE); + + // Synchronous mode + ros::spin(); + // Make for great success + return 0; From 626a51f7e12fde09fb395a7e182d0734a8164c8e Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Mon, 11 Dec 2023 11:00:38 -0800 Subject: [PATCH 08/44] update to scripts with new requirements --- astrobee/survey_manager/CMakeLists.txt | 2 +- .../survey_manager/scripts/command_astrobee | 368 +++++++++++++----- .../survey_manager/scripts/monitor_astrobee | 2 +- 3 files changed, 277 insertions(+), 95 deletions(-) diff --git a/astrobee/survey_manager/CMakeLists.txt b/astrobee/survey_manager/CMakeLists.txt index 181faab7..8e588496 100644 --- a/astrobee/survey_manager/CMakeLists.txt +++ b/astrobee/survey_manager/CMakeLists.txt @@ -58,4 +58,4 @@ include_directories( ############# catkin_install_python(PROGRAMS scripts/command_astrobee scripts/monitor_astrobee - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) \ No newline at end of file + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/astrobee/survey_manager/scripts/command_astrobee b/astrobee/survey_manager/scripts/command_astrobee index b1224590..bc51fe72 100755 --- a/astrobee/survey_manager/scripts/command_astrobee +++ b/astrobee/survey_manager/scripts/command_astrobee @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # # Copyright (c) 2021, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. @@ -25,29 +25,62 @@ import socket import subprocess import sys import threading - - -def get_position(bay): - - if bay == "jem_bay1": - return "'11, -4, 4.8'" - if bay == "jem_bay2": - return "'11, -5, 4.8'" - if bay == "jem_bay3": - return "'11, -6, 4.8'" - if bay == "jem_bay4": - return "'11, -7, 4.8'" - if bay == "jem_bay5": - return "'11, -8, 4.8'" - if bay == "jem_bay6": - return "'11, -9, 4.8'" - if bay == "jem_bay7": - return "'11, -9.7, 4.8'" - -class SurveyExecutor: +import yaml +import pathlib + +import rospy + +from std_msgs.msg import String +from ff_msgs.msg import CommandArg, CommandStamped +from ff_msgs.msg import CommandConstants +from ff_msgs.msg import AckStamped, AckCompletedStatus + +from typing import Any, Dict, List + +# Type alias +YamlMapping = Dict[str, Any] + +# Constants +MAX_COUNTER = 10 + +def exposure_change(bay_origin, bay_destination, value): + # Going to JEM + if bay_origin == "nod2_hatch_to_jem" and bay_destination == "jem_hatch_from_nod2": + value = 100 + return True + # Going to NOD2 + if (bay_origin == "jem_hatch_to_nod2" and bay_destination == "nod2_hatch_from_jem" + or bay_origin == "usl_hatch_to_nod2" and bay_destination == "nod2_hatch_from_usl"): + value = 300 + return True + # Going to USL + if bay_origin == "nod2_hatch_to_usl" and bay_destination == "usl_hatch_from_nod2": + value = 300 + return True + return False + +def map_change(bay_origin, bay_destination, map_name): + # Going to JEM + if bay_origin == "nod2_hatch_to_jem" and bay_destination == "jem_hatch_from_nod2": + map_name = "iss.map" + return True + # Going to NOD2 + if (bay_origin == "jem_hatch_to_nod2" and bay_destination == "nod2_hatch_from_jem" + or bay_origin == "usl_hatch_to_nod2" and bay_destination == "nod2_hatch_from_usl"): + map_name = "20220331_isaac5.map" + return True + # Going to USL + if bay_origin == "nod2_hatch_to_usl" and bay_destination == "usl_hatch_from_nod2": + map_name = "20220617_Isaac10_USLonly.map" + return True + + +# This class starts a new process and lets you monitor the input and output +# Mostly used for actions where user inteference might be required +class ProcessExecutor: def __init__(self, robot_name, goal, bay, run): - self.robot = robot_name + self.input_path = '/tmp/input_' + robot_name self.output_path = '/tmp/output_' + robot_name @@ -62,30 +95,18 @@ class SurveyExecutor: self.sock_input.settimeout(1) # Set a timeout for socket operations self.sock_input.bind(self.input_path) self.sock_input.listen(1) # Listen for one connection - # self.sock_input_connected = False - # self.sock_input.accept(connect_input_callback) # Declare socket for process output self.sock_output = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) self.sock_output.settimeout(1) # Set a timeout for socket operations self.sock_output.bind(self.output_path) self.sock_output.listen(1) # Listen for one connection - # self.sock_output_connected = False - # self.sock_output.accept(connect_output_callback) - - # Open the input FIFO file descriptor with non-blocking flag - # self.input_fifo_fd = os.open(self.input_fifo_path, os.O_RDONLY | os.O_NONBLOCK) - # self.input_fifo = os.fdopen(self.input_fifo_fd) - - # Open the output FIFO file descriptor with non-blocking flag - # self.output_fifo_fd = os.open(self.output_fifo_path, os.O_WRONLY) - # self.output_fifo = os.fdopen(self.output_fifo_fd, 'w') - + # Declare event that will stop input thread self._stop_event = threading.Event() - self.survey_manager_executor(goal, bay, run) + def __del__(self): self.sock_input.close() self.sock_output.close() @@ -102,7 +123,7 @@ class SurveyExecutor: def thread_write_output(self, process): print("starting thread_write_output...") - # Store commulative output + # Store cumulative output output_total = "" connected = False try: @@ -115,30 +136,27 @@ class SurveyExecutor: output_total += output # If socket is not connected try to connect - try: - if not connected: + if not connected: + try: print("trying to connect") conn, addr = self.sock_output.accept() conn.setblocking(False) connected = True conn.send(output_total.encode("utf-8")[:1024]) - except socket.timeout: - continue - except (socket.error, BrokenPipeError): - print("Error sending data. Receiver may have disconnected.") - connected = False + except socket.timeout: + continue + except (socket.error, BrokenPipeError): + print("Error sending data. Receiver may have disconnected.") + connected = False # If socket is already connected, send output - if connected: + elif connected: try: conn.send(output.encode("utf-8")[:1024]) except (socket.error, BrokenPipeError): print("Error sending data. Receiver may have disconnected.") connected = False - - # print(output.strip()) - # self.sock_output.send(output.encode("utf-8")[:1024]) except Exception as e: print("exit output:") print(e) @@ -176,6 +194,7 @@ class SurveyExecutor: if self._stop_event.is_set(): break + # If broken pipe connect again if not request: break print("got: " + request) @@ -188,8 +207,7 @@ class SurveyExecutor: print(e) - - def send_command_with_input(self, command): + def send_command(self, command): print(command) return_code = -1 @@ -217,23 +235,17 @@ class SurveyExecutor: print("Killing input thread...") self._stop_event.set() input_thread.join() + output_thread.join() # In normal scenarios this should have already happened # Get the final exit code return return_code - def send_command(self, command): - print(command) - process = subprocess.Popen(command, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True) - - # Get the output and error (if any) - return process.wait() - - def repeat_inspection(self): + def recursive_command(self, command): while True: user_input = input("Do you want to continue? (y/n): ").lower().strip() if user_input == 'y': - if send_command_with_input("rosrun inspection inspection_tool -resume") != 0: + if send_command_with_input(command) != 0: repeat_inspection() # Continue recursively break elif user_input == 'n': @@ -242,42 +254,202 @@ class SurveyExecutor: else: print("Invalid input. Please enter 'y' or 'n'.") - - def survey_manager_executor(self, goal, bay, run): - - if goal == "dock": - self.send_command("rosrun executive teleop -dock") - - elif goal == "dock": - self.send_command("rosrun executive teleop -undock") - - elif goal == "move": - self.send_command("rosrun executive teleop -move -pos " + bay) - - # Change exposure if needed - - # Change map if needed - - elif goal == "panorama": - - self.send_command("python gds_helper_batch.py -i cmd -- bagger -start pano_" + bay + "_" + run) - if self.send_command_with_input("rosrun inspection inspection_tool -panorama -panorama_mode '5_mapper_and_hugin' -pos " + str(get_position(bay))) != 0: - self.repeat_inspection() - self.send_command("python gds_helper_batch.py -i cmd -- bagger -stop") - - - - elif goal == "stereo": - self.send_command("python gds_helper_batch.py -i cmd -- bagger -start stereo_" + bay + "_" + run) - self.send_command("python gds_helper_batch.py -i cmd -- plan -load plans/ISAAC/" + bay + "_stereo_mapping.fplan") - self.send_command("python gds_helper_batch.py -i cmd -- plan -run") - self.send_command("python gds_helper_batch.py -i cmd -- bagger -stop") + def send_command_recursive(self, command): + if send_command(command): + recursive_command(command) + + +# This class sends a command to the executor and waits to get a response +# Mostly used for short actions that should be immediate and require no feedback +# This method is needed on actions that run remotely and are not controlled by topics +class CommandExecutor: + + + def __init__(self, ns): + self.ns = ns + # Start ROS node + rospy.init_node("survey_namager_cmd_" + robot_name) + # Declare guest science command publisher + self.pub_guest_sci = rospy.Publisher(self.ns + "command", CommandStamped, queue_size=5) + + self.unique_cmd_id = "" + + def __del__(self): + rospy.shutdown() + + def start_recording(): + + # Arg is bagfile name description + arg1 = CommandArg() + arg1.data_type = CommandArg.DATA_TYPE_STRING + arg1.s = "Front" + + # Create CommandStamped message + cmd_args = [arg1] + + cmd = CommandStamped() + cmd.header = Header(stamp=rospy.Time.now()) + cmd.cmd_name = CommandConstants.CMD_NAME_START_RECORDING + cmd.cmd_id = "survey_manager" + str(rospy.Time.now().to_sec()) + self.unique_cmd_id = cmd.cmd_id + cmd.cmd_src = "isaac fsw" + cmd.cmd_origin = "isaac fsw" + + # Publish the CommandStamped message + result = publish_and_wait_response(cmd) + + def stop_recording(bag_description): + cmd = CommandStamped() + cmd.header = Header(stamp=rospy.Time.now()) + cmd.cmd_name = CommandConstants.CMD_NAME_STOP_RECORDING + cmd.cmd_id = "survey_manager" + str(rospy.Time.now().to_sec()) + self.unique_cmd_id = cmd.cmd_id + cmd.cmd_src = "isaac fsw" + cmd.cmd_origin = "isaac fsw" + + # Publish the CommandStamped message + result = publish_and_wait_response(cmd) + + def change_exposure(val): + #TBD + rospy.loginfo("Change exposure to " + val) + + def change_map(map_name): + #TBD + rospy.loginfo("Change map to " + map_name) + + def set_plan(): + # TODO Add call to plan_pub.cc + + cmd = CommandStamped() + cmd.header = Header(stamp=rospy.Time.now()) + cmd.cmd_name = CommandConstants.CMD_NAME_SET_PLAN + cmd.cmd_id = "survey_manager" + str(rospy.Time.now().to_sec()) + self.unique_cmd_id = cmd.cmd_id + cmd.cmd_src = "isaac fsw" + cmd.cmd_origin = "isaac fsw" + cmd.args = cmd_args + + # Publish the CommandStamped message + result = publish_and_wait_response(cmd) + + def run_plan(): + cmd = CommandStamped() + cmd.header = Header(stamp=rospy.Time.now()) + cmd.cmd_name = CommandConstants.CMD_NAME_RUN_PLAN + cmd.cmd_id = "survey_manager" + str(rospy.Time.now().to_sec()) + self.unique_cmd_id = cmd.cmd_id + cmd.cmd_src = "isaac fsw" + cmd.cmd_origin = "isaac fsw" + + # Publish the CommandStamped message + result = publish_and_wait_response(cmd) + + def publish_and_wait_response(cmd): + + # Publish the CommandStamped message + self.pub_guest_sci.publish(cmd) + + # Wait for ack + counter = 0 + while counter < MAX_COUNTER: + try: + msg = rospy.wait_for_message(self.ns + "gt/ack", AckStamped, timeout = 5) + if msg.cmd_id == self.unique_cmd_id: + if msg.completed_status.status == AckCompletedStatus.NOT: + rospy.loginfo("Command is being executed and has not completed.") + elif msg.completed_status.status == AckCompletedStatus.OK: + rospy.loginfo("Command completed successfully!") + return 0 + else: + rospy.loginfo("Command failed! Message: " + Ack.message) + return 1 + except: + continue + counter += 1 + + +def load_yaml(yaml_path: pathlib.Path) -> YamlMapping: + """ + Return the YAML parse result for the file at `yaml_path`. + """ + with yaml_path.open(encoding="utf-8") as yaml_stream: + return yaml.safe_load(yaml_stream) + +def get_stereo_traj(from_bay, to_bay): + # Get stereo trajectory + traj_matches = [ + traj + for traj in static_config["stereo"].values() + if traj["base_location"] == from_bay and traj["bound_location"] == to_bay + ] + assert ( + len(traj_matches) == 1 + ), f"Expected exactly 1 matching stereo trajectory with base {base} and bound {bound}, got {len(traj_matches)}" + fplan = traj_matches[0]["fplan"] + return fplan + +def survey_manager_executor(robot_name, goal, to_bay, from_bay, run, config_static_path: pathlib.Path): + + # Read the static configs that convert constants to values + config_static = load_yaml(config_static_path) + + sim = False + # Figure out robot name and whether we are in simulation or hardware + current_robot = os.environ.get('ROBOTNAME') + if not current_robot: + rospy.loginfo("We're in simulation. Let's get the robotname using the topic") + # This is a latching messge so it shouldn't take long + data = rospy.wait_for_message('/robot_name', String, timeout=2) + current_robot = data.data.lower() + sim = True + + ns = "" + # If we're commanding a robot remotely + if current_robot != robot_name: + rospy.loginfo("We're commanding a robot remotely!") + ns = " -remote -ns " + robot_name + # Command executor will add namespace for bridge forwarding + command_executor = CommandExecutor(robot_name + "/") + else: + command_executor = CommandExecutor("") + process_executor = ProcessExecutor() + + # Initialize exit code + exit_code = 0 + + if goal == "dock": + exit_code += process_executor.send_command_recursive("rosrun executive teleop -dock" + ns) + + elif goal == "undock": + exit_code += process_executor.send_command_recursive("rosrun executive teleop -undock" + ns) + + elif goal == "move": + exit_code += process_executor.send_command_recursive("rosrun executive teleop -move " + bay + ns) + # Change exposure if needed + if exposure_change(from_bay, to_bay, exposure_value): + exit_code += command_executor.change_exposure(exposure_value) + # Change map if needed + if map_change(from_bay, to_bay, map_name): + exit_code += command_executor.change_map(map_name) + + elif goal == "panorama": + exit_code += command_executor.start_recording("pano_" + bay + "_" + run) + exit_code += process_executor.send_command_recursive("rosrun inspection inspection_tool -panorama -panorama_mode '5_mapper_and_hugin' " + static_config["bays"][to_bay] + ns) + exit_code += command_executor.stop_recording() + + elif goal == "stereo": + exit_code += command_executor.start_recording("stereo_" + bay + "_" + run) + exit_code += command_executor.set_plan("plans/ISAAC/" + get_stereo_traj(from_bay, to_bay)) + exit_code += command_executor.run_plan() + exit_code += command_executor.stop_recording() + + return exit_code class CustomFormatter(argparse.ArgumentDefaultsHelpFormatter): pass - if __name__ == "__main__": parser = argparse.ArgumentParser( description=__doc__, formatter_class=CustomFormatter @@ -293,16 +465,26 @@ if __name__ == "__main__": help="Prefix for bagfiles to merge. Bags should all be in the current working directory.", ) parser.add_argument( - "bay", + "to_bay", + default="", + help="Target bay destination.", + ) + parser.add_argument( + "from_bay", default="", help="Target bay start.", ) - parser.add_argument( "run", default="", help="Run number, increases as we add attempts.", ) + parser.add_argument( + "config_static", + help="Path to input static problem config YAML (module geometry, available stereo surveys, etc.)", + type=pathlib.Path, + default="survey_constants.yaml", + ) args = parser.parse_args() - e = SurveyExecutor(args.robot_name, args.command_name, args.bay, args.run) + exit_code = survey_manager_executor(args.robot_name, args.command_name, args.to_bay, args.from_bay, args.run, args.config_static) diff --git a/astrobee/survey_manager/scripts/monitor_astrobee b/astrobee/survey_manager/scripts/monitor_astrobee index 2a9d691c..82f53a4e 100755 --- a/astrobee/survey_manager/scripts/monitor_astrobee +++ b/astrobee/survey_manager/scripts/monitor_astrobee @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # # Copyright (c) 2021, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. From 1686c1215fb3da113e7ee11817ff2fddf6338b73 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Mon, 11 Dec 2023 19:13:13 -0800 Subject: [PATCH 09/44] thread join safeguard --- astrobee/survey_manager/scripts/command_astrobee | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/astrobee/survey_manager/scripts/command_astrobee b/astrobee/survey_manager/scripts/command_astrobee index bc51fe72..9a6e0fda 100755 --- a/astrobee/survey_manager/scripts/command_astrobee +++ b/astrobee/survey_manager/scripts/command_astrobee @@ -235,7 +235,8 @@ class ProcessExecutor: print("Killing input thread...") self._stop_event.set() input_thread.join() - output_thread.join() # In normal scenarios this should have already happened + if output_thread.is_alive(): + output_thread.join() # Get the final exit code return return_code From 30c761d3c8a7418a616777a024094fc70ecdf739 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Mon, 11 Dec 2023 19:17:48 -0800 Subject: [PATCH 10/44] running isort --- .../survey_manager/scripts/command_astrobee | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/astrobee/survey_manager/scripts/command_astrobee b/astrobee/survey_manager/scripts/command_astrobee index 9a6e0fda..aa2de8ea 100755 --- a/astrobee/survey_manager/scripts/command_astrobee +++ b/astrobee/survey_manager/scripts/command_astrobee @@ -20,22 +20,24 @@ import argparse import os +import pathlib import select import socket import subprocess import sys import threading -import yaml -import pathlib +from typing import Any, Dict, List import rospy - +import yaml +from ff_msgs.msg import ( + AckCompletedStatus, + AckStamped, + CommandArg, + CommandConstants, + CommandStamped, +) from std_msgs.msg import String -from ff_msgs.msg import CommandArg, CommandStamped -from ff_msgs.msg import CommandConstants -from ff_msgs.msg import AckStamped, AckCompletedStatus - -from typing import Any, Dict, List # Type alias YamlMapping = Dict[str, Any] From c84eff28ba97f2b60223d05f763c2a3863cf0f59 Mon Sep 17 00:00:00 2001 From: Bckempa Date: Tue, 12 Dec 2023 11:13:20 -0800 Subject: [PATCH 11/44] Plansys2 on Astrobee Noetic (#107) * Add `survey_manager` subdirectory without layout for survey nodes Includes placeholder directories with hyperlinked `readme.md` files in the documentation hierarchy for the manger, planner, and bridge. Per discussion with @marinagmoreira `survey_manager` lives under `astrobee` to signify the code is intended for cross-compilation. * Improve linking to survey manager docs * Add files via upload Adding planning domain, problem, and xml of the atomic actions of the robotic agents * added files to the correct directory * adding soft constraints problem files * Attempt at a complete MVP domain model * Oops, remove redundant effect * Simplified collision checking predicate * In a working state * Add sample_output_plan.txt * Oops, remove obsolete commented-out code * Fixed stereo survey part of the model based on analysis of old stereo surveys by Marina - unfortunately causes additional planner flakiness * Update sample_output_plan.txt to reflect latest domain/problem * Now generate PDDL problem from higher-level problem specification * Tune panorama estimated duration * Add plan_interpreter.py. Minor cleanup in problem_generator.py * Simplify dynamic config just a bit * Add `survey_manager` subdirectory without layout for survey nodes Includes placeholder directories with hyperlinked `readme.md` files in the documentation hierarchy for the manger, planner, and bridge. Per discussion with @marinagmoreira `survey_manager` lives under `astrobee` to signify the code is intended for cross-compilation. * Improve linking to survey manager docs * Remove `survey_bridge` capability will be added to `astrobee` * Add traclabs plansys2 backport via submodule, thanks @ana-GT * Move sub-modules to `survey_manager` path * Upgrade behaviortree to V4 NOTE: If already installed, remove V3 before installing V4 * Cleanup unused, misplaced sub-modules, again. * Remove `survey_manager` package and organize `survey_planner` * Deprecate Ubuntu-16.04 (xenial) builds of Isaac to support Plansys2 - Remove Ubuntu-16.04 (xenial) CI builds - Update dockerfile ubuntu version defaults to Ubuntu-20.04 (focal) - Update apk build environment to Ubuntu-20.04 (focal) * Fix python formatting * Update to forks of traclabs backports * Add readline development files to setup for plansys2 terminal interface If you have already built your VM, run `sudo apt install libreadline-dev` * Removed outdated PDDL files per @trey0 https://github.com/nasa/isaac/pull/107#discussion_r1419159573 * Revert CI upgrades for APK builds --------- Co-authored-by: Abiola Akanni Co-authored-by: Trey Smith --- .github/workflows/apk.yaml | 2 - .github/workflows/ci_pr.yml | 21 -- .github/workflows/ci_push.yml | 59 ---- .github/workflows/ci_release.yml | 62 ---- .gitmodules | 8 + README.md | 1 + astrobee/readme.md | 4 +- astrobee/survey_manager/readme.md | 5 + .../behavior_trees/collecting_panoramas.xml | 7 + .../survey_planner/behavior_trees/move_to.xml | 7 + .../behavior_trees/move_to_inspect.xml | 8 + .../data/jem_survey_dynamic.yaml | 54 +++ .../data/jem_survey_static.yaml | 59 ++++ .../data/sample_output_plan.txt | 31 ++ .../data/sample_output_plan.yaml | 200 +++++++++++ .../survey_planner/pddl/domain_survey.pddl | 331 ++++++++++++++++++ .../pddl/jem_survey_template.pddl | 57 +++ .../pddl/problem_jem_survey.pddl | 296 ++++++++++++++++ .../survey_manager/survey_planner/readme.md | 5 + .../survey_planner/src/ros1_lifecycle | 1 + .../survey_planner/src/ros2_planning_system | 1 + .../survey_planner/tools/plan_interpreter.py | 251 +++++++++++++ .../survey_planner/tools/problem_generator.py | 307 ++++++++++++++++ isaac/Subsystems.md | 2 + scripts/docker/astrobee_msgs.Dockerfile | 6 +- scripts/docker/isaac.Dockerfile | 6 +- scripts/docker/isaac_astrobee.Dockerfile | 6 +- scripts/docker/isaac_msgs.Dockerfile | 6 +- scripts/setup/packages_focal.lst | 2 + 29 files changed, 1648 insertions(+), 157 deletions(-) create mode 100644 astrobee/survey_manager/readme.md create mode 100644 astrobee/survey_manager/survey_planner/behavior_trees/collecting_panoramas.xml create mode 100644 astrobee/survey_manager/survey_planner/behavior_trees/move_to.xml create mode 100644 astrobee/survey_manager/survey_planner/behavior_trees/move_to_inspect.xml create mode 100644 astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml create mode 100644 astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml create mode 100644 astrobee/survey_manager/survey_planner/data/sample_output_plan.txt create mode 100644 astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml create mode 100644 astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl create mode 100644 astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl create mode 100644 astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl create mode 100644 astrobee/survey_manager/survey_planner/readme.md create mode 160000 astrobee/survey_manager/survey_planner/src/ros1_lifecycle create mode 160000 astrobee/survey_manager/survey_planner/src/ros2_planning_system create mode 100755 astrobee/survey_manager/survey_planner/tools/plan_interpreter.py create mode 100755 astrobee/survey_manager/survey_planner/tools/problem_generator.py diff --git a/.github/workflows/apk.yaml b/.github/workflows/apk.yaml index ea3e5768..9127ee7c 100644 --- a/.github/workflows/apk.yaml +++ b/.github/workflows/apk.yaml @@ -1,5 +1,3 @@ -# Check for lint error and auto correct them - name: Compile APK on: ['push', 'pull_request', 'workflow_dispatch'] diff --git a/.github/workflows/ci_pr.yml b/.github/workflows/ci_pr.yml index d768e72b..6a1f767b 100644 --- a/.github/workflows/ci_pr.yml +++ b/.github/workflows/ci_pr.yml @@ -7,27 +7,6 @@ on: jobs: - build-xenial: - - runs-on: ubuntu-20.04 - - steps: - - - name: Checkout Astrobee - uses: actions/checkout@v3 - with: - repository: nasa/astrobee - path: astrobee/ - - - name: Checkout ISAAC - uses: actions/checkout@v3 - with: - submodules: recursive - path: isaac/ - - - name: Build code for isaac:astrobee Ubuntu 16 - run: isaac/scripts/docker/build.sh --remote --xenial --astrobee-source-path astrobee/ - build-focal: runs-on: ubuntu-20.04 diff --git a/.github/workflows/ci_push.yml b/.github/workflows/ci_push.yml index c3fa9ed5..f5a49a6a 100644 --- a/.github/workflows/ci_push.yml +++ b/.github/workflows/ci_push.yml @@ -8,65 +8,6 @@ on: jobs: - build-xenial: - - runs-on: ubuntu-20.04 - - steps: - - - name: Checkout Astrobee - uses: actions/checkout@v3 - with: - repository: nasa/astrobee - path: astrobee/ - - - name: Checkout ISAAC - uses: actions/checkout@v3 - with: - submodules: recursive - path: isaac/ - - - name: Build code for isaac:astrobee Ubuntu 16 - run: docker build isaac -f isaac/scripts/docker/isaac_astrobee.Dockerfile - --build-arg UBUNTU_VERSION=16.04 - --build-arg ROS_VERSION=kinetic - --build-arg PYTHON='' - --build-arg REMOTE=ghcr.io/nasa - -t ghcr.io/${{ github.repository_owner }}/isaac:latest-astrobee-ubuntu16.04 - - - name: Build code for isaac:latest Ubuntu 16 - run: docker build isaac -f isaac/scripts/docker/isaac.Dockerfile - --build-arg UBUNTU_VERSION=16.04 - --build-arg ROS_VERSION=kinetic - --build-arg PYTHON='' - --build-arg REMOTE=ghcr.io/nasa - -t ghcr.io/${{ github.repository_owner }}/isaac:latest-ubuntu16.04 - - - name: Build messages dockers for Ubuntu 16 (astrobee) - run: docker build astrobee -f isaac/scripts/docker/astrobee_msgs.Dockerfile - --build-arg UBUNTU_VERSION=16.04 - --build-arg ROS_VERSION=kinetic - --build-arg PYTHON='' - -t ghcr.io/${{ github.repository_owner }}/isaac:astrobee-msgs-ubuntu16.04 - - - name: Build messages dockers for Ubuntu 16 (isaac) - run: docker build isaac -f isaac/scripts/docker/isaac_msgs.Dockerfile - --build-arg UBUNTU_VERSION=16.04 - --build-arg ROS_VERSION=kinetic - --build-arg PYTHON='' - --build-arg REMOTE=ghcr.io/nasa - -t ghcr.io/${{ github.repository_owner }}/isaac:msgs-ubuntu16.04 - - - name: Log in to registry - run: echo "${{ secrets.GITHUB_TOKEN }}" | docker login ghcr.io -u ${{ github.repository_owner }} --password-stdin - - - name: Push Docker image - run: | - if [ "${{ github.repository_owner }}" = "nasa" ]; then docker push ghcr.io/${{ github.repository_owner }}/isaac:latest-astrobee-ubuntu16.04; fi; - if [ "${{ github.repository_owner }}" = "nasa" ]; then docker push ghcr.io/${{ github.repository_owner }}/isaac:latest-ubuntu16.04; fi; - if [ "${{ github.repository_owner }}" = "nasa" ]; then docker push ghcr.io/${{ github.repository_owner }}/isaac:astrobee-msgs-ubuntu16.04; fi; - if [ "${{ github.repository_owner }}" = "nasa" ]; then docker push ghcr.io/${{ github.repository_owner }}/isaac:msgs-ubuntu16.04; fi; - build-focal: runs-on: ubuntu-20.04 diff --git a/.github/workflows/ci_release.yml b/.github/workflows/ci_release.yml index c7b48cc9..bcaaa749 100644 --- a/.github/workflows/ci_release.yml +++ b/.github/workflows/ci_release.yml @@ -8,68 +8,6 @@ on: jobs: - build-xenial: - - runs-on: ubuntu-20.04 - - steps: - - uses: actions/checkout@v3 - - - name: Checkout Astrobee - uses: actions/checkout@v3 - with: - repository: nasa/astrobee - path: astrobee/ - - - name: Checkout ISAAC - uses: actions/checkout@v3 - with: - submodules: recursive - path: isaac/ - - - name: Build code for isaac:astrobee Ubuntu 16 - run: docker build isaac -f isaac/scripts/docker/isaac_astrobee.Dockerfile - --build-arg UBUNTU_VERSION=16.04 - --build-arg ROS_VERSION=kinetic - --build-arg PYTHON='' - --build-arg REMOTE=ghcr.io/nasa - -t isaac/isaac:latest-astrobee-ubuntu16.04 - - - name: Build code for isaac:latest Ubuntu 16 - run: docker build isaac -f isaac/scripts/docker/isaac.Dockerfile - --build-arg UBUNTU_VERSION=16.04 - --build-arg ROS_VERSION=kinetic - --build-arg PYTHON='' - --build-arg REMOTE=isaac - -t isaac/isaac:latest-ubuntu16.04 - - - name: Build messages dockers for Ubuntu 16 (astrobee) - run: docker build astrobee -f isaac/scripts/docker/astrobee_msgs.Dockerfile - --build-arg UBUNTU_VERSION=16.04 - --build-arg ROS_VERSION=kinetic - --build-arg PYTHON='' - -t isaac/isaac:astrobee-msgs-ubuntu16.04 - - - name: Build messages dockers for Ubuntu 16 (isaac) - run: docker build isaac -f isaac/scripts/docker/isaac_msgs.Dockerfile - --build-arg UBUNTU_VERSION=16.04 - --build-arg ROS_VERSION=kinetic - --build-arg PYTHON='' - --build-arg REMOTE=isaac - -t isaac/isaac:msgs-ubuntu16.04 - - - name: Log in to registry - run: echo "${{ secrets.GITHUB_TOKEN }}" | docker login ghcr.io -u ${{ github.repository_owner }} --password-stdin - - - name: Push Docker image - run: | - cd isaac - export VERSION=`grep -w -m 1 "Release" RELEASE.md | awk '{print $3}'` - docker tag isaac/isaac:latest-astrobee-ubuntu16.04 ghcr.io/${{ github.repository_owner }}/isaac:v${VERSION}-astrobee-ubuntu16.04 - if [ "${{ github.repository_owner }}" = "nasa" ]; then docker push ghcr.io/${{ github.repository_owner }}/isaac:v${VERSION}-astrobee-ubuntu16.04; fi; - docker tag isaac/isaac:latest-ubuntu16.04 ghcr.io/${{ github.repository_owner }}/isaac:v${VERSION}-ubuntu16.04 - if [ "${{ github.repository_owner }}" = "nasa" ]; then docker push ghcr.io/${{ github.repository_owner }}/isaac:v${VERSION}-ubuntu16.04; fi; - build-focal: runs-on: ubuntu-20.04 diff --git a/.gitmodules b/.gitmodules index b94bfb0e..6d14a890 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,11 @@ [submodule "isaac_msgs"] path = communications/isaac_msgs url = https://github.com/nasa/isaac_msgs.git +[submodule "astrobee/survey_manager/survey_planner/src/ros2_planning_system"] + path = astrobee/survey_manager/survey_planner/src/ros2_planning_system + url = https://github.com/bckempa/ros2_planning_system + branch = noetic-devel +[submodule "astrobee/survey_manager/survey_planner/src/ros1_lifecycle"] + path = astrobee/survey_manager/survey_planner/src/ros1_lifecycle + url = https://github.com/bckempa/ros1_lifecycle + branch = noetic-devel diff --git a/README.md b/README.md index 3383cae3..b0287805 100644 --- a/README.md +++ b/README.md @@ -33,6 +33,7 @@ the [Astrobee robot](https://github.com/nasa/astrobee). This repository includes - [Dense mapping](https://nasa.github.io/isaac/html/geometric_streaming_mapper.html) to create a textured 3D map - [Volumetric mapping](https://nasa.github.io/isaac/html/volumetric_mapper.html) to map volumetric signals, such as WiFi. - [Image analysis](https://nasa.github.io/isaac/html/ano.html) module to train a neural network to detect anomalies +- [Survey Manager](https://nasa.github.io/isaac/html/survey.html) semi-autonomous planning and execution of imaging tasks. You may also be interested in the separate repository for the [ISAAC User Interface](https://github.com/nasa/isaac_user_interface), which enables monitoring of multiple robots through a web browser. diff --git a/astrobee/readme.md b/astrobee/readme.md index 89e60ac6..3f2f50d3 100644 --- a/astrobee/readme.md +++ b/astrobee/readme.md @@ -9,4 +9,6 @@ The Astrobeemodule contains the ISAAC additions to the Astrobee FSW \subpage sim : Gazebo plugins for simulation including acoustic and heat camera, RFID reader and emmiter, as well as WiFi reader and emmiters. -\subpage gs_action_helper : Guest science action helper used to communicate with the ground through DDS messages \ No newline at end of file +\subpage gs_action_helper : Guest science action helper used to communicate with the ground through DDS messages + +\subpage survey : Semi-autonomous survey tasking of Astrobees. diff --git a/astrobee/survey_manager/readme.md b/astrobee/survey_manager/readme.md new file mode 100644 index 00000000..75b544a4 --- /dev/null +++ b/astrobee/survey_manager/readme.md @@ -0,0 +1,5 @@ +\page survey Survey Manager + +The ISAAC Survey Manager provides semi-autonomous planning and execution of panorama and stereophotography tasks to reduce operator loading and improve activity utilization. + +\subpage survey_planner : Planning and scheduling of queued survey actions. diff --git a/astrobee/survey_manager/survey_planner/behavior_trees/collecting_panoramas.xml b/astrobee/survey_manager/survey_planner/behavior_trees/collecting_panoramas.xml new file mode 100644 index 00000000..d8cb181f --- /dev/null +++ b/astrobee/survey_manager/survey_planner/behavior_trees/collecting_panoramas.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/astrobee/survey_manager/survey_planner/behavior_trees/move_to.xml b/astrobee/survey_manager/survey_planner/behavior_trees/move_to.xml new file mode 100644 index 00000000..a708b178 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/behavior_trees/move_to.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/astrobee/survey_manager/survey_planner/behavior_trees/move_to_inspect.xml b/astrobee/survey_manager/survey_planner/behavior_trees/move_to_inspect.xml new file mode 100644 index 00000000..b40db393 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/behavior_trees/move_to_inspect.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml b/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml new file mode 100644 index 00000000..680641ff --- /dev/null +++ b/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml @@ -0,0 +1,54 @@ +# Copyright (c) 2023, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking +# platform" software is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +# Example dynamic configuration info used when generating a PDDL problem. For now, this is goal +# conditions and initial state. A likely conops is that the initial version of this file for a +# specific activity would be hand-generated, but it might later be automatically regenerated by the +# survey manager when a replan is needed (remove completed/failed goals, add retry goals, update +# initial state to match actual current state, etc.) See also jem_survey_static.yaml. + +goals: + +- {type: panorama, robot: bumble, order: 0, location: jem_bay4, run: 1} +- {type: panorama, robot: bumble, order: 1, location: jem_bay3, run: 1} +- {type: panorama, robot: bumble, order: 2, location: jem_bay2, run: 1} +- {type: panorama, robot: bumble, order: 3, location: jem_bay1, run: 1} +- {type: stereo, robot: bumble, order: 4, trajectory: jem_bay1_to_bay3, run: 1} + +# We want Bumble to return to its berth at the end of the run, but adding this goal causes POPF to +# get confused and greatly increase the total run time. For some reason, it doesn't notice it can +# use the same plan as without this goal and then add some motion actions at the end to achieve this +# goal. Instead, it falls back to only undocking one robot at a time, which slows things down by +# about 2x. +# - {type: robot_at, robot: bumble, location: berth1} + +- {type: panorama, robot: honey, order: 0, location: jem_bay7, run: 1} +- {type: panorama, robot: honey, order: 1, location: jem_bay6, run: 1} +- {type: panorama, robot: honey, order: 2, location: jem_bay5, run: 1} + +# This is another objective we want to include that for some reason causes POPF to fail to generate +# a plan (hang indefinitely). No obvious reason why it should cause a problem. +# - {type: stereo, robot: honey, order: 3, trajectory: jem_bay4_to_bay7, run: 1} + +- {type: robot_at, robot: honey, location: berth2} + +init: + bumble: + location: berth1 + honey: + location: berth2 diff --git a/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml b/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml new file mode 100644 index 00000000..db617126 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml @@ -0,0 +1,59 @@ +# Copyright (c) 2023, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking +# platform" software is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +# Static configuration info used when generating a PDDL problem and also when executing actions in a +# PDDL plan. This info should be static in the sense that it nominally doesn't change during an ISS +# activity, so the survey manager doesn't have to modify it. However, an edge case is that an +# operator might want to manually edit something in here (like add a new symbolic location or nudge +# the position of a named bay away from an obstacle) and restart the survey manager. On the other +# hand, info that is *expected* to change as part of the survey manager conops belongs in +# jem_survey_dynamic.yaml. + +# Useful reference for positions and stereo survey trajectories: +# https://babelfish.arc.nasa.gov/confluence/display/FFOPS/ISAAC+Phase+1X+Activity+9+Ground+Procedure + +bays: + # 3D coordinates for symbolic bays in ISS Analysis Coordinate System used by Astrobee + jem_bay1: [11.0, -4.0, 4.8] + jem_bay2: [11.0, -5.0, 4.8] + jem_bay3: [11.0, -6.0, 4.8] + jem_bay4: [11.0, -7.0, 4.8] + jem_bay5: [11.0, -8.0, 4.8] + jem_bay6: [11.0, -9.0, 4.8] + jem_bay7: [11.0, -9.7, 4.8] + +bogus_bays: [jem_bay0, jem_bay8] +berths: [berth1, berth2] +robots: [bumble, honey] +num_orders: 10 + +stereo: + # Meta-data about stereo survey options + jem_bay1_to_bay3: + # fplan: Name of external fplan specification of trajectory in astrobee_ops/gds/plans/ISAAC/ + fplan: "jem_stereo_mapping_bay1_to_bay3.fplan" + # base_location: Where trajectory starts and ends for planning purposes (rough location, not exact) + base_location: jem_bay1 + # bound_location: The other end of the interval covered by the trajectory, for planner collision + # check purposes. (Note a trajectory may fly a bit into a bay that it doesn't claim to cover. + # The two surveys that cover the module purposefully overlap.) + bound_location: jem_bay4 + jem_bay4_to_bay7: + fplan: "jem_stereo_mapping_bay4_to_bay7.fplan" + base_location: jem_bay7 + bound_location: jem_bay4 diff --git a/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt b/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt new file mode 100644 index 00000000..6a119031 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt @@ -0,0 +1,31 @@ +$ time popf domain_survey.pddl problem_jem_survey.pddl +Constructing lookup tables: [10%] [20%] [30%] [40%] [50%] [60%] [70%] [80%] [90%] [100%] +Post filtering unreachable actions: [10%] [20%] [30%] [40%] [50%] [60%] [70%] [80%] [90%] [100%] +3% of the ground temporal actions in this problem are compression-safe +b (25.000 | 30.000)b (24.000 | 50.001)b (23.000 | 70.002)b (22.000 | 90.003)b (21.000 | 990.004)b (20.000 | 990.004)b (19.000 | 1010.005)b (18.000 | 1910.006)b (17.000 | 1910.006)b (16.000 | 1930.007)b (15.000 | 2830.008)b (14.000 | 2830.008)b (13.000 | 2850.009)b (12.000 | 3750.010)b (11.000 | 3750.010)b (10.000 | 4350.011)b (9.000 | 4350.011)b (8.000 | 4350.011)b (7.000 | 4350.011)b (6.000 | 4350.011)b (5.000 | 4350.011)b (4.000 | 5270.013)b (3.000 | 5270.013)b (2.000 | 5290.014)b (1.000 | 5310.015);;;; Solution Found +; Time 0.56 +0.000: (undock bumble berth1 jem_bay7 jem_bay8 jem_bay6) [30.000] +30.001: (move bumble jem_bay7 jem_bay6 jem_bay5) [20.000] +50.002: (move bumble jem_bay6 jem_bay5 jem_bay4) [20.000] +70.003: (move bumble jem_bay5 jem_bay4 jem_bay3) [20.000] +70.003: (undock honey berth2 jem_bay7 jem_bay8 jem_bay6) [30.000] +90.004: (panorama bumble o0 jem_bay4 run1) [900.000] +100.004: (panorama honey o0 jem_bay7 run1) [900.000] +990.005: (move bumble jem_bay4 jem_bay3 jem_bay2) [20.000] +1000.005: (move honey jem_bay7 jem_bay6 jem_bay5) [20.000] +1010.006: (panorama bumble o1 jem_bay3 run1) [900.000] +1020.006: (panorama honey o1 jem_bay6 run1) [900.000] +1910.007: (move bumble jem_bay3 jem_bay2 jem_bay1) [20.000] +1930.008: (panorama bumble o2 jem_bay2 run1) [900.000] +2830.009: (move bumble jem_bay2 jem_bay1 jem_bay0) [20.000] +2850.010: (panorama bumble o3 jem_bay1 run1) [900.000] +3750.011: (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3 run1) [600.000] +4350.012: (move honey jem_bay6 jem_bay5 jem_bay4) [20.000] +4370.013: (panorama honey o2 jem_bay5 run1) [900.000] +5270.014: (move honey jem_bay5 jem_bay6 jem_bay7) [20.000] +5290.015: (move honey jem_bay6 jem_bay7 jem_bay8) [20.000] +5310.016: (dock honey jem_bay7 berth2) [30.000] + +real 0m0.585s +user 0m0.553s +sys 0m0.032s diff --git a/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml b/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml new file mode 100644 index 00000000..d17b7e1c --- /dev/null +++ b/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml @@ -0,0 +1,200 @@ +- start_time_seconds: '0.000' + action: + type: undock + robot: bumble + duration_seconds: '30.000' +- start_time_seconds: '30.001' + action: + type: move + robot: bumble + to_name: jem_bay6 + to_pos: + - 11.0 + - -9.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '50.002' + action: + type: move + robot: bumble + to_name: jem_bay5 + to_pos: + - 11.0 + - -8.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '70.003' + action: + type: move + robot: bumble + to_name: jem_bay4 + to_pos: + - 11.0 + - -7.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '70.003' + action: + type: undock + robot: honey + duration_seconds: '30.000' +- start_time_seconds: '90.004' + action: + type: panorama + robot: bumble + location_name: jem_bay4 + location_pos: + - 11.0 + - -7.0 + - 4.8 + run: 1 + duration_seconds: '900.000' +- start_time_seconds: '100.004' + action: + type: panorama + robot: honey + location_name: jem_bay7 + location_pos: + - 11.0 + - -9.7 + - 4.8 + run: 1 + duration_seconds: '900.000' +- start_time_seconds: '990.005' + action: + type: move + robot: bumble + to_name: jem_bay3 + to_pos: + - 11.0 + - -6.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '1000.005' + action: + type: move + robot: honey + to_name: jem_bay6 + to_pos: + - 11.0 + - -9.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '1010.006' + action: + type: panorama + robot: bumble + location_name: jem_bay3 + location_pos: + - 11.0 + - -6.0 + - 4.8 + run: 1 + duration_seconds: '900.000' +- start_time_seconds: '1020.006' + action: + type: panorama + robot: honey + location_name: jem_bay6 + location_pos: + - 11.0 + - -9.0 + - 4.8 + run: 1 + duration_seconds: '900.000' +- start_time_seconds: '1910.007' + action: + type: move + robot: bumble + to_name: jem_bay2 + to_pos: + - 11.0 + - -5.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '1930.008' + action: + type: panorama + robot: bumble + location_name: jem_bay2 + location_pos: + - 11.0 + - -5.0 + - 4.8 + run: 1 + duration_seconds: '900.000' +- start_time_seconds: '2830.009' + action: + type: move + robot: bumble + to_name: jem_bay1 + to_pos: + - 11.0 + - -4.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '2850.010' + action: + type: panorama + robot: bumble + location_name: jem_bay1 + location_pos: + - 11.0 + - -4.0 + - 4.8 + run: 1 + duration_seconds: '900.000' +- start_time_seconds: '3750.011' + action: + type: stereo + robot: bumble + fplan: jem_stereo_mapping_bay1_to_bay3.fplan + run: 1 + duration_seconds: '600.000' +- start_time_seconds: '4350.012' + action: + type: move + robot: honey + to_name: jem_bay5 + to_pos: + - 11.0 + - -8.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '4370.013' + action: + type: panorama + robot: honey + location_name: jem_bay5 + location_pos: + - 11.0 + - -8.0 + - 4.8 + run: 1 + duration_seconds: '900.000' +- start_time_seconds: '5270.014' + action: + type: move + robot: honey + to_name: jem_bay6 + to_pos: + - 11.0 + - -9.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '5290.015' + action: + type: move + robot: honey + to_name: jem_bay7 + to_pos: + - 11.0 + - -9.7 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '5310.016' + action: + type: dock + robot: honey + berth: berth2 + duration_seconds: '30.000' diff --git a/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl new file mode 100644 index 00000000..e31999fd --- /dev/null +++ b/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl @@ -0,0 +1,331 @@ +(define (domain survey-manager) + (:requirements + :strips + :typing + :durative-actions + :fluents + ) + + (:types + location + robot + order + run-number + ) + + (:predicates + ;; === Static predicates === + ;; move-connected: Indicates a robot can travel from ?from to ?to using a single + ;; move action. Note that these are interpreted as directed links, so for the usual case + ;; that you can travel in either direction, you must assert the predicate both ways. + ;; Neither location can be a berth (use the dock-connected predicate for that). However, + ;; both bays 6 and 7 should be move-connected to both berth approach points. + (move-connected ?from ?to - location) + + ;; location-real: Indicates a location is a real place the robot can fly to. We've added + ;; bogus locations bay0 and bay8 to the problem instance to satisfy the implicit assumption + ;; of the collision checking that every bay has two neighbors. We assert location-real for + ;; the other locations and use it as a precondition on moves so the planner can't + ;; accidentally fly to a bogus location. + (location-real ?location - location) + + ;; dock-connected: Indicates a robot can move from ?approach to ?berth using a dock + ;; action, or from ?berth to ?approach using an undock action. + (dock-connected ?approach ?berth - location) + + ;; robots-different: Indicates a != b. Needs to be expressed as a positive predicate + ;; so it can be used as a precondition. Must be asserted in both directions. + (robots-different ?a ?b - robot) + + ;; locations-different: Indicates a != b. Needs to be expressed as a positive predicate + ;; so it can be used as a precondition. Must be asserted in both directions. + (locations-different ?a ?b - location) + + ;; === Dynamic predicates === + ;; robot-available: Since a robot can only perform one action at a time in our domain, each + ;; action grabs this mutex. In the initial state, both robots should be available. + (robot-available ?robot - robot) + + ;; robot-at: Indicates the robot's current position (usually not set during execution of + ;; motion actions). In the initial state, both robots should have robot-at set for their + ;; initial locations. + (robot-at ?robot - robot ?location - location) + + ;; location-available: Indicates that no robot has reserved the location. When stationary, + ;; each robot reserves its current location. During a move, it reserves both its ?from + ;; location and its ?to location. Collision avoidance checks prevent robots from reserving + ;; the same location or reserving adjacent bays while flying. Note: It might be more natural + ;; to express this as a location-reserved predicate with the opposite boolean sense, but we + ;; need it to be this way so we can use it as a precondition without negating it. In the + ;; initial state, we must mark location-available for all locations, real or bogus, except + ;; the initial robot locations. + (location-available ?location - location) + + ;; need-stereo: If you add a completed-stereo goal, you must also add a need-stereo + ;; predicate with identical parameters to the initial state. This is part of a hack that + ;; greatly improves planner performance. The need-stereo predicate has been made part of the + ;; preconditions of the stereo action, and one of its effects is to clear the + ;; predicate. Therefore, the planner won't waste time trying to execute stereo actions that + ;; the user didn't explicitly request. Without this hack, the planner run time blows up. + (need-stereo ?robot - robot ?order - order ?base ?bound - location ?run-number - run-number) + + ;; === Goal predicates === + ;; completed-panorama: The goal to add if you want the plan to include collecting a + ;; panorama. For now, goals specify ?robot and ?order parameters that constrain + ;; multi-robot task allocation and task ordering. The ?run-number is used to indicate + ;; retries and is meaningless to the planner but helpful for post-run analysis. + (completed-panorama + ?robot - robot + ?order - order + ?location - location + ?run-number - run-number + ) + + ;; completed-stereo: The goal to add if you want the plan to include collecting a stereo + ;; survey. For now, goals specify ?robot and ?order parameters that constrain multi-robot + ;; task allocation and task ordering. The ?run-number is used to indicate retries and is + ;; meaningless to the planner but helpful for post-run analysis. The current model for + ;; stereo surveys assumes the robot starts and ends the survey at the same location called + ;; ?base (these locations only need to be same to the effective precision modeled in the + ;; planner, "less than a bay apart"). The ?bound argument indicates the other end of the + ;; interval covered by the survey and is used for collision checking. It's assumed that + ;; ?base and ?bound are not adjacent locations. If future stereo surveys violate these + ;; assumptions the model will need to be revisited. + (completed-stereo + ?robot - robot + ?order - order + ?base ?bound - location + ?run-number - run-number + ) + ) + + (:functions + ;; === Static numeric fluents === + ;; order-identity: An identity operator that maps from a symbolic order like o0 to its + ;; corresponding numeric value 0. + (order-identity ?order - order) + + ;; === Dynamic numeric fluents === + ;; robot-order: Indicates the order of the last action executed by ?robot. Later actions + ;; must not have a lower ?order (this only applies to the panorama and stereo actions that + ;; take an ?order parameter). In the initial state, each robot must have order -1. + (robot-order ?robot - robot) + ) + + (:durative-action dock + :parameters (?robot - robot ?from ?to - location) ;; from bay7 to berth1 or berth2 + :duration (= ?duration 30) + :condition + (and + ;; Check robot mutex + (at start (robot-available ?robot)) + + ;; Check parameters make sense + (at start (robot-at ?robot ?from)) + (at start (dock-connected ?from ?to)) + + ;; Check collision avoidance + (at start (location-available ?to)) + ; Don't need to check berth neighbors + ) + :effect + (and + ;; Grab and release robot mutex + (at start (not (robot-available ?robot))) + (at end (robot-available ?robot)) + + ;; Grab and release reserved locations + (at start (not (location-available ?to))) + (at end (location-available ?from)) + + ;; Update robot location + (at start (not (robot-at ?robot ?from))) + (at end (robot-at ?robot ?to)) + ) + ) + + (:durative-action undock + :parameters ( + ?robot - robot + ?from ?to - location ;; from berth1 or berth2 to bay7 + ?check1 ?check2 - location ;; neighbors of ?to to check for collision avoidance + ) + :duration (= ?duration 30) + :condition + (and + ;; Check robot mutex + (at start (robot-available ?robot)) + + ;; Check parameters make sense + (at start (robot-at ?robot ?from)) + (at start (dock-connected ?to ?from)) + (at start (location-real ?to)) + + ;(at start (robot-can-undock-now ?robot)) + + ;; Check collision avoidance + (at start (location-available ?to)) + (at start (locations-different ?check1 ?check2)) + (at start (move-connected ?check1 ?to)) + (at start (move-connected ?check2 ?to)) + (at start (location-available ?check1)) + (at start (location-available ?check2)) + ) + :effect + (and + ;; Grab and release robot mutex + (at start (not (robot-available ?robot))) + (at end (robot-available ?robot)) + + ;; Grab and release reserved locations + (at start (not (location-available ?to))) + (at end (location-available ?from)) + + ;; Update robot location + (at start (not (robot-at ?robot ?from))) + (at end (robot-at ?robot ?to)) + ) + ) + + (:durative-action move + :parameters ( + ?robot - robot + ?from ?to - location + ?check - location ;; neighbor of ?to to check for collision avoidance + ) + :duration (= ?duration 20) + :condition + (and + ;; Check robot mutex + (at start (robot-available ?robot)) + + ;; Check parameters make sense + (at start (robot-at ?robot ?from)) + (at start (move-connected ?from ?to)) + (at start (location-real ?to)) + + ;; Check collision avoidance + (at start (location-available ?to)) + ;; In general when flying to ?to we collision check whether ?to or either of its + ;; neighbors are reserved (by the other robot). In this case, one of the neighbors + ;; of ?to is ?from, which can't be reserved by the other robot since this robot + ;; previously reserved it. Therefore, we only need to check if the other neighbor + ;; (?check) is reserved. + (at start (locations-different ?check ?from)) + (at start (move-connected ?check ?to)) + (at start (location-available ?check)) + ) + :effect + (and + ;; Grab and release robot mutex + (at start (not (robot-available ?robot))) + (at end (robot-available ?robot)) + + ;; Grab and release reserved locations + (at start (not (location-available ?to))) + (at end (location-available ?from)) + + ;; Update robot location + (at start (not (robot-at ?robot ?from))) + (at end (robot-at ?robot ?to)) + ) + ) + + (:durative-action panorama + :parameters + ( + ?robot - robot + ?order - order + ?location - location + ?run-number - run-number + ) + ;; ~13 minutes, per https://babelfish.arc.nasa.gov/confluence/display/FFOPS/ISAAC+Phase+1X+Activity+9+Ground+Procedure + :duration (= ?duration 780) + :condition + (and + ;; Check robot mutex + (at start (robot-available ?robot)) + + ;; Check order + (at start (< (robot-order ?robot) (order-identity ?order))) + + ;; Check parameters make sense + (at start (robot-at ?robot ?location)) + ) + :effect + (and + ;; Grab and release robot mutex + (at start (not (robot-available ?robot))) + (at end (robot-available ?robot)) + + ;; Update order + (at end (assign (robot-order ?robot) (order-identity ?order))) + + ;; Mark success + (at end (completed-panorama ?robot ?order ?location ?run-number)) + ) + ) + + (:durative-action stereo + :parameters + ( + ?robot - robot + ?order - order + ;; ?base: The bay where we start and also stop. ?bound: The other end of the survey + ?base ?bound - location + ;; ?check1 and ?check2: Planner-selected neighbors of ?bound for collision check + ?check1 ?check2 - location + ?run-number - run-number + ) + :duration (= ?duration 600) ;; 10 minutes + :condition + (and + ;; Check robot mutex + (at start (robot-available ?robot)) + + ;; Check order + (at start (< (robot-order ?robot) (order-identity ?order))) + + ;; Check parameters make sense + (at start (robot-at ?robot ?base)) + (at start (location-real ?bound)) + + ;; Check for need-stereo so the planner only tries this action when the user + ;; explicitly requests it. + (at start (need-stereo ?robot ?order ?base ?bound ?run-number)) + + ;; Check collision avoidance + (at start (location-available ?bound)) + (at start (locations-different ?check1 ?check2)) + (at start (move-connected ?check1 ?bound)) + (at start (move-connected ?check2 ?bound)) + (at start (location-available ?check1)) + (at start (location-available ?check2)) + ) + :effect + (and + ;; Grab and release robot mutex + (at start (not (robot-available ?robot))) + (at end (robot-available ?robot)) + + ;; Update order + (at end (assign (robot-order ?robot) (order-identity ?order))) + + ;; Grab and release reserved locations + (at start (not (location-available ?bound))) + (at end (location-available ?bound)) + + ;; Update robot location - technically correct but not really needed + ;(at start (not (robot-at ?robot ?base))) + ;(at end (robot-at ?robot ?base)) + + ;; Clear need-stereo so the planner won't try to use the stereo action + ;; again after the user request is satisfied. + (at end (not (need-stereo ?robot ?order ?base ?bound ?run-number))) + + ;; Mark success + (at end (completed-stereo ?robot ?order ?base ?bound ?run-number)) + ) + ) + +) diff --git a/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl b/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl new file mode 100644 index 00000000..313ca28f --- /dev/null +++ b/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl @@ -0,0 +1,57 @@ +{{ header }} +(define (problem jem-survey) + (:domain survey-manager) + (:metric minimize (total-time)) + (:objects + {{ bays }} {{ berths }} - location + {{ robots }} - robot + {{ orders }} - order + run1 run2 run3 run4 run5 - run-number + ) + + (:goal + (and + {{ goals }} + ) + ) + + (:init + ;; === Static predicates === + {{ move_connected_predicates }} + + {{ location_real_predicates }} + + {{ dock_connected_predicates }} + + {{ robots_different_predicates }} + + {{ locations_different_predicates }} + + ;; === Dynamic predicates === + {{ robot_available_predicates }} + + {{ robot_at_predicates }} + + {{ location_available_predicates }} + + ;; need-stereo predicates must be asserted with identical parameters to the + ;; stereo-completed goals. See the need-stereo docs for more. + {{ need_stereo_predicates }} + + ;; === Static numeric fluents === + {{ order_identity_fluents }} + + ;; === Dynamic numeric fluents === + {{ robot_order_fluents }} + ) ;; end :init +) ;; end problem + +;; Include raw high-level config in problem in case an (ISAAC-custom) planner prefers to use it. + +;; BEGIN CONFIG_DYNAMIC +{{ config_dynamic }} +;; END CONFIG_DYNAMIC + +;; BEGIN CONFIG_STATIC +{{ config_static }} +;; END CONFIG_STATIC diff --git a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl new file mode 100644 index 00000000..301f8c05 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl @@ -0,0 +1,296 @@ +;; Auto-generated by problem_generator.py. Do not edit! +;; Problem template: jem_survey_template.pddl +;; Dynamic config: jem_survey_dynamic.yaml +;; Static config: jem_survey_static.yaml + +(define (problem jem-survey) + (:domain survey-manager) + (:metric minimize (total-time)) + (:objects + jem_bay0 jem_bay1 jem_bay2 jem_bay3 jem_bay4 jem_bay5 jem_bay6 jem_bay7 jem_bay8 berth1 berth2 - location + bumble honey - robot + o0 o1 o2 o3 o4 o5 o6 o7 o8 o9 - order + run1 run2 run3 run4 run5 - run-number + ) + + (:goal + (and + (completed-panorama bumble o0 jem_bay4 run1) + (completed-panorama bumble o1 jem_bay3 run1) + (completed-panorama bumble o2 jem_bay2 run1) + (completed-panorama bumble o3 jem_bay1 run1) + (completed-stereo bumble o4 jem_bay1 jem_bay4 run1) + (completed-panorama honey o0 jem_bay7 run1) + (completed-panorama honey o1 jem_bay6 run1) + (completed-panorama honey o2 jem_bay5 run1) + (robot-at honey berth2) + ) + ) + + (:init + ;; === Static predicates === + (move-connected jem_bay0 jem_bay1) + (move-connected jem_bay1 jem_bay0) + (move-connected jem_bay1 jem_bay2) + (move-connected jem_bay2 jem_bay1) + (move-connected jem_bay2 jem_bay3) + (move-connected jem_bay3 jem_bay2) + (move-connected jem_bay3 jem_bay4) + (move-connected jem_bay4 jem_bay3) + (move-connected jem_bay4 jem_bay5) + (move-connected jem_bay5 jem_bay4) + (move-connected jem_bay5 jem_bay6) + (move-connected jem_bay6 jem_bay5) + (move-connected jem_bay6 jem_bay7) + (move-connected jem_bay7 jem_bay6) + (move-connected jem_bay7 jem_bay8) + (move-connected jem_bay8 jem_bay7) + + (location-real jem_bay1) + (location-real jem_bay2) + (location-real jem_bay3) + (location-real jem_bay4) + (location-real jem_bay5) + (location-real jem_bay6) + (location-real jem_bay7) + + (dock-connected jem_bay7 berth1) + (dock-connected jem_bay7 berth2) + + (robots-different bumble honey) + (robots-different honey bumble) + + (locations-different jem_bay0 jem_bay1) + (locations-different jem_bay0 jem_bay2) + (locations-different jem_bay0 jem_bay3) + (locations-different jem_bay0 jem_bay4) + (locations-different jem_bay0 jem_bay5) + (locations-different jem_bay0 jem_bay6) + (locations-different jem_bay0 jem_bay7) + (locations-different jem_bay0 jem_bay8) + (locations-different jem_bay1 jem_bay0) + (locations-different jem_bay1 jem_bay2) + (locations-different jem_bay1 jem_bay3) + (locations-different jem_bay1 jem_bay4) + (locations-different jem_bay1 jem_bay5) + (locations-different jem_bay1 jem_bay6) + (locations-different jem_bay1 jem_bay7) + (locations-different jem_bay1 jem_bay8) + (locations-different jem_bay2 jem_bay0) + (locations-different jem_bay2 jem_bay1) + (locations-different jem_bay2 jem_bay3) + (locations-different jem_bay2 jem_bay4) + (locations-different jem_bay2 jem_bay5) + (locations-different jem_bay2 jem_bay6) + (locations-different jem_bay2 jem_bay7) + (locations-different jem_bay2 jem_bay8) + (locations-different jem_bay3 jem_bay0) + (locations-different jem_bay3 jem_bay1) + (locations-different jem_bay3 jem_bay2) + (locations-different jem_bay3 jem_bay4) + (locations-different jem_bay3 jem_bay5) + (locations-different jem_bay3 jem_bay6) + (locations-different jem_bay3 jem_bay7) + (locations-different jem_bay3 jem_bay8) + (locations-different jem_bay4 jem_bay0) + (locations-different jem_bay4 jem_bay1) + (locations-different jem_bay4 jem_bay2) + (locations-different jem_bay4 jem_bay3) + (locations-different jem_bay4 jem_bay5) + (locations-different jem_bay4 jem_bay6) + (locations-different jem_bay4 jem_bay7) + (locations-different jem_bay4 jem_bay8) + (locations-different jem_bay5 jem_bay0) + (locations-different jem_bay5 jem_bay1) + (locations-different jem_bay5 jem_bay2) + (locations-different jem_bay5 jem_bay3) + (locations-different jem_bay5 jem_bay4) + (locations-different jem_bay5 jem_bay6) + (locations-different jem_bay5 jem_bay7) + (locations-different jem_bay5 jem_bay8) + (locations-different jem_bay6 jem_bay0) + (locations-different jem_bay6 jem_bay1) + (locations-different jem_bay6 jem_bay2) + (locations-different jem_bay6 jem_bay3) + (locations-different jem_bay6 jem_bay4) + (locations-different jem_bay6 jem_bay5) + (locations-different jem_bay6 jem_bay7) + (locations-different jem_bay6 jem_bay8) + (locations-different jem_bay7 jem_bay0) + (locations-different jem_bay7 jem_bay1) + (locations-different jem_bay7 jem_bay2) + (locations-different jem_bay7 jem_bay3) + (locations-different jem_bay7 jem_bay4) + (locations-different jem_bay7 jem_bay5) + (locations-different jem_bay7 jem_bay6) + (locations-different jem_bay7 jem_bay8) + (locations-different jem_bay8 jem_bay0) + (locations-different jem_bay8 jem_bay1) + (locations-different jem_bay8 jem_bay2) + (locations-different jem_bay8 jem_bay3) + (locations-different jem_bay8 jem_bay4) + (locations-different jem_bay8 jem_bay5) + (locations-different jem_bay8 jem_bay6) + (locations-different jem_bay8 jem_bay7) + + ;; === Dynamic predicates === + (robot-available bumble) + (robot-available honey) + + (robot-at bumble berth1) + (robot-at honey berth2) + + (location-available jem_bay0) + (location-available jem_bay1) + (location-available jem_bay2) + (location-available jem_bay3) + (location-available jem_bay4) + (location-available jem_bay5) + (location-available jem_bay6) + (location-available jem_bay7) + (location-available jem_bay8) + + ;; need-stereo predicates must be asserted with identical parameters to the + ;; stereo-completed goals. See the need-stereo docs for more. + (need-stereo bumble o4 jem_bay1 jem_bay4 run1) + + ;; === Static numeric fluents === + (= (order-identity o0) 0) + (= (order-identity o1) 1) + (= (order-identity o2) 2) + (= (order-identity o3) 3) + (= (order-identity o4) 4) + (= (order-identity o5) 5) + (= (order-identity o6) 6) + (= (order-identity o7) 7) + (= (order-identity o8) 8) + (= (order-identity o9) 9) + + ;; === Dynamic numeric fluents === + (= (robot-order bumble) -1) + (= (robot-order honey) -1) + ) ;; end :init +) ;; end problem + +;; Include raw high-level config in problem in case an (ISAAC-custom) planner prefers to use it. + +;; BEGIN CONFIG_DYNAMIC +;; # Copyright (c) 2023, United States Government, as represented by the +;; # Administrator of the National Aeronautics and Space Administration. +;; # +;; # All rights reserved. +;; # +;; # The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking +;; # platform" software is licensed under the Apache License, Version 2.0 +;; # (the "License"); you may not use this file except in compliance with the +;; # License. You may obtain a copy of the License at +;; # +;; # http://www.apache.org/licenses/LICENSE-2.0 +;; # +;; # Unless required by applicable law or agreed to in writing, software +;; # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +;; # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +;; # License for the specific language governing permissions and limitations +;; # under the License. +;; +;; # Example dynamic configuration info used when generating a PDDL problem. For now, this is goal +;; # conditions and initial state. A likely conops is that the initial version of this file for a +;; # specific activity would be hand-generated, but it might later be automatically regenerated by the +;; # survey manager when a replan is needed (remove completed/failed goals, add retry goals, update +;; # initial state to match actual current state, etc.) See also jem_survey_static.yaml. +;; +;; goals: +;; +;; - {type: panorama, robot: bumble, order: 0, location: jem_bay4, run: 1} +;; - {type: panorama, robot: bumble, order: 1, location: jem_bay3, run: 1} +;; - {type: panorama, robot: bumble, order: 2, location: jem_bay2, run: 1} +;; - {type: panorama, robot: bumble, order: 3, location: jem_bay1, run: 1} +;; - {type: stereo, robot: bumble, order: 4, trajectory: jem_bay1_to_bay3, run: 1} +;; +;; # We want Bumble to return to its berth at the end of the run, but adding this goal causes POPF to +;; # get confused and greatly increase the total run time. For some reason, it doesn't notice it can +;; # use the same plan as without this goal and then add some motion actions at the end to achieve this +;; # goal. Instead, it falls back to only undocking one robot at a time, which slows things down by +;; # about 2x. +;; # - {type: robot_at, robot: bumble, location: berth1} +;; +;; - {type: panorama, robot: honey, order: 0, location: jem_bay7, run: 1} +;; - {type: panorama, robot: honey, order: 1, location: jem_bay6, run: 1} +;; - {type: panorama, robot: honey, order: 2, location: jem_bay5, run: 1} +;; +;; # This is another objective we want to include that for some reason causes POPF to fail to generate +;; # a plan (hang indefinitely). No obvious reason why it should cause a problem. +;; # - {type: stereo, robot: honey, order: 3, trajectory: jem_bay4_to_bay7, run: 1} +;; +;; - {type: robot_at, robot: honey, location: berth2} +;; +;; init: +;; bumble: +;; location: berth1 +;; honey: +;; location: berth2 +;; +;; END CONFIG_DYNAMIC + +;; BEGIN CONFIG_STATIC +;; # Copyright (c) 2023, United States Government, as represented by the +;; # Administrator of the National Aeronautics and Space Administration. +;; # +;; # All rights reserved. +;; # +;; # The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking +;; # platform" software is licensed under the Apache License, Version 2.0 +;; # (the "License"); you may not use this file except in compliance with the +;; # License. You may obtain a copy of the License at +;; # +;; # http://www.apache.org/licenses/LICENSE-2.0 +;; # +;; # Unless required by applicable law or agreed to in writing, software +;; # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +;; # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +;; # License for the specific language governing permissions and limitations +;; # under the License. +;; +;; # Static configuration info used when generating a PDDL problem and also when executing actions in a +;; # PDDL plan. This info should be static in the sense that it nominally doesn't change during an ISS +;; # activity, so the survey manager doesn't have to modify it. However, an edge case is that an +;; # operator might want to manually edit something in here (like add a new symbolic location or nudge +;; # the position of a named bay away from an obstacle) and restart the survey manager. On the other +;; # hand, info that is *expected* to change as part of the survey manager conops belongs in +;; # jem_survey_dynamic.yaml. +;; +;; # Useful reference for positions and stereo survey trajectories: +;; # https://babelfish.arc.nasa.gov/confluence/display/FFOPS/ISAAC+Phase+1X+Activity+9+Ground+Procedure +;; +;; bays: +;; # 3D coordinates for symbolic bays in ISS Analysis Coordinate System used by Astrobee +;; jem_bay1: [11.0, -4.0, 4.8] +;; jem_bay2: [11.0, -5.0, 4.8] +;; jem_bay3: [11.0, -6.0, 4.8] +;; jem_bay4: [11.0, -7.0, 4.8] +;; jem_bay5: [11.0, -8.0, 4.8] +;; jem_bay6: [11.0, -9.0, 4.8] +;; jem_bay7: [11.0, -9.7, 4.8] +;; +;; bogus_bays: [jem_bay0, jem_bay8] +;; berths: [berth1, berth2] +;; robots: [bumble, honey] +;; num_orders: 10 +;; +;; stereo: +;; # Meta-data about stereo survey options +;; jem_bay1_to_bay3: +;; # fplan: Name of external fplan specification of trajectory in astrobee_ops/gds/plans/ISAAC/ +;; fplan: "jem_stereo_mapping_bay1_to_bay3.fplan" +;; # base_location: Where trajectory starts and ends for planning purposes (rough location, not exact) +;; base_location: jem_bay1 +;; # bound_location: The other end of the interval covered by the trajectory, for planner collision +;; # check purposes. (Note a trajectory may fly a bit into a bay that it doesn't claim to cover. +;; # The two surveys that cover the module purposefully overlap.) +;; bound_location: jem_bay4 +;; jem_bay4_to_bay7: +;; fplan: "jem_stereo_mapping_bay4_to_bay7.fplan" +;; base_location: jem_bay7 +;; bound_location: jem_bay4 +;; +;; END CONFIG_STATIC diff --git a/astrobee/survey_manager/survey_planner/readme.md b/astrobee/survey_manager/survey_planner/readme.md new file mode 100644 index 00000000..6fd77ac8 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/readme.md @@ -0,0 +1,5 @@ +\page survey_planner Survey Planner + +Planning and scheduling of queued survey actions using Playsys2 for PDDL solutions and behavior trees for execution. + +Based on [preliminary work](https://github.com/traclabs/astrobee_task_planning_ws) by [Ana](https://github.com/ana-GT) at [Traclabs](https://traclabs.com). diff --git a/astrobee/survey_manager/survey_planner/src/ros1_lifecycle b/astrobee/survey_manager/survey_planner/src/ros1_lifecycle new file mode 160000 index 00000000..15a50352 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/src/ros1_lifecycle @@ -0,0 +1 @@ +Subproject commit 15a50352f9d522c7815ecedaa2b77b6f99ecdb31 diff --git a/astrobee/survey_manager/survey_planner/src/ros2_planning_system b/astrobee/survey_manager/survey_planner/src/ros2_planning_system new file mode 160000 index 00000000..3a988a5b --- /dev/null +++ b/astrobee/survey_manager/survey_planner/src/ros2_planning_system @@ -0,0 +1 @@ +Subproject commit 3a988a5bc14a1700a8f276e2438c47ac80e7bf8c diff --git a/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py b/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py new file mode 100755 index 00000000..ce98e096 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py @@ -0,0 +1,251 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2023, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking +# platform" software is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +Provides an example of how to interpret the actions in a survey domain plan, dropping the irrelevant +arguments, mapping the positional arguments back to the names used in the dynamic config, and +looking up extra info that's not needed by the planner but is important for execution (e.g., +coordinates of named locations, fplan filenames of stereo surveys). + +For testing, you can call this from the command line on a complete sample output plan. + +For use during execution, if you are receiving one action at a time from an executor, you may +prefer to import this module and call the yaml_action_from_pddl() function directly. +""" + +import argparse +import pathlib +import re +import sys +from typing import Any, Dict, List + +import yaml + +ACTION_TYPE_OPTIONS = ("dock", "undock", "move", "panorama", "stereo") + + +# Type alias +YamlMapping = Dict[str, Any] +FloatStr = str # String representation of a floating-point value + +# POPF plan output weirdly mixes its random debugging output with the actual plan actions that it +# outputs in a standard format. (Maybe there's some way to suppress the debug info?) Anyway, this +# regex works nicely to ignore the debug info and parse the fields of the plan actions we care +# about. It might need fine-tuning if we use other planners. +PLAN_ACTION_REGEX = re.compile( + r"^(?P\d+(\.\d*)?): (?P\(.*?\))\s+\[(?P\d+(\.\d*)?)\]\s*$" +) + + +def load_yaml(yaml_path: pathlib.Path) -> YamlMapping: + """ + Return the YAML parse result for the file at `yaml_path`. + """ + with yaml_path.open(encoding="utf-8") as yaml_stream: + return yaml.safe_load(yaml_stream) + + +class PlanAction: + """ + Class representing one entry in the output plan sequence of a PDDL planner. + """ + + def __init__( + self, start_time_seconds: FloatStr, action: str, duration_seconds: FloatStr + ): + self.start_time_seconds = start_time_seconds + self.action = action + self.duration_seconds = duration_seconds + + def __repr__(self): + return f"{self.start_time_seconds}: {self.action} [{self.duration_seconds}]" + + +def yaml_action_from_pddl(action: str, static_config: YamlMapping) -> YamlMapping: + """ + Return a YamlMapping representation of `action`. This is the only place + we really need domain-specific logic. + """ + action_args = action[1:-1].split() + action_type = action_args[0] + assert ( + action_type in ACTION_TYPE_OPTIONS + ), f"Expected action type in {ACTION_TYPE_OPTIONS}, got {action_type}" + + if action_type == "dock": + robot, _from_bay, to_berth = action_args[1:] + # Can discard from_bay + return {"type": "dock", "robot": robot, "berth": to_berth} + + if action_type == "undock": + robot, _from_berth, _to_bay, _check1, _check2 = action_args[1:] + # Can discard from_berth, to_bay, check1, check2 + return {"type": "undock", "robot": robot} + + if action_type == "move": + robot, _from_bay, to_bay, _check_bay = action_args[1:] + # Can discard from_bay, check_bay. Look up coordinates for to_bay. + return { + "type": "move", + "robot": robot, + "to_name": to_bay, + "to_pos": static_config["bays"][to_bay], + } + + if action_type == "panorama": + robot, _order, location, run_name = action_args[1:] + run_number = int(run_name[-1]) + # Can discard order. Look up coordinates for location. + return { + "type": "panorama", + "robot": robot, + "location_name": location, + "location_pos": static_config["bays"][location], + "run": run_number, + } + + if action_type == "stereo": + robot, _order, base, bound, _check1, _check2, run_name = action_args[1:] + run_number = int(run_name[-1]) + # Use base and bound to look up trajectory. + traj_matches = [ + traj + for traj in static_config["stereo"].values() + if traj["base_location"] == base and traj["bound_location"] == bound + ] + assert ( + len(traj_matches) == 1 + ), f"Expected exactly 1 matching stereo trajectory with base {base} and bound {bound}, got {len(traj_matches)}" + fplan = traj_matches[0]["fplan"] + # Can discard order, base, bound, check1, check2. + return {"type": "stereo", "robot": robot, "fplan": fplan, "run": run_number} + + assert False, "Never reach this point." + return {} # Make pylint happy + + +def yaml_plan_action_from_pddl( + plan_action: PlanAction, static_config: YamlMapping +) -> YamlMapping: + """ + Return a YamlMapping representation of `plan_action`. + """ + return { + "start_time_seconds": plan_action.start_time_seconds, + "action": yaml_action_from_pddl(plan_action.action, static_config), + "duration_seconds": plan_action.duration_seconds, + } + + +def parse_plan(plan_path: pathlib.Path) -> List[PlanAction]: + """ + Return a list of PlanActions read from the PDDL plan at `plan_path`. + """ + actions = [] + with plan_path.open(encoding="utf-8") as plan_stream: + for plan_line in plan_stream: + match = PLAN_ACTION_REGEX.search(plan_line) + if match: + start_time_seconds = match.group("start_time_seconds") + action = match.group("action") + duration_seconds = match.group("duration_seconds") + + # Raise an exception if these values are not the string representation of a + # floating-point value. However, we are storing the original string representation + # rather than storing the float to avoid any weird issues with the values getting + # mangled due to floating-point precision and formatting. + float(start_time_seconds) + float(duration_seconds) + + actions.append(PlanAction(start_time_seconds, action, duration_seconds)) + return actions + + +class NoAliasDumper( + yaml.SafeDumper +): # pylint: disable=too-many-ancestors # It only has 1, so (?) + """ + Configures yaml dump output to avoid using confusing aliases. + """ + + def ignore_aliases(self, data): + return True + + +def plan_interpreter( + config_static_path: pathlib.Path, plan_path: pathlib.Path, output_path: pathlib.Path +) -> None: + """ + The main function that interprets an entire plan file. + + However, if you are receiving one action at a time from an executor, you may prefer to import + this module and call yaml_action_from_pddl() directly. + """ + config_static = load_yaml(config_static_path) + pddl_actions = parse_plan(plan_path) + yaml_actions = [ + yaml_plan_action_from_pddl(plan_action, config_static) + for plan_action in pddl_actions + ] + with output_path.open("w", encoding="utf-8") as output_stream: + yaml.dump(yaml_actions, output_stream, Dumper=NoAliasDumper, sort_keys=False) + print(f"Wrote to {output_path}", file=sys.stderr) + + +class CustomFormatter( + argparse.ArgumentDefaultsHelpFormatter, argparse.RawDescriptionHelpFormatter +): + pass + + +def main(): + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=CustomFormatter + ) + parser.add_argument( + "--config-static", + help="Path to input static problem config YAML (module geometry, available stereo surveys, etc.)", + type=pathlib.Path, + default="jem_survey_static.yaml", + ) + parser.add_argument( + "--plan", + help="Path to input plan generated by PDDL planner (parser currently tuned for POPF idiosyncrasies)", + type=pathlib.Path, + default="sample_output_plan.txt", + ) + parser.add_argument( + "-o", + "--output", + help="Path for output converted to YAML format", + type=pathlib.Path, + default="sample_output_plan.yaml", + ) + args = parser.parse_args() + + plan_interpreter( + config_static_path=args.config_static, + plan_path=args.plan, + output_path=args.output, + ) + + +if __name__ == "__main__": + main() diff --git a/astrobee/survey_manager/survey_planner/tools/problem_generator.py b/astrobee/survey_manager/survey_planner/tools/problem_generator.py new file mode 100755 index 00000000..7b5b2b31 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/tools/problem_generator.py @@ -0,0 +1,307 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2023, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking +# platform" software is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +Generates a PDDL problem specification for the survey domain in domain_survey.pddl. Takes as input +a PDDL problem template and higher-level static and dynamic configuration specified in YAML. + +The generator takes care of error-prone repetitive tasks like asserting predicates for which +locations are different, which robots are different, asserting a need-stereo predicate for every +completed-stereo goal, making the initial location-available predicates agree with the initial robot +locations, etc. +""" + +import argparse +import itertools +import pathlib +import re +import sys +from typing import Any, Dict, Iterable, Sequence, T, Tuple + +import yaml + +GOAL_TYPE_OPTIONS = ("panorama", "stereo", "robot_at") + + +# Type alias +YamlMapping = Dict[str, Any] + +# Replace the text "{{ foo }}" in the template with the value of the foo parameter +TEMPLATE_SUBST_REGEX = re.compile(r"{{\s*([\w]+)\s*}}") + + +def load_yaml(yaml_path: pathlib.Path) -> YamlMapping: + """ + Return the YAML parse result for the file at `yaml_path`. + """ + with yaml_path.open(encoding="utf-8") as yaml_stream: + return yaml.safe_load(yaml_stream) + + +def pddl_goal_from_yaml(goal: YamlMapping, config_static: YamlMapping) -> str: + """ + Convert a YAML goal with named fields from the dynamic config into a PDDL goal predicate. + """ + goal_type = goal["type"] + assert ( + goal_type in GOAL_TYPE_OPTIONS + ), f"Expected goal type in {GOAL_TYPE_OPTIONS}, got {goal_type}" + + if goal_type == "panorama": + robot = goal["robot"] + order = goal["order"] + location = goal["location"] + run = goal["run"] + return f"(completed-panorama {robot} o{order} {location} run{run})" + + if goal_type == "stereo": + robot = goal["robot"] + order = goal["order"] + trajectory = goal["trajectory"] + run = goal["run"] + traj_info = config_static["stereo"][trajectory] + base = traj_info["base_location"] + bound = traj_info["bound_location"] + return f"(completed-stereo {robot} o{order} {base} {bound} run{run})" + + if goal_type == "robot_at": + robot = goal["robot"] + location = goal["location"] + return f"(robot-at {robot} {location})" + + assert False, "Never reach this point" + return {} # Make pylint happy + + +def indent_lines(lines: Sequence[str], indent: int) -> str: + """ + Return `lines` joined with carriage returns, prepending `indent` spaces to each line after the + first. + """ + sep = "\n" + (" " * indent) + return sep.join(lines) + + +def pairwise(elts: Iterable[T]) -> Iterable[Tuple[T, T]]: + """ + Returns consecutive pairs drawn from `elts`. Back-port itertools.pairwise() to earlier Python + versions. + """ + a, b = itertools.tee(elts) + next(b, None) + return zip(a, b) + + +def both_ways(elts: Iterable[Tuple[T, T]]) -> Iterable[Tuple[T, T]]: + """ + Given `elts` an iterable of 2-tuples, return a new iterable that includes each 2-tuple twice, + once in its original order and once reversed. + """ + for a, b in elts: + yield a, b + yield b, a + + +def distinct_pairs(elts: Iterable[T]) -> Iterable[Tuple[T, T]]: + """ + Return distinct pairs of items drawn from `elts`. + """ + for a, b in itertools.product(elts, repeat=2): + if a != b: + yield a, b + + +class TemplateFiller: + """ + Class for substituting parameters into a template. + """ + + def __init__(self, params): + self.params = params + + def __call__(self, match: re.match) -> str: + param = match.group(1) + if param not in self.params: + raise KeyError( + "Expected template param in {self.params.keys()}, got {{{{ {param} }}}}" + ) + return self.params[param] + + +def comment_for_pddl(text: str) -> str: + """ + Return the result of commenting `text` using PDDL (Lisp-like) comment syntax. + """ + return ";; " + text.replace("\n", "\n;; ") + + +def problem_generator( + problem_template_path: pathlib.Path, + config_dynamic_path: pathlib.Path, + config_static_path: pathlib.Path, + output_path: pathlib.Path, +) -> None: + """ + The main function that generates the problem. + """ + problem_template = problem_template_path.read_text() + config_dynamic = load_yaml(config_dynamic_path) + config_static = load_yaml(config_static_path) + params = {} + + params["header"] = ( + ";; Auto-generated by problem_generator.py. Do not edit!\n" + f";; Problem template: {problem_template_path}\n" + f";; Dynamic config: {config_dynamic_path}\n" + f";; Static config: {config_static_path}\n" + ) + + bays = list(config_static["bays"].keys()) + bogus_bays = config_static["bogus_bays"] + all_bays = sorted(bays + bogus_bays) + params["bays"] = " ".join(all_bays) + + berths = config_static["berths"] + params["berths"] = " ".join(berths) + + robots = config_static["robots"] + params["robots"] = " ".join(robots) + + num_orders = config_static["num_orders"] + params["orders"] = " ".join([f"o{i}" for i in range(num_orders)]) + + yaml_goals = config_dynamic["goals"] + pddl_goals = [pddl_goal_from_yaml(goal, config_static) for goal in yaml_goals] + params["goals"] = indent_lines(pddl_goals, 12) + + move_connected_lines = [ + f"(move-connected {a} {b})" for a, b in both_ways(pairwise(all_bays)) + ] + params["move_connected_predicates"] = indent_lines(move_connected_lines, 8) + + location_real_lines = [f"(location-real {bay})" for bay in bays] + params["location_real_predicates"] = indent_lines(location_real_lines, 8) + + candidates = (("jem_bay7", "berth1"), ("jem_bay7", "berth2")) + dock_connected_lines = [ + f"(dock-connected {bay} {berth})" + for bay, berth in candidates + if bay in bays and berth in berths + ] + params["dock_connected_predicates"] = indent_lines(dock_connected_lines, 8) + + robots_different_lines = [ + f"(robots-different {a} {b})" for a, b in distinct_pairs(robots) + ] + params["robots_different_predicates"] = indent_lines(robots_different_lines, 8) + + locations_different_lines = [ + f"(locations-different {a} {b})" for a, b in distinct_pairs(all_bays) + ] + params["locations_different_predicates"] = indent_lines( + locations_different_lines, 8 + ) + + robot_available_lines = [f"(robot-available {robot})" for robot in robots] + params["robot_available_predicates"] = indent_lines(robot_available_lines, 8) + + init = config_dynamic["init"] + robot_at_lines = [ + f"(robot-at {robot} {init[robot]['location']})" for robot in robots + ] + params["robot_at_predicates"] = indent_lines(robot_at_lines, 8) + + all_locations = all_bays + berths + occupied_locations = [init[robot]["location"] for robot in robots] + available_locations = sorted(set(all_locations).difference(occupied_locations)) + location_available_lines = [ + f"(location-available {location})" for location in available_locations + ] + params["location_available_predicates"] = indent_lines(location_available_lines, 8) + + need_stereo_lines = [ + goal.replace("completed-stereo", "need-stereo") + for goal in pddl_goals + if "completed-stereo" in goal + ] + params["need_stereo_predicates"] = indent_lines(need_stereo_lines, 8) + + order_identity_lines = [f"(= (order-identity o{i}) {i})" for i in range(num_orders)] + params["order_identity_fluents"] = indent_lines(order_identity_lines, 8) + + robot_order_lines = [f"(= (robot-order {robot}) -1)" for robot in robots] + params["robot_order_fluents"] = indent_lines(robot_order_lines, 8) + + params["config_dynamic"] = comment_for_pddl(config_dynamic_path.read_text()) + params["config_static"] = comment_for_pddl(config_static_path.read_text()) + + # print(yaml.safe_dump(params, indent=4, sort_keys=False)) + filled_template = TEMPLATE_SUBST_REGEX.sub(TemplateFiller(params), problem_template) + output_path.write_text(filled_template) + print(f"Wrote to {output_path}", file=sys.stderr) + + +class CustomFormatter( + argparse.ArgumentDefaultsHelpFormatter, argparse.RawDescriptionHelpFormatter +): + pass + + +def main(): + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=CustomFormatter + ) + parser.add_argument( + "--problem-template", + help="Path to input PDDL problem template", + type=pathlib.Path, + default="jem_survey_template.pddl", + ) + parser.add_argument( + "--config-dynamic", + help="Path to input dynamic problem config YAML (goals, initial state)", + type=pathlib.Path, + default="jem_survey_dynamic.yaml", + ) + parser.add_argument( + "--config-static", + help="Path to input static problem config YAML (module geometry, available stereo surveys, etc.)", + type=pathlib.Path, + default="jem_survey_static.yaml", + ) + parser.add_argument( + "-o", + "--output", + help="Path for output PDDL problem", + type=pathlib.Path, + default="problem_jem_survey.pddl", + ) + args = parser.parse_args() + + problem_generator( + problem_template_path=args.problem_template, + config_dynamic_path=args.config_dynamic, + config_static_path=args.config_static, + output_path=args.output, + ) + + +if __name__ == "__main__": + main() diff --git a/isaac/Subsystems.md b/isaac/Subsystems.md index bff1f708..ddd1e7a8 100644 --- a/isaac/Subsystems.md +++ b/isaac/Subsystems.md @@ -13,4 +13,6 @@ The ISAAC Repo contains the following Modules: \subpage astrobee : High-level actions that support complex maneuvers. +\subpage survey : Semi-autonomous survey tasking of Astrobees. + \subpage shared : Shared tools common to the entire ISAAC repo. diff --git a/scripts/docker/astrobee_msgs.Dockerfile b/scripts/docker/astrobee_msgs.Dockerfile index 1ac4e76c..1a8e4716 100644 --- a/scripts/docker/astrobee_msgs.Dockerfile +++ b/scripts/docker/astrobee_msgs.Dockerfile @@ -19,11 +19,11 @@ # This will set up an Astrobee docker container using the non-NASA install instructions. # You must set the docker context to be the repository root directory -ARG UBUNTU_VERSION=16.04 +ARG UBUNTU_VERSION=20.04 FROM ubuntu:${UBUNTU_VERSION} -ARG ROS_VERSION=kinetic -ARG PYTHON="" +ARG ROS_VERSION=noetic +ARG PYTHON=3 # try to suppress certain warnings during apt-get calls ARG DEBIAN_FRONTEND=noninteractive diff --git a/scripts/docker/isaac.Dockerfile b/scripts/docker/isaac.Dockerfile index 18fb3cd3..4ee67c98 100644 --- a/scripts/docker/isaac.Dockerfile +++ b/scripts/docker/isaac.Dockerfile @@ -19,12 +19,12 @@ # This will set up an Astrobee docker container using the non-NASA install instructions. # You must set the docker context to be the repository root directory -ARG UBUNTU_VERSION=16.04 +ARG UBUNTU_VERSION=20.04 ARG REMOTE=isaac FROM ${REMOTE}/isaac:latest-astrobee-ubuntu${UBUNTU_VERSION} -ARG ROS_VERSION=kinetic -ARG PYTHON="" +ARG ROS_VERSION=noetic +ARG PYTHON=3 # suppress detached head warnings later RUN git config --global advice.detachedHead false diff --git a/scripts/docker/isaac_astrobee.Dockerfile b/scripts/docker/isaac_astrobee.Dockerfile index 269c85ca..12afb06d 100644 --- a/scripts/docker/isaac_astrobee.Dockerfile +++ b/scripts/docker/isaac_astrobee.Dockerfile @@ -19,13 +19,13 @@ # This will set up an Astrobee docker container using the non-NASA install instructions. # You must set the docker context to be the repository root directory -ARG UBUNTU_VERSION=16.04 +ARG UBUNTU_VERSION=20.04 ARG REMOTE=astrobee FROM ${REMOTE}/astrobee:latest-ubuntu${UBUNTU_VERSION} # Already inherited from astrobee:base-latest-ubuntu... -ARG ROS_VERSION=kinetic -ARG PYTHON="" +ARG ROS_VERSION=noetic +ARG PYTHON=3 RUN apt-get update && apt-get install -y \ libmnl-dev \ diff --git a/scripts/docker/isaac_msgs.Dockerfile b/scripts/docker/isaac_msgs.Dockerfile index 27190252..280c5ae8 100644 --- a/scripts/docker/isaac_msgs.Dockerfile +++ b/scripts/docker/isaac_msgs.Dockerfile @@ -19,12 +19,12 @@ # This will set up an Astrobee docker container using the non-NASA install instructions. # You must set the docker context to be the repository root directory -ARG UBUNTU_VERSION=16.04 +ARG UBUNTU_VERSION=20.04 ARG REMOTE=isaac FROM ${REMOTE}/isaac:astrobee-msgs-ubuntu${UBUNTU_VERSION} -ARG ROS_VERSION=kinetic -ARG PYTHON="" +ARG ROS_VERSION=noetic +ARG PYTHON=3 # Copy over the isaac_msgs COPY communications/isaac_msgs /src/msgs/src/communications/ diff --git a/scripts/setup/packages_focal.lst b/scripts/setup/packages_focal.lst index 87954901..e1c70e58 100644 --- a/scripts/setup/packages_focal.lst +++ b/scripts/setup/packages_focal.lst @@ -2,7 +2,9 @@ doxygen python3-catkin-tools python3-osrf-pycommon python3-rosdep +libreadline-dev ros-noetic-eigen-conversions ros-noetic-pcl-ros +ros-noetic-behaviortree-cpp libmnl-dev libproj-dev From 4508014c5c5f18c0483196f1ee47d9c620768839 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Thu, 14 Dec 2023 13:29:22 -0800 Subject: [PATCH 12/44] bug fixes --- .../survey_manager/scripts/command_astrobee | 112 ++++++++++-------- 1 file changed, 62 insertions(+), 50 deletions(-) diff --git a/astrobee/survey_manager/scripts/command_astrobee b/astrobee/survey_manager/scripts/command_astrobee index aa2de8ea..94c623c3 100755 --- a/astrobee/survey_manager/scripts/command_astrobee +++ b/astrobee/survey_manager/scripts/command_astrobee @@ -37,13 +37,13 @@ from ff_msgs.msg import ( CommandConstants, CommandStamped, ) -from std_msgs.msg import String +from std_msgs.msg import String, Header # Type alias YamlMapping = Dict[str, Any] # Constants -MAX_COUNTER = 10 +MAX_COUNTER = 3 def exposure_change(bay_origin, bay_destination, value): # Going to JEM @@ -81,7 +81,7 @@ def map_change(bay_origin, bay_destination, map_name): # Mostly used for actions where user inteference might be required class ProcessExecutor: - def __init__(self, robot_name, goal, bay, run): + def __init__(self, robot_name): self.input_path = '/tmp/input_' + robot_name self.output_path = '/tmp/output_' + robot_name @@ -258,8 +258,8 @@ class ProcessExecutor: print("Invalid input. Please enter 'y' or 'n'.") def send_command_recursive(self, command): - if send_command(command): - recursive_command(command) + if self.send_command(command): + self.recursive_command(command) # This class sends a command to the executor and waits to get a response @@ -270,25 +270,21 @@ class CommandExecutor: def __init__(self, ns): self.ns = ns - # Start ROS node - rospy.init_node("survey_namager_cmd_" + robot_name) + rospy.loginfo(self.ns + "/command") # Declare guest science command publisher - self.pub_guest_sci = rospy.Publisher(self.ns + "command", CommandStamped, queue_size=5) - + self.sub_ack = rospy.Subscriber(self.ns + "/mgt/ack", AckStamped, self.ack_callback) + self.ack_needed = False + self.pub_command = rospy.Publisher(self.ns + "/command", CommandStamped, queue_size=5) + while self.pub_command.get_num_connections() == 0: + rospy.loginfo("Waiting for subscriber to connect") + rospy.sleep(1) self.unique_cmd_id = "" - def __del__(self): - rospy.shutdown() - - def start_recording(): - + def start_recording(self, bag_description): # Arg is bagfile name description arg1 = CommandArg() arg1.data_type = CommandArg.DATA_TYPE_STRING - arg1.s = "Front" - - # Create CommandStamped message - cmd_args = [arg1] + arg1.s = bag_description cmd = CommandStamped() cmd.header = Header(stamp=rospy.Time.now()) @@ -297,11 +293,13 @@ class CommandExecutor: self.unique_cmd_id = cmd.cmd_id cmd.cmd_src = "isaac fsw" cmd.cmd_origin = "isaac fsw" + cmd.args = [arg1] # Publish the CommandStamped message - result = publish_and_wait_response(cmd) + result = self.publish_and_wait_response(cmd) + return result - def stop_recording(bag_description): + def stop_recording(self): cmd = CommandStamped() cmd.header = Header(stamp=rospy.Time.now()) cmd.cmd_name = CommandConstants.CMD_NAME_STOP_RECORDING @@ -311,17 +309,18 @@ class CommandExecutor: cmd.cmd_origin = "isaac fsw" # Publish the CommandStamped message - result = publish_and_wait_response(cmd) + result = self.publish_and_wait_response(cmd) + return result - def change_exposure(val): + def change_exposure(self, val): #TBD rospy.loginfo("Change exposure to " + val) - def change_map(map_name): + def change_map(self, map_name): #TBD rospy.loginfo("Change map to " + map_name) - def set_plan(): + def set_plan(self): # TODO Add call to plan_pub.cc cmd = CommandStamped() @@ -334,9 +333,10 @@ class CommandExecutor: cmd.args = cmd_args # Publish the CommandStamped message - result = publish_and_wait_response(cmd) + result = self.publish_and_wait_response(cmd) + return result - def run_plan(): + def run_plan(self): cmd = CommandStamped() cmd.header = Header(stamp=rospy.Time.now()) cmd.cmd_name = CommandConstants.CMD_NAME_RUN_PLAN @@ -346,30 +346,39 @@ class CommandExecutor: cmd.cmd_origin = "isaac fsw" # Publish the CommandStamped message - result = publish_and_wait_response(cmd) + result = self.publish_and_wait_response(cmd) + return result + + def ack_callback(self, msg): + if self.ack_needed == True and msg.cmd_id == self.unique_cmd_id: + self.ack_msg = msg + self.ack_needed = False - def publish_and_wait_response(cmd): + def publish_and_wait_response(self, cmd): # Publish the CommandStamped message - self.pub_guest_sci.publish(cmd) + self.ack_needed = True + self.pub_command.publish(cmd) # Wait for ack counter = 0 while counter < MAX_COUNTER: - try: - msg = rospy.wait_for_message(self.ns + "gt/ack", AckStamped, timeout = 5) - if msg.cmd_id == self.unique_cmd_id: - if msg.completed_status.status == AckCompletedStatus.NOT: - rospy.loginfo("Command is being executed and has not completed.") - elif msg.completed_status.status == AckCompletedStatus.OK: - rospy.loginfo("Command completed successfully!") - return 0 - else: - rospy.loginfo("Command failed! Message: " + Ack.message) - return 1 - except: - continue - counter += 1 + # got message + if self.ack_needed == False: + rospy.loginfo("GOT MSG.") + if self.ack_msg.completed_status.status == AckCompletedStatus.NOT: + rospy.loginfo("Command is being executed and has not completed.") + self.ack_needed = True + elif self.ack_msg.completed_status.status == AckCompletedStatus.OK: + rospy.loginfo("Command completed successfully!") + return 0 + else: + rospy.loginfo("Command failed! Message: " + self.ack_msg.message) + return 1 + else: + rospy.sleep(1) + counter += 1 + return 1 def load_yaml(yaml_path: pathlib.Path) -> YamlMapping: @@ -379,11 +388,11 @@ def load_yaml(yaml_path: pathlib.Path) -> YamlMapping: with yaml_path.open(encoding="utf-8") as yaml_stream: return yaml.safe_load(yaml_stream) -def get_stereo_traj(from_bay, to_bay): +def get_stereo_traj(config_static, from_bay, to_bay): # Get stereo trajectory traj_matches = [ traj - for traj in static_config["stereo"].values() + for traj in config_static["stereo"].values() if traj["base_location"] == from_bay and traj["bound_location"] == to_bay ] assert ( @@ -394,6 +403,9 @@ def get_stereo_traj(from_bay, to_bay): def survey_manager_executor(robot_name, goal, to_bay, from_bay, run, config_static_path: pathlib.Path): + # Start ROS node + rospy.init_node("survey_namager_cmd_" + robot_name, anonymous=True) + # Read the static configs that convert constants to values config_static = load_yaml(config_static_path) @@ -403,7 +415,7 @@ def survey_manager_executor(robot_name, goal, to_bay, from_bay, run, config_stat if not current_robot: rospy.loginfo("We're in simulation. Let's get the robotname using the topic") # This is a latching messge so it shouldn't take long - data = rospy.wait_for_message('/robot_name', String, timeout=2) + data = rospy.wait_for_message('/robot_name', String, timeout=5) current_robot = data.data.lower() sim = True @@ -413,10 +425,10 @@ def survey_manager_executor(robot_name, goal, to_bay, from_bay, run, config_stat rospy.loginfo("We're commanding a robot remotely!") ns = " -remote -ns " + robot_name # Command executor will add namespace for bridge forwarding - command_executor = CommandExecutor(robot_name + "/") + command_executor = CommandExecutor(robot_name) else: command_executor = CommandExecutor("") - process_executor = ProcessExecutor() + process_executor = ProcessExecutor(robot_name) # Initialize exit code exit_code = 0 @@ -437,8 +449,8 @@ def survey_manager_executor(robot_name, goal, to_bay, from_bay, run, config_stat exit_code += command_executor.change_map(map_name) elif goal == "panorama": - exit_code += command_executor.start_recording("pano_" + bay + "_" + run) - exit_code += process_executor.send_command_recursive("rosrun inspection inspection_tool -panorama -panorama_mode '5_mapper_and_hugin' " + static_config["bays"][to_bay] + ns) + exit_code += command_executor.start_recording("pano_" + to_bay + "_" + run) + exit_code += process_executor.send_command_recursive("rosrun inspection inspection_tool -panorama -panorama_mode '5_mapper_and_hugin' " + config_static["bays"][to_bay] + ns) exit_code += command_executor.stop_recording() elif goal == "stereo": From 649ea5ce3a96245a5005a760114ea91576358645 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Thu, 14 Dec 2023 13:35:38 -0800 Subject: [PATCH 13/44] fixes --- astrobee/survey_manager/scripts/command_astrobee | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/astrobee/survey_manager/scripts/command_astrobee b/astrobee/survey_manager/scripts/command_astrobee index 94c623c3..604641da 100755 --- a/astrobee/survey_manager/scripts/command_astrobee +++ b/astrobee/survey_manager/scripts/command_astrobee @@ -37,7 +37,7 @@ from ff_msgs.msg import ( CommandConstants, CommandStamped, ) -from std_msgs.msg import String, Header +from std_msgs.msg import Header, String # Type alias YamlMapping = Dict[str, Any] @@ -145,6 +145,8 @@ class ProcessExecutor: conn.setblocking(False) connected = True + # Always send full history to each newly connecting client so history + # is not lost if the connection is dropped conn.send(output_total.encode("utf-8")[:1024]) except socket.timeout: continue From 2647bac73bfa0222e8448029594d37cbb8a3f44c Mon Sep 17 00:00:00 2001 From: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com> Date: Thu, 14 Dec 2023 13:40:50 -0800 Subject: [PATCH 14/44] Update ci_pr.yml make survey manager PR's run CI --- .github/workflows/ci_pr.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci_pr.yml b/.github/workflows/ci_pr.yml index 6a1f767b..6cc8b9ec 100644 --- a/.github/workflows/ci_pr.yml +++ b/.github/workflows/ci_pr.yml @@ -2,7 +2,7 @@ name: Build and Test CI for Pull Requests on: pull_request: - branches: [ 'master', 'develop' ] + branches: [ 'master', 'develop', 'survey_manager' ] workflow_dispatch: jobs: From d7ea2f57c2dff214475eb4f5a6f1e5308480233e Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Wed, 20 Dec 2023 18:35:01 -0800 Subject: [PATCH 15/44] moving executor to different package hierarchy --- .../{ => survey_executor}/CMakeLists.txt | 2 +- .../{ => survey_executor}/package.xml | 2 +- .../scripts/command_astrobee | 34 +++++++++---------- .../scripts/monitor_astrobee | 2 +- .../src/move_action_node.cpp | 0 5 files changed, 19 insertions(+), 21 deletions(-) rename astrobee/survey_manager/{ => survey_executor}/CMakeLists.txt (98%) rename astrobee/survey_manager/{ => survey_executor}/package.xml (94%) rename astrobee/survey_manager/{ => survey_executor}/scripts/command_astrobee (95%) rename astrobee/survey_manager/{ => survey_executor}/scripts/monitor_astrobee (98%) rename astrobee/survey_manager/{ => survey_executor}/src/move_action_node.cpp (100%) diff --git a/astrobee/survey_manager/CMakeLists.txt b/astrobee/survey_manager/survey_executor/CMakeLists.txt similarity index 98% rename from astrobee/survey_manager/CMakeLists.txt rename to astrobee/survey_manager/survey_executor/CMakeLists.txt index 8e588496..7ac3317c 100644 --- a/astrobee/survey_manager/CMakeLists.txt +++ b/astrobee/survey_manager/survey_executor/CMakeLists.txt @@ -17,7 +17,7 @@ # under the License. cmake_minimum_required(VERSION 3.0) -project(survey_manager) +project(survey_executor) ## Compile as C++14, supported in ROS Kinetic and newer add_compile_options(-std=c++14) diff --git a/astrobee/survey_manager/package.xml b/astrobee/survey_manager/survey_executor/package.xml similarity index 94% rename from astrobee/survey_manager/package.xml rename to astrobee/survey_manager/survey_executor/package.xml index ecb3d537..6fe97cac 100644 --- a/astrobee/survey_manager/package.xml +++ b/astrobee/survey_manager/survey_executor/package.xml @@ -1,6 +1,6 @@ - survey_manager + survey_executor 0.0.0 The survey_manager package diff --git a/astrobee/survey_manager/scripts/command_astrobee b/astrobee/survey_manager/survey_executor/scripts/command_astrobee similarity index 95% rename from astrobee/survey_manager/scripts/command_astrobee rename to astrobee/survey_manager/survey_executor/scripts/command_astrobee index 604641da..e36b32dd 100755 --- a/astrobee/survey_manager/scripts/command_astrobee +++ b/astrobee/survey_manager/survey_executor/scripts/command_astrobee @@ -113,16 +113,6 @@ class ProcessExecutor: self.sock_input.close() self.sock_output.close() - # def connect_input_callback(sock, addr): - # """Callback function that is called when a connection is made to the socket.""" - # print("Input connection made from", addr) - # sock_input_connected = False - - # def connect_output_callback(sock, addr): - # """Callback function that is called when a connection is made to the socket.""" - # print("Output connection made from", addr) - # sock_output_connected = False - def thread_write_output(self, process): print("starting thread_write_output...") # Store cumulative output @@ -131,22 +121,30 @@ class ProcessExecutor: try: while True: # Get output from process - output = process.stdout.readline() - if (output == '' and process.poll() is not None) or self._stop_event.is_set(): + char = process.stdout.read(1) + # output = process.stdout.readline() + if (char == '' and process.poll() is not None) or self._stop_event.is_set(): break - if output: - output_total += output + if char: + if char == '\r': + print("got rrrr") + last_newline_index = output_total.rfind('\n') + output_total = output_total[:last_newline_index + 1] if last_newline_index != -1 else "" + else: + output_total += char + # Check if output has \r # If socket is not connected try to connect if not connected: try: - print("trying to connect") + # print("trying to connect output") conn, addr = self.sock_output.accept() conn.setblocking(False) connected = True # Always send full history to each newly connecting client so history # is not lost if the connection is dropped + print(output_total) conn.send(output_total.encode("utf-8")[:1024]) except socket.timeout: continue @@ -157,7 +155,7 @@ class ProcessExecutor: # If socket is already connected, send output elif connected: try: - conn.send(output.encode("utf-8")[:1024]) + conn.send(char.encode("utf-8")[:1024]) except (socket.error, BrokenPipeError): print("Error sending data. Receiver may have disconnected.") connected = False @@ -165,7 +163,7 @@ class ProcessExecutor: print("exit output:") print(e) # finally: - # # Save total output into a log + # # Save total output into a log file # print(output_total) def thread_read_input(self, process): @@ -173,7 +171,7 @@ class ProcessExecutor: try: while True: while not self._stop_event.is_set(): - print("waiting for connection") + # print("waiting for connection") try: client_socket, client_address = self.sock_input.accept() break diff --git a/astrobee/survey_manager/scripts/monitor_astrobee b/astrobee/survey_manager/survey_executor/scripts/monitor_astrobee similarity index 98% rename from astrobee/survey_manager/scripts/monitor_astrobee rename to astrobee/survey_manager/survey_executor/scripts/monitor_astrobee index 82f53a4e..91b9e535 100755 --- a/astrobee/survey_manager/scripts/monitor_astrobee +++ b/astrobee/survey_manager/survey_executor/scripts/monitor_astrobee @@ -53,7 +53,7 @@ def thread_read_output(sock_output): request = sock_output.recv(1024) request = request.decode("utf-8") # convert bytes tro str - print(request) + print(request, end='') except: print("exit input") diff --git a/astrobee/survey_manager/src/move_action_node.cpp b/astrobee/survey_manager/survey_executor/src/move_action_node.cpp similarity index 100% rename from astrobee/survey_manager/src/move_action_node.cpp rename to astrobee/survey_manager/survey_executor/src/move_action_node.cpp From b8ae5aa4381ac9d1880d052f62912c382510803c Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Fri, 5 Jan 2024 12:21:02 -0800 Subject: [PATCH 16/44] consolidating packages; action compiling --- .../survey_executor/CMakeLists.txt | 61 ------------------- .../survey_executor/package.xml | 21 ------- .../survey_planner/CMakeLists.txt | 27 ++++++++ .../behavior_trees/collecting_panoramas.xml | 7 --- .../survey_planner/behavior_trees/move_to.xml | 7 --- .../behavior_trees/move_to_inspect.xml | 8 --- .../launch/survey_domain.launch | 4 ++ .../survey_manager/survey_planner/package.xml | 11 ++-- .../src/move_action_node.cpp | 15 ++++- .../survey_planner/src/ros1_lifecycle | 1 - .../survey_planner/src/ros2_planning_system | 1 - .../tools}/command_astrobee | 0 .../tools}/monitor_astrobee | 0 13 files changed, 51 insertions(+), 112 deletions(-) delete mode 100644 astrobee/survey_manager/survey_executor/CMakeLists.txt delete mode 100644 astrobee/survey_manager/survey_executor/package.xml delete mode 100644 astrobee/survey_manager/survey_planner/behavior_trees/collecting_panoramas.xml delete mode 100644 astrobee/survey_manager/survey_planner/behavior_trees/move_to.xml delete mode 100644 astrobee/survey_manager/survey_planner/behavior_trees/move_to_inspect.xml rename astrobee/survey_manager/{survey_executor => survey_planner}/src/move_action_node.cpp (82%) delete mode 160000 astrobee/survey_manager/survey_planner/src/ros1_lifecycle delete mode 160000 astrobee/survey_manager/survey_planner/src/ros2_planning_system rename astrobee/survey_manager/{survey_executor/scripts => survey_planner/tools}/command_astrobee (100%) rename astrobee/survey_manager/{survey_executor/scripts => survey_planner/tools}/monitor_astrobee (100%) diff --git a/astrobee/survey_manager/survey_executor/CMakeLists.txt b/astrobee/survey_manager/survey_executor/CMakeLists.txt deleted file mode 100644 index 7ac3317c..00000000 --- a/astrobee/survey_manager/survey_executor/CMakeLists.txt +++ /dev/null @@ -1,61 +0,0 @@ -# Copyright (c) 2021, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking -# platform" software is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -cmake_minimum_required(VERSION 3.0) -project(survey_executor) - -## Compile as C++14, supported in ROS Kinetic and newer -add_compile_options(-std=c++14) - -## Find catkin macros and libraries -SET(catkin2_DIR "${CMAKE_SOURCE_DIR}/../../../cmake") -find_package(catkin2 REQUIRED COMPONENTS -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( -) - -########### -## Build ## -########### - -# Specify additional locations of header files -# Your package locations should be listed before other locations -include_directories( - ${catkin_INCLUDE_DIRS} -) - - -# Uncomment this when plansys merged -# # Action for move_inspect -# add_executable(move_action_node src/move_action_node.cpp) -# target_link_libraries(move_action_node ${catkin_LIBRARIES} ) - -############# -## Install ## -############# - -catkin_install_python(PROGRAMS scripts/command_astrobee scripts/monitor_astrobee - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/astrobee/survey_manager/survey_executor/package.xml b/astrobee/survey_manager/survey_executor/package.xml deleted file mode 100644 index 6fe97cac..00000000 --- a/astrobee/survey_manager/survey_executor/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - survey_executor - 0.0.0 - The survey_manager package - - Apache License, Version 2.0 - - - ISAAC Flight Software - - - ISAAC Flight Software - - - catkin - - - - - diff --git a/astrobee/survey_manager/survey_planner/CMakeLists.txt b/astrobee/survey_manager/survey_planner/CMakeLists.txt index eb0bc8e4..070a153e 100644 --- a/astrobee/survey_manager/survey_planner/CMakeLists.txt +++ b/astrobee/survey_manager/survey_planner/CMakeLists.txt @@ -26,12 +26,39 @@ SET(catkin2_DIR "${CMAKE_SOURCE_DIR}/../../../cmake") find_package(catkin2 REQUIRED COMPONENTS roscpp ff_msgs + lifecycle + plansys2_executor ) catkin_package( CATKIN_DEPENDS ) +########### +## Build ## +########### + +# Specify additional locations of header files +# Your package locations should be listed before other locations +include_directories( + ${catkin_INCLUDE_DIRS} +) + +# Action for move_inspect +add_executable(move_action_node src/move_action_node.cpp) +target_link_libraries(move_action_node ${catkin_LIBRARIES} ) + +############# +## Install ## +############# + +catkin_install_python(PROGRAMS tools/command_astrobee tools/monitor_astrobee + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +# Install actions +install(TARGETS move_action_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + foreach( dir pddl launch) install( DIRECTORY ${dir}/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir} ) diff --git a/astrobee/survey_manager/survey_planner/behavior_trees/collecting_panoramas.xml b/astrobee/survey_manager/survey_planner/behavior_trees/collecting_panoramas.xml deleted file mode 100644 index d8cb181f..00000000 --- a/astrobee/survey_manager/survey_planner/behavior_trees/collecting_panoramas.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/astrobee/survey_manager/survey_planner/behavior_trees/move_to.xml b/astrobee/survey_manager/survey_planner/behavior_trees/move_to.xml deleted file mode 100644 index a708b178..00000000 --- a/astrobee/survey_manager/survey_planner/behavior_trees/move_to.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/astrobee/survey_manager/survey_planner/behavior_trees/move_to_inspect.xml b/astrobee/survey_manager/survey_planner/behavior_trees/move_to_inspect.xml deleted file mode 100644 index b40db393..00000000 --- a/astrobee/survey_manager/survey_planner/behavior_trees/move_to_inspect.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/astrobee/survey_manager/survey_planner/launch/survey_domain.launch b/astrobee/survey_manager/survey_planner/launch/survey_domain.launch index bbcd5145..b0cfad2d 100644 --- a/astrobee/survey_manager/survey_planner/launch/survey_domain.launch +++ b/astrobee/survey_manager/survey_planner/launch/survey_domain.launch @@ -39,5 +39,9 @@ + + + + diff --git a/astrobee/survey_manager/survey_planner/package.xml b/astrobee/survey_manager/survey_planner/package.xml index 279b7f24..0a530666 100644 --- a/astrobee/survey_manager/survey_planner/package.xml +++ b/astrobee/survey_manager/survey_planner/package.xml @@ -13,11 +13,14 @@ ISAAC Flight Software - catkin - roscpp - std_srvs - lifecycle_msgs + catkin + roscpp + lifecycle + roscpp + lifecycle + roscpp + lifecycle plansys2_domain_expert plansys2_problem_expert diff --git a/astrobee/survey_manager/survey_executor/src/move_action_node.cpp b/astrobee/survey_manager/survey_planner/src/move_action_node.cpp similarity index 82% rename from astrobee/survey_manager/survey_executor/src/move_action_node.cpp rename to astrobee/survey_manager/survey_planner/src/move_action_node.cpp index ecba8813..03a69d6c 100644 --- a/astrobee/survey_manager/survey_executor/src/move_action_node.cpp +++ b/astrobee/survey_manager/survey_planner/src/move_action_node.cpp @@ -45,15 +45,26 @@ class MoveAction : public plansys2::ActionExecutorClient { int main(int argc, char *argv[]) { // Initialize a ros node ros::init(argc, argv, "move_action"); - ros::NodeHandle nh(name); + + std::string name = ros::this_node::getName(); + if (name.empty() || (name.size() == 1 && name[0] == '/')) + name = "default"; + else if (name[0] == '/') + name = name.substr(1); + + ros::NodeHandle nh("~"); + nh.setParam("action_name", "move"); // Start action node // We could actually add multiple action nodes here being aware that we might need a ros::AsyncSpinner // (https://github.com/Bckempa/ros2_planning_system/blob/noetic-devel/plansys2_bt_actions/src/bt_action_node.cpp#L41) - auto action_node = std::make_shared(nh, name, std::chrono_literals::1s); + auto action_node = std::make_shared(nh, "move", std::chrono::seconds(1)); action_node->trigger_transition(ros::lifecycle::CONFIGURE); + ROS_ERROR_STREAM("Starting action"); + // Synchronous mode ros::spin(); // Make for great success return 0; +} diff --git a/astrobee/survey_manager/survey_planner/src/ros1_lifecycle b/astrobee/survey_manager/survey_planner/src/ros1_lifecycle deleted file mode 160000 index 15a50352..00000000 --- a/astrobee/survey_manager/survey_planner/src/ros1_lifecycle +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 15a50352f9d522c7815ecedaa2b77b6f99ecdb31 diff --git a/astrobee/survey_manager/survey_planner/src/ros2_planning_system b/astrobee/survey_manager/survey_planner/src/ros2_planning_system deleted file mode 160000 index 3a988a5b..00000000 --- a/astrobee/survey_manager/survey_planner/src/ros2_planning_system +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 3a988a5bc14a1700a8f276e2438c47ac80e7bf8c diff --git a/astrobee/survey_manager/survey_executor/scripts/command_astrobee b/astrobee/survey_manager/survey_planner/tools/command_astrobee similarity index 100% rename from astrobee/survey_manager/survey_executor/scripts/command_astrobee rename to astrobee/survey_manager/survey_planner/tools/command_astrobee diff --git a/astrobee/survey_manager/survey_executor/scripts/monitor_astrobee b/astrobee/survey_manager/survey_planner/tools/monitor_astrobee similarity index 100% rename from astrobee/survey_manager/survey_executor/scripts/monitor_astrobee rename to astrobee/survey_manager/survey_planner/tools/monitor_astrobee From 2376fc6a1ab880a87d8e6ef307cad1055721f43d Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Mon, 8 Jan 2024 13:33:56 -0800 Subject: [PATCH 17/44] tested action --- .../survey_planner/CMakeLists.txt | 4 +- .../survey_planner/src/move_action_node.cpp | 37 +++++++++++++++---- 2 files changed, 32 insertions(+), 9 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/CMakeLists.txt b/astrobee/survey_manager/survey_planner/CMakeLists.txt index 070a153e..3e1412b4 100644 --- a/astrobee/survey_manager/survey_planner/CMakeLists.txt +++ b/astrobee/survey_manager/survey_planner/CMakeLists.txt @@ -16,10 +16,10 @@ # License for the specific language governing permissions and limitations # under the License. -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(survey_planner) -add_compile_options(-std=c++17) +set(CMAKE_CXX_STANDARD 17) find_package(catkin REQUIRED) SET(catkin2_DIR "${CMAKE_SOURCE_DIR}/../../../cmake") diff --git a/astrobee/survey_manager/survey_planner/src/move_action_node.cpp b/astrobee/survey_manager/survey_planner/src/move_action_node.cpp index 03a69d6c..e6a6d4da 100644 --- a/astrobee/survey_manager/survey_planner/src/move_action_node.cpp +++ b/astrobee/survey_manager/survey_planner/src/move_action_node.cpp @@ -21,6 +21,7 @@ #include +#include #include namespace plansys2_actions { @@ -28,15 +29,39 @@ namespace plansys2_actions { class MoveAction : public plansys2::ActionExecutorClient { public: MoveAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate) - : ActionExecutorClient(nh, action, rate) {} + : ActionExecutorClient(nh, action, rate) { + progress_ = 0.0; + } protected: void do_work() { - ROS_ERROR_STREAM("Executing [MOVE]"); - // std::string from, towards; - // from = get_arguments()[1]; - // towards = get_arguments()[2]; + std::string from, towards; + + if (get_arguments().size() > 2) { + robot_name_ = get_arguments()[0]; + from = get_arguments()[1]; + towards = get_arguments()[2]; + } else { + finish(false, 1.0, "Not enough arguments for [MOVE] command"); + } + + if (progress_ < 1.0) { + progress_ += 0.05; + send_feedback(progress_, "Move and Inspect running"); + } else { + finish(true, 1.0, "Move and Inspect completed"); + + progress_ = 0.0; + std::cout << std::endl; + } + + std::cout << "\t ** [Move and Inspect] Robot " << robot_name_ << " moving from " << from << " towards " << towards + << " ... [" << std::min(100.0, progress_ * 100.0) << "%] " << std::flush; } + + + float progress_; + std::string robot_name_; }; } // namespace plansys2_actions @@ -61,8 +86,6 @@ int main(int argc, char *argv[]) { auto action_node = std::make_shared(nh, "move", std::chrono::seconds(1)); action_node->trigger_transition(ros::lifecycle::CONFIGURE); - ROS_ERROR_STREAM("Starting action"); - // Synchronous mode ros::spin(); // Make for great success From fb0c57ceba92fa8dfec52ae5d46787992b436d33 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Mon, 8 Jan 2024 19:11:31 -0800 Subject: [PATCH 18/44] submodule update --- .../survey_manager/survey_dependencies/ros2_planning_system | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/astrobee/survey_manager/survey_dependencies/ros2_planning_system b/astrobee/survey_manager/survey_dependencies/ros2_planning_system index 87d1656d..60e9a235 160000 --- a/astrobee/survey_manager/survey_dependencies/ros2_planning_system +++ b/astrobee/survey_manager/survey_dependencies/ros2_planning_system @@ -1 +1 @@ -Subproject commit 87d1656d31ebec1eb6c825e601c352cdaffdb95f +Subproject commit 60e9a23591c4afb0b0aa04140e5bb329657d7f23 From adf0b817caa16a58ee83bcb7dd21280462f9fea9 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Tue, 9 Jan 2024 13:21:29 -0800 Subject: [PATCH 19/44] move scripts into package --- astrobee/survey_manager/{ => survey_planner}/scripts/pidwrap.sh | 0 .../survey_manager/{ => survey_planner}/scripts/tmux_inject.py | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename astrobee/survey_manager/{ => survey_planner}/scripts/pidwrap.sh (100%) rename astrobee/survey_manager/{ => survey_planner}/scripts/tmux_inject.py (100%) diff --git a/astrobee/survey_manager/scripts/pidwrap.sh b/astrobee/survey_manager/survey_planner/scripts/pidwrap.sh similarity index 100% rename from astrobee/survey_manager/scripts/pidwrap.sh rename to astrobee/survey_manager/survey_planner/scripts/pidwrap.sh diff --git a/astrobee/survey_manager/scripts/tmux_inject.py b/astrobee/survey_manager/survey_planner/scripts/tmux_inject.py similarity index 100% rename from astrobee/survey_manager/scripts/tmux_inject.py rename to astrobee/survey_manager/survey_planner/scripts/tmux_inject.py From d724113ce7794e380e41e301490de1ab707ec314 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Tue, 9 Jan 2024 13:22:17 -0800 Subject: [PATCH 20/44] changing static config to be more general --- ..._survey_static.yaml => survey_static.yaml} | 28 ++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) rename astrobee/survey_manager/survey_planner/data/{jem_survey_static.yaml => survey_static.yaml} (71%) diff --git a/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml b/astrobee/survey_manager/survey_planner/data/survey_static.yaml similarity index 71% rename from astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml rename to astrobee/survey_manager/survey_planner/data/survey_static.yaml index fbfd7e02..552cbd79 100644 --- a/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml +++ b/astrobee/survey_manager/survey_planner/data/survey_static.yaml @@ -37,6 +37,32 @@ bays: jem_bay6: [11.0, -9.0, 4.8] jem_bay7: [11.0, -9.7, 4.8] +bays_move: + jem_bay1: "-pos '11 -4 4.8'" + jem_bay2: "-pos '11 -5 4.8'" + jem_bay3: "-pos '11 -6 4.8'" + jem_bay4: " -pos '11 -7 4.8'" + jem_bay5: "-pos '11 -8 4.8'" + jem_bay6: "-pos '11 -9 4.8'" + jem_bay7: "-pos '11 -9.7 4.8'" + jem_hatch_to_nod2: "-move -pos '11 -3.5 4.8' -att '0 0 90'" + jem_hatch_from_nod2: "-move -pos '11 -3.5 4.8' -att '0 0 -90'" + nod2_hatch_from_jem: "-move -pos '11 -1.0 4.8' -att '0 0 90'" + nod2_hatch_to_jem: "-move -pos '11 -1.0 4.8' -att '0 0 -90'" + nod2_bay2: "-pos '11 0 4.8'" + nod2_bay3: "-pos '10 0 4.8'" + nod2_bay4: "-pos '9 0 4.8'" + nod2_hatch_to_usl: "-move -pos '7.8 -3.5 4.8' -att '0 0 180'" + nod2_hatch_from_usl: "-move -pos '7.8 -3.5 4.8' -att '0 0 0'" + usl_hatch_from_nod2: "-move -pos '5.3 -1.0 4.8' -att '0 0 180'" + usl_hatch_to_nod2: "-move -pos '5.3 -1.0 4.8' -att '0 0 0'" + usl_bay1: "-pos '4.7 0 4.8'" + usl_bay2: "-pos '3.65 0 4.8'" + usl_bay3: "-pos '2.6 0 4.8'" + usl_bay4: " -pos '1.55 0 4.8'" + usl_bay5: "-pos '0.5 0 4.8'" + usl_bay6: "-pos '-0.5 0 4.8'" + bogus_bays: [jem_bay0, jem_bay8] berths: [berth1, berth2] robots: [bumble, honey] @@ -53,6 +79,6 @@ stereo: # check purposes. bound_location: jem_bay4 # The survey flies into bay4 even though it only covers up to bay3 jem_bay4_to_bay7: - fplan: "jem_stereo_mapping_bay4_to_bay7.fplan" + fplan: "jem_stereo_mapping_bay7_to_bay4.fplan" base_location: jem_bay7 bound_location: jem_bay4 From a11b640b6b8dde3bfbeae60237f96d81b2d5a9a2 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Tue, 9 Jan 2024 13:28:34 -0800 Subject: [PATCH 21/44] fixing config name + install data folder --- astrobee/survey_manager/survey_planner/CMakeLists.txt | 2 +- .../survey_planner/pddl/problem_jem_survey.pddl | 4 ++-- .../survey_manager/survey_planner/tools/plan_interpreter.py | 2 +- .../survey_manager/survey_planner/tools/problem_generator.py | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/CMakeLists.txt b/astrobee/survey_manager/survey_planner/CMakeLists.txt index 3e1412b4..e29addac 100644 --- a/astrobee/survey_manager/survey_planner/CMakeLists.txt +++ b/astrobee/survey_manager/survey_planner/CMakeLists.txt @@ -59,7 +59,7 @@ catkin_install_python(PROGRAMS tools/command_astrobee tools/monitor_astrobee install(TARGETS move_action_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -foreach( dir pddl launch) +foreach( dir pddl launch data) install( DIRECTORY ${dir}/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir} ) endforeach(dir) diff --git a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl index d099ca79..b9ea2ba3 100644 --- a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl @@ -2,7 +2,7 @@ ;; Command was: ./tools/problem_generator.py ;; Working directory was: /home/vagrant/isaac/astrobee/survey_manager/survey_planner ;; Problem template: pddl/jem_survey_template.pddl -;; Config 1: data/jem_survey_static.yaml +;; Config 1: data/survey_static.yaml ;; Config 2: data/jem_survey_dynamic.yaml (define (problem jem-survey) @@ -246,7 +246,7 @@ ;; # conditions and initial state. A likely conops is that the initial version of this file for a ;; # specific activity would be hand-generated, but it might later be automatically regenerated by the ;; # survey manager when a replan is needed (remove completed/failed goals, add retry goals, update -;; # initial state to match actual current state, etc.) See also jem_survey_static.yaml. +;; # initial state to match actual current state, etc.) See also survey_static.yaml. ;; ;; goals: ;; diff --git a/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py b/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py index 73477859..9798a393 100755 --- a/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py +++ b/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py @@ -46,7 +46,7 @@ from problem_generator import DATA_DIR, load_yaml, path_list DEFAULT_CONFIGS = [ - DATA_DIR / "jem_survey_static.yaml", + DATA_DIR / "survey_static.yaml", # Dynamic config not needed for interpreting the plan ] diff --git a/astrobee/survey_manager/survey_planner/tools/problem_generator.py b/astrobee/survey_manager/survey_planner/tools/problem_generator.py index 72c77327..bf966281 100755 --- a/astrobee/survey_manager/survey_planner/tools/problem_generator.py +++ b/astrobee/survey_manager/survey_planner/tools/problem_generator.py @@ -56,7 +56,7 @@ DATA_DIR = pathlib.Path(os.path.relpath(str((THIS_DIR / ".." / "data").resolve()), CWD)) PDDL_DIR = pathlib.Path(os.path.relpath(str((THIS_DIR / ".." / "pddl").resolve()), CWD)) DEFAULT_CONFIGS = [ - DATA_DIR / "jem_survey_static.yaml", + DATA_DIR / "survey_static.yaml", DATA_DIR / "jem_survey_dynamic.yaml", ] From af52e73fb6585ee79c45018d8b6206ee012943c2 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Wed, 10 Jan 2024 11:41:20 -0800 Subject: [PATCH 22/44] restoring comms to readline --- .../survey_planner/tools/command_astrobee | 142 +++++++++--------- .../survey_planner/tools/monitor_astrobee | 14 +- 2 files changed, 74 insertions(+), 82 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/tools/command_astrobee b/astrobee/survey_manager/survey_planner/tools/command_astrobee index e36b32dd..6cac9a61 100755 --- a/astrobee/survey_manager/survey_planner/tools/command_astrobee +++ b/astrobee/survey_manager/survey_planner/tools/command_astrobee @@ -29,6 +29,7 @@ import threading from typing import Any, Dict, List import rospy +import rospkg import yaml from ff_msgs.msg import ( AckCompletedStatus, @@ -39,11 +40,27 @@ from ff_msgs.msg import ( ) from std_msgs.msg import Header, String -# Type alias -YamlMapping = Dict[str, Any] +# # Get the directory of the current script +sys.path.append(os.path.dirname(os.path.realpath(__file__))) +from problem_generator import load_yaml # Constants MAX_COUNTER = 3 +CHUNK_SIZE = 1024 + +def get_stereo_traj(config_static, from_bay, to_bay): + # Get stereo trajectory + traj_matches = [ + traj + for traj in config_static["stereo"].values() + if traj["base_location"] == from_bay and traj["bound_location"] == to_bay + ] + assert ( + len(traj_matches) == 1 + ), f"Expected exactly 1 matching stereo trajectory with base {base} and bound {bound}, got {len(traj_matches)}" + fplan = traj_matches[0]["fplan"] + return fplan + def exposure_change(bay_origin, bay_destination, value): # Going to JEM @@ -115,55 +132,46 @@ class ProcessExecutor: def thread_write_output(self, process): print("starting thread_write_output...") - # Store cumulative output + # Store commulative output output_total = "" connected = False try: while True: # Get output from process - char = process.stdout.read(1) - # output = process.stdout.readline() - if (char == '' and process.poll() is not None) or self._stop_event.is_set(): + output = process.stdout.readline() + if (output == '' and process.poll() is not None) or self._stop_event.is_set(): break - if char: - if char == '\r': - print("got rrrr") - last_newline_index = output_total.rfind('\n') - output_total = output_total[:last_newline_index + 1] if last_newline_index != -1 else "" - else: - output_total += char - # Check if output has \r - - # If socket is not connected try to connect - if not connected: - try: - # print("trying to connect output") + if output: + output_total += output + + try: + # If socket is not connected try to connect + if not connected: + print("trying to connect") conn, addr = self.sock_output.accept() conn.setblocking(False) connected = True - # Always send full history to each newly connecting client so history - # is not lost if the connection is dropped - print(output_total) - conn.send(output_total.encode("utf-8")[:1024]) - except socket.timeout: - continue - except (socket.error, BrokenPipeError): - print("Error sending data. Receiver may have disconnected.") - connected = False + encoded_message = output_total.encode("ascii", errors="replace") + + for i in range(0, len(encoded_message), CHUNK_SIZE): + chunk = encoded_message[i:i + CHUNK_SIZE] + conn.sendall(chunk) + + # If socket is already connected, send output + elif connected: + conn.send(output.encode("ascii", errors="replace")[:CHUNK_SIZE]) + except socket.timeout: + continue + except (socket.error, BrokenPipeError): + print("Error sending data. Receiver may have disconnected.") + connected = False - # If socket is already connected, send output - elif connected: - try: - conn.send(char.encode("utf-8")[:1024]) - except (socket.error, BrokenPipeError): - print("Error sending data. Receiver may have disconnected.") - connected = False except Exception as e: print("exit output:") print(e) # finally: - # # Save total output into a log file + # # Save total output into a log # print(output_total) def thread_read_input(self, process): @@ -171,7 +179,7 @@ class ProcessExecutor: try: while True: while not self._stop_event.is_set(): - # print("waiting for connection") + print("waiting for connection") try: client_socket, client_address = self.sock_input.accept() break @@ -189,7 +197,7 @@ class ProcessExecutor: while not self._stop_event.is_set(): print("waiting to receive") try: - request = client_socket.recv(1024).decode("utf-8") + request = client_socket.recv(CHUNK_SIZE).decode("ascii", errors="replace") break except socket.timeout: continue @@ -211,7 +219,7 @@ class ProcessExecutor: def send_command(self, command): print(command) - return_code = -1 + return_code = 1 try: # Start the process @@ -240,26 +248,30 @@ class ProcessExecutor: if output_thread.is_alive(): output_thread.join() - # Get the final exit code - return return_code - + # Get the final exit code + return return_code def recursive_command(self, command): while True: user_input = input("Do you want to continue? (y/n): ").lower().strip() if user_input == 'y': - if send_command_with_input(command) != 0: - repeat_inspection() # Continue recursively - break + exit_code = self.send_command(command) + if exit_code != 0: + exit_code = recursive_command(command) # Continue recursively + return exit_code elif user_input == 'n': print("Exiting.") - break + return 1 else: print("Invalid input. Please enter 'y' or 'n'.") def send_command_recursive(self, command): - if self.send_command(command): - self.recursive_command(command) + print("Sending recursive command") + exit_code = self.send_command(command) + print("Exit code " + str(exit_code)) + if (exit_code): + exit_code = self.recursive_command(command) + return exit_code # This class sends a command to the executor and waits to get a response @@ -365,7 +377,6 @@ class CommandExecutor: while counter < MAX_COUNTER: # got message if self.ack_needed == False: - rospy.loginfo("GOT MSG.") if self.ack_msg.completed_status.status == AckCompletedStatus.NOT: rospy.loginfo("Command is being executed and has not completed.") self.ack_needed = True @@ -381,25 +392,6 @@ class CommandExecutor: return 1 -def load_yaml(yaml_path: pathlib.Path) -> YamlMapping: - """ - Return the YAML parse result for the file at `yaml_path`. - """ - with yaml_path.open(encoding="utf-8") as yaml_stream: - return yaml.safe_load(yaml_stream) - -def get_stereo_traj(config_static, from_bay, to_bay): - # Get stereo trajectory - traj_matches = [ - traj - for traj in config_static["stereo"].values() - if traj["base_location"] == from_bay and traj["bound_location"] == to_bay - ] - assert ( - len(traj_matches) == 1 - ), f"Expected exactly 1 matching stereo trajectory with base {base} and bound {bound}, got {len(traj_matches)}" - fplan = traj_matches[0]["fplan"] - return fplan def survey_manager_executor(robot_name, goal, to_bay, from_bay, run, config_static_path: pathlib.Path): @@ -434,23 +426,25 @@ def survey_manager_executor(robot_name, goal, to_bay, from_bay, run, config_stat exit_code = 0 if goal == "dock": - exit_code += process_executor.send_command_recursive("rosrun executive teleop -dock" + ns) + exit_code += process_executor.send_command_recursive("rosrun executive teleop_tool -dock" + ns) elif goal == "undock": - exit_code += process_executor.send_command_recursive("rosrun executive teleop -undock" + ns) + exit_code += process_executor.send_command_recursive("rosrun executive teleop_tool -undock" + ns) elif goal == "move": - exit_code += process_executor.send_command_recursive("rosrun executive teleop -move " + bay + ns) + exit_code += process_executor.send_command_recursive("rosrun executive teleop_tool -move " + config_static["bays_move"][to_bay] + ns) # Change exposure if needed + exposure_value = 0 if exposure_change(from_bay, to_bay, exposure_value): exit_code += command_executor.change_exposure(exposure_value) # Change map if needed + map_name = "" if map_change(from_bay, to_bay, map_name): exit_code += command_executor.change_map(map_name) elif goal == "panorama": exit_code += command_executor.start_recording("pano_" + to_bay + "_" + run) - exit_code += process_executor.send_command_recursive("rosrun inspection inspection_tool -panorama -panorama_mode '5_mapper_and_hugin' " + config_static["bays"][to_bay] + ns) + exit_code += process_executor.send_command_recursive("rosrun inspection inspection_tool -panorama -panorama_mode '5_mapper_and_hugin' " + config_static["bays_move"][to_bay] + ns) exit_code += command_executor.stop_recording() elif goal == "stereo": @@ -495,10 +489,10 @@ if __name__ == "__main__": help="Run number, increases as we add attempts.", ) parser.add_argument( - "config_static", + "--config_static", help="Path to input static problem config YAML (module geometry, available stereo surveys, etc.)", type=pathlib.Path, - default="survey_constants.yaml", + default=os.path.join(rospkg.RosPack().get_path('survey_planner'), "data/survey_static.yaml"), ) args = parser.parse_args() diff --git a/astrobee/survey_manager/survey_planner/tools/monitor_astrobee b/astrobee/survey_manager/survey_planner/tools/monitor_astrobee index 91b9e535..37c74f3e 100755 --- a/astrobee/survey_manager/survey_planner/tools/monitor_astrobee +++ b/astrobee/survey_manager/survey_planner/tools/monitor_astrobee @@ -25,10 +25,11 @@ import sys import threading +# Constants +CHUNK_SIZE = 1024 + def thread_write_input(sock_input): print("starting thread_write_input...") - # Store commulative output - output_total = "" try: while True: # Get user input dynamically @@ -38,20 +39,17 @@ def thread_write_input(sock_input): # Check if the user wants to exit if user_input.lower().strip() == 'exit': break - sock_input.send(user_input.encode("utf-8")[:1024]) + sock_input.send(user_input.encode("ascii", errors="replace")[:CHUNK_SIZE]) except: print("exit output") - finally: - # Save total output into a log - print(output_total) def thread_read_output(sock_output): print("starting thread_read_output...") try: while True: - request = sock_output.recv(1024) - request = request.decode("utf-8") # convert bytes tro str + request = sock_output.recv(CHUNK_SIZE) + request = request.decode("ascii", errors="replace") # convert bytes to str print(request, end='') except: From 362763bb0f5f366acfaa4d13c580107a6268af8a Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Wed, 10 Jan 2024 15:39:50 -0800 Subject: [PATCH 23/44] getting details sorted --- .../survey_planner/tools/command_astrobee | 25 +++++++++++-------- .../survey_planner/tools/monitor_astrobee | 5 ++-- 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/tools/command_astrobee b/astrobee/survey_manager/survey_planner/tools/command_astrobee index 6cac9a61..92d2bab5 100755 --- a/astrobee/survey_manager/survey_planner/tools/command_astrobee +++ b/astrobee/survey_manager/survey_planner/tools/command_astrobee @@ -131,13 +131,14 @@ class ProcessExecutor: self.sock_output.close() def thread_write_output(self, process): - print("starting thread_write_output...") + # print("starting thread_write_output...") # Store commulative output output_total = "" connected = False try: while True: # Get output from process + # print("waiting for output") output = process.stdout.readline() if (output == '' and process.poll() is not None) or self._stop_event.is_set(): break @@ -147,7 +148,7 @@ class ProcessExecutor: try: # If socket is not connected try to connect if not connected: - print("trying to connect") + # print("trying to connect") conn, addr = self.sock_output.accept() conn.setblocking(False) @@ -175,11 +176,11 @@ class ProcessExecutor: # print(output_total) def thread_read_input(self, process): - print("starting thread_read_input...") + # print("starting thread_read_input...") try: while True: while not self._stop_event.is_set(): - print("waiting for connection") + # print("waiting for connection") try: client_socket, client_address = self.sock_input.accept() break @@ -191,11 +192,11 @@ class ProcessExecutor: while True: - print("accepted connection:") + # print("accepted connection:") print(client_address) while not self._stop_event.is_set(): - print("waiting to receive") + # print("waiting to receive") try: request = client_socket.recv(CHUNK_SIZE).decode("ascii", errors="replace") break @@ -231,6 +232,8 @@ class ProcessExecutor: output_thread = threading.Thread(target=self.thread_write_output, args=(process,)) output_thread.start() + while output_thread.is_alive(): + rospy.sleep(1) output_thread.join() # Get the return code of the process return_code = process.poll() @@ -252,24 +255,25 @@ class ProcessExecutor: return return_code def recursive_command(self, command): - while True: + while not rospy.is_shutdown(): user_input = input("Do you want to continue? (y/n): ").lower().strip() if user_input == 'y': exit_code = self.send_command(command) if exit_code != 0: - exit_code = recursive_command(command) # Continue recursively + exit_code = self.recursive_command(command) # Continue recursively return exit_code elif user_input == 'n': print("Exiting.") return 1 else: print("Invalid input. Please enter 'y' or 'n'.") + return 1 def send_command_recursive(self, command): print("Sending recursive command") exit_code = self.send_command(command) print("Exit code " + str(exit_code)) - if (exit_code): + if (exit_code and not rospy.is_shutdown()): exit_code = self.recursive_command(command) return exit_code @@ -367,7 +371,8 @@ class CommandExecutor: self.ack_needed = False def publish_and_wait_response(self, cmd): - + if rospy.is_shutdown(): + return 1 # Publish the CommandStamped message self.ack_needed = True self.pub_command.publish(cmd) diff --git a/astrobee/survey_manager/survey_planner/tools/monitor_astrobee b/astrobee/survey_manager/survey_planner/tools/monitor_astrobee index 37c74f3e..7b772b65 100755 --- a/astrobee/survey_manager/survey_planner/tools/monitor_astrobee +++ b/astrobee/survey_manager/survey_planner/tools/monitor_astrobee @@ -29,11 +29,10 @@ import threading CHUNK_SIZE = 1024 def thread_write_input(sock_input): - print("starting thread_write_input...") + # print("starting thread_write_input...") try: while True: # Get user input dynamically - print("get input") user_input = input() print("user input: " + user_input) # Check if the user wants to exit @@ -45,7 +44,7 @@ def thread_write_input(sock_input): print("exit output") def thread_read_output(sock_output): - print("starting thread_read_output...") + # print("starting thread_read_output...") try: while True: request = sock_output.recv(CHUNK_SIZE) From d3d3228adfed0d7b26aa4e0d5835837472a803f8 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Wed, 10 Jan 2024 15:40:15 -0800 Subject: [PATCH 24/44] making output more readable --- astrobee/behaviors/inspection/tools/inspection_tool.cc | 3 +++ 1 file changed, 3 insertions(+) diff --git a/astrobee/behaviors/inspection/tools/inspection_tool.cc b/astrobee/behaviors/inspection/tools/inspection_tool.cc index 4a5de4c4..48b6235d 100644 --- a/astrobee/behaviors/inspection/tools/inspection_tool.cc +++ b/astrobee/behaviors/inspection/tools/inspection_tool.cc @@ -113,6 +113,7 @@ DEFINE_double(deadline, -1.0, "Action deadline timeout"); // Match the internal states and responses with the message definition using STATE = isaac_msgs::InspectionState; bool stopflag_ = false; +std::string feedback_old = ""; bool has_only_whitespace_or_comments(const std::string & str) { for (std::string::const_iterator it = str.begin(); it != str.end(); it++) { @@ -209,6 +210,8 @@ void FeedbackCallback(isaac_msgs::InspectionFeedbackConstPtr const& feedback) { + " -> " + feedback->state.fsm_state + " (" + feedback->state.fsm_subevent + " -> " + feedback->state.fsm_substate + ")"; + if (s == feedback_old) return; + feedback_old = s; if (s.size() < 70) s.append(70 - s.size(), ' '); std::cout << "\r" << s.substr(0, 70) << "|Input: " << std::flush; } From 5e89aa473ae18c93a97eb8e1356c24fc95472ded Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Fri, 12 Jan 2024 14:58:24 -0800 Subject: [PATCH 25/44] installing script to be found in robot install --- astrobee/survey_manager/survey_planner/CMakeLists.txt | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/astrobee/survey_manager/survey_planner/CMakeLists.txt b/astrobee/survey_manager/survey_planner/CMakeLists.txt index e29addac..6766fa7d 100644 --- a/astrobee/survey_manager/survey_planner/CMakeLists.txt +++ b/astrobee/survey_manager/survey_planner/CMakeLists.txt @@ -52,7 +52,10 @@ target_link_libraries(move_action_node ${catkin_LIBRARIES} ) ## Install ## ############# -catkin_install_python(PROGRAMS tools/command_astrobee tools/monitor_astrobee +catkin_install_python(PROGRAMS + tools/command_astrobee + tools/monitor_astrobee + tools/problem_generator.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) # Install actions From 8038c1f471e514444362a7f911653e763faa0424 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Mon, 15 Jan 2024 12:14:07 -0800 Subject: [PATCH 26/44] adding move action starting correct process --- .../survey_planner/src/move_action_node.cpp | 58 +++++++++++++++++-- 1 file changed, 52 insertions(+), 6 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/src/move_action_node.cpp b/astrobee/survey_manager/survey_planner/src/move_action_node.cpp index e6a6d4da..83d253e0 100644 --- a/astrobee/survey_manager/survey_planner/src/move_action_node.cpp +++ b/astrobee/survey_manager/survey_planner/src/move_action_node.cpp @@ -21,8 +21,12 @@ #include +#include +#include + #include #include +#include namespace plansys2_actions { @@ -31,6 +35,8 @@ class MoveAction : public plansys2::ActionExecutorClient { MoveAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate) : ActionExecutorClient(nh, action, rate) { progress_ = 0.0; + process_pipe_ = nullptr; + process_pid_ = -1; } protected: @@ -45,14 +51,52 @@ class MoveAction : public plansys2::ActionExecutorClient { finish(false, 1.0, "Not enough arguments for [MOVE] command"); } - if (progress_ < 1.0) { - progress_ += 0.05; - send_feedback(progress_, "Move and Inspect running"); + // Start process if not started yet + if (progress_ == 0.0) { + std::string command = + "rosrun survey_planner command_astrobee " + robot_name_ + " move " + towards + " " + from + " run1"; + + std::cout << command << std::flush; + // Open a pipe to a command and get a FILE* for reading + process_pipe_ = popen(command.c_str(), "r"); + + if (!process_pipe_) { + perror("popen"); + finish(false, 1.0, "Failed to start the process"); + return; + } + + // Get the process ID + process_pid_ = fileno(process_pipe_); + progress_ = 0.02; + return; + } + + // Check if the process is still running + int status; + pid_t result = waitpid(process_pid_, &status, WNOHANG); + + if (result == 0) { + // Process still running, do nothing + } else if (result > 0) { + // Process completed + if (status == 0) { + std::cout << "Command exited with status success " << std::endl; + finish(true, 1.0, "Move and Inspect completed"); + } else { + std::cout << "Command terminated with status fail: " << status << std::endl; + finish(false, 1.0, "Move and Inspect terminated by signal"); + } } else { - finish(true, 1.0, "Move and Inspect completed"); + perror("waitpid"); + finish(false, 1.0, "Error while waiting for process"); + } + + - progress_ = 0.0; - std::cout << std::endl; + if (progress_ < 1.0) { + progress_ += 0.02; + send_feedback(progress_, "Move and Inspect running"); } std::cout << "\t ** [Move and Inspect] Robot " << robot_name_ << " moving from " << from << " towards " << towards @@ -62,6 +106,8 @@ class MoveAction : public plansys2::ActionExecutorClient { float progress_; std::string robot_name_; + FILE* process_pipe_; + pid_t process_pid_; }; } // namespace plansys2_actions From ab97e9393452ecafafcf22055fe07a8cd6049c5b Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Mon, 15 Jan 2024 12:16:27 -0800 Subject: [PATCH 27/44] adding parameters for easy testing --- .../survey_planner/data/survey_static.yaml | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/astrobee/survey_manager/survey_planner/data/survey_static.yaml b/astrobee/survey_manager/survey_planner/data/survey_static.yaml index 552cbd79..968e6396 100644 --- a/astrobee/survey_manager/survey_planner/data/survey_static.yaml +++ b/astrobee/survey_manager/survey_planner/data/survey_static.yaml @@ -63,6 +63,25 @@ bays_move: usl_bay5: "-pos '0.5 0 4.8'" usl_bay6: "-pos '-0.5 0 4.8'" + # Granite testing + # jem_hatch_to_nod2: "-move -pos '-0.3 -0.1 -0.68' -att '0 0 0'" + # jem_hatch_from_nod2: "-move -pos '-0.3 -0.1 -0.68' -att '0 0 180'" + # nod2_hatch_from_jem: "-move -pos '0.3 -0.1 -0.68' -att '0 0 0'" + # nod2_hatch_to_jem: "-move -pos '0.3 -0.1 -0.68' -att '0 0 180'" + +maps: + jem: "iss.map" + nod2: "isaac.map" + usl: "usl_only.map" + +exposure: + jem: 175 + nod2: 300 + usl: 300 +berth: + berth1: "1" + berth2: "2" + bogus_bays: [jem_bay0, jem_bay8] berths: [berth1, berth2] robots: [bumble, honey] From 55d4402df21b210f4c4f0bfd43a4e8b5cf1d1524 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Mon, 15 Jan 2024 14:23:33 -0800 Subject: [PATCH 28/44] fixes on command astrobee --- .../survey_planner/CMakeLists.txt | 9 +-- .../survey_planner/data/survey_static.yaml | 16 ++-- .../survey_manager/survey_planner/setup.py | 7 ++ .../tools/survey_planner/__init__.py | 0 .../{ => survey_planner}/command_astrobee | 74 ++++++++++--------- .../{ => survey_planner}/monitor_astrobee | 0 .../{ => survey_planner}/plan_interpreter.py | 1 - .../{ => survey_planner}/problem_generator.py | 0 8 files changed, 60 insertions(+), 47 deletions(-) create mode 100644 astrobee/survey_manager/survey_planner/setup.py create mode 100644 astrobee/survey_manager/survey_planner/tools/survey_planner/__init__.py rename astrobee/survey_manager/survey_planner/tools/{ => survey_planner}/command_astrobee (91%) rename astrobee/survey_manager/survey_planner/tools/{ => survey_planner}/monitor_astrobee (100%) rename astrobee/survey_manager/survey_planner/tools/{ => survey_planner}/plan_interpreter.py (99%) rename astrobee/survey_manager/survey_planner/tools/{ => survey_planner}/problem_generator.py (100%) diff --git a/astrobee/survey_manager/survey_planner/CMakeLists.txt b/astrobee/survey_manager/survey_planner/CMakeLists.txt index 6766fa7d..196f5c56 100644 --- a/astrobee/survey_manager/survey_planner/CMakeLists.txt +++ b/astrobee/survey_manager/survey_planner/CMakeLists.txt @@ -52,11 +52,10 @@ target_link_libraries(move_action_node ${catkin_LIBRARIES} ) ## Install ## ############# -catkin_install_python(PROGRAMS - tools/command_astrobee - tools/monitor_astrobee - tools/problem_generator.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +# Allow other packages to use python scripts from this package +catkin_python_setup() + +catkin_package() # Install actions install(TARGETS move_action_node diff --git a/astrobee/survey_manager/survey_planner/data/survey_static.yaml b/astrobee/survey_manager/survey_planner/data/survey_static.yaml index 968e6396..2602a325 100644 --- a/astrobee/survey_manager/survey_planner/data/survey_static.yaml +++ b/astrobee/survey_manager/survey_planner/data/survey_static.yaml @@ -45,17 +45,17 @@ bays_move: jem_bay5: "-pos '11 -8 4.8'" jem_bay6: "-pos '11 -9 4.8'" jem_bay7: "-pos '11 -9.7 4.8'" - jem_hatch_to_nod2: "-move -pos '11 -3.5 4.8' -att '0 0 90'" - jem_hatch_from_nod2: "-move -pos '11 -3.5 4.8' -att '0 0 -90'" - nod2_hatch_from_jem: "-move -pos '11 -1.0 4.8' -att '0 0 90'" - nod2_hatch_to_jem: "-move -pos '11 -1.0 4.8' -att '0 0 -90'" + jem_hatch_to_nod2: "-move -pos '11 -3.5 4.8' -att '0 0 1 90'" + jem_hatch_from_nod2: "-move -pos '11 -3.5 4.8' -att '0 0 1 -90'" + nod2_hatch_from_jem: "-move -pos '11 -1.0 4.8' -att '0 0 1 90'" + nod2_hatch_to_jem: "-move -pos '11 -1.0 4.8' -att '0 0 1 -90'" nod2_bay2: "-pos '11 0 4.8'" nod2_bay3: "-pos '10 0 4.8'" nod2_bay4: "-pos '9 0 4.8'" - nod2_hatch_to_usl: "-move -pos '7.8 -3.5 4.8' -att '0 0 180'" - nod2_hatch_from_usl: "-move -pos '7.8 -3.5 4.8' -att '0 0 0'" - usl_hatch_from_nod2: "-move -pos '5.3 -1.0 4.8' -att '0 0 180'" - usl_hatch_to_nod2: "-move -pos '5.3 -1.0 4.8' -att '0 0 0'" + nod2_hatch_to_usl: "-move -pos '7.8 -3.5 4.8' -att '0 0 1 180'" + nod2_hatch_from_usl: "-move -pos '7.8 -3.5 4.8' -att '0 0 1 0'" + usl_hatch_from_nod2: "-move -pos '5.3 -1.0 4.8' -att '0 0 1 180'" + usl_hatch_to_nod2: "-move -pos '5.3 -1.0 4.8' -att '0 0 1 0'" usl_bay1: "-pos '4.7 0 4.8'" usl_bay2: "-pos '3.65 0 4.8'" usl_bay3: "-pos '2.6 0 4.8'" diff --git a/astrobee/survey_manager/survey_planner/setup.py b/astrobee/survey_manager/survey_planner/setup.py new file mode 100644 index 00000000..ef0e38a1 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/setup.py @@ -0,0 +1,7 @@ +from distutils.core import setup + +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup(packages=["survey_planner"], package_dir={"": "tools"}) + +setup(**d) diff --git a/astrobee/survey_manager/survey_planner/tools/survey_planner/__init__.py b/astrobee/survey_manager/survey_planner/tools/survey_planner/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/astrobee/survey_manager/survey_planner/tools/command_astrobee b/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee similarity index 91% rename from astrobee/survey_manager/survey_planner/tools/command_astrobee rename to astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee index 92d2bab5..f3f6af26 100755 --- a/astrobee/survey_manager/survey_planner/tools/command_astrobee +++ b/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee @@ -41,11 +41,10 @@ from ff_msgs.msg import ( from std_msgs.msg import Header, String # # Get the directory of the current script -sys.path.append(os.path.dirname(os.path.realpath(__file__))) -from problem_generator import load_yaml +from survey_planner.problem_generator import load_yaml # Constants -MAX_COUNTER = 3 +MAX_COUNTER = 10 CHUNK_SIZE = 1024 def get_stereo_traj(config_static, from_bay, to_bay): @@ -62,36 +61,38 @@ def get_stereo_traj(config_static, from_bay, to_bay): return fplan -def exposure_change(bay_origin, bay_destination, value): +def exposure_change(config_static, bay_origin, bay_destination): # Going to JEM if bay_origin == "nod2_hatch_to_jem" and bay_destination == "jem_hatch_from_nod2": - value = 100 - return True + print("CHANGING EXPOSURE TO JEM") + return config_static["exposure"]["jem"] + # Going to NOD2 if (bay_origin == "jem_hatch_to_nod2" and bay_destination == "nod2_hatch_from_jem" or bay_origin == "usl_hatch_to_nod2" and bay_destination == "nod2_hatch_from_usl"): - value = 300 - return True + print("CHANGING EXPOSURE TO NOD2") + return config_static["exposure"]["nod2"] + # Going to USL if bay_origin == "nod2_hatch_to_usl" and bay_destination == "usl_hatch_from_nod2": - value = 300 - return True - return False + return config_static["exposure"]["usl"] -def map_change(bay_origin, bay_destination, map_name): + return 0 + +def map_change(config_static, bay_origin, bay_destination): # Going to JEM if bay_origin == "nod2_hatch_to_jem" and bay_destination == "jem_hatch_from_nod2": - map_name = "iss.map" - return True + print("CHANGING MAP TO JEM") + return config_static["maps"]["jem"] # Going to NOD2 if (bay_origin == "jem_hatch_to_nod2" and bay_destination == "nod2_hatch_from_jem" or bay_origin == "usl_hatch_to_nod2" and bay_destination == "nod2_hatch_from_usl"): - map_name = "20220331_isaac5.map" - return True + print("CHANGING MAP TO NOD2") + return config_static["maps"]["nod2"] # Going to USL if bay_origin == "nod2_hatch_to_usl" and bay_destination == "usl_hatch_from_nod2": - map_name = "20220617_Isaac10_USLonly.map" - return True + return config_static["maps"]["usl"] + return "" # This class starts a new process and lets you monitor the input and output @@ -114,12 +115,14 @@ class ProcessExecutor: self.sock_input.settimeout(1) # Set a timeout for socket operations self.sock_input.bind(self.input_path) self.sock_input.listen(1) # Listen for one connection + self.sock_input_connected = False # Declare socket for process output self.sock_output = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) self.sock_output.settimeout(1) # Set a timeout for socket operations self.sock_output.bind(self.output_path) self.sock_output.listen(1) # Listen for one connection + self.sock_output_connected = False # Declare event that will stop input thread self._stop_event = threading.Event() @@ -134,7 +137,6 @@ class ProcessExecutor: # print("starting thread_write_output...") # Store commulative output output_total = "" - connected = False try: while True: # Get output from process @@ -143,16 +145,17 @@ class ProcessExecutor: if (output == '' and process.poll() is not None) or self._stop_event.is_set(): break if output: + rospy.loginfo(output) output_total += output try: # If socket is not connected try to connect - if not connected: + if not self.sock_output_connected: # print("trying to connect") conn, addr = self.sock_output.accept() conn.setblocking(False) - connected = True + self.sock_output_connected = True encoded_message = output_total.encode("ascii", errors="replace") for i in range(0, len(encoded_message), CHUNK_SIZE): @@ -160,20 +163,20 @@ class ProcessExecutor: conn.sendall(chunk) # If socket is already connected, send output - elif connected: + elif self.sock_output_connected: conn.send(output.encode("ascii", errors="replace")[:CHUNK_SIZE]) except socket.timeout: continue except (socket.error, BrokenPipeError): print("Error sending data. Receiver may have disconnected.") - connected = False + self.sock_output_connected = False except Exception as e: print("exit output:") print(e) # finally: # # Save total output into a log - # print(output_total) + # rospy.loginfo(output_total) def thread_read_input(self, process): # print("starting thread_read_input...") @@ -330,11 +333,13 @@ class CommandExecutor: def change_exposure(self, val): #TBD - rospy.loginfo("Change exposure to " + val) + rospy.loginfo("Change exposure to " + str(val)) + return 0 def change_map(self, map_name): #TBD rospy.loginfo("Change map to " + map_name) + return 0 def set_plan(self): # TODO Add call to plan_pub.cc @@ -412,14 +417,17 @@ def survey_manager_executor(robot_name, goal, to_bay, from_bay, run, config_stat if not current_robot: rospy.loginfo("We're in simulation. Let's get the robotname using the topic") # This is a latching messge so it shouldn't take long - data = rospy.wait_for_message('/robot_name', String, timeout=5) - current_robot = data.data.lower() + try: + data = rospy.wait_for_message('/robot_name', String, timeout=5) + current_robot = data.data.lower() + except: + current_robot = "" sim = True ns = "" # If we're commanding a robot remotely if current_robot != robot_name: - rospy.loginfo("We're commanding a robot remotely!") + rospy.loginfo("We're commanding a namespaced robot!") ns = " -remote -ns " + robot_name # Command executor will add namespace for bridge forwarding command_executor = CommandExecutor(robot_name) @@ -431,7 +439,7 @@ def survey_manager_executor(robot_name, goal, to_bay, from_bay, run, config_stat exit_code = 0 if goal == "dock": - exit_code += process_executor.send_command_recursive("rosrun executive teleop_tool -dock" + ns) + exit_code += process_executor.send_command_recursive("rosrun executive teleop_tool -dock" + ns + " -berth " + config_static["berth"][to_bay]) elif goal == "undock": exit_code += process_executor.send_command_recursive("rosrun executive teleop_tool -undock" + ns) @@ -439,12 +447,12 @@ def survey_manager_executor(robot_name, goal, to_bay, from_bay, run, config_stat elif goal == "move": exit_code += process_executor.send_command_recursive("rosrun executive teleop_tool -move " + config_static["bays_move"][to_bay] + ns) # Change exposure if needed - exposure_value = 0 - if exposure_change(from_bay, to_bay, exposure_value): + exposure_value = exposure_change(config_static, from_bay, to_bay) + if exposure_value != 0: exit_code += command_executor.change_exposure(exposure_value) # Change map if needed - map_name = "" - if map_change(from_bay, to_bay, map_name): + map_name = map_change(config_static, from_bay, to_bay) + if map_name != "": exit_code += command_executor.change_map(map_name) elif goal == "panorama": diff --git a/astrobee/survey_manager/survey_planner/tools/monitor_astrobee b/astrobee/survey_manager/survey_planner/tools/survey_planner/monitor_astrobee similarity index 100% rename from astrobee/survey_manager/survey_planner/tools/monitor_astrobee rename to astrobee/survey_manager/survey_planner/tools/survey_planner/monitor_astrobee diff --git a/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py b/astrobee/survey_manager/survey_planner/tools/survey_planner/plan_interpreter.py similarity index 99% rename from astrobee/survey_manager/survey_planner/tools/plan_interpreter.py rename to astrobee/survey_manager/survey_planner/tools/survey_planner/plan_interpreter.py index 9798a393..a3df6c92 100755 --- a/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py +++ b/astrobee/survey_manager/survey_planner/tools/survey_planner/plan_interpreter.py @@ -42,7 +42,6 @@ from matplotlib import collections as mc from matplotlib import patches as mp from matplotlib import pyplot as plt - from problem_generator import DATA_DIR, load_yaml, path_list DEFAULT_CONFIGS = [ diff --git a/astrobee/survey_manager/survey_planner/tools/problem_generator.py b/astrobee/survey_manager/survey_planner/tools/survey_planner/problem_generator.py similarity index 100% rename from astrobee/survey_manager/survey_planner/tools/problem_generator.py rename to astrobee/survey_manager/survey_planner/tools/survey_planner/problem_generator.py From 9a3e19157f2bdc294dea52c78970151dcc1271d5 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Wed, 17 Jan 2024 09:06:58 -0800 Subject: [PATCH 29/44] add remaining actions + documentation --- .../survey_planner/CMakeLists.txt | 14 +- .../launch/survey_domain.launch | 8 ++ .../survey_manager/survey_planner/readme.md | 124 +++++++++++++++++ .../survey_planner/src/dock_action_node.cpp | 128 ++++++++++++++++++ .../survey_planner/src/move_action_node.cpp | 39 ++---- .../src/panorama_action_node.cpp | 128 ++++++++++++++++++ .../survey_planner/src/stereo_action_node.cpp | 128 ++++++++++++++++++ .../survey_planner/src/undock_action_node.cpp | 128 ++++++++++++++++++ 8 files changed, 671 insertions(+), 26 deletions(-) create mode 100644 astrobee/survey_manager/survey_planner/src/dock_action_node.cpp create mode 100644 astrobee/survey_manager/survey_planner/src/panorama_action_node.cpp create mode 100644 astrobee/survey_manager/survey_planner/src/stereo_action_node.cpp create mode 100644 astrobee/survey_manager/survey_planner/src/undock_action_node.cpp diff --git a/astrobee/survey_manager/survey_planner/CMakeLists.txt b/astrobee/survey_manager/survey_planner/CMakeLists.txt index 196f5c56..58a958c5 100644 --- a/astrobee/survey_manager/survey_planner/CMakeLists.txt +++ b/astrobee/survey_manager/survey_planner/CMakeLists.txt @@ -44,9 +44,21 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -# Action for move_inspect +# Action for move add_executable(move_action_node src/move_action_node.cpp) target_link_libraries(move_action_node ${catkin_LIBRARIES} ) +# Action for dock +add_executable(dock_action_node src/dock_action_node.cpp) +target_link_libraries(dock_action_node ${catkin_LIBRARIES} ) +# Action for undock +add_executable(undock_action_node src/undock_action_node.cpp) +target_link_libraries(undock_action_node ${catkin_LIBRARIES} ) +# Action for panorama +add_executable(panorama_action_node src/panorama_action_node.cpp) +target_link_libraries(panorama_action_node ${catkin_LIBRARIES} ) +# Action for stereo +add_executable(stereo_action_node src/stereo_action_node.cpp) +target_link_libraries(stereo_action_node ${catkin_LIBRARIES} ) ############# ## Install ## diff --git a/astrobee/survey_manager/survey_planner/launch/survey_domain.launch b/astrobee/survey_manager/survey_planner/launch/survey_domain.launch index b0cfad2d..5306ab7f 100644 --- a/astrobee/survey_manager/survey_planner/launch/survey_domain.launch +++ b/astrobee/survey_manager/survey_planner/launch/survey_domain.launch @@ -42,6 +42,14 @@ + + + + + + + + diff --git a/astrobee/survey_manager/survey_planner/readme.md b/astrobee/survey_manager/survey_planner/readme.md index 6fd77ac8..c4a56854 100644 --- a/astrobee/survey_manager/survey_planner/readme.md +++ b/astrobee/survey_manager/survey_planner/readme.md @@ -3,3 +3,127 @@ Planning and scheduling of queued survey actions using Playsys2 for PDDL solutions and behavior trees for execution. Based on [preliminary work](https://github.com/traclabs/astrobee_task_planning_ws) by [Ana](https://github.com/ana-GT) at [Traclabs](https://traclabs.com). + +## Starting suvey planner + +The survey_planner as of now starts separately from the isaac fsw. This is necessary because the planner crashes very easily, so having the ability to easily restart this component is necessary. + +To start the planner and the terminal interface (in the robot, add the prefix `/opt/isaac/env_wrapper.sh`): + + roslaunch survey_planner survey_domain.launch + rosrun plansys2_terminal plansys2_terminal + +To monitor an execution and to input commands to the execution: + + rosrun survey_planner monitor_astrobee $ROBOTNAME + +## Running actions manually + +There are 5 different actions at the moment 'move', 'dock', 'undock', 'panorama', 'stereo'. +To run each action individually through the survey_planner you must add the correct predicates before sending the action command or else the plansys2 executor will crash. Also, if you want to re-send the action you'll want to re-send the predicates too. + +Don't forget to change the commands to use the target robot's name (bumble is what is used in simulation) + +If the survey_planner crashes sometimes the simulation has to be restarted, for it to restart in a good state. + +### Move + + set instance bumble robot + set instance jem_bay7 location + set instance jem_bay6 location + set instance jem_bay5 location + + set predicate (robot-available bumble) + set predicate (robot-at bumble jem_bay7) + set predicate (location-available jem_bay6) + + set predicate (move-connected jem_bay7 jem_bay6) + set predicate (location-real jem_bay6) + set predicate (locations-different jem_bay5 jem_bay7) + set predicate (move-connected jem_bay5 jem_bay6) + set predicate (location-available jem_bay5) + + + run action (move bumble jem_bay7 jem_bay6 jem_bay5) + +### Dock + + set instance bumble robot + set instance jem_bay7 location + set instance berth1 location + + set predicate (robot-available bumble) + set predicate (robot-at bumble jem_bay7) + set predicate (dock-connected jem_bay7 berth1) + set predicate (location-available berth1) + + run action (dock bumble jem_bay7 berth1) + + +### Undock + + + set instance bumble robot + set instance jem_bay6 location + set instance jem_bay7 location + set instance jem_bay8 location + set instance berth1 location + + set predicate (robot-available bumble) + set predicate (robot-at bumble berth1) + set predicate (dock-connected jem_bay7 berth1) + set predicate (location-available jem_bay7) + set predicate (location-real jem_bay7) + set predicate (locations-different jem_bay8 jem_bay6) + set predicate (move-connected jem_bay8 jem_bay7) + set predicate (move-connected jem_bay6 jem_bay7) + set predicate (location-available jem_bay8) + set predicate (location-available jem_bay6) + + run action (undock bumble berth1 jem_bay7 jem_bay8 jem_bay6) + + +### Panorama + + set instance bumble robot + set instance jem_bay6 location + set instance o0 order + + set predicate (robot-available bumble) + + set function (= (order-identity o0) 0) + set function (= (robot-order bumble) -1) + + set predicate (robot-at bumble jem_bay6) + + run action (panorama bumble o0 jem_bay6) + + +### Stereo + + + set instance bumble robot + set instance jem_bay7 location + set instance jem_bay6 location + set instance jem_bay5 location + set instance jem_bay4 location + set instance jem_bay3 location + set instance o0 order + + + set predicate (robot-available bumble) + set predicate (robot-at bumble jem_bay7) + set predicate (location-real jem_bay4) + set predicate (need-stereo bumble o0 jem_bay7 jem_bay4) + set predicate (location-available jem_bay4) + set predicate (locations-different jem_bay5 jem_bay3) + set predicate (move-connected jem_bay5 jem_bay4) + set predicate (move-connected jem_bay3 jem_bay4) + set predicate (location-available jem_bay5) + set predicate (location-available jem_bay3) + + set function (= (order-identity o0) 0) + set function (= (robot-order bumble) -1) + + + run action (stereo bumble o0 jem_bay7 jem_bay4 jem_bay5 jem_bay3) \ No newline at end of file diff --git a/astrobee/survey_manager/survey_planner/src/dock_action_node.cpp b/astrobee/survey_manager/survey_planner/src/dock_action_node.cpp new file mode 100644 index 00000000..b3bbb0d5 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/src/dock_action_node.cpp @@ -0,0 +1,128 @@ +/* Copyright (c) 2021, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking + * platform" software is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +#include + +#include +#include + +#include +#include +#include + +namespace plansys2_actions { + +class DockAction : public plansys2::ActionExecutorClient { + public: + DockAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate) + : ActionExecutorClient(nh, action, rate) { + progress_ = 0.0; + process_pipe_ = nullptr; + } + + protected: + void do_work() { + std::string from, towards; + + if (get_arguments().size() > 2) { + robot_name_ = get_arguments()[0]; + from = get_arguments()[1]; + towards = get_arguments()[2]; + } else { + finish(false, 1.0, "Not enough arguments for [DOCK] command"); + } + + // Start process if not started yet + if (progress_ == 0.0) { + std::string command = + "rosrun survey_planner command_astrobee " + robot_name_ + " dock " + towards + " " + from + " run1"; + + std::cout << command << std::endl; + // Open a pipe to a command and get a FILE* for reading + process_pipe_ = popen(command.c_str(), "r"); + + if (!process_pipe_) { + perror("popen"); + finish(false, 1.0, "Failed to start the process"); + return; + } + + // Get the process ID + progress_ = 0.02; + return; + } + + char buffer[1028]; + + if (fgets(buffer, 1028, process_pipe_) != NULL) { + std::cout << buffer << std::endl; + if (progress_ < 1.0) { + progress_ += 0.02; + send_feedback(progress_, "Dock and Inspect running"); + } + + std::cout << "\t ** [Dock] Robot " << robot_name_ << " moving from " << from << " towards " << towards + << " ... [" << std::min(100.0, progress_ * 100.0) << "%] " << std::endl; + } else { + int status = pclose(process_pipe_); + std::cout << "Finished.\n" << std::endl; + if (status == 0) { + std::cout << "Command exited with status success " << std::endl; + finish(true, 1.0, "Dock completed"); + } else { + std::cout << "Command terminated with status fail: " << status << std::endl; + finish(false, 1.0, "Dock terminated by signal"); + } + } + } + + float progress_; + std::string robot_name_; + FILE* process_pipe_; +}; +} // namespace plansys2_actions + + +// Main entry point for application +int main(int argc, char *argv[]) { + // Initialize a ros node + ros::init(argc, argv, "dock_action"); + + std::string name = ros::this_node::getName(); + if (name.empty() || (name.size() == 1 && name[0] == '/')) + name = "default"; + else if (name[0] == '/') + name = name.substr(1); + + ros::NodeHandle nh("~"); + nh.setParam("action_name", "dock"); + + // Start action node + // We could actually add multiple action nodes here being aware that we might need a ros::AsyncSpinner + // (https://github.com/Bckempa/ros2_planning_system/blob/noetic-devel/plansys2_bt_actions/src/bt_action_node.cpp#L41) + auto action_node = std::make_shared(nh, "dock", std::chrono::seconds(1)); + action_node->trigger_transition(ros::lifecycle::CONFIGURE); + + // Synchronous mode + ros::spin(); + // Make for great success + return 0; +} diff --git a/astrobee/survey_manager/survey_planner/src/move_action_node.cpp b/astrobee/survey_manager/survey_planner/src/move_action_node.cpp index 83d253e0..39bc1664 100644 --- a/astrobee/survey_manager/survey_planner/src/move_action_node.cpp +++ b/astrobee/survey_manager/survey_planner/src/move_action_node.cpp @@ -36,7 +36,6 @@ class MoveAction : public plansys2::ActionExecutorClient { : ActionExecutorClient(nh, action, rate) { progress_ = 0.0; process_pipe_ = nullptr; - process_pid_ = -1; } protected: @@ -56,7 +55,7 @@ class MoveAction : public plansys2::ActionExecutorClient { std::string command = "rosrun survey_planner command_astrobee " + robot_name_ + " move " + towards + " " + from + " run1"; - std::cout << command << std::flush; + std::cout << command << std::endl; // Open a pipe to a command and get a FILE* for reading process_pipe_ = popen(command.c_str(), "r"); @@ -67,19 +66,24 @@ class MoveAction : public plansys2::ActionExecutorClient { } // Get the process ID - process_pid_ = fileno(process_pipe_); progress_ = 0.02; return; } - // Check if the process is still running - int status; - pid_t result = waitpid(process_pid_, &status, WNOHANG); + char buffer[1028]; - if (result == 0) { - // Process still running, do nothing - } else if (result > 0) { - // Process completed + if (fgets(buffer, 1028, process_pipe_) != NULL) { + std::cout << buffer << std::endl; + if (progress_ < 1.0) { + progress_ += 0.02; + send_feedback(progress_, "Move and Inspect running"); + } + + std::cout << "\t ** [Move and Inspect] Robot " << robot_name_ << " moving from " << from << " towards " << towards + << " ... [" << std::min(100.0, progress_ * 100.0) << "%] " << std::endl; + } else { + int status = pclose(process_pipe_); + std::cout << "Finished.\n" << std::endl; if (status == 0) { std::cout << "Command exited with status success " << std::endl; finish(true, 1.0, "Move and Inspect completed"); @@ -87,27 +91,12 @@ class MoveAction : public plansys2::ActionExecutorClient { std::cout << "Command terminated with status fail: " << status << std::endl; finish(false, 1.0, "Move and Inspect terminated by signal"); } - } else { - perror("waitpid"); - finish(false, 1.0, "Error while waiting for process"); - } - - - - if (progress_ < 1.0) { - progress_ += 0.02; - send_feedback(progress_, "Move and Inspect running"); } - - std::cout << "\t ** [Move and Inspect] Robot " << robot_name_ << " moving from " << from << " towards " << towards - << " ... [" << std::min(100.0, progress_ * 100.0) << "%] " << std::flush; } - float progress_; std::string robot_name_; FILE* process_pipe_; - pid_t process_pid_; }; } // namespace plansys2_actions diff --git a/astrobee/survey_manager/survey_planner/src/panorama_action_node.cpp b/astrobee/survey_manager/survey_planner/src/panorama_action_node.cpp new file mode 100644 index 00000000..1ce9c219 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/src/panorama_action_node.cpp @@ -0,0 +1,128 @@ +/* Copyright (c) 2021, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking + * platform" software is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +#include + +#include +#include + +#include +#include +#include + +namespace plansys2_actions { + +class PanoramaAction : public plansys2::ActionExecutorClient { + public: + PanoramaAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate) + : ActionExecutorClient(nh, action, rate) { + progress_ = 0.0; + process_pipe_ = nullptr; + } + + protected: + void do_work() { + std::string from, towards; + + if (get_arguments().size() > 2) { + robot_name_ = get_arguments()[0]; + from = get_arguments()[1]; + towards = get_arguments()[2]; + } else { + finish(false, 1.0, "Not enough arguments for [PANORAMA] command"); + } + + // Start process if not started yet + if (progress_ == 0.0) { + std::string command = + "rosrun survey_planner command_astrobee " + robot_name_ + " panorama " + towards + " " + from + " run1"; + + std::cout << command << std::endl; + // Open a pipe to a command and get a FILE* for reading + process_pipe_ = popen(command.c_str(), "r"); + + if (!process_pipe_) { + perror("popen"); + finish(false, 1.0, "Failed to start the process"); + return; + } + + // Get the process ID + progress_ = 0.02; + return; + } + + char buffer[1028]; + + if (fgets(buffer, 1028, process_pipe_) != NULL) { + std::cout << buffer << std::endl; + if (progress_ < 1.0) { + progress_ += 0.02; + send_feedback(progress_, "Panorama running"); + } + + std::cout << "\t ** [Panorama] Robot " << robot_name_ << " moving from " << from << " towards " << towards + << " ... [" << std::min(100.0, progress_ * 100.0) << "%] " << std::endl; + } else { + int status = pclose(process_pipe_); + std::cout << "Finished.\n" << std::endl; + if (status == 0) { + std::cout << "Command exited with status success " << std::endl; + finish(true, 1.0, "Panorama completed"); + } else { + std::cout << "Command terminated with status fail: " << status << std::endl; + finish(false, 1.0, "Panorama terminated by signal"); + } + } + } + + float progress_; + std::string robot_name_; + FILE* process_pipe_; +}; +} // namespace plansys2_actions + + +// Main entry point for application +int main(int argc, char *argv[]) { + // Initialize a ros node + ros::init(argc, argv, "panorama_action"); + + std::string name = ros::this_node::getName(); + if (name.empty() || (name.size() == 1 && name[0] == '/')) + name = "default"; + else if (name[0] == '/') + name = name.substr(1); + + ros::NodeHandle nh("~"); + nh.setParam("action_name", "panorama"); + + // Start action node + // We could actually add multiple action nodes here being aware that we might need a ros::AsyncSpinner + // (https://github.com/Bckempa/ros2_planning_system/blob/noetic-devel/plansys2_bt_actions/src/bt_action_node.cpp#L41) + auto action_node = std::make_shared(nh, "panorama", std::chrono::seconds(1)); + action_node->trigger_transition(ros::lifecycle::CONFIGURE); + + // Synchronous mode + ros::spin(); + // Make for great success + return 0; +} diff --git a/astrobee/survey_manager/survey_planner/src/stereo_action_node.cpp b/astrobee/survey_manager/survey_planner/src/stereo_action_node.cpp new file mode 100644 index 00000000..96f42281 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/src/stereo_action_node.cpp @@ -0,0 +1,128 @@ +/* Copyright (c) 2021, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking + * platform" software is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +#include + +#include +#include + +#include +#include +#include + +namespace plansys2_actions { + +class StereoAction : public plansys2::ActionExecutorClient { + public: + StereoAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate) + : ActionExecutorClient(nh, action, rate) { + progress_ = 0.0; + process_pipe_ = nullptr; + } + + protected: + void do_work() { + std::string from, towards; + + if (get_arguments().size() > 2) { + robot_name_ = get_arguments()[0]; + from = get_arguments()[1]; + towards = get_arguments()[2]; + } else { + finish(false, 1.0, "Not enough arguments for [STEREO] command"); + } + + // Start process if not started yet + if (progress_ == 0.0) { + std::string command = + "rosrun survey_planner command_astrobee " + robot_name_ + " stereo " + towards + " " + from + " run1"; + + std::cout << command << std::endl; + // Open a pipe to a command and get a FILE* for reading + process_pipe_ = popen(command.c_str(), "r"); + + if (!process_pipe_) { + perror("popen"); + finish(false, 1.0, "Failed to start the process"); + return; + } + + // Get the process ID + progress_ = 0.02; + return; + } + + char buffer[1028]; + + if (fgets(buffer, 1028, process_pipe_) != NULL) { + std::cout << buffer << std::endl; + if (progress_ < 1.0) { + progress_ += 0.02; + send_feedback(progress_, "Stereo and Inspect running"); + } + + std::cout << "\t ** [Stereo] Robot " << robot_name_ << " moving from " << from << " towards " << towards + << " ... [" << std::min(100.0, progress_ * 100.0) << "%] " << std::endl; + } else { + int status = pclose(process_pipe_); + std::cout << "Finished.\n" << std::endl; + if (status == 0) { + std::cout << "Command exited with status success " << std::endl; + finish(true, 1.0, "Stereo completed"); + } else { + std::cout << "Command terminated with status fail: " << status << std::endl; + finish(false, 1.0, "Stereo terminated by signal"); + } + } + } + + float progress_; + std::string robot_name_; + FILE* process_pipe_; +}; +} // namespace plansys2_actions + + +// Main entry point for application +int main(int argc, char *argv[]) { + // Initialize a ros node + ros::init(argc, argv, "stereo_action"); + + std::string name = ros::this_node::getName(); + if (name.empty() || (name.size() == 1 && name[0] == '/')) + name = "default"; + else if (name[0] == '/') + name = name.substr(1); + + ros::NodeHandle nh("~"); + nh.setParam("action_name", "stereo"); + + // Start action node + // We could actually add multiple action nodes here being aware that we might need a ros::AsyncSpinner + // (https://github.com/Bckempa/ros2_planning_system/blob/noetic-devel/plansys2_bt_actions/src/bt_action_node.cpp#L41) + auto action_node = std::make_shared(nh, "stereo", std::chrono::seconds(1)); + action_node->trigger_transition(ros::lifecycle::CONFIGURE); + + // Synchronous mode + ros::spin(); + // Make for great success + return 0; +} diff --git a/astrobee/survey_manager/survey_planner/src/undock_action_node.cpp b/astrobee/survey_manager/survey_planner/src/undock_action_node.cpp new file mode 100644 index 00000000..1ad2d042 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/src/undock_action_node.cpp @@ -0,0 +1,128 @@ +/* Copyright (c) 2021, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking + * platform" software is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +#include + +#include +#include + +#include +#include +#include + +namespace plansys2_actions { + +class UndockAction : public plansys2::ActionExecutorClient { + public: + UndockAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate) + : ActionExecutorClient(nh, action, rate) { + progress_ = 0.0; + process_pipe_ = nullptr; + } + + protected: + void do_work() { + std::string from, towards; + + if (get_arguments().size() > 2) { + robot_name_ = get_arguments()[0]; + from = get_arguments()[1]; + towards = get_arguments()[2]; + } else { + finish(false, 1.0, "Not enough arguments for [MOVE] command"); + } + + // Start process if not started yet + if (progress_ == 0.0) { + std::string command = + "rosrun survey_planner command_astrobee " + robot_name_ + " undock " + towards + " " + from + " run1"; + + std::cout << command << std::endl; + // Open a pipe to a command and get a FILE* for reading + process_pipe_ = popen(command.c_str(), "r"); + + if (!process_pipe_) { + perror("popen"); + finish(false, 1.0, "Failed to start the process"); + return; + } + + // Get the process ID + progress_ = 0.02; + return; + } + + char buffer[1028]; + + if (fgets(buffer, 1028, process_pipe_) != NULL) { + std::cout << buffer << std::endl; + if (progress_ < 1.0) { + progress_ += 0.02; + send_feedback(progress_, "Undock running"); + } + + std::cout << "\t ** [Undock] Robot " << robot_name_ << " moving from " << from << " towards " << towards + << " ... [" << std::min(100.0, progress_ * 100.0) << "%] " << std::endl; + } else { + int status = pclose(process_pipe_); + std::cout << "Finished.\n" << std::endl; + if (status == 0) { + std::cout << "Command exited with status success " << std::endl; + finish(true, 1.0, "Undock completed"); + } else { + std::cout << "Command terminated with status fail: " << status << std::endl; + finish(false, 1.0, "Undock terminated by signal"); + } + } + } + + float progress_; + std::string robot_name_; + FILE* process_pipe_; +}; +} // namespace plansys2_actions + + +// Main entry point for application +int main(int argc, char *argv[]) { + // Initialize a ros node + ros::init(argc, argv, "undock_action"); + + std::string name = ros::this_node::getName(); + if (name.empty() || (name.size() == 1 && name[0] == '/')) + name = "default"; + else if (name[0] == '/') + name = name.substr(1); + + ros::NodeHandle nh("~"); + nh.setParam("action_name", "undock"); + + // Start action node + // We could actually add multiple action nodes here being aware that we might need a ros::AsyncSpinner + // (https://github.com/Bckempa/ros2_planning_system/blob/noetic-devel/plansys2_bt_actions/src/bt_action_node.cpp#L41) + auto action_node = std::make_shared(nh, "undock", std::chrono::seconds(1)); + action_node->trigger_transition(ros::lifecycle::CONFIGURE); + + // Synchronous mode + ros::spin(); + // Make for great success + return 0; +} From 86b7990aba48428e3e6bba8cbc3565d674f36818 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Wed, 17 Jan 2024 09:10:28 -0800 Subject: [PATCH 30/44] running isort --- .../survey_planner/tools/survey_planner/command_astrobee | 2 +- .../survey_planner/tools/survey_planner/monitor_astrobee | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee b/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee index f3f6af26..6dfd3bfb 100755 --- a/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee +++ b/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee @@ -28,8 +28,8 @@ import sys import threading from typing import Any, Dict, List -import rospy import rospkg +import rospy import yaml from ff_msgs.msg import ( AckCompletedStatus, diff --git a/astrobee/survey_manager/survey_planner/tools/survey_planner/monitor_astrobee b/astrobee/survey_manager/survey_planner/tools/survey_planner/monitor_astrobee index 7b772b65..9c7acbd0 100755 --- a/astrobee/survey_manager/survey_planner/tools/survey_planner/monitor_astrobee +++ b/astrobee/survey_manager/survey_planner/tools/survey_planner/monitor_astrobee @@ -24,7 +24,6 @@ import socket import sys import threading - # Constants CHUNK_SIZE = 1024 From 8d4f72af0cdd56ae66928d9b802d391c88aa4e73 Mon Sep 17 00:00:00 2001 From: Brian Coltin Date: Wed, 17 Jan 2024 16:07:30 -0800 Subject: [PATCH 31/44] Use exec and waitpid instead of reading output. --- .../survey_planner/src/undock_action_node.cpp | 62 ++++++++++--------- 1 file changed, 34 insertions(+), 28 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/src/undock_action_node.cpp b/astrobee/survey_manager/survey_planner/src/undock_action_node.cpp index 1ad2d042..fb897bbd 100644 --- a/astrobee/survey_manager/survey_planner/src/undock_action_node.cpp +++ b/astrobee/survey_manager/survey_planner/src/undock_action_node.cpp @@ -35,7 +35,7 @@ class UndockAction : public plansys2::ActionExecutorClient { UndockAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate) : ActionExecutorClient(nh, action, rate) { progress_ = 0.0; - process_pipe_ = nullptr; + pid_ = 0; } protected: @@ -52,38 +52,44 @@ class UndockAction : public plansys2::ActionExecutorClient { // Start process if not started yet if (progress_ == 0.0) { - std::string command = - "rosrun survey_planner command_astrobee " + robot_name_ + " undock " + towards + " " + from + " run1"; - - std::cout << command << std::endl; - // Open a pipe to a command and get a FILE* for reading - process_pipe_ = popen(command.c_str(), "r"); - - if (!process_pipe_) { - perror("popen"); + pid_ = fork(); + if (pid_ < 0) { + perror("Fork failed."); finish(false, 1.0, "Failed to start the process"); + } else if (pid_ == 0) { + printf("rosrun rosrun survey_planner command_astrobee %s undock %s %s run1\n", robot_name_.c_str(), + towards.c_str(), from.c_str()); + const char* args[4]; + args[0] = "sh"; + args[1] = "-c"; + args[2] = + ("rosrun survey_planner command_astrobee " + robot_name_ + " undock " + towards + " " + from + " run1") + .c_str(); + args[3] = NULL; + execvpe("sh", (char* const*)args, environ); + perror("Failed to execute command."); + printf("EXITING FAILURE %d\n", getpid()); + exit(-1); + } else { + progress_ = 0.02; return; } - - // Get the process ID - progress_ = 0.02; - return; } - char buffer[1028]; - - if (fgets(buffer, 1028, process_pipe_) != NULL) { - std::cout << buffer << std::endl; - if (progress_ < 1.0) { - progress_ += 0.02; - send_feedback(progress_, "Undock running"); - } + if (progress_ < 1.0) { + progress_ += 0.02; + send_feedback(progress_, "Undock running"); + } - std::cout << "\t ** [Undock] Robot " << robot_name_ << " moving from " << from << " towards " << towards - << " ... [" << std::min(100.0, progress_ * 100.0) << "%] " << std::endl; - } else { - int status = pclose(process_pipe_); - std::cout << "Finished.\n" << std::endl; + std::cout << "\t ** [Undock] Robot " << robot_name_ << " moving from " << from << " towards " << towards + << " ... [" << std::min(100.0, progress_ * 100.0) << "%] " << std::endl; + int status; + int result = waitpid(-1, &status, WNOHANG); + printf("Result: %d %d %d\n", result, pid_, status); + if (result < 0) { + perror("Failed to wait for pid."); + finish(false, 1.0, "Unexpected error waiting for process."); + } else if (result == pid_) { if (status == 0) { std::cout << "Command exited with status success " << std::endl; finish(true, 1.0, "Undock completed"); @@ -96,7 +102,7 @@ class UndockAction : public plansys2::ActionExecutorClient { float progress_; std::string robot_name_; - FILE* process_pipe_; + int pid_; }; } // namespace plansys2_actions From 6a18416882d4fb64b51abec8aefd06f9bc452243 Mon Sep 17 00:00:00 2001 From: Brian Coltin Date: Wed, 17 Jan 2024 16:34:15 -0800 Subject: [PATCH 32/44] Change all action nodes to use the same class. --- .../survey_planner/CMakeLists.txt | 12 +- .../survey_planner/isaac_action_node.h | 46 +++++++ .../survey_planner/src/dock_action_node.cpp | 108 +-------------- .../survey_planner/src/isaac_action_node.cpp | 128 ++++++++++++++++++ .../survey_planner/src/move_action_node.cpp | 108 +-------------- .../src/panorama_action_node.cpp | 108 +-------------- .../survey_planner/src/stereo_action_node.cpp | 108 +-------------- .../survey_planner/src/undock_action_node.cpp | 114 +--------------- 8 files changed, 191 insertions(+), 541 deletions(-) create mode 100644 astrobee/survey_manager/survey_planner/include/survey_planner/isaac_action_node.h create mode 100644 astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp diff --git a/astrobee/survey_manager/survey_planner/CMakeLists.txt b/astrobee/survey_manager/survey_planner/CMakeLists.txt index 58a958c5..f8f2f0aa 100644 --- a/astrobee/survey_manager/survey_planner/CMakeLists.txt +++ b/astrobee/survey_manager/survey_planner/CMakeLists.txt @@ -41,24 +41,26 @@ catkin_package( # Specify additional locations of header files # Your package locations should be listed before other locations include_directories( + include ${catkin_INCLUDE_DIRS} ) +add_library(isaac_action_node src/isaac_action_node.cpp) # Action for move add_executable(move_action_node src/move_action_node.cpp) -target_link_libraries(move_action_node ${catkin_LIBRARIES} ) +target_link_libraries(move_action_node isaac_action_node ${catkin_LIBRARIES} ) # Action for dock add_executable(dock_action_node src/dock_action_node.cpp) -target_link_libraries(dock_action_node ${catkin_LIBRARIES} ) +target_link_libraries(dock_action_node isaac_action_node ${catkin_LIBRARIES} ) # Action for undock add_executable(undock_action_node src/undock_action_node.cpp) -target_link_libraries(undock_action_node ${catkin_LIBRARIES} ) +target_link_libraries(undock_action_node isaac_action_node ${catkin_LIBRARIES} ) # Action for panorama add_executable(panorama_action_node src/panorama_action_node.cpp) -target_link_libraries(panorama_action_node ${catkin_LIBRARIES} ) +target_link_libraries(panorama_action_node isaac_action_node ${catkin_LIBRARIES} ) # Action for stereo add_executable(stereo_action_node src/stereo_action_node.cpp) -target_link_libraries(stereo_action_node ${catkin_LIBRARIES} ) +target_link_libraries(stereo_action_node isaac_action_node ${catkin_LIBRARIES} ) ############# ## Install ## diff --git a/astrobee/survey_manager/survey_planner/include/survey_planner/isaac_action_node.h b/astrobee/survey_manager/survey_planner/include/survey_planner/isaac_action_node.h new file mode 100644 index 00000000..0d60a331 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/include/survey_planner/isaac_action_node.h @@ -0,0 +1,46 @@ +/* Copyright (c) 2021, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking + * platform" software is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef SURVEY_PLANNER_ISAAC_ACTION_NODE_H_ +#define SURVEY_PLANNER_ISAAC_ACTION_NODE_H_ + +#include + +#include + +namespace plansys2_actions { + +class IsaacAction : public plansys2::ActionExecutorClient { + public: + IsaacAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate); + + protected: + void do_work(); + + float progress_; + std::string robot_name_, action_name_; + int pid_; +}; +} // namespace plansys2_actions + + +// Main entry point for application +int isaac_action_main(int argc, char *argv[], const char* action_name); + +#endif // SURVEY_PLANNER_ISAAC_ACTION_NODE_H_ diff --git a/astrobee/survey_manager/survey_planner/src/dock_action_node.cpp b/astrobee/survey_manager/survey_planner/src/dock_action_node.cpp index b3bbb0d5..a06abca6 100644 --- a/astrobee/survey_manager/survey_planner/src/dock_action_node.cpp +++ b/astrobee/survey_manager/survey_planner/src/dock_action_node.cpp @@ -17,112 +17,8 @@ * under the License. */ -#include +#include -#include - -#include -#include - -#include -#include -#include - -namespace plansys2_actions { - -class DockAction : public plansys2::ActionExecutorClient { - public: - DockAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate) - : ActionExecutorClient(nh, action, rate) { - progress_ = 0.0; - process_pipe_ = nullptr; - } - - protected: - void do_work() { - std::string from, towards; - - if (get_arguments().size() > 2) { - robot_name_ = get_arguments()[0]; - from = get_arguments()[1]; - towards = get_arguments()[2]; - } else { - finish(false, 1.0, "Not enough arguments for [DOCK] command"); - } - - // Start process if not started yet - if (progress_ == 0.0) { - std::string command = - "rosrun survey_planner command_astrobee " + robot_name_ + " dock " + towards + " " + from + " run1"; - - std::cout << command << std::endl; - // Open a pipe to a command and get a FILE* for reading - process_pipe_ = popen(command.c_str(), "r"); - - if (!process_pipe_) { - perror("popen"); - finish(false, 1.0, "Failed to start the process"); - return; - } - - // Get the process ID - progress_ = 0.02; - return; - } - - char buffer[1028]; - - if (fgets(buffer, 1028, process_pipe_) != NULL) { - std::cout << buffer << std::endl; - if (progress_ < 1.0) { - progress_ += 0.02; - send_feedback(progress_, "Dock and Inspect running"); - } - - std::cout << "\t ** [Dock] Robot " << robot_name_ << " moving from " << from << " towards " << towards - << " ... [" << std::min(100.0, progress_ * 100.0) << "%] " << std::endl; - } else { - int status = pclose(process_pipe_); - std::cout << "Finished.\n" << std::endl; - if (status == 0) { - std::cout << "Command exited with status success " << std::endl; - finish(true, 1.0, "Dock completed"); - } else { - std::cout << "Command terminated with status fail: " << status << std::endl; - finish(false, 1.0, "Dock terminated by signal"); - } - } - } - - float progress_; - std::string robot_name_; - FILE* process_pipe_; -}; -} // namespace plansys2_actions - - -// Main entry point for application int main(int argc, char *argv[]) { - // Initialize a ros node - ros::init(argc, argv, "dock_action"); - - std::string name = ros::this_node::getName(); - if (name.empty() || (name.size() == 1 && name[0] == '/')) - name = "default"; - else if (name[0] == '/') - name = name.substr(1); - - ros::NodeHandle nh("~"); - nh.setParam("action_name", "dock"); - - // Start action node - // We could actually add multiple action nodes here being aware that we might need a ros::AsyncSpinner - // (https://github.com/Bckempa/ros2_planning_system/blob/noetic-devel/plansys2_bt_actions/src/bt_action_node.cpp#L41) - auto action_node = std::make_shared(nh, "dock", std::chrono::seconds(1)); - action_node->trigger_transition(ros::lifecycle::CONFIGURE); - - // Synchronous mode - ros::spin(); - // Make for great success - return 0; + return isaac_action_main(argc, argv, "dock"); } diff --git a/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp b/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp new file mode 100644 index 00000000..3c60dc0e --- /dev/null +++ b/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp @@ -0,0 +1,128 @@ +/* Copyright (c) 2021, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking + * platform" software is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +#include + +#include +#include + +#include +#include +#include + +#include "survey_planner/isaac_action_node.h" + +namespace plansys2_actions { + +IsaacAction::IsaacAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate) + : ActionExecutorClient(nh, action, rate) { + action_name_ = action; + progress_ = 0.0; + pid_ = 0; +} + +void IsaacAction::do_work() { + std::string from, towards; + + if (get_arguments().size() > 2) { + robot_name_ = get_arguments()[0]; + from = get_arguments()[1]; + towards = get_arguments()[2]; + } else { + finish(false, 1.0, "Not enough arguments for [MOVE] command"); + } + + // Start process if not started yet + if (progress_ == 0.0) { + pid_ = fork(); + if (pid_ < 0) { + perror("Fork failed."); + finish(false, 1.0, "Failed to start the process"); + } else if (pid_ == 0) { + printf("rosrun rosrun survey_planner command_astrobee %s %s %s %s run1\n", + robot_name_.c_str(), action_name_.c_str(), towards.c_str(), from.c_str()); + const char* args[4]; + args[0] = "sh"; + args[1] = "-c"; + args[2] = ("rosrun survey_planner command_astrobee " + robot_name_ + " " + + action_name_ + " " + towards + " " + from + " run1").c_str(); + args[3] = NULL; + execvpe("sh", (char* const*)args, environ); + perror("Failed to execute command."); + printf("EXITING FAILURE %d\n", getpid()); + exit(-1); + } else { + progress_ = 0.02; + return; + } + } + + if (progress_ < 1.0) { + progress_ += 0.02; + send_feedback(progress_, action_name_ + " running"); + } + + std::cout << "\t ** [" << action_name_ << "] Robot " << robot_name_ << " moving from " << from << " towards " + << towards << " ... [" << std::min(100.0, progress_ * 100.0) << "%] " << std::endl; + int status; + int result = waitpid(-1, &status, WNOHANG); + printf("Result: %d %d %d\n", result, pid_, status); + if (result < 0) { + perror("Failed to wait for pid."); + finish(false, 1.0, "Unexpected error waiting for process."); + } else if (result == pid_) { + if (status == 0) { + std::cout << "Command exited with status success " << std::endl; + finish(true, 1.0, action_name_ + " completed"); + } else { + std::cout << "Command terminated with status fail: " << status << std::endl; + finish(false, 1.0, action_name_ + " terminated by signal"); + } + } +} +} // namespace plansys2_actions + + +// Main entry point for application +int isaac_action_main(int argc, char *argv[], const char* action_name) { + // Initialize a ros node + ros::init(argc, argv, (std::string(action_name) + "_action").c_str()); + + std::string name = ros::this_node::getName(); + if (name.empty() || (name.size() == 1 && name[0] == '/')) + name = "default"; + else if (name[0] == '/') + name = name.substr(1); + + ros::NodeHandle nh("~"); + nh.setParam("action_name", action_name); + + // Start action node + // We could actually add multiple action nodes here being aware that we might need a ros::AsyncSpinner + // (https://github.com/Bckempa/ros2_planning_system/blob/noetic-devel/plansys2_bt_actions/src/bt_action_node.cpp#L41) + auto action_node = std::make_shared(nh, action_name, std::chrono::seconds(1)); + action_node->trigger_transition(ros::lifecycle::CONFIGURE); + + // Synchronous mode + ros::spin(); + // Make for great success + return 0; +} diff --git a/astrobee/survey_manager/survey_planner/src/move_action_node.cpp b/astrobee/survey_manager/survey_planner/src/move_action_node.cpp index 39bc1664..ca12cb92 100644 --- a/astrobee/survey_manager/survey_planner/src/move_action_node.cpp +++ b/astrobee/survey_manager/survey_planner/src/move_action_node.cpp @@ -17,112 +17,8 @@ * under the License. */ -#include +#include -#include - -#include -#include - -#include -#include -#include - -namespace plansys2_actions { - -class MoveAction : public plansys2::ActionExecutorClient { - public: - MoveAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate) - : ActionExecutorClient(nh, action, rate) { - progress_ = 0.0; - process_pipe_ = nullptr; - } - - protected: - void do_work() { - std::string from, towards; - - if (get_arguments().size() > 2) { - robot_name_ = get_arguments()[0]; - from = get_arguments()[1]; - towards = get_arguments()[2]; - } else { - finish(false, 1.0, "Not enough arguments for [MOVE] command"); - } - - // Start process if not started yet - if (progress_ == 0.0) { - std::string command = - "rosrun survey_planner command_astrobee " + robot_name_ + " move " + towards + " " + from + " run1"; - - std::cout << command << std::endl; - // Open a pipe to a command and get a FILE* for reading - process_pipe_ = popen(command.c_str(), "r"); - - if (!process_pipe_) { - perror("popen"); - finish(false, 1.0, "Failed to start the process"); - return; - } - - // Get the process ID - progress_ = 0.02; - return; - } - - char buffer[1028]; - - if (fgets(buffer, 1028, process_pipe_) != NULL) { - std::cout << buffer << std::endl; - if (progress_ < 1.0) { - progress_ += 0.02; - send_feedback(progress_, "Move and Inspect running"); - } - - std::cout << "\t ** [Move and Inspect] Robot " << robot_name_ << " moving from " << from << " towards " << towards - << " ... [" << std::min(100.0, progress_ * 100.0) << "%] " << std::endl; - } else { - int status = pclose(process_pipe_); - std::cout << "Finished.\n" << std::endl; - if (status == 0) { - std::cout << "Command exited with status success " << std::endl; - finish(true, 1.0, "Move and Inspect completed"); - } else { - std::cout << "Command terminated with status fail: " << status << std::endl; - finish(false, 1.0, "Move and Inspect terminated by signal"); - } - } - } - - float progress_; - std::string robot_name_; - FILE* process_pipe_; -}; -} // namespace plansys2_actions - - -// Main entry point for application int main(int argc, char *argv[]) { - // Initialize a ros node - ros::init(argc, argv, "move_action"); - - std::string name = ros::this_node::getName(); - if (name.empty() || (name.size() == 1 && name[0] == '/')) - name = "default"; - else if (name[0] == '/') - name = name.substr(1); - - ros::NodeHandle nh("~"); - nh.setParam("action_name", "move"); - - // Start action node - // We could actually add multiple action nodes here being aware that we might need a ros::AsyncSpinner - // (https://github.com/Bckempa/ros2_planning_system/blob/noetic-devel/plansys2_bt_actions/src/bt_action_node.cpp#L41) - auto action_node = std::make_shared(nh, "move", std::chrono::seconds(1)); - action_node->trigger_transition(ros::lifecycle::CONFIGURE); - - // Synchronous mode - ros::spin(); - // Make for great success - return 0; + return isaac_action_main(argc, argv, "move"); } diff --git a/astrobee/survey_manager/survey_planner/src/panorama_action_node.cpp b/astrobee/survey_manager/survey_planner/src/panorama_action_node.cpp index 1ce9c219..b63386a4 100644 --- a/astrobee/survey_manager/survey_planner/src/panorama_action_node.cpp +++ b/astrobee/survey_manager/survey_planner/src/panorama_action_node.cpp @@ -17,112 +17,8 @@ * under the License. */ -#include +#include -#include - -#include -#include - -#include -#include -#include - -namespace plansys2_actions { - -class PanoramaAction : public plansys2::ActionExecutorClient { - public: - PanoramaAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate) - : ActionExecutorClient(nh, action, rate) { - progress_ = 0.0; - process_pipe_ = nullptr; - } - - protected: - void do_work() { - std::string from, towards; - - if (get_arguments().size() > 2) { - robot_name_ = get_arguments()[0]; - from = get_arguments()[1]; - towards = get_arguments()[2]; - } else { - finish(false, 1.0, "Not enough arguments for [PANORAMA] command"); - } - - // Start process if not started yet - if (progress_ == 0.0) { - std::string command = - "rosrun survey_planner command_astrobee " + robot_name_ + " panorama " + towards + " " + from + " run1"; - - std::cout << command << std::endl; - // Open a pipe to a command and get a FILE* for reading - process_pipe_ = popen(command.c_str(), "r"); - - if (!process_pipe_) { - perror("popen"); - finish(false, 1.0, "Failed to start the process"); - return; - } - - // Get the process ID - progress_ = 0.02; - return; - } - - char buffer[1028]; - - if (fgets(buffer, 1028, process_pipe_) != NULL) { - std::cout << buffer << std::endl; - if (progress_ < 1.0) { - progress_ += 0.02; - send_feedback(progress_, "Panorama running"); - } - - std::cout << "\t ** [Panorama] Robot " << robot_name_ << " moving from " << from << " towards " << towards - << " ... [" << std::min(100.0, progress_ * 100.0) << "%] " << std::endl; - } else { - int status = pclose(process_pipe_); - std::cout << "Finished.\n" << std::endl; - if (status == 0) { - std::cout << "Command exited with status success " << std::endl; - finish(true, 1.0, "Panorama completed"); - } else { - std::cout << "Command terminated with status fail: " << status << std::endl; - finish(false, 1.0, "Panorama terminated by signal"); - } - } - } - - float progress_; - std::string robot_name_; - FILE* process_pipe_; -}; -} // namespace plansys2_actions - - -// Main entry point for application int main(int argc, char *argv[]) { - // Initialize a ros node - ros::init(argc, argv, "panorama_action"); - - std::string name = ros::this_node::getName(); - if (name.empty() || (name.size() == 1 && name[0] == '/')) - name = "default"; - else if (name[0] == '/') - name = name.substr(1); - - ros::NodeHandle nh("~"); - nh.setParam("action_name", "panorama"); - - // Start action node - // We could actually add multiple action nodes here being aware that we might need a ros::AsyncSpinner - // (https://github.com/Bckempa/ros2_planning_system/blob/noetic-devel/plansys2_bt_actions/src/bt_action_node.cpp#L41) - auto action_node = std::make_shared(nh, "panorama", std::chrono::seconds(1)); - action_node->trigger_transition(ros::lifecycle::CONFIGURE); - - // Synchronous mode - ros::spin(); - // Make for great success - return 0; + return isaac_action_main(argc, argv, "panorama"); } diff --git a/astrobee/survey_manager/survey_planner/src/stereo_action_node.cpp b/astrobee/survey_manager/survey_planner/src/stereo_action_node.cpp index 96f42281..ddd0bd7b 100644 --- a/astrobee/survey_manager/survey_planner/src/stereo_action_node.cpp +++ b/astrobee/survey_manager/survey_planner/src/stereo_action_node.cpp @@ -17,112 +17,8 @@ * under the License. */ -#include +#include -#include - -#include -#include - -#include -#include -#include - -namespace plansys2_actions { - -class StereoAction : public plansys2::ActionExecutorClient { - public: - StereoAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate) - : ActionExecutorClient(nh, action, rate) { - progress_ = 0.0; - process_pipe_ = nullptr; - } - - protected: - void do_work() { - std::string from, towards; - - if (get_arguments().size() > 2) { - robot_name_ = get_arguments()[0]; - from = get_arguments()[1]; - towards = get_arguments()[2]; - } else { - finish(false, 1.0, "Not enough arguments for [STEREO] command"); - } - - // Start process if not started yet - if (progress_ == 0.0) { - std::string command = - "rosrun survey_planner command_astrobee " + robot_name_ + " stereo " + towards + " " + from + " run1"; - - std::cout << command << std::endl; - // Open a pipe to a command and get a FILE* for reading - process_pipe_ = popen(command.c_str(), "r"); - - if (!process_pipe_) { - perror("popen"); - finish(false, 1.0, "Failed to start the process"); - return; - } - - // Get the process ID - progress_ = 0.02; - return; - } - - char buffer[1028]; - - if (fgets(buffer, 1028, process_pipe_) != NULL) { - std::cout << buffer << std::endl; - if (progress_ < 1.0) { - progress_ += 0.02; - send_feedback(progress_, "Stereo and Inspect running"); - } - - std::cout << "\t ** [Stereo] Robot " << robot_name_ << " moving from " << from << " towards " << towards - << " ... [" << std::min(100.0, progress_ * 100.0) << "%] " << std::endl; - } else { - int status = pclose(process_pipe_); - std::cout << "Finished.\n" << std::endl; - if (status == 0) { - std::cout << "Command exited with status success " << std::endl; - finish(true, 1.0, "Stereo completed"); - } else { - std::cout << "Command terminated with status fail: " << status << std::endl; - finish(false, 1.0, "Stereo terminated by signal"); - } - } - } - - float progress_; - std::string robot_name_; - FILE* process_pipe_; -}; -} // namespace plansys2_actions - - -// Main entry point for application int main(int argc, char *argv[]) { - // Initialize a ros node - ros::init(argc, argv, "stereo_action"); - - std::string name = ros::this_node::getName(); - if (name.empty() || (name.size() == 1 && name[0] == '/')) - name = "default"; - else if (name[0] == '/') - name = name.substr(1); - - ros::NodeHandle nh("~"); - nh.setParam("action_name", "stereo"); - - // Start action node - // We could actually add multiple action nodes here being aware that we might need a ros::AsyncSpinner - // (https://github.com/Bckempa/ros2_planning_system/blob/noetic-devel/plansys2_bt_actions/src/bt_action_node.cpp#L41) - auto action_node = std::make_shared(nh, "stereo", std::chrono::seconds(1)); - action_node->trigger_transition(ros::lifecycle::CONFIGURE); - - // Synchronous mode - ros::spin(); - // Make for great success - return 0; + return isaac_action_main(argc, argv, "stereo"); } diff --git a/astrobee/survey_manager/survey_planner/src/undock_action_node.cpp b/astrobee/survey_manager/survey_planner/src/undock_action_node.cpp index fb897bbd..09d3ca6e 100644 --- a/astrobee/survey_manager/survey_planner/src/undock_action_node.cpp +++ b/astrobee/survey_manager/survey_planner/src/undock_action_node.cpp @@ -17,118 +17,8 @@ * under the License. */ -#include +#include -#include - -#include -#include - -#include -#include -#include - -namespace plansys2_actions { - -class UndockAction : public plansys2::ActionExecutorClient { - public: - UndockAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate) - : ActionExecutorClient(nh, action, rate) { - progress_ = 0.0; - pid_ = 0; - } - - protected: - void do_work() { - std::string from, towards; - - if (get_arguments().size() > 2) { - robot_name_ = get_arguments()[0]; - from = get_arguments()[1]; - towards = get_arguments()[2]; - } else { - finish(false, 1.0, "Not enough arguments for [MOVE] command"); - } - - // Start process if not started yet - if (progress_ == 0.0) { - pid_ = fork(); - if (pid_ < 0) { - perror("Fork failed."); - finish(false, 1.0, "Failed to start the process"); - } else if (pid_ == 0) { - printf("rosrun rosrun survey_planner command_astrobee %s undock %s %s run1\n", robot_name_.c_str(), - towards.c_str(), from.c_str()); - const char* args[4]; - args[0] = "sh"; - args[1] = "-c"; - args[2] = - ("rosrun survey_planner command_astrobee " + robot_name_ + " undock " + towards + " " + from + " run1") - .c_str(); - args[3] = NULL; - execvpe("sh", (char* const*)args, environ); - perror("Failed to execute command."); - printf("EXITING FAILURE %d\n", getpid()); - exit(-1); - } else { - progress_ = 0.02; - return; - } - } - - if (progress_ < 1.0) { - progress_ += 0.02; - send_feedback(progress_, "Undock running"); - } - - std::cout << "\t ** [Undock] Robot " << robot_name_ << " moving from " << from << " towards " << towards - << " ... [" << std::min(100.0, progress_ * 100.0) << "%] " << std::endl; - int status; - int result = waitpid(-1, &status, WNOHANG); - printf("Result: %d %d %d\n", result, pid_, status); - if (result < 0) { - perror("Failed to wait for pid."); - finish(false, 1.0, "Unexpected error waiting for process."); - } else if (result == pid_) { - if (status == 0) { - std::cout << "Command exited with status success " << std::endl; - finish(true, 1.0, "Undock completed"); - } else { - std::cout << "Command terminated with status fail: " << status << std::endl; - finish(false, 1.0, "Undock terminated by signal"); - } - } - } - - float progress_; - std::string robot_name_; - int pid_; -}; -} // namespace plansys2_actions - - -// Main entry point for application int main(int argc, char *argv[]) { - // Initialize a ros node - ros::init(argc, argv, "undock_action"); - - std::string name = ros::this_node::getName(); - if (name.empty() || (name.size() == 1 && name[0] == '/')) - name = "default"; - else if (name[0] == '/') - name = name.substr(1); - - ros::NodeHandle nh("~"); - nh.setParam("action_name", "undock"); - - // Start action node - // We could actually add multiple action nodes here being aware that we might need a ros::AsyncSpinner - // (https://github.com/Bckempa/ros2_planning_system/blob/noetic-devel/plansys2_bt_actions/src/bt_action_node.cpp#L41) - auto action_node = std::make_shared(nh, "undock", std::chrono::seconds(1)); - action_node->trigger_transition(ros::lifecycle::CONFIGURE); - - // Synchronous mode - ros::spin(); - // Make for great success - return 0; + return isaac_action_main(argc, argv, "undock"); } From 8ef602dc3174cdbbba0aef916dbdd410713da8a5 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Thu, 18 Jan 2024 12:18:15 -0800 Subject: [PATCH 33/44] install all nodes --- astrobee/survey_manager/survey_planner/CMakeLists.txt | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/CMakeLists.txt b/astrobee/survey_manager/survey_planner/CMakeLists.txt index f8f2f0aa..e4a0fd15 100644 --- a/astrobee/survey_manager/survey_planner/CMakeLists.txt +++ b/astrobee/survey_manager/survey_planner/CMakeLists.txt @@ -72,8 +72,13 @@ catkin_python_setup() catkin_package() # Install actions -install(TARGETS move_action_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install(TARGETS + move_action_node + dock_action_node + undock_action_node + panorama_action_node + stereo_action_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) foreach( dir pddl launch data) install( DIRECTORY ${dir}/ From efd9ac710fcfc87031c47c73e9167c2e9954c99c Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Thu, 18 Jan 2024 22:32:19 -0800 Subject: [PATCH 34/44] adding remote option with no parameter adjusting, we'll do panoramas using geometry --- .../inspection/tools/inspection_tool.cc | 24 +++++++++++++++---- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/astrobee/behaviors/inspection/tools/inspection_tool.cc b/astrobee/behaviors/inspection/tools/inspection_tool.cc index 48b6235d..29de2af5 100644 --- a/astrobee/behaviors/inspection/tools/inspection_tool.cc +++ b/astrobee/behaviors/inspection/tools/inspection_tool.cc @@ -66,6 +66,7 @@ DEFINE_bool(anomaly, false, "Send the inspection command"); DEFINE_bool(geometry, false, "Send the inspection command"); DEFINE_bool(panorama, false, "Send the inspection command"); DEFINE_bool(volumetric, false, "Send the inspection command"); +DEFINE_bool(remote, false, "Whether target command is remote robot"); // General parameters DEFINE_string(camera, "sci_cam", "Camera to use"); @@ -427,8 +428,11 @@ int main(int argc, char *argv[]) { // Create a node handle ros::NodeHandle nh(std::string("/") + FLAGS_ns); // Setup SWITCH action - client.SetConnectedTimeout(FLAGS_connect); - client.SetActiveTimeout(FLAGS_active); + if (!FLAGS_remote) { + client.SetConnectedTimeout(FLAGS_connect); + client.SetActiveTimeout(FLAGS_active); + client.SetConnectedCallback(std::bind(ConnectedCallback, &client)); + } client.SetResponseTimeout(FLAGS_response); if (FLAGS_deadline > 0) client.SetDeadlineTimeout(FLAGS_deadline); @@ -436,11 +440,10 @@ int main(int argc, char *argv[]) { std::placeholders::_1)); client.SetResultCallback(std::bind(ResultCallback, std::placeholders::_1, std::placeholders::_2)); - client.SetConnectedCallback(std::bind(ConnectedCallback, &client)); client.Create(&nh, ACTION_BEHAVIORS_INSPECTION); // Configure panorama anomaly parameters - if (FLAGS_anomaly) { + if (FLAGS_anomaly && !FLAGS_remote) { ff_util::ConfigClient cfg(&nh, NODE_INSPECTION); cfg.Set("target_distance", FLAGS_target_distance); cfg.Set("min_distance", FLAGS_min_distance); @@ -459,7 +462,7 @@ int main(int argc, char *argv[]) { } // Configure panorama inspection parameters - if (FLAGS_panorama) { + if (FLAGS_panorama && !FLAGS_remote) { ff_util::ConfigClient cfg(&nh, NODE_INSPECTION); if (FLAGS_panorama_mode == "") { @@ -506,6 +509,17 @@ int main(int argc, char *argv[]) { // Start input thread boost::thread inp(GetInput, &client); + + if (FLAGS_remote) { + ros::Rate loop_rate(10); + ros::Time start_time = ros::Time::now(); + + // Spin for 3 seconds + while (ros::Time::now() - start_time < ros::Duration(3.0)) + loop_rate.sleep(); + + SendGoal(&client); + } // Synchronous mode while (!stopflag_) { ros::spinOnce(); From 903105c3028121421c714afd11a076aa65ed2bbb Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Thu, 18 Jan 2024 22:33:03 -0800 Subject: [PATCH 35/44] reset progress after one action --- .../survey_manager/survey_planner/src/isaac_action_node.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp b/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp index 3c60dc0e..a2337e5e 100644 --- a/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp +++ b/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp @@ -57,7 +57,7 @@ void IsaacAction::do_work() { perror("Fork failed."); finish(false, 1.0, "Failed to start the process"); } else if (pid_ == 0) { - printf("rosrun rosrun survey_planner command_astrobee %s %s %s %s run1\n", + printf("rosrun survey_planner command_astrobee %s %s %s %s run1\n", robot_name_.c_str(), action_name_.c_str(), towards.c_str(), from.c_str()); const char* args[4]; args[0] = "sh"; @@ -87,13 +87,16 @@ void IsaacAction::do_work() { printf("Result: %d %d %d\n", result, pid_, status); if (result < 0) { perror("Failed to wait for pid."); + progress_ = 0.0; finish(false, 1.0, "Unexpected error waiting for process."); } else if (result == pid_) { if (status == 0) { std::cout << "Command exited with status success " << std::endl; + progress_ = 0.0; finish(true, 1.0, action_name_ + " completed"); } else { std::cout << "Command terminated with status fail: " << status << std::endl; + progress_ = 0.0; finish(false, 1.0, action_name_ + " terminated by signal"); } } From adf6eb2b609716f56327ffd0fcd8ad40f1164720 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Fri, 19 Jan 2024 21:20:56 -0800 Subject: [PATCH 36/44] adding inspection lib; fixing output --- astrobee/behaviors/inspection/CMakeLists.txt | 2 +- astrobee/behaviors/inspection/src/inspection_node.cc | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/astrobee/behaviors/inspection/CMakeLists.txt b/astrobee/behaviors/inspection/CMakeLists.txt index 4e80113d..87bb379e 100644 --- a/astrobee/behaviors/inspection/CMakeLists.txt +++ b/astrobee/behaviors/inspection/CMakeLists.txt @@ -106,7 +106,7 @@ target_link_libraries(test_pano ############# # Mark executables and/or libraries for installation -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME} +install(TARGETS pano_survey ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/astrobee/behaviors/inspection/src/inspection_node.cc b/astrobee/behaviors/inspection/src/inspection_node.cc index 1b744f89..8b628483 100644 --- a/astrobee/behaviors/inspection/src/inspection_node.cc +++ b/astrobee/behaviors/inspection/src/inspection_node.cc @@ -368,7 +368,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet { ff_msgs::MotionResultConstPtr const& result) { // Check for invalid results if (result == nullptr) { - ROS_ERROR_STREAM("Invalid result received Motion"); + ROS_INFO_STREAM("Invalid result received Motion"); return fsm_.Update(MOTION_FAILED); } @@ -607,7 +607,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet { if (result != nullptr) result_.anomaly_result.push_back(result->anomaly_result); else - ROS_ERROR_STREAM("Invalid result received Image Analysis"); + ROS_INFO_STREAM("Invalid result received Image Analysis"); return fsm_.Update(NEXT_INSPECT); } From 4ad2bdd1a81a01b9a9c8a436614f4418e76903db Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Fri, 19 Jan 2024 21:21:24 -0800 Subject: [PATCH 37/44] fixing robot install --- .../survey_manager/survey_planner/CMakeLists.txt | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/astrobee/survey_manager/survey_planner/CMakeLists.txt b/astrobee/survey_manager/survey_planner/CMakeLists.txt index e4a0fd15..84346094 100644 --- a/astrobee/survey_manager/survey_planner/CMakeLists.txt +++ b/astrobee/survey_manager/survey_planner/CMakeLists.txt @@ -71,6 +71,18 @@ catkin_python_setup() catkin_package() +catkin_install_python(PROGRAMS + tools/survey_planner/command_astrobee + tools/survey_planner/monitor_astrobee + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +# Mark executables and/or libraries for installation +install(TARGETS isaac_action_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + # Install actions install(TARGETS move_action_node From 64e8bf5792ee1c09dfd52c586dd509b525f13e9e Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Fri, 19 Jan 2024 21:33:38 -0800 Subject: [PATCH 38/44] tested all 5 actions in the granite table --- .../survey_planner/data/survey_static.yaml | 40 +++++- .../tools/survey_planner/command_astrobee | 114 ++++++++++-------- .../tools/survey_planner/plan_interpreter.py | 13 +- .../tools/survey_planner/problem_generator.py | 13 ++ 4 files changed, 114 insertions(+), 66 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/data/survey_static.yaml b/astrobee/survey_manager/survey_planner/data/survey_static.yaml index 2602a325..59de50ce 100644 --- a/astrobee/survey_manager/survey_planner/data/survey_static.yaml +++ b/astrobee/survey_manager/survey_planner/data/survey_static.yaml @@ -64,10 +64,32 @@ bays_move: usl_bay6: "-pos '-0.5 0 4.8'" # Granite testing - # jem_hatch_to_nod2: "-move -pos '-0.3 -0.1 -0.68' -att '0 0 0'" - # jem_hatch_from_nod2: "-move -pos '-0.3 -0.1 -0.68' -att '0 0 180'" - # nod2_hatch_from_jem: "-move -pos '0.3 -0.1 -0.68' -att '0 0 0'" - # nod2_hatch_to_jem: "-move -pos '0.3 -0.1 -0.68' -att '0 0 180'" + gra_p1_bsharp: "-move -pos '-0.3 -0.1 -0.68'" + gra_p2_bsharp: "-move -pos '0.3 -0.1 -0.68'" + gra_p1_wannabee: "-move -pos '-0.3 -0.1 -0.68' -att '1 0 0 180'" + gra_p2_wannabee: "-move -pos '0.3 -0.1 -0.68' -att '1 0 0 180'" + +bays_pano: + jem_bay1: "isaac9/jem_bay1_std_panorama.txt" + jem_bay2: "isaac9/jem_bay2_std_panorama.txt" + jem_bay3: "isaac9/jem_bay3_std_panorama.txt" + jem_bay4: "isaac9/jem_bay4_std_panorama.txt" + jem_bay5: "isaac9/jem_bay5_std_panorama.txt" + jem_bay6: "isaac9/jem_bay6_std_panorama.txt" + jem_bay7: "isaac9/jem_bay7_safe_std_panorama.txt" + nod2_bay2: "isaac10/nod2_bay2_std_panorama.txt" + nod2_bay3: "isaac10/nod2_bay3_std_panorama.txt" + nod2_bay4: "isaac10/nod2_bay4_std_panorama.txt" + usl_bay1: "isaac11/usl_bay1_std_panorama.txt" + usl_bay2: "isaac11/usl_bay2_std_panorama.txt" + usl_bay3: "isaac11/usl_bay3_std_panorama.txt" + usl_bay4: "isaac11/usl_bay4_std_panorama.txt" + usl_bay5: "isaac11/usl_bay5_std_panorama.txt" + usl_bay6: "isaac11/usl_bay6_std_panorama.txt" + + # Granite testing + gra_bsharp: "panorama_granite.txt" + gra_wannabee: "panorama_granite_wannabee.txt" maps: jem: "iss.map" @@ -91,13 +113,19 @@ stereo: jem_bay1_to_bay3: # fplan: Name of external fplan specification of trajectory in astrobee_ops/gds/plans/ISAAC/ . The # bay names are intended to indicate which bays are covered by the stereo survey. - fplan: "jem_stereo_mapping_bay1_to_bay3.fplan" + fplan: "ISAAC/jem_stereo_mapping_bay1_to_bay3" # base_location: Where trajectory starts and ends for planning purposes (rough location, not exact) base_location: jem_bay1 # bound_location: The other end of the interval visited by the trajectory, for planner collision # check purposes. bound_location: jem_bay4 # The survey flies into bay4 even though it only covers up to bay3 jem_bay4_to_bay7: - fplan: "jem_stereo_mapping_bay7_to_bay4.fplan" + fplan: "ISAAC/jem_stereo_mapping_bay7_to_bay4" base_location: jem_bay7 bound_location: jem_bay4 + + # Granite testing + gra_p1_to_gra_p2: + fplan: "startup" + base_location: gra_p1 + bound_location: gra_p2 \ No newline at end of file diff --git a/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee b/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee index 6dfd3bfb..d7bf905a 100755 --- a/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee +++ b/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee @@ -37,30 +37,17 @@ from ff_msgs.msg import ( CommandArg, CommandConstants, CommandStamped, + PlanStatusStamped ) from std_msgs.msg import Header, String -# # Get the directory of the current script -from survey_planner.problem_generator import load_yaml +# Imports from survey_planner package +from survey_planner.problem_generator import load_yaml, get_stereo_traj # Constants MAX_COUNTER = 10 CHUNK_SIZE = 1024 -def get_stereo_traj(config_static, from_bay, to_bay): - # Get stereo trajectory - traj_matches = [ - traj - for traj in config_static["stereo"].values() - if traj["base_location"] == from_bay and traj["bound_location"] == to_bay - ] - assert ( - len(traj_matches) == 1 - ), f"Expected exactly 1 matching stereo trajectory with base {base} and bound {bound}, got {len(traj_matches)}" - fplan = traj_matches[0]["fplan"] - return fplan - - def exposure_change(config_static, bay_origin, bay_destination): # Going to JEM if bay_origin == "nod2_hatch_to_jem" and bay_destination == "jem_hatch_from_nod2": @@ -94,6 +81,27 @@ def map_change(config_static, bay_origin, bay_destination): return config_static["maps"]["usl"] return "" +def get_ops_plan_path(): + # Check if the path /opt/astrobee/ops/gds/plans/ exists + if os.path.exists("/opt/astrobee/ops/gds/plans/"): + return "/opt/astrobee/ops/gds/plans/" + + # Check if the environment variable $ASTROBEE_OPS exists + astrobee_ops_path = os.getenv("ASTROBEE_OPS") + if astrobee_ops_path: + return os.path.join(astrobee_ops_path, "gds/plans") + + # Check if the symlink ~/gds/latest/ControlStationConfig/IssWorld exists + symlink_path = os.path.expanduser("~/gds/latest/ControlStationConfig/IssWorld") + if os.path.islink(symlink_path): + # Get the target of the symlink + target_path = os.path.realpath(symlink_path) + # Construct the relative path ../../plans + relative_path = os.path.join(target_path, "../../plans") + return relative_path + + # Return None if none of the conditions are met + return None # This class starts a new process and lets you monitor the input and output # Mostly used for actions where user inteference might be required @@ -293,6 +301,9 @@ class CommandExecutor: # Declare guest science command publisher self.sub_ack = rospy.Subscriber(self.ns + "/mgt/ack", AckStamped, self.ack_callback) self.ack_needed = False + self.sub_plan_status = rospy.Subscriber(self.ns + "/mgt/executive/plan_status", PlanStatusStamped, self.plan_status_callback) + self.plan_status_needed = False + self.plan_name = "" self.pub_command = rospy.Publisher(self.ns + "/command", CommandStamped, queue_size=5) while self.pub_command.get_num_connections() == 0: rospy.loginfo("Waiting for subscriber to connect") @@ -341,40 +352,23 @@ class CommandExecutor: rospy.loginfo("Change map to " + map_name) return 0 - def set_plan(self): - # TODO Add call to plan_pub.cc - - cmd = CommandStamped() - cmd.header = Header(stamp=rospy.Time.now()) - cmd.cmd_name = CommandConstants.CMD_NAME_SET_PLAN - cmd.cmd_id = "survey_manager" + str(rospy.Time.now().to_sec()) - self.unique_cmd_id = cmd.cmd_id - cmd.cmd_src = "isaac fsw" - cmd.cmd_origin = "isaac fsw" - cmd.args = cmd_args - - # Publish the CommandStamped message - result = self.publish_and_wait_response(cmd) - return result - - def run_plan(self): - cmd = CommandStamped() - cmd.header = Header(stamp=rospy.Time.now()) - cmd.cmd_name = CommandConstants.CMD_NAME_RUN_PLAN - cmd.cmd_id = "survey_manager" + str(rospy.Time.now().to_sec()) - self.unique_cmd_id = cmd.cmd_id - cmd.cmd_src = "isaac fsw" - cmd.cmd_origin = "isaac fsw" - - # Publish the CommandStamped message - result = self.publish_and_wait_response(cmd) - return result - def ack_callback(self, msg): if self.ack_needed == True and msg.cmd_id == self.unique_cmd_id: self.ack_msg = msg self.ack_needed = False + def plan_status_callback(self, msg): + if self.plan_status_needed == True: + rospy.loginfo("plan_name" + self.plan_name + "; msg name " + msg.name) + if self.plan_name in msg.name: + rospy.loginfo("In point " + str(msg.point) + " status " + str(msg.status.status)) + if msg.status.status == 3: + self.plan_status_needed = False + else: + # Plan changed, and previous plan did not complete + rospy.loginfo("Plan changed, exiting.") + self.plan_status_needed = False + def publish_and_wait_response(self, cmd): if rospy.is_shutdown(): return 1 @@ -401,6 +395,17 @@ class CommandExecutor: counter += 1 return 1 + def wait_plan(self, plan_name): + if rospy.is_shutdown(): + return 1 + # Wait for ack + counter = 0 + while counter < MAX_COUNTER: + # got message + if self.plan_status_needed == False: + return 0 + return 1 + def survey_manager_executor(robot_name, goal, to_bay, from_bay, run, config_static_path: pathlib.Path): @@ -415,7 +420,7 @@ def survey_manager_executor(robot_name, goal, to_bay, from_bay, run, config_stat # Figure out robot name and whether we are in simulation or hardware current_robot = os.environ.get('ROBOTNAME') if not current_robot: - rospy.loginfo("We're in simulation. Let's get the robotname using the topic") + rospy.loginfo("ROBOTNAME not defined. Let's get the robotname using the topic") # This is a latching messge so it shouldn't take long try: data = rospy.wait_for_message('/robot_name', String, timeout=5) @@ -457,13 +462,20 @@ def survey_manager_executor(robot_name, goal, to_bay, from_bay, run, config_stat elif goal == "panorama": exit_code += command_executor.start_recording("pano_" + to_bay + "_" + run) - exit_code += process_executor.send_command_recursive("rosrun inspection inspection_tool -panorama -panorama_mode '5_mapper_and_hugin' " + config_static["bays_move"][to_bay] + ns) + exit_code += process_executor.send_command_recursive("rosrun inspection inspection_tool -geometry -geometry_poses /resources/" + config_static["bays_pano"][to_bay] + ns) exit_code += command_executor.stop_recording() elif goal == "stereo": - exit_code += command_executor.start_recording("stereo_" + bay + "_" + run) - exit_code += command_executor.set_plan("plans/ISAAC/" + get_stereo_traj(from_bay, to_bay)) - exit_code += command_executor.run_plan() + exit_code += command_executor.start_recording("stereo_" + to_bay + "_" + run) + # This starts the plan + plan_path = get_ops_plan_path() + plan_name = get_stereo_traj(config_static, to_bay, from_bay) + + command_executor.plan_status_needed = True + command_executor.plan_name = plan_name + exit_code += process_executor.send_command_recursive("rosrun executive plan_pub " + os.path.join(plan_path, plan_name + ".fplan") + ns) + if exit_code == 0: + exit_code += command_executor.wait_plan(os.path.basename(plan_name)) exit_code += command_executor.stop_recording() return exit_code @@ -510,3 +522,5 @@ if __name__ == "__main__": args = parser.parse_args() exit_code = survey_manager_executor(args.robot_name, args.command_name, args.to_bay, args.from_bay, args.run, args.config_static) + + print("Finished plan action with code " + str(exit_code)) diff --git a/astrobee/survey_manager/survey_planner/tools/survey_planner/plan_interpreter.py b/astrobee/survey_manager/survey_planner/tools/survey_planner/plan_interpreter.py index a3df6c92..550fa514 100755 --- a/astrobee/survey_manager/survey_planner/tools/survey_planner/plan_interpreter.py +++ b/astrobee/survey_manager/survey_planner/tools/survey_planner/plan_interpreter.py @@ -42,7 +42,7 @@ from matplotlib import collections as mc from matplotlib import patches as mp from matplotlib import pyplot as plt -from problem_generator import DATA_DIR, load_yaml, path_list +from problem_generator import DATA_DIR, get_stereo_traj, load_yaml, path_list DEFAULT_CONFIGS = [ DATA_DIR / "survey_static.yaml", @@ -138,15 +138,8 @@ def yaml_action_from_pddl( if action_type == "stereo": robot, _order, base, bound, _check1, _check2 = action_args[1:] # Use base and bound to look up trajectory. - traj_matches = [ - traj - for traj in static_config["stereo"].values() - if traj["base_location"] == base and traj["bound_location"] == bound - ] - assert ( - len(traj_matches) == 1 - ), f"Expected exactly 1 matching stereo trajectory with base {base} and bound {bound}, got {len(traj_matches)}" - fplan = traj_matches[0]["fplan"] + fplan = get_stereo_traj(static_config, base, bound) + # Can discard order check1, check2. return { "type": "stereo", diff --git a/astrobee/survey_manager/survey_planner/tools/survey_planner/problem_generator.py b/astrobee/survey_manager/survey_planner/tools/survey_planner/problem_generator.py index bf966281..4ee335dd 100755 --- a/astrobee/survey_manager/survey_planner/tools/survey_planner/problem_generator.py +++ b/astrobee/survey_manager/survey_planner/tools/survey_planner/problem_generator.py @@ -76,6 +76,19 @@ def load_yaml(yaml_path: pathlib.Path) -> YamlMapping: return yaml.safe_load(yaml_stream) +def get_stereo_traj(static_config, base, bound): + traj_matches = [ + traj + for traj in static_config["stereo"].values() + if traj["base_location"] == base and traj["bound_location"] == bound + ] + assert ( + len(traj_matches) == 1 + ), f"Expected exactly 1 matching stereo trajectory with base {base} and bound {bound}, got {len(traj_matches)}" + fplan = traj_matches[0]["fplan"] + return fplan + + def pddl_goal_from_yaml(goal: YamlMapping, config_static: YamlMapping) -> str: """ Convert a YAML goal with named fields from the dynamic config into a PDDL goal predicate. From 12663c33adb9813413ff0e990e38108e3ef7ec5e Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Fri, 19 Jan 2024 21:50:30 -0800 Subject: [PATCH 39/44] remote plansys2 submodules from doxygen --- isaac.doxyfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/isaac.doxyfile b/isaac.doxyfile index 857f52e3..29ac3c37 100644 --- a/isaac.doxyfile +++ b/isaac.doxyfile @@ -899,7 +899,7 @@ RECURSIVE = YES # Note that relative paths are relative to the directory from which doxygen is # run. -EXCLUDE = +EXCLUDE =astrobee/survey_manager/survey_dependencies # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or # directories that are symbolic links (a Unix file system feature) are excluded From 8b61b23b46f4eebc2762fcbe2d6aa5e36155a712 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Fri, 19 Jan 2024 21:51:41 -0800 Subject: [PATCH 40/44] splitting documentation --- .../survey_planner/action_testing.md | 209 ++++++++++++++++++ .../survey_manager/survey_planner/readme.md | 106 +-------- 2 files changed, 212 insertions(+), 103 deletions(-) create mode 100644 astrobee/survey_manager/survey_planner/action_testing.md diff --git a/astrobee/survey_manager/survey_planner/action_testing.md b/astrobee/survey_manager/survey_planner/action_testing.md new file mode 100644 index 00000000..0853b2bc --- /dev/null +++ b/astrobee/survey_manager/survey_planner/action_testing.md @@ -0,0 +1,209 @@ +\page action_testing Action Testing + +## Simulation + +### Move + + set instance bumble robot + set instance jem_bay7 location + set instance jem_bay6 location + set instance jem_bay5 location + + set predicate (robot-available bumble) + set predicate (robot-at bumble jem_bay7) + set predicate (location-available jem_bay6) + + set predicate (move-connected jem_bay7 jem_bay6) + set predicate (location-real jem_bay6) + set predicate (locations-different jem_bay5 jem_bay7) + set predicate (move-connected jem_bay5 jem_bay6) + set predicate (location-available jem_bay5) + + + run action (move bumble jem_bay7 jem_bay6 jem_bay5) + +### Dock + + set instance bumble robot + set instance jem_bay7 location + set instance berth1 location + + set predicate (robot-available bumble) + set predicate (robot-at bumble jem_bay7) + set predicate (dock-connected jem_bay7 berth1) + set predicate (location-available berth1) + + run action (dock bumble jem_bay7 berth1) + + +### Undock + + + set instance bumble robot + set instance jem_bay6 location + set instance jem_bay7 location + set instance jem_bay8 location + set instance berth1 location + + set predicate (robot-available bumble) + set predicate (robot-at bumble berth1) + set predicate (dock-connected jem_bay7 berth1) + set predicate (location-available jem_bay7) + set predicate (location-real jem_bay7) + set predicate (locations-different jem_bay8 jem_bay6) + set predicate (move-connected jem_bay8 jem_bay7) + set predicate (move-connected jem_bay6 jem_bay7) + set predicate (location-available jem_bay8) + set predicate (location-available jem_bay6) + + run action (undock bumble berth1 jem_bay7 jem_bay8 jem_bay6) + + +### Panorama + + set instance bumble robot + set instance jem_bay6 location + set instance o0 order + + set predicate (robot-available bumble) + + set function (= (order-identity o0) 0) + set function (= (robot-order bumble) -1) + + set predicate (robot-at bumble jem_bay6) + + run action (panorama bumble o0 jem_bay6) + + +### Stereo + + + set instance bumble robot + set instance jem_bay7 location + set instance jem_bay6 location + set instance jem_bay5 location + set instance jem_bay4 location + set instance jem_bay3 location + set instance o0 order + + + set predicate (robot-available bumble) + set predicate (robot-at bumble jem_bay7) + set predicate (location-real jem_bay4) + set predicate (need-stereo bumble o0 jem_bay7 jem_bay4) + set predicate (location-available jem_bay4) + set predicate (locations-different jem_bay5 jem_bay3) + set predicate (move-connected jem_bay5 jem_bay4) + set predicate (move-connected jem_bay3 jem_bay4) + set predicate (location-available jem_bay5) + set predicate (location-available jem_bay3) + + set function (= (order-identity o0) 0) + set function (= (robot-order bumble) -1) + + + run action (stereo bumble o0 jem_bay7 jem_bay4 jem_bay5 jem_bay3) + +## Granite table + +### Move + + set instance wannabee robot + set instance gra_p1 location + set instance gra_p2 location + set instance jem_bay5 location + + set predicate (robot-available wannabee) + set predicate (robot-at wannabee gra_p1) + set predicate (location-available gra_p2) + + set predicate (move-connected gra_p1 gra_p2) + set predicate (location-real gra_p2) + set predicate (locations-different jem_bay5 gra_p1) + set predicate (move-connected jem_bay5 gra_p2) + set predicate (location-available jem_bay5) + + + run action (move wannabee gra_p1 gra_p2 jem_bay5) + +### Dock + + set instance wannabee robot + set instance gra_p1 location + set instance berth2 location + + set predicate (robot-available wannabee) + set predicate (robot-at wannabee gra_p1) + set predicate (dock-connected gra_p1 berth2) + set predicate (location-available berth2) + + run action (dock wannabee gra_p1 berth2) + + +### Undock + + + set instance wannabee robot + set instance gra_p2 location + set instance gra_p1 location + set instance gra_p0 location + set instance berth1 location + + set predicate (robot-available wannabee) + set predicate (robot-at wannabee berth1) + set predicate (dock-connected gra_p1 berth1) + set predicate (location-available gra_p1) + set predicate (location-real gra_p1) + set predicate (locations-different gra_p0 gra_p2) + set predicate (move-connected gra_p0 gra_p1) + set predicate (move-connected gra_p2 gra_p1) + set predicate (location-available gra_p0) + set predicate (location-available gra_p2) + + run action (undock wannabee berth1 gra_p1 gra_p0 gra_p2) + + +### Panorama + + set instance wannabee robot + set instance gra_p2 location + set instance o0 order + + set predicate (robot-available wannabee) + + set function (= (order-identity o0) 0) + set function (= (robot-order wannabee) -1) + + set predicate (robot-at wannabee gra_p2) + + run action (panorama wannabee o0 gra_p2) + + +### Stereo + + + set instance wannabee robot + set instance gra_p1 location + set instance gra_p2 location + set instance jem_bay5 location + set instance jem_bay4 location + set instance jem_bay3 location + set instance o0 order + + + set predicate (robot-available wannabee) + set predicate (robot-at wannabee gra_p1) + set predicate (location-real jem_bay4) + set predicate (need-stereo wannabee o0 gra_p1 jem_bay4) + set predicate (location-available jem_bay4) + set predicate (locations-different jem_bay5 jem_bay3) + set predicate (move-connected jem_bay5 jem_bay4) + set predicate (move-connected jem_bay3 jem_bay4) + set predicate (location-available jem_bay5) + set predicate (location-available jem_bay3) + + set function (= (order-identity o0) 0) + set function (= (robot-order wannabee) -1) + + + run action (stereo wannabee o0 gra_p1 jem_bay4 jem_bay5 jem_bay3) \ No newline at end of file diff --git a/astrobee/survey_manager/survey_planner/readme.md b/astrobee/survey_manager/survey_planner/readme.md index c4a56854..fe277abd 100644 --- a/astrobee/survey_manager/survey_planner/readme.md +++ b/astrobee/survey_manager/survey_planner/readme.md @@ -4,7 +4,7 @@ Planning and scheduling of queued survey actions using Playsys2 for PDDL solutio Based on [preliminary work](https://github.com/traclabs/astrobee_task_planning_ws) by [Ana](https://github.com/ana-GT) at [Traclabs](https://traclabs.com). -## Starting suvey planner +## Starting survey planner The survey_planner as of now starts separately from the isaac fsw. This is necessary because the planner crashes very easily, so having the ability to easily restart this component is necessary. @@ -24,106 +24,6 @@ To run each action individually through the survey_planner you must add the corr Don't forget to change the commands to use the target robot's name (bumble is what is used in simulation) -If the survey_planner crashes sometimes the simulation has to be restarted, for it to restart in a good state. +If the survey_planner crashes sometimes the simulation has to be restarted for it to restart in a good state. -### Move - - set instance bumble robot - set instance jem_bay7 location - set instance jem_bay6 location - set instance jem_bay5 location - - set predicate (robot-available bumble) - set predicate (robot-at bumble jem_bay7) - set predicate (location-available jem_bay6) - - set predicate (move-connected jem_bay7 jem_bay6) - set predicate (location-real jem_bay6) - set predicate (locations-different jem_bay5 jem_bay7) - set predicate (move-connected jem_bay5 jem_bay6) - set predicate (location-available jem_bay5) - - - run action (move bumble jem_bay7 jem_bay6 jem_bay5) - -### Dock - - set instance bumble robot - set instance jem_bay7 location - set instance berth1 location - - set predicate (robot-available bumble) - set predicate (robot-at bumble jem_bay7) - set predicate (dock-connected jem_bay7 berth1) - set predicate (location-available berth1) - - run action (dock bumble jem_bay7 berth1) - - -### Undock - - - set instance bumble robot - set instance jem_bay6 location - set instance jem_bay7 location - set instance jem_bay8 location - set instance berth1 location - - set predicate (robot-available bumble) - set predicate (robot-at bumble berth1) - set predicate (dock-connected jem_bay7 berth1) - set predicate (location-available jem_bay7) - set predicate (location-real jem_bay7) - set predicate (locations-different jem_bay8 jem_bay6) - set predicate (move-connected jem_bay8 jem_bay7) - set predicate (move-connected jem_bay6 jem_bay7) - set predicate (location-available jem_bay8) - set predicate (location-available jem_bay6) - - run action (undock bumble berth1 jem_bay7 jem_bay8 jem_bay6) - - -### Panorama - - set instance bumble robot - set instance jem_bay6 location - set instance o0 order - - set predicate (robot-available bumble) - - set function (= (order-identity o0) 0) - set function (= (robot-order bumble) -1) - - set predicate (robot-at bumble jem_bay6) - - run action (panorama bumble o0 jem_bay6) - - -### Stereo - - - set instance bumble robot - set instance jem_bay7 location - set instance jem_bay6 location - set instance jem_bay5 location - set instance jem_bay4 location - set instance jem_bay3 location - set instance o0 order - - - set predicate (robot-available bumble) - set predicate (robot-at bumble jem_bay7) - set predicate (location-real jem_bay4) - set predicate (need-stereo bumble o0 jem_bay7 jem_bay4) - set predicate (location-available jem_bay4) - set predicate (locations-different jem_bay5 jem_bay3) - set predicate (move-connected jem_bay5 jem_bay4) - set predicate (move-connected jem_bay3 jem_bay4) - set predicate (location-available jem_bay5) - set predicate (location-available jem_bay3) - - set function (= (order-identity o0) 0) - set function (= (robot-order bumble) -1) - - - run action (stereo bumble o0 jem_bay7 jem_bay4 jem_bay5 jem_bay3) \ No newline at end of file +\subpage action_testing \ No newline at end of file From 49add1423a81071a59af06de2204245262238a46 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Sat, 20 Jan 2024 01:18:35 -0800 Subject: [PATCH 41/44] addressing tons of comments on PR, lab tested --- .../inspection/tools/inspection_tool.cc | 37 +++++- .../survey_planner/isaac_action_node.h | 1 + .../survey_planner/src/isaac_action_node.cpp | 22 ++-- .../tools/survey_planner/command_astrobee | 119 ++++++++---------- .../tools/survey_planner/plan_interpreter.py | 81 ++---------- .../tools/survey_planner/problem_generator.py | 86 ++++++++++++- 6 files changed, 186 insertions(+), 160 deletions(-) diff --git a/astrobee/behaviors/inspection/tools/inspection_tool.cc b/astrobee/behaviors/inspection/tools/inspection_tool.cc index 29de2af5..2749f107 100644 --- a/astrobee/behaviors/inspection/tools/inspection_tool.cc +++ b/astrobee/behaviors/inspection/tools/inspection_tool.cc @@ -311,10 +311,37 @@ void SendGoal(ff_util::FreeFlyerActionClient *clie client->SendGoal(goal); } +bool GetlineAsync(std::istream& is, std::string& str, char delim = '\n') { + static std::string linesofar; + char inchar; + int charsread = 0; + bool lineread = false; + str = ""; + + do { + charsread = is.readsome(&inchar, 1); + if (charsread == 1) { + // if the delimiter is read then return the string so far + if (inchar == delim) { + str = linesofar; + linesofar = ""; + lineread = true; + } else { // otherwise add it to the string so far + linesofar.append(1, inchar); + } + } + } while (charsread != 0 && !lineread && !!stopflag_); + + return lineread; +} + void GetInput(ff_util::FreeFlyerActionClient *client) { while (!stopflag_ && ros::ok()) { std::string line, val; - std::getline(std::cin, line); + + if (!GetlineAsync(std::cin, line)) + continue; + std::string s; try { switch (std::stoi(line)) { @@ -331,7 +358,7 @@ void GetInput(ff_util::FreeFlyerActionClient *clie SendGoal(client); s = "\r Input: " + line + ") Pausing"; if (s.size() < 80) s.append(80 - s.size(), ' '); - std::cout << s << std::flush; + std::cout << s << std::endl; break; case 2: FLAGS_pause = false; @@ -348,7 +375,7 @@ void GetInput(ff_util::FreeFlyerActionClient *clie SendGoal(client); s = "\r Input: " + line + ") Pausing and repeating pose (needs resume)"; if (s.size() < 80) s.append(80 - s.size(), ' '); - std::cout << s << std::flush; + std::cout << s << std::endl; break; case 4: FLAGS_pause = false; @@ -358,7 +385,7 @@ void GetInput(ff_util::FreeFlyerActionClient *clie SendGoal(client); s = "\r Input: " + line + ") Pausing and skipping pose (needs resume)"; if (s.size() < 80) s.append(80 - s.size(), ' '); - std::cout << s << std::flush; + std::cout << s << std::endl; break; case 5: FLAGS_pause = false; @@ -369,7 +396,7 @@ void GetInput(ff_util::FreeFlyerActionClient *clie SendGoal(client); s = "\r Input: " + line + ") Pausing and saving (needs resume)"; if (s.size() < 80) s.append(80 - s.size(), ' '); - std::cout << s << std::flush; + std::cout << s << std::endl; break; default: s = "\r Input: " + line + ") Invalid option"; diff --git a/astrobee/survey_manager/survey_planner/include/survey_planner/isaac_action_node.h b/astrobee/survey_manager/survey_planner/include/survey_planner/isaac_action_node.h index 0d60a331..bf28a343 100644 --- a/astrobee/survey_manager/survey_planner/include/survey_planner/isaac_action_node.h +++ b/astrobee/survey_manager/survey_planner/include/survey_planner/isaac_action_node.h @@ -36,6 +36,7 @@ class IsaacAction : public plansys2::ActionExecutorClient { float progress_; std::string robot_name_, action_name_; int pid_; + std::string command_; }; } // namespace plansys2_actions diff --git a/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp b/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp index a2337e5e..dfae9d16 100644 --- a/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp +++ b/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp @@ -37,16 +37,13 @@ IsaacAction::IsaacAction(ros::NodeHandle nh, const std::string& action, const st action_name_ = action; progress_ = 0.0; pid_ = 0; + command_ = ""; } void IsaacAction::do_work() { std::string from, towards; - if (get_arguments().size() > 2) { - robot_name_ = get_arguments()[0]; - from = get_arguments()[1]; - towards = get_arguments()[2]; - } else { + if (get_arguments().size() < 3) { finish(false, 1.0, "Not enough arguments for [MOVE] command"); } @@ -57,14 +54,17 @@ void IsaacAction::do_work() { perror("Fork failed."); finish(false, 1.0, "Failed to start the process"); } else if (pid_ == 0) { - printf("rosrun survey_planner command_astrobee %s %s %s %s run1\n", - robot_name_.c_str(), action_name_.c_str(), towards.c_str(), from.c_str()); const char* args[4]; args[0] = "sh"; args[1] = "-c"; - args[2] = ("rosrun survey_planner command_astrobee " + robot_name_ + " " + - action_name_ + " " + towards + " " + from + " run1").c_str(); + command_ = "rosrun survey_planner command_astrobee "; + for (const auto& arg : get_arguments()) { + command_ += arg + " "; + } + command_ += "run1"; + args[2] = command_.c_str(); args[3] = NULL; + printf("%s\n", args[2]); execvpe("sh", (char* const*)args, environ); perror("Failed to execute command."); printf("EXITING FAILURE %d\n", getpid()); @@ -80,8 +80,8 @@ void IsaacAction::do_work() { send_feedback(progress_, action_name_ + " running"); } - std::cout << "\t ** [" << action_name_ << "] Robot " << robot_name_ << " moving from " << from << " towards " - << towards << " ... [" << std::min(100.0, progress_ * 100.0) << "%] " << std::endl; + std::cout << "\t ** [" << action_name_ << "] " << command_ << " [" << std::min(100.0, progress_ * 100.0) << "%] " + << std::endl; int status; int result = waitpid(-1, &status, WNOHANG); printf("Result: %d %d %d\n", result, pid_, status); diff --git a/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee b/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee index d7bf905a..04f870f4 100755 --- a/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee +++ b/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee @@ -42,7 +42,7 @@ from ff_msgs.msg import ( from std_msgs.msg import Header, String # Imports from survey_planner package -from survey_planner.problem_generator import load_yaml, get_stereo_traj +from survey_planner.problem_generator import load_yaml, yaml_action_from_pddl # Constants MAX_COUNTER = 10 @@ -120,14 +120,14 @@ class ProcessExecutor: # Declare socket for process input self.sock_input = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) - self.sock_input.settimeout(1) # Set a timeout for socket operations + self.sock_input.settimeout(0.05) # Set a timeout for socket operations self.sock_input.bind(self.input_path) self.sock_input.listen(1) # Listen for one connection self.sock_input_connected = False # Declare socket for process output self.sock_output = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) - self.sock_output.settimeout(1) # Set a timeout for socket operations + self.sock_output.settimeout(0.05) # Set a timeout for socket operations self.sock_output.bind(self.output_path) self.sock_output.listen(1) # Listen for one connection self.sock_output_connected = False @@ -265,29 +265,18 @@ class ProcessExecutor: # Get the final exit code return return_code - def recursive_command(self, command): - while not rospy.is_shutdown(): - user_input = input("Do you want to continue? (y/n): ").lower().strip() - if user_input == 'y': - exit_code = self.send_command(command) - if exit_code != 0: - exit_code = self.recursive_command(command) # Continue recursively - return exit_code - elif user_input == 'n': - print("Exiting.") - return 1 - else: - print("Invalid input. Please enter 'y' or 'n'.") - return 1 - def send_command_recursive(self, command): print("Sending recursive command") + exit_code = self.send_command(command) print("Exit code " + str(exit_code)) - if (exit_code and not rospy.is_shutdown()): - exit_code = self.recursive_command(command) - return exit_code + if exit_code != 0 and not rospy.is_shutdown(): + repeat = input("Do you want to repeat the command? (yes/no): ").lower() + print(repeat) + if repeat == "yes": + exit_code = exit_code = self.send_command_recursive(command) + return exit_code # This class sends a command to the executor and waits to get a response # Mostly used for short actions that should be immediate and require no feedback @@ -395,7 +384,7 @@ class CommandExecutor: counter += 1 return 1 - def wait_plan(self, plan_name): + def wait_plan(self): if rospy.is_shutdown(): return 1 # Wait for ack @@ -408,14 +397,16 @@ class CommandExecutor: -def survey_manager_executor(robot_name, goal, to_bay, from_bay, run, config_static_path: pathlib.Path): - - # Start ROS node - rospy.init_node("survey_namager_cmd_" + robot_name, anonymous=True) +def survey_manager_executor(command_names, run, config_static_path: pathlib.Path): # Read the static configs that convert constants to values config_static = load_yaml(config_static_path) + args = yaml_action_from_pddl(f"[{' '.join(command_names)}]", config_static) + + # Start ROS node + rospy.init_node("survey_namager_cmd_" + args["robot"], anonymous=True) + sim = False # Figure out robot name and whether we are in simulation or hardware current_robot = os.environ.get('ROBOTNAME') @@ -431,55 +422,64 @@ def survey_manager_executor(robot_name, goal, to_bay, from_bay, run, config_stat ns = "" # If we're commanding a robot remotely - if current_robot != robot_name: + if current_robot != args["robot"]: rospy.loginfo("We're commanding a namespaced robot!") - ns = " -remote -ns " + robot_name + ns = " -remote -ns " + args["robot"] # Command executor will add namespace for bridge forwarding - command_executor = CommandExecutor(robot_name) + command_executor = CommandExecutor(args["robot"]) else: command_executor = CommandExecutor("") - process_executor = ProcessExecutor(robot_name) + process_executor = ProcessExecutor(args["robot"]) # Initialize exit code exit_code = 0 - if goal == "dock": - exit_code += process_executor.send_command_recursive("rosrun executive teleop_tool -dock" + ns + " -berth " + config_static["berth"][to_bay]) + if args["type"] == "dock": + exit_code += process_executor.send_command_recursive("rosrun executive teleop_tool -dock" + ns + " -berth " + config_static["berth"][args["berth"]]) - elif goal == "undock": + elif args["type"] == "undock": exit_code += process_executor.send_command_recursive("rosrun executive teleop_tool -undock" + ns) - elif goal == "move": - exit_code += process_executor.send_command_recursive("rosrun executive teleop_tool -move " + config_static["bays_move"][to_bay] + ns) + elif args["type"] == "move": + exit_code += process_executor.send_command_recursive("rosrun executive teleop_tool -move " + config_static["bays_move"][args["to_name"]] + ns) # Change exposure if needed - exposure_value = exposure_change(config_static, from_bay, to_bay) + exposure_value = exposure_change(config_static, args["from_name"], args["to_name"]) if exposure_value != 0: exit_code += command_executor.change_exposure(exposure_value) # Change map if needed - map_name = map_change(config_static, from_bay, to_bay) + map_name = map_change(config_static, args["from_name"], args["to_name"]) if map_name != "": exit_code += command_executor.change_map(map_name) - elif goal == "panorama": - exit_code += command_executor.start_recording("pano_" + to_bay + "_" + run) - exit_code += process_executor.send_command_recursive("rosrun inspection inspection_tool -geometry -geometry_poses /resources/" + config_static["bays_pano"][to_bay] + ns) + elif args["type"] == "panorama": + exit_code += command_executor.start_recording("pano_" + args["location_name"] + "_" + run) + exit_code += process_executor.send_command_recursive("rosrun inspection inspection_tool -geometry -geometry_poses /resources/" + config_static["bays_pano"][args["location_name"]] + ns) exit_code += command_executor.stop_recording() - elif goal == "stereo": - exit_code += command_executor.start_recording("stereo_" + to_bay + "_" + run) + elif args["type"] == "stereo": + exit_code += command_executor.start_recording("stereo_" + os.path.basename(args["fplan"]) + "_" + run) # This starts the plan plan_path = get_ops_plan_path() - plan_name = get_stereo_traj(config_static, to_bay, from_bay) command_executor.plan_status_needed = True - command_executor.plan_name = plan_name - exit_code += process_executor.send_command_recursive("rosrun executive plan_pub " + os.path.join(plan_path, plan_name + ".fplan") + ns) + command_executor.plan_name = os.path.basename(args["fplan"]) + exit_code += process_executor.send_command_recursive("rosrun executive plan_pub " + os.path.join(plan_path, args["fplan"] + ".fplan") + ns) if exit_code == 0: - exit_code += command_executor.wait_plan(os.path.basename(plan_name)) + exit_code += command_executor.wait_plan() exit_code += command_executor.stop_recording() return exit_code +def survey_manager_executor_recursive(command_names, run_number, config_static_path: pathlib.Path): + exit_code = survey_manager_executor(command_names, f"run{run_number}", config_static_path) + + if exit_code != 0: + repeat = input("Do you want to repeat the survey? (yes/no): ").lower() + if repeat == "yes": + run_number += 1 + exit_code = survey_manager_executor_recursive(command_names, run_number, config_static_path) + + return exit_code class CustomFormatter(argparse.ArgumentDefaultsHelpFormatter): pass @@ -490,28 +490,9 @@ if __name__ == "__main__": ) parser.add_argument( - "robot_name", - default="", - help="Robot name executing the command.", - ) - parser.add_argument( - "command_name", - help="Prefix for bagfiles to merge. Bags should all be in the current working directory.", - ) - parser.add_argument( - "to_bay", - default="", - help="Target bay destination.", - ) - parser.add_argument( - "from_bay", - default="", - help="Target bay start.", - ) - parser.add_argument( - "run", - default="", - help="Run number, increases as we add attempts.", + "command_names", + nargs='*', + help="Prefixes for bagfiles to merge. Bags should all be in the current working directory.", ) parser.add_argument( "--config_static", @@ -521,6 +502,6 @@ if __name__ == "__main__": ) args = parser.parse_args() - exit_code = survey_manager_executor(args.robot_name, args.command_name, args.to_bay, args.from_bay, args.run, args.config_static) + exit_code = survey_manager_executor_recursive(args.command_names, 1, args.config_static) print("Finished plan action with code " + str(exit_code)) diff --git a/astrobee/survey_manager/survey_planner/tools/survey_planner/plan_interpreter.py b/astrobee/survey_manager/survey_planner/tools/survey_planner/plan_interpreter.py index 550fa514..0939e803 100755 --- a/astrobee/survey_manager/survey_planner/tools/survey_planner/plan_interpreter.py +++ b/astrobee/survey_manager/survey_planner/tools/survey_planner/plan_interpreter.py @@ -42,21 +42,19 @@ from matplotlib import collections as mc from matplotlib import patches as mp from matplotlib import pyplot as plt -from problem_generator import DATA_DIR, get_stereo_traj, load_yaml, path_list +from problem_generator import ( + DATA_DIR, + get_stereo_traj, + load_yaml, + path_list, + yaml_action_from_pddl, +) DEFAULT_CONFIGS = [ DATA_DIR / "survey_static.yaml", # Dynamic config not needed for interpreting the plan ] -ACTION_TYPE_OPTIONS = ( - "dock", - "undock", - "move", - "panorama", - "stereo", - "let-other-robot-reach", -) COLORS = {"bumble": "#4080ffff", "honey": "#c0c080ff"} ROBOTS = list(COLORS.keys()) @@ -91,71 +89,6 @@ def __repr__(self): return f"{self.start_time_seconds}: {self.action} [{self.duration_seconds}]" -def yaml_action_from_pddl( - action: str, static_config: YamlMapping -) -> Optional[YamlMapping]: - """ - Return a YamlMapping representation of `action`. This is the only place - we really need domain-specific logic. - """ - action_args = action[1:-1].split() - action_type = action_args[0] - assert ( - action_type in ACTION_TYPE_OPTIONS - ), f"Expected action type in {ACTION_TYPE_OPTIONS}, got {action_type}" - - if action_type == "dock": - robot, _from_bay, to_berth = action_args[1:] - # Can discard from_bay - return {"type": "dock", "robot": robot, "berth": to_berth} - - if action_type == "undock": - robot, _from_berth, _to_bay, _check1, _check2 = action_args[1:] - # Can discard from_berth, to_bay, check1, check2 - return {"type": "undock", "robot": robot} - - if action_type == "move": - robot, from_bay, to_bay, _check_bay = action_args[1:] - # Can discard check_bay. Look up coordinates for to_bay. - return { - "type": "move", - "robot": robot, - "from_name": from_bay, - "to_name": to_bay, - "to_pos": static_config["bays"][to_bay], - } - - if action_type == "panorama": - robot, _order, location = action_args[1:] - # Can discard order. Look up coordinates for location. - return { - "type": "panorama", - "robot": robot, - "location_name": location, - "location_pos": static_config["bays"][location], - } - - if action_type == "stereo": - robot, _order, base, bound, _check1, _check2 = action_args[1:] - # Use base and bound to look up trajectory. - fplan = get_stereo_traj(static_config, base, bound) - - # Can discard order check1, check2. - return { - "type": "stereo", - "robot": robot, - "fplan": fplan, - "base_name": base, - "bound_name": bound, - } - - if action_type == "let-other-robot-reach": - return None # Action is a no-op intended only to constrain the planner - - assert False, "Never reach this point." - return {} # Make pylint happy - - def yaml_plan_action_from_pddl( plan_action: PlanAction, static_config: YamlMapping ) -> Optional[YamlMapping]: diff --git a/astrobee/survey_manager/survey_planner/tools/survey_planner/problem_generator.py b/astrobee/survey_manager/survey_planner/tools/survey_planner/problem_generator.py index 4ee335dd..746ad507 100755 --- a/astrobee/survey_manager/survey_planner/tools/survey_planner/problem_generator.py +++ b/astrobee/survey_manager/survey_planner/tools/survey_planner/problem_generator.py @@ -45,12 +45,31 @@ import shlex import sys from abc import ABC, abstractmethod -from typing import Any, Dict, Iterable, List, Mapping, Sequence, Tuple, TypeVar +from typing import ( + Any, + Dict, + Iterable, + List, + Mapping, + Optional, + Sequence, + Tuple, + TypeVar, +) import yaml GOAL_TYPE_OPTIONS = ("panorama", "stereo", "robot_at", "let_other_robot_reach") +ACTION_TYPE_OPTIONS = ( + "dock", + "undock", + "move", + "panorama", + "stereo", + "let-other-robot-reach", +) + THIS_DIR = pathlib.Path(__file__).resolve().parent CWD = pathlib.Path.cwd() DATA_DIR = pathlib.Path(os.path.relpath(str((THIS_DIR / ".." / "data").resolve()), CWD)) @@ -89,6 +108,71 @@ def get_stereo_traj(static_config, base, bound): return fplan +def yaml_action_from_pddl( + action: str, static_config: YamlMapping +) -> Optional[YamlMapping]: + """ + Return a YamlMapping representation of `action`. This is the only place + we really need domain-specific logic. + """ + action_args = action[1:-1].split() + action_type = action_args[0] + assert ( + action_type in ACTION_TYPE_OPTIONS + ), f"Expected action type in {ACTION_TYPE_OPTIONS}, got {action_type}" + + if action_type == "dock": + robot, _from_bay, to_berth = action_args[1:] + # Can discard from_bay + return {"type": "dock", "robot": robot, "berth": to_berth} + + if action_type == "undock": + robot, _from_berth, _to_bay, _check1, _check2 = action_args[1:] + # Can discard from_berth, to_bay, check1, check2 + return {"type": "undock", "robot": robot} + + if action_type == "move": + robot, from_bay, to_bay, _check_bay = action_args[1:] + # Can discard check_bay. Look up coordinates for to_bay. + return { + "type": "move", + "robot": robot, + "from_name": from_bay, + "to_name": to_bay, + "to_pos": static_config["bays"][to_bay], + } + + if action_type == "panorama": + robot, _order, location = action_args[1:] + # Can discard order. Look up coordinates for location. + return { + "type": "panorama", + "robot": robot, + "location_name": location, + "location_pos": static_config["bays"][location], + } + + if action_type == "stereo": + robot, _order, base, bound, _check1, _check2 = action_args[1:] + # Use base and bound to look up trajectory. + fplan = get_stereo_traj(static_config, base, bound) + + # Can discard order check1, check2. + return { + "type": "stereo", + "robot": robot, + "fplan": fplan, + "base_name": base, + "bound_name": bound, + } + + if action_type == "let-other-robot-reach": + return None # Action is a no-op intended only to constrain the planner + + assert False, "Never reach this point." + return {} # Make pylint happy + + def pddl_goal_from_yaml(goal: YamlMapping, config_static: YamlMapping) -> str: """ Convert a YAML goal with named fields from the dynamic config into a PDDL goal predicate. From a5ca4c802ba5df5d8b70af2050d473529466f4c7 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Sat, 20 Jan 2024 01:32:01 -0800 Subject: [PATCH 42/44] more PR reviews --- astrobee/survey_manager/survey_planner/data/survey_static.yaml | 2 +- .../survey_planner/tools/survey_planner/command_astrobee | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/data/survey_static.yaml b/astrobee/survey_manager/survey_planner/data/survey_static.yaml index 59de50ce..00a362a4 100644 --- a/astrobee/survey_manager/survey_planner/data/survey_static.yaml +++ b/astrobee/survey_manager/survey_planner/data/survey_static.yaml @@ -119,7 +119,7 @@ stereo: # bound_location: The other end of the interval visited by the trajectory, for planner collision # check purposes. bound_location: jem_bay4 # The survey flies into bay4 even though it only covers up to bay3 - jem_bay4_to_bay7: + jem_bay7_to_bay4: fplan: "ISAAC/jem_stereo_mapping_bay7_to_bay4" base_location: jem_bay7 bound_location: jem_bay4 diff --git a/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee b/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee index 04f870f4..ed5c0dbc 100755 --- a/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee +++ b/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee @@ -278,7 +278,7 @@ class ProcessExecutor: exit_code = exit_code = self.send_command_recursive(command) return exit_code -# This class sends a command to the executor and waits to get a response +# This class sends a command to the astrobee executor and waits to get a response # Mostly used for short actions that should be immediate and require no feedback # This method is needed on actions that run remotely and are not controlled by topics class CommandExecutor: From d1522ba71d06d44611dbb677cdf4359f3d797f90 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Sat, 20 Jan 2024 01:33:27 -0800 Subject: [PATCH 43/44] more PR reviews --- .../survey_planner/tools/survey_planner/command_astrobee | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee b/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee index ed5c0dbc..151160e5 100755 --- a/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee +++ b/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee @@ -143,7 +143,7 @@ class ProcessExecutor: def thread_write_output(self, process): # print("starting thread_write_output...") - # Store commulative output + # Store cumulative output output_total = "" try: while True: From 29ff61ba3ae41659acdfc9ad933fc7717a67f257 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Sat, 20 Jan 2024 01:40:32 -0800 Subject: [PATCH 44/44] more PR reviews --- .../survey_planner/tools/survey_planner/command_astrobee | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee b/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee index 151160e5..2f6b0498 100755 --- a/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee +++ b/astrobee/survey_manager/survey_planner/tools/survey_planner/command_astrobee @@ -37,7 +37,7 @@ from ff_msgs.msg import ( CommandArg, CommandConstants, CommandStamped, - PlanStatusStamped + PlanStatusStamped, ) from std_msgs.msg import Header, String