Merge branch 'feature/webots_force_service' into feature/step_length_…

…adjustment

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

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
 

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()

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

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

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

bitbots_misc/bitbots_bringup/launch/demo.launch

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="sim" default="false" description="Whether the robot is running in simulation or on real hardware" />
+    <arg name="behavior_dsd_file" default="demo.dsd" description="The behavior dsd file that should be used" />
+
+    <!-- load teamplayer software stack without some unnecessary stuff, that is not needed in the demo -->
+    <include file="$(find-pkg-share bitbots_bringup)/launch/teamplayer.launch">
+        <arg name="behavior_dsd_file" value="$(var behavior_dsd_file)" />
+        <arg name="game_controller" value="false"/>
+        <arg name="localization" value="false" />
+        <arg name="sim" value="$(var sim)" />
+        <arg name="teamcom" value="false" />
+        <arg name="path_planning" value="false" />
+    </include>
+
+    <!-- load the path planning node in dummy mode, because we are limited by the map size otherwise and together with no localization 
+    this could lead to the robot not working after a while, because due to odometry errors the robot could be outside of the map -->
+    <include file="$(find-pkg-share bitbots_path_planning)/launch/path_planning.launch">
+        <arg name="sim" value="$(var sim)" />
+        <arg name="dummy" value="true" />
+    </include>
+</launch>

bitbots_misc/bitbots_teleop/scripts/teleop_keyboard.py

@@ -20,6 +20,7 @@
 
 from bitbots_msgs.action import Dynup, Kick
 from bitbots_msgs.msg import HeadMode, JointCommand
+from bitbots_msgs.srv import SimulatorPush
 
 msg = """
 BitBots Teleop
@@ -57,6 +58,13 @@
 4: Ball Mode adapted for Penalty Kick
 5: Do a pattern which only looks in front of the robot
 
+Pushing:
+p: execute Push
+Shift-p: reset Power to 0
+ü/ä: increase/decrease power forward (x axis)
++/#: increase/decrease power left (y axis)
+SHIFT increases/decreases with factor 10
+
 CTRL-C to quit
 
 
@@ -121,6 +129,9 @@ def __init__(self):
         self.th = 0
         self.status = 0
 
+        self.push_force_x = 0.0
+        self.push_force_y = 0.0
+
         # Head Part
         self.create_subscription(JointState, "joint_states", self.joint_state_cb, 1)
         self.head_mode_pub = self.create_publisher(HeadMode, "head_mode", 1)
@@ -145,6 +156,7 @@ def __init__(self):
 
         self.reset_robot = self.create_client(Empty, "/reset_pose")
         self.reset_ball = self.create_client(Empty, "/reset_ball")
+        self.simulator_push = self.create_client(SimulatorPush, "/simulator_push")
 
         self.frame_prefix = "" if os.environ.get("ROS_NAMESPACE") is None else os.environ.get("ROS_NAMESPACE") + "/"
 
@@ -308,6 +320,32 @@ def loop(self):
                     self.z = 0
                     self.a_x = -1
                     self.th = 0
+                elif key == "p":
+                    # push robot in simulation
+                    push_request = SimulatorPush.Request()
+                    push_request.force.x = self.push_force_x
+                    push_request.force.y = self.push_force_y
+                    push_request.relative = True
+                    self.simulator_push.call_async(push_request)
+                elif key == "P":
+                    self.push_force_x = 0.0
+                    self.push_force_y = 0.0
+                elif key == "ü":
+                    self.push_force_x += 1
+                elif key == "Ü":
+                    self.push_force_x += 10
+                elif key == "ä":
+                    self.push_force_x -= 1
+                elif key == "Ä":
+                    self.push_force_x -= 10
+                elif key == "+":
+                    self.push_force_y += 1
+                elif key == "*":
+                    self.push_force_y += 10
+                elif key == "#":
+                    self.push_force_y -= 1
+                elif key == "'":
+                    self.push_force_y -= 10
                 else:
                     self.x = 0
                     self.y = 0
@@ -324,7 +362,15 @@ def loop(self):
                 twist.linear = Vector3(x=float(self.x), y=float(self.y), z=0.0)
                 twist.angular = Vector3(x=float(self.a_x), y=0.0, z=float(self.th))
                 self.pub.publish(twist)
-                state_str = f"x:    {self.x}          \ny:    {self.y}          \nturn: {self.th}          \nhead mode: {self.head_mode_msg.head_mode}      \n"
+                state_str = (
+                    f"x:    {self.x}     \n"
+                    f"y:    {self.y}     \n"
+                    f"turn: {self.th}     \n"
+                    f"head mode: {self.head_mode_msg.head_mode}     \n"
+                    f"push force x (+forward/-back): {self.push_force_x}     \n"
+                    f"push force y (+left/-right):   {self.push_force_y}     "
+                )
+
                 for _ in range(state_str.count("\n") + 1):
                     sys.stdout.write("\x1b[A")
                 print(state_str)

bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py

@@ -659,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
 

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<double> 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_
+#endif  // BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_UTILS_H_

bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_visualizer.hpp

@@ -10,6 +10,7 @@
 #include <bitbots_quintic_walk/walk_engine.hpp>
 #include <bitbots_quintic_walk/walk_utils.hpp>
 #include <moveit_msgs/msg/robot_state.hpp>
+#include <ranges>
 #include <rclcpp/rclcpp.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
@@ -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<bitbots_quintic_walk::msg::WalkEngineDebug, visualization_msgs::msg::MarkerArray> buildEngineDebugMsgs(
+      WalkResponse response);
+  std::pair<bitbots_quintic_walk::msg::WalkDebug, visualization_msgs::msg::MarkerArray> 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 &current_response, const moveit::core::RobotStatePtr &current_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<bitbots_quintic_walk::msg::WalkDebug>::SharedPtr pub_debug_;
   rclcpp::Publisher<bitbots_quintic_walk::msg::WalkEngineDebug>::SharedPtr pub_engine_debug_;
-  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub_debug_marker_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_debug_marker_;
 
   moveit::core::RobotModelPtr kinematic_model_;
 };

bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp

@@ -858,12 +858,23 @@ void WalkEngine::stepFromOrders(const std::vector<double>& linear_orders, double
   // No change in forward step and upward step
   tmp_diff.getOrigin()[0] = linear_orders[0];
   tmp_diff.getOrigin()[2] = linear_orders[2];
+
   // Add lateral foot offset
-  if (is_left_support_foot_) {
-    tmp_diff.getOrigin()[1] = config_.foot_distance;
-  } else {
-    tmp_diff.getOrigin()[1] = -1 * config_.foot_distance;
+  // This is normally just the foot distance, but if we turn we need to also move the foot forward/backward
+  geometry_msgs::msg::Vector3 foot_offset;
+  foot_offset.x = -sin(angular_z / 2) * config_.foot_distance;
+  foot_offset.y = cos(angular_z / 2) * config_.foot_distance;
+
+  // Invert lateral offset for right foot
+  if (!is_left_support_foot_) {
+    foot_offset.x *= -1;
+    foot_offset.y *= -1;
   }
+
+  // Add foot offset to step diff
+  tmp_diff.getOrigin()[0] += foot_offset.x;
+  tmp_diff.getOrigin()[1] += foot_offset.y;
+
   // Allow lateral step only on external foot
   //(internal foot will return to zero pose)
   if ((is_left_support_foot_ && linear_orders[1] > 0.0) || (!is_left_support_foot_ && linear_orders[1] < 0.0)) {

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_);