diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py
index a929d40f9..a621bf0a3 100644
--- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py
+++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py
@@ -26,7 +26,6 @@ class PathfindingCapsule(AbstractBlackboardCapsule):
def __init__(self, node, blackboard):
super().__init__(node, blackboard)
- self.map_frame: str = self._node.get_parameter("map_frame").value
self.position_threshold: str = self._node.get_parameter("pathfinding_position_threshold").value
self.orientation_threshold: str = self._node.get_parameter("pathfinding_orientation_threshold").value
@@ -175,16 +174,21 @@ def get_ball_goal(self, target: BallGoalType, distance: float, side_offset: floa
elif BallGoalType.CLOSE == target:
ball_u, ball_v = self._blackboard.world_model.get_ball_position_uv()
angle = math.atan2(ball_v, ball_u)
- ball_point = (ball_u, ball_v, angle, self._blackboard.world_model.base_footprint_frame)
+ goal_u = ball_u - math.cos(angle) * distance
+ goal_v = ball_v - math.sin(angle) * distance + side_offset
+ ball_point = (goal_u, goal_v, angle, self._blackboard.world_model.base_footprint_frame)
else:
self._node.get_logger().error(f"Target {target} for go_to_ball action not implemented.")
return
+ # Create the goal pose message
pose_msg = PoseStamped()
- pose_msg.header.stamp = self._node.get_clock().now().to_msg()
pose_msg.header.frame_id = ball_point[3]
pose_msg.pose.position = Point(x=ball_point[0], y=ball_point[1], z=0.0)
pose_msg.pose.orientation = quat_from_yaw(ball_point[2])
+ # Convert the goal to the map frame
+ pose_msg = self._blackboard.tf_buffer.transform(pose_msg, self._blackboard.map_frame)
+
return pose_msg
diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py
index 4b1bdc710..e22dafd38 100644
--- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py
+++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py
@@ -189,6 +189,19 @@ def is_not_goalie(team_data: TeamData) -> bool:
# Count valid team data infos (aka robots with valid team data)
return sum(map(self.is_valid, team_data_infos))
+ def get_is_goalie_active(self) -> bool:
+ def is_a_goalie(team_data: TeamData) -> bool:
+ return team_data.strategy.role == Strategy.ROLE_GOALIE
+
+ # Get the team data infos for all robots (ignoring the robot id/name)
+ team_data_infos = self.team_data.values()
+
+ # Remove none goalie Data
+ team_data_infos = filter(is_a_goalie, team_data_infos)
+
+ # Count valid team data infos (aka robots with valid team data)
+ return sum(map(self.is_valid, team_data_infos)) == 1
+
def get_own_time_to_ball(self) -> float:
return self.own_time_to_ball
diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py
index 86a04b5e1..ebb24ec83 100644
--- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py
+++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py
@@ -22,9 +22,11 @@ def __init__(self, blackboard, dsd, parameters):
self.blocking = parameters.get("blocking", True)
self.distance = parameters.get("distance", self.blackboard.config["ball_approach_dist"])
+ # Offset so we kick the ball with one foot instead of the center between the feet
+ self.side_offset = parameters.get("side_offset", 0.08)
def perform(self, reevaluate=False):
- pose_msg = self.blackboard.pathfinding.get_ball_goal(self.target, self.distance, 0.08)
+ pose_msg = self.blackboard.pathfinding.get_ball_goal(self.target, self.distance, self.side_offset)
self.blackboard.pathfinding.publish(pose_msg)
approach_marker = Marker()
diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/goalie_active.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/goalie_active.py
new file mode 100644
index 000000000..c0783318c
--- /dev/null
+++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/goalie_active.py
@@ -0,0 +1,22 @@
+from bitbots_blackboard.body_blackboard import BodyBlackboard
+from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement
+
+
+class GoalieActive(AbstractDecisionElement):
+ """
+ Decides whether the goalie is on field or not
+ """
+
+ blackboard: BodyBlackboard
+
+ def __init__(self, blackboard, dsd, parameters):
+ super().__init__(blackboard, dsd, parameters)
+
+ def perform(self, reevaluate=False):
+ if self.blackboard.team_data.get_is_goalie_active():
+ return "YES"
+ else:
+ return "NO"
+
+ def get_reevaluate(self):
+ return True
diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd
new file mode 100644
index 000000000..f9b0a3048
--- /dev/null
+++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd
@@ -0,0 +1,28 @@
+#SearchBall
+@ChangeAction + action:searching, @LookAtFieldFeatures, @WalkInPlace + duration:3, @Turn
+
+#PerformKickLeft
+@ChangeAction + action:kicking, @LookAtFront, @WalkKick + foot:left + r:false, @WalkInPlace + duration:1 + r:false, @ForgetBall + r:false, @WalkInPlace + duration:1 + r:false, @LookAtFieldFeatures + r:false
+
+#PerformKickRight
+@ChangeAction + action:kicking, @LookAtFront, @WalkKick + foot:right + r:false, @WalkInPlace + duration:1 + r:false, @ForgetBall + r:false, @WalkInPlace + duration:1 + r:false, @LookAtFieldFeatures + r:false
+
+#KickWithAvoidance
+$AvoidBall
+ NO --> $BallClose + distance:%ball_reapproach_dist + angle:%ball_reapproach_angle
+ YES --> $BallKickArea
+ NEAR --> $FootSelection
+ LEFT --> #PerformKickLeft
+ RIGHT --> #PerformKickRight
+ FAR --> @ChangeAction + action:going_to_ball, @LookAtFront, @GoToBall + target:close
+ NO --> @ChangeAction + action:going_to_ball + r:false, @LookAtFieldFeatures + r:false, @AvoidBallActive + r:false, @GoToBall + target:close + blocking:false + distance:%ball_far_approach_dist
+ YES --> $ReachedPathPlanningGoalPosition + threshold:%ball_far_approach_position_thresh
+ YES --> @AvoidBallInactive
+ NO --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:close + distance:%ball_far_approach_dist
+
+-->BodyBehavior
+$DoOnce
+ NOT_DONE --> @Stand + duration:15
+ DONE --> $BallSeen
+ YES --> #KickWithAvoidance
+ NO --> #SearchBall
diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd
index c71286d92..f120e145d 100644
--- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd
+++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd
@@ -71,7 +71,9 @@ $GoalieHandlingBall
NO --> #KickWithAvoidance
#DefensePositioning
-@LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToDefensePosition
+$GoalieActive
+ YES --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToDefensePosition
+ NO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToBlockPosition
#SupporterRole
$PassStarted
diff --git a/bitbots_misc/bitbots_bringup/launch/demo.launch b/bitbots_misc/bitbots_bringup/launch/demo.launch
new file mode 100644
index 000000000..6076d5490
--- /dev/null
+++ b/bitbots_misc/bitbots_bringup/launch/demo.launch
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/bitbots_misc/system_monitor/config/config.yaml b/bitbots_misc/system_monitor/config/config.yaml
index 0e2d5d1e4..4ee829080 100644
--- a/bitbots_misc/system_monitor/config/config.yaml
+++ b/bitbots_misc/system_monitor/config/config.yaml
@@ -5,11 +5,13 @@ system_monitor:
# These settings are quick_switches to completely disable certain parts of statistic collection
do_cpu: true
+ do_gpu: true
do_memory: true
do_network: false
# these are the threshold values at which we start going into a warn state
cpu_load_percentage: 80.0
+ gpu_load_percentage: 95.0
memory_load_percentage: 80.0
network_rate_received_errors: 10.0
network_rate_send_errors: 10.0
diff --git a/bitbots_misc/system_monitor/system_monitor/cpus.py b/bitbots_misc/system_monitor/system_monitor/cpus.py
index 391a3a2b0..886f31af9 100644
--- a/bitbots_misc/system_monitor/system_monitor/cpus.py
+++ b/bitbots_misc/system_monitor/system_monitor/cpus.py
@@ -36,7 +36,8 @@ def collect_all():
def _get_cpu_stats():
"""
read and parse /proc/stat
- :returns timings which contains accumulative busy and total cpu time
+
+ :returns: timings which contains accumulative busy and total cpu time
"""
timings = {}
with open("/proc/stat") as file_obj:
diff --git a/bitbots_misc/system_monitor/system_monitor/gpu.py b/bitbots_misc/system_monitor/system_monitor/gpu.py
new file mode 100644
index 000000000..2f440c031
--- /dev/null
+++ b/bitbots_misc/system_monitor/system_monitor/gpu.py
@@ -0,0 +1,19 @@
+import pyamdgpuinfo
+
+
+def collect_all():
+ """
+ use pyamdgpuinfo to get gpu metrics
+
+ :return: (load, vram_used, vram_total, temperature)
+ """
+ if pyamdgpuinfo.detect_gpus() == 0:
+ return (0, 0, 0, 0)
+
+ gpu = pyamdgpuinfo.get_gpu(0)
+ load = gpu.query_load()
+ vram_total = gpu.memory_info["vram_size"]
+ vram_used = gpu.query_vram_usage()
+ temperature = gpu.query_temperature()
+
+ return (load, vram_used, vram_total, temperature)
diff --git a/bitbots_misc/system_monitor/system_monitor/monitor.py b/bitbots_misc/system_monitor/system_monitor/monitor.py
index c1ee224fb..56304400d 100755
--- a/bitbots_misc/system_monitor/system_monitor/monitor.py
+++ b/bitbots_misc/system_monitor/system_monitor/monitor.py
@@ -7,7 +7,7 @@
from rclpy.node import Node
from bitbots_msgs.msg import Workload as WorkloadMsg
-from system_monitor import cpus, memory, network_interfaces
+from system_monitor import cpus, gpu, memory, network_interfaces
def main():
@@ -23,24 +23,31 @@ def main():
# start all names with "SYSTEM" for diagnostic analyzer
diag_cpu.name = "SYSTEMCPU"
diag_cpu.hardware_id = "CPU"
+ diag_gpu = DiagnosticStatus()
+ diag_gpu.name = "SYSTEMGPU"
+ diag_gpu.hardware_id = "GPU"
diag_mem = DiagnosticStatus()
diag_mem.name = "SYSTEMMemory"
diag_mem.hardware_id = "Memory"
node.declare_parameter("update_frequency", 10.0)
- node.declare_parameter("do_memory", True)
node.declare_parameter("do_cpu", True)
+ node.declare_parameter("do_gpu", True)
+ node.declare_parameter("do_memory", True)
node.declare_parameter("do_network", True)
node.declare_parameter("cpu_load_percentage", 80.0)
+ node.declare_parameter("gpu_load_percentage", 80.0)
node.declare_parameter("memory_load_percentage", 80.0)
node.declare_parameter("network_rate_received_errors", 10.0)
node.declare_parameter("network_rate_send_errors", 10.0)
rate = node.get_parameter("update_frequency").get_parameter_value().double_value
- do_memory = node.get_parameter("do_memory").get_parameter_value().bool_value
do_cpu = node.get_parameter("do_cpu").get_parameter_value().bool_value
+ do_gpu = node.get_parameter("do_gpu").get_parameter_value().bool_value
+ do_memory = node.get_parameter("do_memory").get_parameter_value().bool_value
do_network = node.get_parameter("do_network").get_parameter_value().bool_value
cpu_load_percentage = node.get_parameter("cpu_load_percentage").get_parameter_value().double_value
+ gpu_load_percentage = node.get_parameter("gpu_load_percentage").get_parameter_value().double_value
memory_load_percentage = node.get_parameter("memory_load_percentage").get_parameter_value().double_value
network_rate_received_errors = node.get_parameter("network_rate_received_errors").get_parameter_value().double_value
network_rate_send_errors = node.get_parameter("network_rate_send_errors").get_parameter_value().double_value
@@ -48,6 +55,7 @@ def main():
while rclpy.ok():
last_send_time = time.time()
running_processes, cpu_usages, overall_usage_percentage = cpus.collect_all() if do_cpu else (-1, [], 0)
+ gpu_load, gpu_vram_used, gpu_vram_total, gpu_temperature = gpu.collect_all() if do_gpu else (0, 0, 0, 0)
memory_available, memory_used, memory_total = memory.collect_all() if do_memory else (-1, -1, -1)
interfaces = network_interfaces.collect_all(node.get_clock()) if do_network else []
@@ -56,6 +64,10 @@ def main():
cpus=cpu_usages,
running_processes=running_processes,
cpu_usage_overall=overall_usage_percentage,
+ gpu_load=gpu_load,
+ gpu_vram_used=gpu_vram_used,
+ gpu_vram_total=gpu_vram_total,
+ gpu_temperature=gpu_temperature,
memory_available=memory_available,
memory_used=memory_used,
memory_total=memory_total,
@@ -73,6 +85,14 @@ def main():
diag_cpu.level = DiagnosticStatus.OK
diag_array.status.append(diag_cpu)
+ gpu_usage = gpu_load * 100
+ diag_gpu.message = str(gpu_usage) + "%"
+ if gpu_usage >= gpu_load_percentage:
+ diag_gpu.level = DiagnosticStatus.WARN
+ else:
+ diag_gpu.level = DiagnosticStatus.OK
+ diag_array.status.append(diag_gpu)
+
memory_usage = round((memory_used / memory_total) * 100, 2)
diag_mem.message = str(memory_usage) + "%"
if memory_usage >= memory_load_percentage:
diff --git a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py
index 32a83bb96..d89806360 100755
--- a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py
+++ b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py
@@ -2,17 +2,18 @@
import math
import os
import sys
+from typing import Literal
import rclpy
from ament_index_python import get_package_share_directory
-from PyQt5.QtCore import Qt
+from PyQt5.QtCore import QLocale, Qt
from PyQt5.QtGui import QKeySequence
from PyQt5.QtWidgets import (
QAbstractItemView,
+ QDoubleSpinBox,
QFileDialog,
QGroupBox,
QLabel,
- QLineEdit,
QListWidgetItem,
QMainWindow,
QMessageBox,
@@ -84,8 +85,6 @@ def __init__(self, context):
# Initialize the working values
self._working_angles: dict[str, float] = {}
- self.update_time = 0.1 # TODO what is this for?
-
# QT directory for saving files
self._save_directory = None
@@ -200,8 +199,8 @@ def q_joint_state_update(self, joint_states: JointState) -> None:
# Check if the we are currently positioning the motor and want to store the value
if motor_active and motor_torqueless:
# Update textfield
- self._motor_controller_text_fields[motor_name].setText(
- str(round(math.degrees(joint_states.position[joint_states.name.index(motor_name)]), 2))
+ self._motor_controller_text_fields[motor_name].setValue(
+ round(math.degrees(joint_states.position[joint_states.name.index(motor_name)]), 2)
)
# Update working values
self._working_angles[motor_name] = joint_states.position[joint_states.name.index(motor_name)]
@@ -224,9 +223,12 @@ def create_motor_controller(self) -> None:
layout.addWidget(label)
# Add a textfield to display the exact value of the motor
- textfield = QLineEdit()
- textfield.setText("0.0")
- textfield.textEdited.connect(self.textfield_update)
+ textfield = QDoubleSpinBox()
+ textfield.setLocale(QLocale("C"))
+ textfield.setMaximum(180.0)
+ textfield.setMinimum(-180.0)
+ textfield.setValue(0.0)
+ textfield.valueChanged.connect(self.textfield_update)
layout.addWidget(textfield)
self._motor_controller_text_fields[motor_name] = textfield
@@ -299,8 +301,8 @@ def connect_gui_callbacks(self) -> None:
self._widget.actionAnimation_until_Frame.triggered.connect(self.play_until)
self._widget.actionDuplicate_Frame.triggered.connect(self.duplicate)
self._widget.actionDelete_Frame.triggered.connect(self.delete)
- self._widget.actionLeft.triggered.connect(lambda: self.mirror_frame("L"))
- self._widget.actionRight.triggered.connect(lambda: self.mirror_frame("R"))
+ self._widget.actionLeft.triggered.connect(lambda: self.mirror_frame("R"))
+ self._widget.actionRight.triggered.connect(lambda: self.mirror_frame("L"))
self._widget.actionInvert.triggered.connect(self.invert_frame)
self._widget.actionUndo.triggered.connect(self.undo)
self._widget.actionRedo.triggered.connect(self.redo)
@@ -520,6 +522,10 @@ def delete(self):
self._node.get_logger().error(str(e))
return
assert self._recorder.get_keyframe(frame) is not None, "Selected frame not found in list of keyframes"
+ # Check if only one frame is remaining
+ if len(self._widget.frameList) == 1:
+ QMessageBox.warning(self._widget, "Warning", "Cannot delete last remaining frame")
+ return
self._recorder.delete(frame)
self._widget.statusBar.showMessage(f"Deleted frame {frame}")
self.update_frames()
@@ -580,11 +586,32 @@ def redo(self):
self._widget.statusBar.showMessage(status)
self.update_frames()
- def mirror_frame(self, direction):
+ def mirror_frame(self, source: Literal["L", "R"]) -> None:
"""
Copies all motor values from one side of the robot to the other. Inverts values, if necessary
"""
- raise NotImplementedError("This function is not implemented yet")
+ # Get direction to mirror to
+ mirrored_source = {"R": "L", "L": "R"}[source]
+
+ # Go through all active motors
+ for motor_name, angle in self._working_angles.items():
+ # Set mirrored angles
+ if motor_name.startswith(source):
+ mirrored_motor_name = mirrored_source + motor_name[1:]
+ # Make -0.0 to 0.0
+ mirrored_angle = -angle if angle != 0 else 0.0
+ self._working_angles[mirrored_motor_name] = mirrored_angle
+
+ # Update the UI
+ for motor_name, angle in self._working_angles.items():
+ # Block signals
+ self._motor_controller_text_fields[motor_name].blockSignals(True)
+ # Set values
+ self._motor_controller_text_fields[motor_name].setValue(round(math.degrees(angle), 2))
+ # Enable signals again
+ self._motor_controller_text_fields[motor_name].blockSignals(False)
+
+ self._widget.statusBar.showMessage("Mirrored frame")
def invert_frame(self):
"""
@@ -613,7 +640,12 @@ def invert_frame(self):
# Update the UI
for motor_name, angle in self._working_angles.items():
- self._motor_controller_text_fields[motor_name].setText(str(round(math.degrees(angle), 2)))
+ # Block signals
+ self._motor_controller_text_fields[motor_name].blockSignals(True)
+ # Set values
+ self._motor_controller_text_fields[motor_name].setValue(round(math.degrees(angle), 2))
+ # Enable signals again
+ self._motor_controller_text_fields[motor_name].blockSignals(False)
self._widget.statusBar.showMessage("Inverted frame")
@@ -627,6 +659,26 @@ def frame_select(self):
selected_frame_name = self._widget.frameList.currentItem().text()
selected_frame = self._recorder.get_keyframe(selected_frame_name)
if selected_frame is not None:
+ # check if unrecorded changes would be lost
+ unrecorded_changes = []
+ current_keyframe_goals = self._recorder.get_keyframe(self._selected_frame)["goals"]
+
+ for motor_name, text_field in self._motor_controller_text_fields.items():
+ # Get the angle from the textfield
+ angle = text_field.value()
+ # compare with angles in current keyframe
+ if not current_keyframe_goals[motor_name] == math.radians(angle):
+ unrecorded_changes.append(motor_name)
+
+ # warn user about unrecorded changes
+ if unrecorded_changes:
+ message = (
+ f"""This will discard your unrecorded changes for {", ".join(unrecorded_changes)}. Continue?"""
+ )
+ sure = QMessageBox.question(self._widget, "Sure?", message, QMessageBox.Yes | QMessageBox.No)
+ # Cancel the open if the user does not want to discard the changes
+ if sure == QMessageBox.No:
+ return
# Update state so we have a new selected frame
self._selected_frame = selected_frame_name
@@ -657,12 +709,13 @@ def react_to_frame_change(self):
# Update the motor angle controls (value and active state)
if active:
- self._motor_controller_text_fields[motor_name].setText(
- str(round(math.degrees(selected_frame["goals"][motor_name]), 2))
+ self._motor_controller_text_fields[motor_name].setValue(
+ round(math.degrees(selected_frame["goals"][motor_name]), 2)
)
+
self._working_angles[motor_name] = selected_frame["goals"][motor_name]
else:
- self._motor_controller_text_fields[motor_name].setText("0.0")
+ self._motor_controller_text_fields[motor_name].setValue(0.0)
# Update the duration and pause
self._widget.spinBoxDuration.setValue(selected_frame["duration"])
@@ -675,23 +728,12 @@ def textfield_update(self):
If the textfield is updated, update working values
"""
for motor_name, text_field in self._motor_controller_text_fields.items():
- try:
- # Get the angle from the textfield
- angle = float(text_field.text())
- except ValueError:
- # Display QMessageBox stating that the value is not a number
- QMessageBox.warning(
- self._widget,
- "Warning",
- f"Please enter a valid number.\n '{text_field.text()}' is not a valid number.",
- )
- continue
- # Clip the angle to the maximum and minimum, we do this in degrees,
- # because we do not want introduce rounding errors in the textfield
- angle = round(max(-180.0, min(angle, 180.0)), 2)
+ # Get the angle from the textfield
+ angle = text_field.value()
+ angle = round(angle, 2)
# Set the angle in the textfield
- if float(text_field.text()) != float(angle):
- text_field.setText(str(angle))
+ if text_field.value() != angle:
+ text_field.setValue(angle)
# Set the angle in the working values if the motor is active
if self._motor_switcher_active_checkbox[motor_name].checkState(0) == Qt.CheckState.Checked:
self._working_angles[motor_name] = math.radians(angle)
diff --git a/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml b/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml
index 4c38e7350..843c69281 100644
--- a/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml
+++ b/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml
@@ -75,23 +75,23 @@ walking:
phase_reset:
min_phase: 0.90
foot_pressure:
- active: False
+ active: True
ground_min_pressure: 1.5
effort:
active: False
joint_min_effort: 30.0
imu:
- active: True
+ active: False
y_acceleration_threshold: 1.4
trunk_pid:
pitch:
- p: 0.0
+ p: 0.0035
i: 0.0
- d: 0.0
+ d: 0.004
i_clamp_min: 0.0
i_clamp_max: 0.0
- antiwindup: False
+ antiwindup: false
roll:
p: 0.0
i: 0.0
diff --git a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_utils.hpp b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_utils.hpp
index 96ffe4161..cb50c5b7a 100644
--- a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_utils.hpp
+++ b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_utils.hpp
@@ -13,7 +13,16 @@
namespace bitbots_quintic_walk {
-enum WalkState { PAUSED, WALKING, IDLE, START_MOVEMENT, STOP_MOVEMENT, START_STEP, STOP_STEP, KICK };
+enum WalkState {
+ IDLE = 0,
+ START_MOVEMENT = 1,
+ START_STEP = 2,
+ WALKING = 3,
+ PAUSED = 4,
+ KICK = 5,
+ STOP_STEP = 6,
+ STOP_MOVEMENT = 7
+};
struct WalkRequest {
std::vector linear_orders = {0, 0, 0};
@@ -106,4 +115,4 @@ inline void tf_pose_to_msg(tf2::Transform &tf_pose, geometry_msgs::msg::Pose &ms
} // namespace bitbots_quintic_walk
-#endif // BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_UTILS_H_
\ No newline at end of file
+#endif // BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_UTILS_H_
diff --git a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_visualizer.hpp b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_visualizer.hpp
index 31dfa798c..490296fab 100644
--- a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_visualizer.hpp
+++ b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_visualizer.hpp
@@ -10,6 +10,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -24,26 +25,46 @@ class WalkVisualizer : public bitbots_splines::AbstractVisualizer {
public:
explicit WalkVisualizer(rclcpp::Node::SharedPtr node, walking::Params::Node::Tf tf_config);
- void publishArrowMarker(std::string name_space, std::string frame, geometry_msgs::msg::Pose pose, float r, float g,
- float b, float a);
+ visualization_msgs::msg::Marker createArrowMarker(const std::string &name_space, const std::string &frame,
+ const geometry_msgs::msg::Pose &pose,
+ const std_msgs::msg::ColorRGBA &color);
- void publishEngineDebug(WalkResponse response);
- void publishIKDebug(WalkResponse response, moveit::core::RobotStatePtr current_state,
- bitbots_splines::JointGoals joint_goals);
- void publishWalkMarkers(WalkResponse response);
+ std::pair buildEngineDebugMsgs(
+ WalkResponse response);
+ std::pair buildIKDebugMsgs(
+ WalkResponse response, moveit::core::RobotStatePtr current_state, bitbots_splines::JointGoals joint_goals);
+ visualization_msgs::msg::MarkerArray buildWalkMarkers(WalkResponse response);
void init(moveit::core::RobotModelPtr kinematic_model);
+ void publishDebug(const WalkResponse ¤t_response, const moveit::core::RobotStatePtr ¤t_state,
+ const bitbots_splines::JointGoals &motor_goals);
+
+ std_msgs::msg::ColorRGBA colorFactory(double r, double g, double b) {
+ std_msgs::msg::ColorRGBA color;
+ color.r = r;
+ color.g = g;
+ color.b = b;
+ color.a = 1.0;
+ return color;
+ }
+
+ const std_msgs::msg::ColorRGBA BLACK = colorFactory(0.0, 0.0, 0.0);
+ const std_msgs::msg::ColorRGBA BLUE = colorFactory(0.0, 0.0, 1.0);
+ const std_msgs::msg::ColorRGBA GREEN = colorFactory(0.0, 1.0, 0.0);
+ const std_msgs::msg::ColorRGBA ORANGE = colorFactory(1.0, 0.5, 0.0);
+ const std_msgs::msg::ColorRGBA RED = colorFactory(1.0, 0.0, 0.0);
+ const std_msgs::msg::ColorRGBA WHITE = colorFactory(1.0, 1.0, 1.0);
+ const std_msgs::msg::ColorRGBA YELLOW = colorFactory(1.0, 1.0, 0.0);
+
private:
rclcpp::Node::SharedPtr node_;
walking::Params::Node::Tf tf_config_;
- int marker_id_ = 1;
-
rclcpp::Publisher::SharedPtr pub_debug_;
rclcpp::Publisher::SharedPtr pub_engine_debug_;
- rclcpp::Publisher::SharedPtr pub_debug_marker_;
+ rclcpp::Publisher::SharedPtr pub_debug_marker_;
moveit::core::RobotModelPtr kinematic_model_;
};
diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp
index 8f238f2ff..74e41eb37 100644
--- a/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp
+++ b/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp
@@ -172,11 +172,7 @@ void WalkNode::run() {
}
}
-void WalkNode::publish_debug() {
- visualizer_.publishIKDebug(current_stabilized_response_, current_state_, motor_goals_);
- visualizer_.publishWalkMarkers(current_stabilized_response_);
- visualizer_.publishEngineDebug(current_response_);
-}
+void WalkNode::publish_debug() { visualizer_.publishDebug(current_stabilized_response_, current_state_, motor_goals_); }
bitbots_msgs::msg::JointCommand WalkNode::step(double dt) {
WalkRequest request(current_request_);
diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_visualizer.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_visualizer.cpp
index 273bb9a1a..0fc9e7823 100644
--- a/bitbots_motion/bitbots_quintic_walk/src/walk_visualizer.cpp
+++ b/bitbots_motion/bitbots_quintic_walk/src/walk_visualizer.cpp
@@ -6,191 +6,160 @@ WalkVisualizer::WalkVisualizer(rclcpp::Node::SharedPtr node, walking::Params::No
tf_config_(tf_config),
pub_debug_(node_->create_publisher("walk_debug", 1)),
pub_engine_debug_(node_->create_publisher("walk_engine_debug", 1)),
- pub_debug_marker_(node_->create_publisher("walk_debug_marker", 1)) {}
+ pub_debug_marker_(node_->create_publisher("walk_debug_marker", 1)) {}
void WalkVisualizer::init(moveit::core::RobotModelPtr kinematic_model) { kinematic_model_ = kinematic_model; }
-void WalkVisualizer::publishEngineDebug(WalkResponse response) {
- // only do something if someone is listing
- if (pub_engine_debug_->get_subscription_count() == 0 && pub_debug_marker_->get_subscription_count() == 0) {
- return;
- }
+void WalkVisualizer::publishDebug(const WalkResponse ¤t_response,
+ const moveit::core::RobotStatePtr ¤t_state,
+ const bitbots_splines::JointGoals &motor_goals) {
+ visualization_msgs::msg::MarkerArray marker_array;
+
+ auto [engine_debug, engine_markers] = buildEngineDebugMsgs(current_response);
+ marker_array.markers.insert(marker_array.markers.end(), engine_markers.markers.begin(), engine_markers.markers.end());
+ pub_engine_debug_->publish(engine_debug);
+
+ auto [ik_debug, ik_markers] = buildIKDebugMsgs(current_response, current_state, motor_goals);
+ marker_array.markers.insert(marker_array.markers.end(), ik_markers.markers.begin(), ik_markers.markers.end());
+ pub_debug_->publish(ik_debug);
+
+ auto walk_markers = buildWalkMarkers(current_response);
+ marker_array.markers.insert(marker_array.markers.end(), walk_markers.markers.begin(), walk_markers.markers.end());
+
+ pub_debug_marker_->publish(marker_array);
+}
- /*
- This method publishes various debug / visualization information.
- */
+std::pair
+WalkVisualizer::buildEngineDebugMsgs(WalkResponse response) {
+ // Here we will convert the walk engine response to a various debug messages and RViz markers
+ // Initialize output containers
bitbots_quintic_walk::msg::WalkEngineDebug msg;
- bool is_left_support = response.is_left_support_foot;
- msg.is_left_support = is_left_support;
+ visualization_msgs::msg::MarkerArray marker_array;
+
+ // Copy some data into the debug message
+ msg.is_left_support = response.is_left_support_foot;
msg.is_double_support = response.is_double_support;
msg.header.stamp = node_->now();
-
- // define current support frame
+ msg.phase_time = response.phase;
+ msg.traj_time = response.traj_time;
+ // Copy walk engine state
+ static const std::unordered_map state_string_mapping = {
+ {WalkState::IDLE, "idle"},
+ {WalkState::START_MOVEMENT, "start_movement"},
+ {WalkState::START_STEP, "start_step"},
+ {WalkState::WALKING, "walking"},
+ {WalkState::PAUSED, "paused"},
+ {WalkState::KICK, "kick"},
+ {WalkState::STOP_STEP, "stop_step"},
+ {WalkState::STOP_MOVEMENT, "stop_movement"}};
+ msg.state.data = state_string_mapping.at(response.state);
+ msg.state_number = static_cast(response.state);
+
+ // Define current support foot frame
std::string current_support_frame;
- if (is_left_support) {
+ if (response.is_left_support_foot) {
current_support_frame = tf_config_.l_sole_frame;
} else {
current_support_frame = tf_config_.r_sole_frame;
}
- // define colors based on current support state
- float r, g, b, a;
- if (response.is_double_support) {
- r = 0;
- g = 0;
- b = 1;
- a = 1;
- } else if (response.is_left_support_foot) {
- r = 1;
- g = 0;
- b = 0;
- a = 1;
- } else {
- r = 1;
- g = 1;
- b = 0;
- a = 1;
- }
-
- // times
- msg.phase_time = response.phase;
- msg.traj_time = response.traj_time;
-
- if (response.state == WalkState::IDLE) {
- msg.state_number = 0;
- msg.state.data = "idle";
- } else if (response.state == WalkState::START_MOVEMENT) {
- msg.state_number = 1;
- msg.state.data = "start_movement";
- } else if (response.state == WalkState::START_STEP) {
- msg.state_number = 2;
- msg.state.data = "start_step";
- } else if (response.state == WalkState::WALKING) {
- msg.state_number = 3;
- msg.state.data = "walking";
- } else if (response.state == WalkState::PAUSED) {
- msg.state_number = 4;
- msg.state.data = "paused";
- } else if (response.state == WalkState::KICK) {
- msg.state_number = 5;
- msg.state.data = "kick";
- } else if (response.state == WalkState::STOP_STEP) {
- msg.state_number = 6;
- msg.state.data = "stop_step";
- } else if (response.state == WalkState::STOP_MOVEMENT) {
- msg.state_number = 7;
- msg.state.data = "stop_movement";
- }
-
- // footsteps
- double roll, pitch, yaw;
+ // Create placeholder floats
+ double _1, _2;
+ // Copy transform of the last footstep position (and orientation) to the debug message
msg.footstep_last.x = response.support_to_last.getOrigin()[0];
msg.footstep_last.y = response.support_to_last.getOrigin()[1];
- tf2::Matrix3x3(response.support_to_last.getRotation()).getRPY(roll, pitch, yaw);
- msg.footstep_last.z = yaw;
+ tf2::Matrix3x3(response.support_to_last.getRotation()).getRPY(_1, _2, msg.footstep_last.z);
+ // Copy transform of the next footstep position (and orientation) to the debug message
msg.footstep_next.x = response.support_to_next.getOrigin()[0];
msg.footstep_next.y = response.support_to_next.getOrigin()[1];
- tf2::Matrix3x3(response.support_to_next.getRotation()).getRPY(roll, pitch, yaw);
- msg.footstep_next.z = yaw;
-
- // engine output
- geometry_msgs::msg::Pose pose_msg;
- tf2::toMsg(response.support_foot_to_flying_foot, pose_msg);
- msg.fly_goal = pose_msg;
- publishArrowMarker("engine_fly_goal", current_support_frame, pose_msg, 0, 0, 1, a);
-
- tf2::Matrix3x3(response.support_foot_to_flying_foot.getRotation()).getRPY(roll, pitch, yaw);
- msg.fly_euler.x = roll;
- msg.fly_euler.y = pitch;
- msg.fly_euler.z = yaw;
-
- tf2::toMsg(response.support_foot_to_trunk, pose_msg);
- msg.trunk_goal = pose_msg;
- publishArrowMarker("engine_trunk_goal", current_support_frame, pose_msg, r, g, b, a);
-
- msg.trunk_goal_abs = pose_msg;
+ tf2::Matrix3x3(response.support_to_next.getRotation()).getRPY(_1, _2, msg.footstep_next.z);
+
+ // Copy cartesian coordinates of the currently flying foot relative to the support foot to the debug message
+ tf2::toMsg(response.support_foot_to_flying_foot, msg.fly_goal);
+ // Create an additional marker for the flying foot goal
+ marker_array.markers.push_back(createArrowMarker("engine_fly_goal", current_support_frame, msg.fly_goal, BLUE));
+ RCLCPP_INFO_ONCE(node_->get_logger(),
+ "Color for the Engine Debug marker, showing where the flying foot and trunk should be, is blue!");
+
+ // Copy the rotation of the flying foot relative to the support foot to the debug message
+ tf2::Matrix3x3(response.support_foot_to_flying_foot.getRotation())
+ .getRPY(msg.fly_euler.x, msg.fly_euler.y, msg.fly_euler.z);
+
+ // Copy cartesian coordinates of the trunk goal relative to the support foot to the debug message
+ tf2::toMsg(response.support_foot_to_trunk, msg.trunk_goal);
+ // Create an additional marker for the trunk goal
+ marker_array.markers.push_back(createArrowMarker("engine_trunk_goal", current_support_frame, msg.trunk_goal, BLUE));
+
+ // TODO check this!!!
+ msg.trunk_goal_abs = msg.trunk_goal;
if (msg.trunk_goal_abs.position.y > 0) {
msg.trunk_goal_abs.position.y -= response.foot_distance / 2;
} else {
msg.trunk_goal_abs.position.y += response.foot_distance / 2;
}
- tf2::Matrix3x3(response.support_foot_to_flying_foot.getRotation()).getRPY(roll, pitch, yaw);
- msg.trunk_euler.x = roll;
- msg.trunk_euler.y = pitch;
- msg.trunk_euler.z = yaw;
+ tf2::Matrix3x3(response.support_foot_to_flying_foot.getRotation())
+ .getRPY(msg.trunk_euler.x, msg.trunk_euler.y, msg.trunk_euler.z);
// resulting trunk pose
geometry_msgs::msg::Pose pose;
- geometry_msgs::msg::Point point;
- point.x = 0;
- point.y = 0;
- point.z = 0;
- pose.position = point;
- pose.orientation.x = 0;
- pose.orientation.y = 0;
- pose.orientation.z = 0;
pose.orientation.w = 1;
- publishArrowMarker("trunk_result", tf_config_.base_link_frame, pose, r, g, b, a);
+ marker_array.markers.push_back(createArrowMarker("trunk_result", tf_config_.base_link_frame, pose, GREEN));
+ RCLCPP_INFO_ONCE(node_->get_logger(), "Color for the Engine Debug marker, showing where the trunk is, is green!");
- pub_engine_debug_->publish(msg);
+ return {msg, marker_array};
}
-void WalkVisualizer::publishIKDebug(WalkResponse response, moveit::core::RobotStatePtr current_state,
- bitbots_splines::JointGoals joint_goals) {
- // only do something if someone is listing
- if (pub_debug_->get_subscription_count() == 0 && pub_debug_marker_->get_subscription_count() == 0) {
- return;
- }
+std::pair WalkVisualizer::buildIKDebugMsgs(
+ WalkResponse response, moveit::core::RobotStatePtr current_state, bitbots_splines::JointGoals joint_goals) {
bitbots_quintic_walk::msg::WalkDebug msg;
+ visualization_msgs::msg::MarkerArray marker_array;
tf2::Transform trunk_to_support_foot = response.support_foot_to_trunk.inverse();
tf2::Transform trunk_to_flying_foot = trunk_to_support_foot * response.support_foot_to_flying_foot;
- // goals
- geometry_msgs::msg::Pose pose_support_foot_goal;
- tf2::toMsg(trunk_to_support_foot, pose_support_foot_goal);
- msg.support_foot_goal = pose_support_foot_goal;
- geometry_msgs::msg::Pose pose_fly_foot_goal;
- tf2::toMsg(trunk_to_flying_foot, pose_fly_foot_goal);
- msg.fly_foot_goal = pose_fly_foot_goal;
+ // Copy goals into the message
+ tf2::toMsg(trunk_to_support_foot, msg.support_foot_goal);
+ tf2::toMsg(trunk_to_flying_foot, msg.fly_foot_goal);
+ // Set left and right foot goals based on the support foot
if (response.is_left_support_foot) {
- msg.left_foot_goal = pose_support_foot_goal;
- msg.right_foot_goal = pose_fly_foot_goal;
+ msg.left_foot_goal = msg.support_foot_goal;
+ msg.right_foot_goal = msg.fly_foot_goal;
} else {
- msg.left_foot_goal = pose_fly_foot_goal;
- msg.right_foot_goal = pose_support_foot_goal;
+ msg.left_foot_goal = msg.fly_foot_goal;
+ msg.right_foot_goal = msg.support_foot_goal;
}
- publishArrowMarker("engine_left_goal", tf_config_.base_link_frame, msg.left_foot_goal, 0, 1, 0, 1);
- publishArrowMarker("engine_right_goal", tf_config_.base_link_frame, msg.right_foot_goal, 1, 0, 0, 1);
+ // Create additional markers for the foot goals
+ marker_array.markers.push_back(
+ createArrowMarker("engine_left_goal", tf_config_.base_link_frame, msg.left_foot_goal, ORANGE));
+ marker_array.markers.push_back(
+ createArrowMarker("engine_right_goal", tf_config_.base_link_frame, msg.right_foot_goal, ORANGE));
+ RCLCPP_INFO_ONCE(node_->get_logger(), "Color for the IK marker, showing where the feet should be, is orange!");
// IK results
moveit::core::RobotStatePtr goal_state;
goal_state.reset(new moveit::core::RobotState(kinematic_model_));
- std::vector names = joint_goals.first;
- std::vector goals = joint_goals.second;
+ auto &[names, goals] = joint_goals;
for (size_t i = 0; i < names.size(); i++) {
// besides its name, this method only changes a single joint position...
goal_state->setJointPositions(names[i], &goals[i]);
}
-
goal_state->updateLinkTransforms();
- geometry_msgs::msg::Pose pose_left_result;
- tf2::convert(goal_state->getFrameTransform("l_sole"), pose_left_result);
- msg.left_foot_ik_result = pose_left_result;
- geometry_msgs::msg::Pose pose_right_result;
- tf2::convert(goal_state->getFrameTransform("r_sole"), pose_right_result);
- msg.right_foot_ik_result = pose_right_result;
+ tf2::convert(goal_state->getFrameTransform("l_sole"), msg.left_foot_ik_result);
+ tf2::convert(goal_state->getFrameTransform("r_sole"), msg.right_foot_ik_result);
if (response.is_left_support_foot) {
- msg.support_foot_ik_result = pose_left_result;
- msg.fly_foot_ik_result = pose_right_result;
+ msg.support_foot_ik_result = msg.left_foot_ik_result;
+ msg.fly_foot_ik_result = msg.right_foot_ik_result;
} else {
- msg.support_foot_ik_result = pose_right_result;
- msg.fly_foot_ik_result = pose_left_result;
+ msg.support_foot_ik_result = msg.right_foot_ik_result;
+ msg.fly_foot_ik_result = msg.left_foot_ik_result;
}
- publishArrowMarker("ik_left", tf_config_.base_link_frame, pose_left_result, 0, 1, 0, 1);
- publishArrowMarker("ik_right", tf_config_.base_link_frame, pose_right_result, 1, 0, 0, 1);
+ marker_array.markers.push_back(
+ createArrowMarker("ik_left", tf_config_.base_link_frame, msg.left_foot_ik_result, GREEN));
+ marker_array.markers.push_back(
+ createArrowMarker("ik_right", tf_config_.base_link_frame, msg.right_foot_ik_result, GREEN));
+ RCLCPP_INFO_ONCE(node_->get_logger(), "Color for the IK marker, showing the ik result, is green!");
// IK offsets
tf2::Vector3 support_off;
@@ -234,22 +203,18 @@ void WalkVisualizer::publishIKDebug(WalkResponse response, moveit::core::RobotSt
vect_msg.z = fly_off.z();
msg.fly_foot_ik_offset = vect_msg;
- // actual positions
- geometry_msgs::msg::Pose pose_left_actual;
- tf2::convert(current_state->getGlobalLinkTransform("l_sole"), pose_left_actual);
- msg.left_foot_position = pose_left_actual;
- geometry_msgs::msg::Pose pose_right_actual;
- tf2::convert(current_state->getGlobalLinkTransform("r_sole"), pose_right_actual);
- msg.right_foot_position = pose_right_actual;
+ // Actual foot positions determined by the IK solver (not strictly equal to the goals)
+ tf2::convert(current_state->getGlobalLinkTransform("l_sole"), msg.left_foot_position);
+ tf2::convert(current_state->getGlobalLinkTransform("r_sole"), msg.right_foot_position);
if (response.is_left_support_foot) {
- msg.support_foot_position = pose_left_actual;
- msg.fly_foot_position = pose_right_actual;
+ msg.support_foot_position = msg.left_foot_position;
+ msg.fly_foot_position = msg.right_foot_position;
} else {
- msg.support_foot_position = pose_right_actual;
- msg.fly_foot_position = pose_left_actual;
+ msg.support_foot_position = msg.right_foot_position;
+ msg.fly_foot_position = msg.left_foot_position;
}
- // actual offsets
+ // Calculate offsets between the actual foot positions and the goals (meaning the IK solver error)
l_transform = current_state->getGlobalLinkTransform("l_sole").translation();
r_transform = current_state->getGlobalLinkTransform("r_sole").translation();
tf2::convert(l_transform, tf_vec_left);
@@ -286,25 +251,20 @@ void WalkVisualizer::publishIKDebug(WalkResponse response, moveit::core::RobotSt
vect_msg.z = fly_off.z();
msg.fly_foot_actual_offset = vect_msg;
- pub_debug_->publish(msg);
+ return {msg, marker_array};
}
-void WalkVisualizer::publishArrowMarker(std::string name_space, std::string frame, geometry_msgs::msg::Pose pose,
- float r, float g, float b, float a) {
+visualization_msgs::msg::Marker WalkVisualizer::createArrowMarker(const std::string &name_space,
+ const std::string &frame,
+ const geometry_msgs::msg::Pose &pose,
+ const std_msgs::msg::ColorRGBA &color) {
visualization_msgs::msg::Marker marker_msg;
- marker_msg.header.stamp = node_->now();
marker_msg.header.frame_id = frame;
marker_msg.type = marker_msg.ARROW;
marker_msg.ns = name_space;
marker_msg.action = marker_msg.ADD;
marker_msg.pose = pose;
-
- std_msgs::msg::ColorRGBA color;
- color.r = r;
- color.g = g;
- color.b = b;
- color.a = a;
marker_msg.color = color;
geometry_msgs::msg::Vector3 scale;
@@ -312,87 +272,56 @@ void WalkVisualizer::publishArrowMarker(std::string name_space, std::string fram
scale.y = 0.003;
scale.z = 0.003;
marker_msg.scale = scale;
+ marker_msg.id = 0;
- marker_msg.id = marker_id_;
- marker_id_++;
-
- pub_debug_marker_->publish(marker_msg);
+ return marker_msg;
}
-void WalkVisualizer::publishWalkMarkers(WalkResponse response) {
- // only do something if someone is listing
- if (pub_debug_marker_->get_subscription_count() == 0) {
- return;
- }
- // publish markers
- visualization_msgs::msg::Marker marker_msg;
- marker_msg.header.stamp = node_->now();
+visualization_msgs::msg::MarkerArray WalkVisualizer::buildWalkMarkers(WalkResponse response) {
+ visualization_msgs::msg::MarkerArray marker_array;
+
+ // Create a marker for the last step
+ visualization_msgs::msg::Marker support_foot_marker_msg;
if (response.is_left_support_foot) {
- marker_msg.header.frame_id = tf_config_.l_sole_frame;
+ support_foot_marker_msg.header.frame_id = tf_config_.l_sole_frame;
} else {
- marker_msg.header.frame_id = tf_config_.r_sole_frame;
+ support_foot_marker_msg.header.frame_id = tf_config_.r_sole_frame;
}
- marker_msg.type = marker_msg.CUBE;
- marker_msg.action = 0;
- marker_msg.lifetime = rclcpp::Duration::from_nanoseconds(0.0);
- geometry_msgs::msg::Vector3 scale;
- scale.x = 0.20;
- scale.y = 0.10;
- scale.z = 0.01;
- marker_msg.scale = scale;
- // last step
- marker_msg.ns = "last_step";
- marker_msg.id = 1;
- std_msgs::msg::ColorRGBA color;
- color.r = 0;
- color.g = 0;
- color.b = 0;
- color.a = 1;
- marker_msg.color = color;
- geometry_msgs::msg::Pose pose;
- tf2::Vector3 step_pos = response.support_to_last.getOrigin();
- geometry_msgs::msg::Point point;
- point.x = step_pos[0];
- point.y = step_pos[1];
- point.z = 0;
- pose.position = point;
- tf2::Quaternion q;
- q.setRPY(0.0, 0.0, step_pos[2]);
- tf2::convert(q, pose.orientation);
- marker_msg.pose = pose;
- pub_debug_marker_->publish(marker_msg);
-
- // last step center
- marker_msg.ns = "step_center";
- marker_msg.id = marker_id_;
- scale.x = 0.01;
- scale.y = 0.01;
- scale.z = 0.01;
- marker_msg.scale = scale;
- pub_debug_marker_->publish(marker_msg);
-
- // next step
- marker_msg.id = marker_id_;
- marker_msg.ns = "next_step";
- scale.x = 0.20;
- scale.y = 0.10;
- scale.z = 0.01;
- marker_msg.scale = scale;
- color.r = 1;
- color.g = 1;
- color.b = 1;
- color.a = 0.5;
- marker_msg.color = color;
- step_pos = response.support_to_next.getOrigin();
- point.x = step_pos[0];
- point.y = step_pos[1];
- pose.position = point;
- q.setRPY(0.0, 0.0, step_pos[2]);
- tf2::convert(q, pose.orientation);
- marker_msg.pose = pose;
- pub_debug_marker_->publish(marker_msg);
-
- marker_id_++;
+ support_foot_marker_msg.type = support_foot_marker_msg.CUBE;
+ support_foot_marker_msg.action = 0;
+ support_foot_marker_msg.lifetime = rclcpp::Duration::from_nanoseconds(0.0);
+ support_foot_marker_msg.scale.x = 0.2;
+ support_foot_marker_msg.scale.y = 0.1;
+ support_foot_marker_msg.scale.z = 0.01;
+ support_foot_marker_msg.ns = "last_step";
+ support_foot_marker_msg.id = 1;
+ support_foot_marker_msg.color = BLACK;
+ support_foot_marker_msg.color.a = 0.5;
+ tf2::toMsg(response.support_to_last, support_foot_marker_msg.pose);
+ marker_array.markers.push_back(support_foot_marker_msg);
+
+ // This step center
+ auto step_center_marker_msg(support_foot_marker_msg);
+ step_center_marker_msg.ns = "step_center";
+ step_center_marker_msg.id = 2;
+ step_center_marker_msg.scale.x = 0.01;
+ step_center_marker_msg.scale.y = 0.01;
+ step_center_marker_msg.scale.z = 0.01;
+ marker_array.markers.push_back(step_center_marker_msg);
+
+ // Next step
+ auto next_step_marker_msg(support_foot_marker_msg);
+ next_step_marker_msg.id = 3;
+ next_step_marker_msg.ns = "next_step";
+ next_step_marker_msg.scale.x = 0.20;
+ next_step_marker_msg.scale.y = 0.10;
+ next_step_marker_msg.scale.z = 0.01;
+ next_step_marker_msg.color = WHITE;
+ next_step_marker_msg.color.a = 0.5;
+ tf2::toMsg(response.support_to_next, next_step_marker_msg.pose);
+ marker_array.markers.push_back(next_step_marker_msg);
+
+ return marker_array;
}
} // namespace bitbots_quintic_walk
diff --git a/bitbots_msgs/msg/Workload.msg b/bitbots_msgs/msg/Workload.msg
index 9b708ddcf..cdde1e0bf 100644
--- a/bitbots_msgs/msg/Workload.msg
+++ b/bitbots_msgs/msg/Workload.msg
@@ -4,6 +4,11 @@ Cpu[] cpus
int32 running_processes
float32 cpu_usage_overall
+float32 gpu_load
+int64 gpu_vram_used
+int64 gpu_vram_total
+float32 gpu_temperature
+
int64 memory_available
int64 memory_used
int64 memory_total
diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py
index e655ae375..3a812d5c1 100644
--- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py
+++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py
@@ -12,7 +12,7 @@
from bitbots_path_planning.controller import Controller
from bitbots_path_planning.map import Map
from bitbots_path_planning.path_planning_parameters import bitbots_path_planning as parameters
-from bitbots_path_planning.planner import Planner
+from bitbots_path_planning.planner import planner_factory
class PathPlanning(Node):
@@ -30,7 +30,7 @@ def __init__(self) -> None:
# Create submodules
self.map = Map(node=self, buffer=self.tf_buffer)
- self.planner = Planner(node=self, buffer=self.tf_buffer, map=self.map)
+ self.planner = planner_factory(self)(node=self, buffer=self.tf_buffer, map=self.map)
self.controller = Controller(node=self, buffer=self.tf_buffer)
# Subscriber
diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py
index 9c3254d0e..08a5392cb 100644
--- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py
+++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py
@@ -1,7 +1,7 @@
import numpy as np
import pyastar2d
import tf2_ros as tf2
-from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import PoseStamped, Vector3
from nav_msgs.msg import Path
from rclpy.duration import Duration
from rclpy.node import Node
@@ -44,6 +44,14 @@ def active(self) -> bool:
"""
return self.goal is not None
+ def get_my_position(self) -> Vector3:
+ """
+ Returns the current position of the robot
+ """
+ return self.buffer.lookup_transform(
+ self.map.frame, self.base_footprint_frame, Time(), Duration(seconds=0.2)
+ ).transform.translation
+
def step(self) -> Path:
"""
Generates a new A* path to the goal pose with respect to the costmap
@@ -54,9 +62,7 @@ def step(self) -> Path:
navigation_grid = self.map.get_map()
# Get my pose and position on the map
- my_position = self.buffer.lookup_transform(
- self.map.frame, self.base_footprint_frame, Time(), Duration(seconds=0.2)
- ).transform.translation
+ my_position = self.get_my_position()
# Transform goal pose to map frame if needed
if goal.header.frame_id != self.map.frame:
@@ -94,3 +100,31 @@ def get_path(self) -> Path | None:
Returns the most recent path
"""
return self.path
+
+
+class DummyPlanner(Planner):
+ def __init__(self, node: Node, buffer: tf2.Buffer, map: Map) -> None:
+ super().__init__(node, buffer, map)
+
+ def step(self) -> Path:
+ return self.get_path()
+
+ def get_path(self) -> Path:
+ pose = PoseStamped()
+ my_position = self.get_my_position()
+ pose.pose.position.x = my_position.x
+ pose.pose.position.y = my_position.y
+
+ self.path = Path(
+ header=Header(frame_id=self.map.get_frame(), stamp=self.node.get_clock().now().to_msg()),
+ poses=[pose, self.goal],
+ )
+
+ return self.path
+
+
+def planner_factory(node: Node) -> type:
+ if node.config.planner.dummy:
+ return DummyPlanner
+ else:
+ return Planner
diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml
index 1f7c94211..498c04350 100644
--- a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml
+++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml
@@ -17,6 +17,12 @@ bitbots_path_planning:
validation:
bounds<>: [0.0, 100.0]
+ planner:
+ dummy:
+ type: bool
+ default_value: false
+ description: 'If true, the dummy planner is used, which just returns a straight line to the goal. This ignores all obstacles, but also has no limitations on the map size.'
+
map:
planning_frame:
type: string
diff --git a/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch b/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch
index 0774f196a..d87afb55c 100755
--- a/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch
+++ b/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch
@@ -1,7 +1,9 @@
+
+
diff --git a/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_robot_controller.py b/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_robot_controller.py
index 0ba93ed38..925fb1db3 100644
--- a/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_robot_controller.py
+++ b/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_robot_controller.py
@@ -645,6 +645,12 @@ def __init__(
self.pub_pres_left = self.ros_node.create_publisher(FootPressure, base_ns + "foot_pressure_left/raw", 1)
self.pub_pres_right = self.ros_node.create_publisher(FootPressure, base_ns + "foot_pressure_right/raw", 1)
+ self.pub_pres_left_filtered = self.ros_node.create_publisher(
+ FootPressure, base_ns + "foot_pressure_left/filtered", 1
+ )
+ self.pub_pres_right_filtered = self.ros_node.create_publisher(
+ FootPressure, base_ns + "foot_pressure_right/filtered", 1
+ )
self.cop_l_pub_ = self.ros_node.create_publisher(PointStamped, base_ns + "cop_l", 1)
self.cop_r_pub_ = self.ros_node.create_publisher(PointStamped, base_ns + "cop_r", 1)
self.ros_node.create_subscription(JointCommand, base_ns + "DynamixelController/command", self.command_cb, 1)
@@ -1009,6 +1015,8 @@ def get_pressure_message(self):
def publish_pressure(self):
left, right, cop_l, cop_r = self.get_pressure_message()
self.pub_pres_left.publish(left)
+ self.pub_pres_left_filtered.publish(left)
self.pub_pres_right.publish(right)
+ self.pub_pres_right_filtered.publish(right)
self.cop_l_pub_.publish(cop_l)
self.cop_r_pub_.publish(cop_r)
diff --git a/requirements/robot.txt b/requirements/robot.txt
index 0c6ffe9b3..a41d3b8cd 100644
--- a/requirements/robot.txt
+++ b/requirements/robot.txt
@@ -4,3 +4,4 @@ mycroft-mimic3-tts
protobuf==3.20.3 # Required for mycroft-mimic3-tts, but we want to enshure that the version is compatible binaries build using the system version, but it should also be compatiple with all the python dependencies
pyttsx3
playsound
+pyamdgpuinfo
diff --git a/scripts/make_basler.sh b/scripts/make_basler.sh
index 7becc3e72..e3ad8a01f 100755
--- a/scripts/make_basler.sh
+++ b/scripts/make_basler.sh
@@ -5,7 +5,7 @@ set -eEo pipefail
# The pylon driver can be found in the download section of the following link:
# https://www.baslerweb.com/en/downloads/software-downloads/
# Go to the download button and copy the link address.
-PYLON_DOWNLOAD_URL="https://www2.baslerweb.com/media/downloads/software/pylon_software/pylon_7_4_0_14900_linux_x86_64_debs.tar.gz"
+PYLON_DOWNLOAD_URL="https://data.bit-bots.de/pylon_7_4_0_14900_linux_x86_64_debs.tar.gz.gpg"
PYLON_VERSION="7.4.0"
# Check let the user confirm that they read the license agreement on the basler website and agree with it.
@@ -47,9 +47,12 @@ else
exit 1
fi
# Download the pylon driver to temp folder
- wget --no-verbose $SHOW_PROGRESS $PYLON_DOWNLOAD_URL -O /tmp/pylon_${PYLON_VERSION}.tar.gz
+ wget --no-verbose $SHOW_PROGRESS $PYLON_DOWNLOAD_URL -O /tmp/pylon_${PYLON_VERSION}.tar.gz.gpg
# Extract the pylon driver
mkdir /tmp/pylon
+ # Decrypt the pylon driver
+ gpg --batch --yes --passphrase "12987318371043223" -o /tmp/pylon_${PYLON_VERSION}.tar.gz -d /tmp/pylon_${PYLON_VERSION}.tar.gz.gpg
+ # Extract the pylon driver
tar -xzf /tmp/pylon_${PYLON_VERSION}.tar.gz -C /tmp/pylon/
# Install the pylon driver
sudo apt-get install /tmp/pylon/pylon_${PYLON_VERSION}*.deb -y