Skip to content

Commit f4f793e

Browse files
jorgenfjvortexuser
jorgenfj
and
vortexuser
authored
Development (#718)
* Added ins origin logic. Rotated tf2 dvl frame * added choices to joystick ori mode launch arg * add type for drone param in joystick node * new waypoint mode. Renamed util scripts --------- Co-authored-by: vortexuser <vortex.git@vortexntnu.no>
1 parent c46b3b5 commit f4f793e

6 files changed

Lines changed: 104 additions & 70 deletions

File tree

‎guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,8 @@ enum class WaypointMode : uint8_t {
2020
FORWARD_HEADING = 2, ///< Control x, y, z with yaw toward target.
2121
ONLY_ORIENTATION = 3, ///< Control roll, pitch, yaw; hold current position.
2222
POSITION_AND_YAW = 4, ///< Control x, y, z and yaw; force roll=pitch=0.
23-
XY_AND_YAW = 5, ///< Control x, y and yaw; hold z, force roll=pitch=0.
23+
XY_AND_YAW = 5, ///< Control x, y and yaw; hold z, force roll=pitch=0.
24+
XY_FORWARD_DIR = 6, ///< Control x, y; hold z; yaw auto-computed toward target.
2425
};
2526

2627
/**

‎guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,8 @@ inline WaypointMode waypoint_mode_from_ros(
3232
return WaypointMode::POSITION_AND_YAW;
3333
case vortex_msgs::msg::WaypointMode::XY_AND_YAW:
3434
return WaypointMode::XY_AND_YAW;
35+
case vortex_msgs::msg::WaypointMode::XY_FORWARD_DIR:
36+
return WaypointMode::XY_FORWARD_DIR;
3537
default:
3638
throw std::invalid_argument("Invalid ROS waypoint mode: " +
3739
std::to_string(mode_msg.mode));

‎guidance/reference_filter_dp/src/lib/waypoint_utils.cpp‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,16 @@ Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& reference_in,
6464
reference_out(5) =
6565
current_state(5) + ssa(reference_in(5) - current_state(5));
6666
break;
67+
68+
case WaypointMode::XY_FORWARD_DIR: {
69+
const double dx = reference_in(0) - current_state(0);
70+
const double dy = reference_in(1) - current_state(1);
71+
reference_out(2) = current_state(2);
72+
reference_out(3) = 0.0;
73+
reference_out(4) = 0.0;
74+
reference_out(5) = ssa(std::atan2(dy, dx));
75+
break;
76+
}
6777
}
6878

6979
return reference_out;
@@ -93,6 +103,8 @@ bool has_converged(const Eigen::Vector6d& measured_pose,
93103
return std::sqrt(ep.squaredNorm() + ea(2) * ea(2));
94104
case WaypointMode::XY_AND_YAW:
95105
return std::sqrt(ep.head<2>().squaredNorm() + ea(2) * ea(2));
106+
case WaypointMode::XY_FORWARD_DIR:
107+
return ep.head<2>().norm();
96108
case WaypointMode::FULL_POSE:
97109
default:
98110
return std::sqrt(ep.squaredNorm() + ea.squaredNorm());

utility_scripts/launch_drone_vehicle.sh renamed to utility_scripts/launch_drone_real.sh

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -70,16 +70,16 @@ tmux kill-session -t "$SESSION" 2>/dev/null
7070
tmux new-session -d -s "$SESSION" -n "control"
7171

7272
PANE_C1=$(tmux list-panes -t "$SESSION:control" -F '#{pane_id}')
73-
tmux send-keys -t "$PANE_C1" "source ~/ros2_ws/install/setup.bash && ros2 launch auv_setup thruster.launch.py" Enter
73+
tmux send-keys -t "$PANE_C1" "source install/setup.bash && ros2 launch auv_setup thruster.launch.py" Enter
7474

7575
PANE_C2=$(tmux split-window -h -t "$PANE_C1" -P -F '#{pane_id}')
76-
tmux send-keys -t "$PANE_C2" "source ~/ros2_ws/install/setup.bash && ros2 launch operation_mode_manager operation_mode_manager.launch.py" Enter
76+
tmux send-keys -t "$PANE_C2" "source install/setup.bash && ros2 launch operation_mode_manager operation_mode_manager.launch.py" Enter
7777

7878
PANE_C3=$(tmux split-window -v -t "$PANE_C1" -P -F '#{pane_id}')
79-
tmux send-keys -t "$PANE_C3" "source ~/ros2_ws/install/setup.bash && ros2 launch $DP_LAUNCH" Enter
79+
tmux send-keys -t "$PANE_C3" "source install/setup.bash && ros2 launch $DP_LAUNCH" Enter
8080

8181
PANE_C4=$(tmux split-window -v -t "$PANE_C2" -P -F '#{pane_id}')
82-
tmux send-keys -t "$PANE_C4" "source ~/ros2_ws/install/setup.bash" Enter
82+
tmux send-keys -t "$PANE_C4" "source install/setup.bash" Enter
8383

8484
tmux select-layout -t "$SESSION:control" tiled
8585

@@ -89,16 +89,16 @@ tmux select-layout -t "$SESSION:control" tiled
8989
tmux new-window -t "$SESSION" -n "perception"
9090

9191
PANE_P1=$(tmux list-panes -t "$SESSION:perception" -F '#{pane_id}')
92-
tmux send-keys -t "$PANE_P1" "source ~/ros2_ws/install/setup.bash && ros2 launch auv_setup nucleus_odom_transformer.launch.py" Enter
92+
tmux send-keys -t "$PANE_P1" "source install/setup.bash && ros2 launch auv_setup nucleus_odom_transformer.launch.py" Enter
9393

9494
PANE_P2=$(tmux split-window -h -t "$PANE_P1" -P -F '#{pane_id}')
95-
tmux send-keys -t "$PANE_P2" "source ~/ros2_ws/install/setup.bash" Enter
95+
tmux send-keys -t "$PANE_P2" "source install/setup.bash" Enter
9696

9797
PANE_P3=$(tmux split-window -v -t "$PANE_P1" -P -F '#{pane_id}')
98-
tmux send-keys -t "$PANE_P3" "source ~/ros2_ws/install/setup.bash" Enter
98+
tmux send-keys -t "$PANE_P3" "source install/setup.bash" Enter
9999

100100
PANE_P4=$(tmux split-window -v -t "$PANE_P2" -P -F '#{pane_id}')
101-
tmux send-keys -t "$PANE_P4" "source ~/ros2_ws/install/setup.bash" Enter
101+
tmux send-keys -t "$PANE_P4" "source install/setup.bash" Enter
102102

103103
tmux select-layout -t "$SESSION:perception" tiled
104104

@@ -108,16 +108,16 @@ tmux select-layout -t "$SESSION:perception" tiled
108108
tmux new-window -t "$SESSION" -n "misc"
109109

110110
PANE_M1=$(tmux list-panes -t "$SESSION:misc" -F '#{pane_id}')
111-
tmux send-keys -t "$PANE_M1" "source ~/ros2_ws/install/setup.bash" Enter
111+
tmux send-keys -t "$PANE_M1" "source install/setup.bash" Enter
112112

113113
PANE_M2=$(tmux split-window -h -t "$PANE_M1" -P -F '#{pane_id}')
114-
tmux send-keys -t "$PANE_M2" "source ~/ros2_ws/install/setup.bash" Enter
114+
tmux send-keys -t "$PANE_M2" "source install/setup.bash" Enter
115115

116116
PANE_M3=$(tmux split-window -v -t "$PANE_M1" -P -F '#{pane_id}')
117-
tmux send-keys -t "$PANE_M3" "source ~/ros2_ws/install/setup.bash" Enter
117+
tmux send-keys -t "$PANE_M3" "source install/setup.bash" Enter
118118

119119
PANE_M4=$(tmux split-window -v -t "$PANE_M2" -P -F '#{pane_id}')
120-
tmux send-keys -t "$PANE_M4" "source ~/ros2_ws/install/setup.bash" Enter
120+
tmux send-keys -t "$PANE_M4" "source install/setup.bash" Enter
121121

122122
tmux select-layout -t "$SESSION:misc" tiled
123123

‎utility_scripts/launch_drone_sim.sh‎

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
#!/bin/bash
2+
# Launch drone simulation stack in a tmux session
3+
# Usage: ./launch_drone_sim.sh [--scenario <name>]
4+
# --scenario Stonefish scenario to load (default: default)
5+
# GPU scenarios: default, docking, pipeline, structure,
6+
# orca_demo, freya_demo, orca_freya_demo, tacc
7+
# No-GPU scenarios: nautilus_no_gpu, orca_no_gpu, freya_no_gpu
8+
9+
usage() {
10+
cat <<EOF
11+
Usage: $(basename "$0") [OPTIONS]
12+
13+
Options:
14+
--scenario <name> Stonefish scenario to load (default: default)
15+
GPU: default, docking, pipeline, structure,
16+
orca_demo, freya_demo, orca_freya_demo, tacc
17+
No-GPU: nautilus_no_gpu, orca_no_gpu, freya_no_gpu
18+
-h, --help Show this help message
19+
EOF
20+
}
21+
22+
SCENARIO="default"
23+
while [[ $# -gt 0 ]]; do
24+
case "$1" in
25+
--scenario) SCENARIO="$2"; shift 2 ;;
26+
-h|--help) usage; exit 0 ;;
27+
*) echo "Unknown argument: $1"; usage; exit 1 ;;
28+
esac
29+
done
30+
31+
SESSION="drone_launch"
32+
S="source install/setup.bash"
33+
34+
# Kill existing session if it exists
35+
tmux kill-session -t "$SESSION" 2>/dev/null
36+
37+
# Launch Foxglove Studio only if not already running
38+
if ! pgrep -f foxglove-studio &>/dev/null; then
39+
foxglove-studio &>/dev/null &
40+
fi
41+
42+
# =============================================
43+
# Window 1: sim (4 equal panes)
44+
# =============================================
45+
tmux new-session -d -s "$SESSION" -n "sim"
46+
47+
PANE_SIM=$(tmux list-panes -t "$SESSION:sim" -F '#{pane_id}')
48+
tmux send-keys -t "$PANE_SIM" "clear && $S && ros2 launch stonefish_sim vortex_sim_launch.py keyboard_joy:=true scenario:=$SCENARIO" Enter
49+
50+
PANE_P2=$(tmux split-window -h -t "$PANE_SIM" -P -F '#{pane_id}')
51+
tmux send-keys -t "$PANE_P2" "clear && $S && ros2 launch auv_setup dp_quat.launch.py" Enter
52+
53+
PANE_P3=$(tmux split-window -v -t "$PANE_SIM" -P -F '#{pane_id}')
54+
tmux send-keys -t "$PANE_P3" "clear && $S" Enter
55+
56+
PANE_P4=$(tmux split-window -v -t "$PANE_P2" -P -F '#{pane_id}')
57+
tmux send-keys -t "$PANE_P4" "clear && $S" Enter
58+
59+
tmux select-layout -t "$SESSION:sim" tiled
60+
61+
# =============================================
62+
# Window 2: tools (2 panes)
63+
# =============================================
64+
tmux new-window -t "$SESSION" -n "tools"
65+
66+
PANE_FOX=$(tmux list-panes -t "$SESSION:tools" -F '#{pane_id}')
67+
tmux send-keys -t "$PANE_FOX" "clear && $S && ros2 launch foxglove_bridge foxglove_bridge_launch.xml" Enter
68+
69+
PANE_MSG=$(tmux split-window -v -t "$PANE_FOX" -P -F '#{pane_id}')
70+
tmux send-keys -t "$PANE_MSG" "clear && $S && ros2 launch vortex_utility_nodes message_publisher.launch.py" Enter
71+
72+
# =============================================
73+
# Focus first window and attach
74+
# =============================================
75+
tmux select-window -t "$SESSION:sim"
76+
tmux attach-session -t "$SESSION"

‎utility_scripts/launch_drone_topside.sh‎

Lines changed: 0 additions & 57 deletions
This file was deleted.

0 commit comments

Comments
 (0)