Kind of working state

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py

@@ -7,6 +7,7 @@
 from rclpy.publisher import Publisher
 from rclpy.time import Time
 from std_msgs.msg import Bool
+from geometry_msgs.msg import PoseStamped
 
 from bitbots_blackboard.capsules import AbstractBlackboardCapsule
 from bitbots_msgs.action import Kick
@@ -40,15 +41,15 @@ def __init__(self, node, blackboard):
         """
         :param blackboard: Global blackboard instance
         """
-        self.walk_kick_pub = self._node.create_publisher(Bool, "/kick", 1)
+        self.walk_kick_pub = self._node.create_publisher(PoseStamped, "/kick", 1)
         # self.connect_dynamic_kick()  Do not connect if dynamic_kick is disabled
 
-    def walk_kick(self, target: WalkKickTargets) -> None:
+    def walk_kick(self, kick_point: PoseStamped) -> None:
         """
         Kick the ball while walking
-        :param target: Target for the walk kick (e.g. left or right foot)
+        :param kick_point: Target point to kick at
         """
-        self.walk_kick_pub.publish(Bool(data=target.value))
+        self.walk_kick_pub.publish(kick_point)
 
     def connect_dynamic_kick(self) -> None:
         topic = self._blackboard.config["dynamic_kick"]["topic"]

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/kick_ball.py

@@ -2,6 +2,7 @@
 from bitbots_blackboard.capsules.kick_capsule import KickCapsule
 from bitbots_utils.transforms import quat_from_yaw
 from dynamic_stack_decider.abstract_action_element import AbstractActionElement
+from geometry_msgs.msg import PoseStamped
 
 from bitbots_msgs.action import Kick
 
@@ -19,17 +20,17 @@ class WalkKick(AbstractKickAction):
 
     def __init__(self, blackboard, dsd, parameters):
         super().__init__(blackboard, dsd, parameters)
-        if "foot" not in parameters.keys():
-            raise ValueError("No foot specified for walk kick")
-        elif "right" == parameters["foot"]:
-            self.target = KickCapsule.WalkKickTargets.RIGHT
-        elif "left" == parameters["foot"]:
-            self.target = KickCapsule.WalkKickTargets.LEFT
-        else:
-            raise ValueError(f"Invalid foot specified for walk kick: {parameters['foot']}")
 
     def perform(self, reevaluate=False):
-        self.blackboard.kick.walk_kick(self.target)
+
+        ball_u, ball_v = self.blackboard.world_model.get_ball_position_uv()
+
+        pose = PoseStamped()
+        pose.header.frame_id = self.blackboard.world_model.base_footprint_frame
+        pose.pose.position.x = ball_u - 0.13 / 2 - 0.02
+        pose.pose.position.y = ball_v
+
+        self.blackboard.kick.walk_kick(pose)
         self.pop()
 
 

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd

@@ -20,11 +20,8 @@ $DoOnce
 #GetWalkreadyAndLocalize
 @ChangeAction + action:waiting + r:false, @PlayAnimationInitInSim + r:false, @LookAtFieldFeatures + r:false, @GetWalkready + r:false, @WalkInPlace
 
-#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
+#PerformKick
+@ChangeAction + action:kicking, @LookAtFront, @WalkKick + r:false, @WalkInPlace + duration:1 + r:false, @ForgetBall + r:false, @WalkInPlace + duration:1 + r:false, @LookAtFieldFeatures + r:false
 
 #Dribble
 @ChangeAction + action:going_to_ball, @CancelPathplanning, @LookAtBall, @LookAtFront, @DribbleForward
@@ -40,9 +37,7 @@ $DoOnce
 $AvoidBall
     NO --> $BallClose + distance:%ball_reapproach_dist + angle:%ball_reapproach_angle
         YES --> $BallKickArea
-            NEAR --> $FootSelection
-                LEFT --> #PerformKickLeft
-                RIGHT --> #PerformKickRight
+            NEAR --> #PerformKick
             FAR --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:map
         NO --> @ChangeAction + action:going_to_ball + r:false, @LookAtFieldFeatures + r:false, @AvoidBallActive + r:false, @GoToBall + target:map + blocking:false + distance:%ball_far_approach_dist
     YES --> $ReachedPathPlanningGoalPosition + threshold:%ball_far_approach_position_thresh

bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml

@@ -61,8 +61,8 @@ body_behavior:
     # An area in which the ball can be kicked
     # defined by min/max x/y values in meters which represent ball positions relative to base_footprint
     # http://www.ros.org/reps/rep-0103.html#axis-orientation
-    kick_x_enter: 0.2
-    kick_x_leave: 0.25
+    kick_x_enter: 0.32
+    kick_x_leave: 0.35
     kick_y_enter: 0.12
     kick_y_leave: 0.14
 
@@ -113,14 +113,14 @@ body_behavior:
     ball_reapproach_dist: 1.0
 
     # Distance at which the ball is normally approached
-    ball_approach_dist: 0.2                                                          
+    ball_approach_dist: 0.2
 
     # Angle at which the ball is normally approached again
     ball_reapproach_angle: 1.2
 
     # The position where the supporter robot should place itself in order to accept a pass
     pass_position_x: 0.1
-    pass_position_y: 1.0                                                                                                                                                                                                                                                                                                                                               
+    pass_position_y: 1.0
     supporter_max_x: 4.0
 
     # maximal angle of a ball kick

bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp

@@ -66,7 +66,7 @@ class WalkEngine : public bitbots_splines::AbstractEngine<WalkRequest, WalkRespo
    */
   bool isDoubleSupport();
 
-  void requestKick(tf2::Transform kick_point);
+  void requestKick(tf2::Transform kick_point, bool left_kick);
 
   void requestPause();
 

bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.hpp

@@ -14,14 +14,14 @@ The original files can be found at:
 #include <tf2/LinearMath/Transform.h>
 #include <tf2/LinearMath/Vector3.h>
 #include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 #include <unistd.h>
 
 #include <Eigen/Dense>
 #include <chrono>
 #include <control_toolbox/pid_ros.hpp>
 #include <geometry_msgs/msg/point_stamped.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <geometry_msgs/msg/twist.hpp>

bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp

@@ -124,7 +124,7 @@ WalkResponse WalkEngine::update(double dt) {
       pause_requested_ = false;
       return createResponse();
     } else if (half_step_finished && positioning_step_to_kick_point_ &&
-               ((left_kick_requested_ && !is_left_support_foot_) || (right_kick_requested_ && is_left_support_foot_))) {
+               ((left_kick_requested_ && is_left_support_foot_) || (right_kick_requested_ && !is_left_support_foot_))) {
       // lets do a kick
       buildTrajectories(TrajectoryType::KICK);
       engine_state_ = WalkState::KICK;
@@ -820,15 +820,16 @@ bool WalkEngine::isDoubleSupport() {
   return is_double_support_spline_.pos(getTrajsTime()) >= 0.5;
 }
 
-void WalkEngine::requestKick(tf2::Transform kick_point) {
+void WalkEngine::requestKick(tf2::Transform kick_point, bool left_kick) {
   // We assume that the kick point is given in the support foot frame
   if (is_left_support_foot_) {
-    kick_point = left_in_world_ * kick_point;
+    kick_point_ = left_in_world_ * kick_point;
   } else {
-    kick_point = right_in_world_ * kick_point;
+    kick_point_ = right_in_world_ * kick_point;
   }
 
-  left_kick_requested_ = true;  // TODO better feet selection
+  left_kick_requested_ = left_kick;
+  right_kick_requested_ = !left_kick;
 }
 
 void WalkEngine::requestPause() { pause_requested_ = true; }
@@ -881,7 +882,7 @@ void WalkEngine::stepToApproachKick(const tf2::Transform kick_point_and_directio
 
   tf2::Transform foot_goal;
 
-  if (is_left_support_foot_ == kick_with_left) {
+  if (is_left_support_foot_ != kick_with_left) {
     // We can not place the foot on the kick point, because it is the support foot
     // Therfore we try to place the other foot next to it in an attempt to get as close as possible
     // and place the kick foot in the step after
@@ -908,17 +909,17 @@ void WalkEngine::stepToApproachKick(const tf2::Transform kick_point_and_directio
   // We need to subtract a small value from the foot distance, otherwise the claming is triggered which,
   // in turn, delays the kick, as we might not be able to reach the kick point yet
   if ((is_left_support_foot_ && foot_goal.getOrigin().getY() < config_.foot_distance - 0.01) ||
-      (!is_left_support_foot_ && foot_goal.getOrigin().getY() > -config_.foot_distance + 0.01)) {
+     (!is_left_support_foot_ && foot_goal.getOrigin().getY() > -config_.foot_distance + 0.01)) {
     RCLCPP_WARN(node_->get_logger(), "Side step not possible, placing the foot back to zero pose");
 
-    if (is_left_support_foot_) {
-      foot_goal.getOrigin().setY(config_.foot_distance);
-    } else {
-      foot_goal.getOrigin().setY(-config_.foot_distance);
-    }
+   if (is_left_support_foot_) {
+     foot_goal.getOrigin().setY(config_.foot_distance);
+   } else {
+     foot_goal.getOrigin().setY(-config_.foot_distance);
+   }
 
-    // If we tried to position the foot with this step this is not possible anymore and we need to do a normal step
-    positioning_step_to_kick_point_ = false;
+   // If we tried to position the foot with this step this is not possible anymore and we need to do a normal step
+   positioning_step_to_kick_point_ = false;
   }
 
   // Now we can perform the step

bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp

@@ -528,9 +528,17 @@ void WalkNode::kickCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
   std::string support_foot_frame =
       walk_engine_.isLeftSupport() ? config_.node.tf.l_sole_frame : config_.node.tf.r_sole_frame;
   try {
-    tf2::Transform transform;
-    tf2::fromMsg(tf_buffer_.transform(msg, support_foot_frame)->pose, transform);
-    walk_engine_.requestKick(transform);
+      // Select foot used for kick
+      auto pose_base_footprint = tf_buffer_.transform(*msg, "base_footprint", tf2::durationFromSec(0.1)).pose;
+      bool use_left_foot = pose_base_footprint.position.y > 0;
+
+      // Transform the goal pose of the contact point to the support foot frame
+      auto pose = tf_buffer_.transform(*msg, support_foot_frame, tf2::durationFromSec(0.1)).pose;
+      tf2::Transform transform;
+      tf2::fromMsg(pose, transform);
+
+      // Request the kick
+      walk_engine_.requestKick(transform, use_left_foot);
   } catch (tf2::TransformException& ex) {
     RCLCPP_ERROR(node_->get_logger(), "Skipping kick, Could not transform kick goal to support foot frame: %s",
                  ex.what());