diff --git a/bitbots_team_communication/scripts/show_team_comm.py b/bitbots_team_communication/scripts/show_team_comm.py index 80c96c7d4..14e137f6b 100755 --- a/bitbots_team_communication/scripts/show_team_comm.py +++ b/bitbots_team_communication/scripts/show_team_comm.py @@ -105,7 +105,7 @@ def generate_string(self, data: TeamData): lines.append(f" y: {round(data.ball_absolute.pose.position.y, 3):<4}") lines.append(f" x_cov: {round(data.ball_absolute.covariance[0], 3):<4}") lines.append(f" y_cov: {round(data.ball_absolute.covariance[7], 3):<4}") - lines.append(f"Obstacles found: {len(data.obstacles.obstacles)}") + lines.append(f"Robots found: {len(data.robots.robots)}") lines.append("Strategy") lines.append(f" Role: {self.roles[data.strategy.role]:<9}") lines.append(f" Action: {self.actions[data.strategy.action]:<15}") diff --git a/bitbots_team_communication/scripts/test_team_comm.py b/bitbots_team_communication/scripts/test_team_comm.py index 53d37c014..d392b3309 100755 --- a/bitbots_team_communication/scripts/test_team_comm.py +++ b/bitbots_team_communication/scripts/test_team_comm.py @@ -7,6 +7,7 @@ import rclpy from game_controller_hl_interfaces.msg import GameState from geometry_msgs.msg import Point, Pose, PoseWithCovariance, PoseWithCovarianceStamped, Quaternion, TransformStamped +from rclpy.qos import QoSDurabilityPolicy, QoSProfile from soccer_vision_3d_msgs.msg import Robot, RobotArray from soccer_vision_attribute_msgs.msg import Robot as RobotAttributes from tf2_ros import StaticTransformBroadcaster @@ -51,7 +52,9 @@ def base_footprint_transform(): node = rclpy.create_node("test_team_comm") tf_static_broadcaster = StaticTransformBroadcaster(node) - gamestate_pub = node.create_publisher(GameState, "gamestate", 1) + gamestate_pub = node.create_publisher( + GameState, "gamestate", qos_profile=QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + ) strategy_pub = node.create_publisher(Strategy, "strategy", 1) ball_pub = node.create_publisher(PoseWithCovarianceStamped, "ball_position_relative_filtered", 1) position_pub = node.create_publisher(PoseWithCovarianceStamped, "pose_with_covariance", 1) diff --git a/bitbots_team_communication/scripts/tmux_testing_setup.zsh b/bitbots_team_communication/scripts/tmux_testing_setup.zsh index dd6cf059e..877e939c8 100755 --- a/bitbots_team_communication/scripts/tmux_testing_setup.zsh +++ b/bitbots_team_communication/scripts/tmux_testing_setup.zsh @@ -10,18 +10,16 @@ run_tmux_session() { tmux new-session -d -s "$session" # window/pane setup - tmux rename-window -t "$session:1" "Launch" - tmux new-window -t "$session:2" -n "Test" + tmux rename-window -t "$session:0" "Launch" + tmux new-window -t "$session:1" -n "Test" tmux split-window -h - tmux new-window -t "$session:3" -n "Base" # run required launch files in order - tmux send-keys -t "$session:Base" "rl bitbots_utils base.launch" Enter - tmux send-keys -t "$session:Launch" "rl bitbots_team_communication team_comm.launch" Enter + tmux send-keys -t "$session:Launch" "rl bitbots_team_communication team_comm_standalone.launch" Enter # start test publisher/subscriber - tmux send-keys -t "$session:Test.top" "rr bitbots_team_communication test_team_comm.py" Enter - tmux send-keys -t "$session:Test.bottom" "rr bitbots_team_communication show_team_comm.py" Enter + tmux send-keys -t "$session:Test.left" "rr bitbots_team_communication test_team_comm.py" Enter + tmux send-keys -t "$session:Test.right" "rr bitbots_team_communication show_team_comm.py" Enter fi tmux attach-session -t "$session:Test" @@ -35,4 +33,5 @@ trap kill_session INT cd "$COLCON_WS" colcon build --symlink-install --packages-up-to "$pkg" +source install/setup.zsh run_tmux_session