Merge branch 'main' into fix/docs_extrinsic_calibration_command_2

.devcontainer/Dockerfile

@@ -32,7 +32,6 @@ RUN apt-get update -y \
     tree \
     uvcdynctrl \
     vim \
-    vlc \
     wget \
     x11-apps \
     zsh

.vscode/settings.json

@@ -112,6 +112,7 @@
         "throwin",
         "timespec",
         "tldr",
+        "torqueless",
         "tqdm",
         "unpenalize",
         "unpenalized",
@@ -212,7 +213,8 @@
         "variant": "cpp",
         "regex": "cpp",
         "future": "cpp",
-        "*.ipp": "cpp"
+        "*.ipp": "cpp",
+        "span": "cpp"
     },
     // Tell the ROS extension where to find the setup.bash
     // This also utilizes the COLCON_WS environment variable, which needs to be set

Makefile

@@ -47,6 +47,8 @@ pull-files:
 	wget \
 		--no-verbose \
 		--show-progress \
+		--timeout=15 \
+		--tries=2 \
 		--recursive \
 		--timestamping \
 		--no-parent \
@@ -57,6 +59,8 @@ pull-files:
 	wget \
 		--no-verbose \
 		--show-progress \
+		--timeout=15 \
+		--tries=2 \
 		--recursive \
 		--timestamping \
 		--no-parent \
@@ -86,7 +90,7 @@ rosdep:
 	# Initialize rosdep if not already done
 	[ -f /etc/ros/rosdep/sources.list.d/20-default.list ] || sudo rosdep init
 	# Update rosdep and install dependencies from meta directory
-	rosdep update
+	rosdep update --include-eol-distros
 	rosdep install --from-paths . --ignore-src --rosdistro iron -y
 
 status:

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.py

@@ -56,6 +56,7 @@ def __init__(self, blackboard, dsd, parameters):
         """Go to an absolute position on the field"""
         super().__init__(blackboard, dsd, parameters)
         self.point = float(parameters.get("x", 0)), float(parameters.get("y", 0)), float(parameters.get("t", 0))
+        self.blocking = parameters.get("blocking", True)
 
     def perform(self, reevaluate=False):
         pose_msg = PoseStamped()
@@ -64,11 +65,26 @@ def perform(self, reevaluate=False):
 
         pose_msg.pose.position.x = self.point[0]
         pose_msg.pose.position.y = self.point[1]
-        pose_msg.pose.position.z = 0
+        pose_msg.pose.position.z = 0.0
         pose_msg.pose.orientation = quat_from_yaw(math.radians(self.point[2]))
 
         self.blackboard.pathfinding.publish(pose_msg)
 
+        if not self.blocking:
+            self.pop()
+
+
+class GoToAbsolutePositionFieldFraction(GoToAbsolutePosition):
+    def __init__(self, blackboard, dsd, parameters):
+        """Go to an absolute position of the field, specified by the fraction of the field size"""
+        super().__init__(blackboard, dsd, parameters)
+        point = float(parameters.get("x", 0)), float(parameters.get("y", 0)), float(parameters.get("t", 0))
+        self.point = (
+            point[0] * self.blackboard.world_model.field_length / 2,
+            point[1] * self.blackboard.world_model.field_width / 2,
+            self.point[2],
+        )
+
 
 class GoToOwnGoal(GoToAbsolutePosition):
     def __init__(self, blackboard, dsd, parameters):
@@ -77,7 +93,7 @@ def __init__(self, blackboard, dsd, parameters):
         self.point = (
             self.blackboard.world_model.get_map_based_own_goal_center_xy()[0],
             self.blackboard.world_model.get_map_based_own_goal_center_xy()[1],
-            parameters,
+            self.point[2],
         )
 
 
@@ -88,12 +104,12 @@ def __init__(self, blackboard, dsd, parameters):
         self.point = (
             self.blackboard.world_model.get_map_based_opp_goal_center_xy()[0],
             self.blackboard.world_model.get_map_based_opp_goal_center_xy()[1],
-            parameters,
+            self.point[2],
         )
 
 
 class GoToCenterpoint(GoToAbsolutePosition):
     def __init__(self, blackboard, dsd, parameters):
         """Go to the center of the field and look towards the enemy goal"""
         super().__init__(blackboard, dsd, parameters)
-        self.point = 0, 0, 0
+        self.point = 0.0, 0.0, 0.0

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

@@ -1,5 +1,15 @@
 #SearchBall
-@ChangeAction + action:searching, @LookAtFieldFeatures, @WalkInPlace + duration:3, @Turn
+$DoOnce
+    NOT_DONE --> @ChangeAction + action:searching, @LookAtFieldFeatures, @WalkInPlace + duration:3, @Turn + duration:15, @GoToAbsolutePositionFieldFraction + x:0.5 + blocking:false
+    DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:0.5 + latch:true
+        NO --> @LookAtFieldFeatures, @GoToAbsolutePositionFieldFraction + x:0.5
+        YES --> $DoOnce
+            NOT_DONE --> @Turn + duration:15
+            DONE --> $DoOnce
+                NOT_DONE --> @GoToAbsolutePositionFieldFraction + y:1.0 + t:-90 + blocking:false
+                DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:0.2 + latch:true
+                    YES --> @Stand
+                    NO --> @GoToAbsolutePositionFieldFraction + y:1.0 + t:-90
 
 #DoNothing
 @ChangeAction + action:waiting, @LookForward, @Stand
@@ -61,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
@@ -148,7 +160,7 @@ $IsPenalized
         READY --> $AnyGoalScoreRecently + time:50
             YES --> #PositioningReady
             NO --> $DoOnce
-                NOT_DONE --> @ChangeAction + action:waiting + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:1, @GetWalkready + r:false
+                NOT_DONE --> @ChangeAction + action:waiting + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:2
                 DONE --> #PositioningReady
         SET --> $SecondaryStateDecider
             PENALTYSHOOT --> $SecondaryStateTeamDecider

bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml

@@ -104,7 +104,7 @@ body_behavior:
     goalpost_safety_distance: 0.05
 
     # Distance at which the ball is first approached before the ball obstacle is deactivated and we approach closer for the kick
-    ball_far_approach_dist: 0.5
+    ball_far_approach_dist: 0.3
 
     # Range in which the ball far approach point is counted as reached
     ball_far_approach_position_thresh: 0.2
@@ -113,14 +113,14 @@ body_behavior:
     ball_reapproach_dist: 1.0
 
     # Distance at which the ball is normally approached
-    ball_approach_dist: 0.20
+    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_lowlevel/bitbots_ros_control/CMakeLists.txt

@@ -16,6 +16,8 @@ find_package(controller_manager REQUIRED)
 find_package(diagnostic_msgs REQUIRED)
 find_package(dynamixel_workbench_toolbox REQUIRED)
 find_package(hardware_interface REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(moveit_ros_planning REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(realtime_tools REQUIRED)
@@ -60,6 +62,8 @@ ament_target_dependencies(
   diagnostic_msgs
   dynamixel_workbench_toolbox
   hardware_interface
+  moveit_core
+  moveit_ros_planning
   pluginlib
   rclcpp
   realtime_tools

bitbots_lowlevel/bitbots_ros_control/include/bitbots_ros_control/dynamixel_servo_hardware_interface.hpp

@@ -70,6 +70,8 @@ class DynamixelServoHardwareInterface : public bitbots_ros_control::HardwareInte
   unsigned int joint_count_;
 
   std::vector<std::string> joint_names_;
+  std::vector<double> lower_joint_limits_;
+  std::vector<double> upper_joint_limits_;
 
   std::vector<double> goal_position_;
   std::vector<double> goal_effort_;

bitbots_lowlevel/bitbots_ros_control/src/dynamixel_servo_hardware_interface.cpp

@@ -1,3 +1,6 @@
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+
 #include <bitbots_ros_control/dynamixel_servo_hardware_interface.hpp>
 #include <bitbots_ros_control/utils.hpp>
 #include <utility>
@@ -63,6 +66,18 @@ bool DynamixelServoHardwareInterface::init() {
     joint_map_[joint_names_[i]] = i;
   }
 
+  // read lower and upper limits
+  robot_model_loader::RobotModelLoader rml(nh_, "robot_description", false);
+  moveit::core::RobotModelPtr model = rml.getModel();
+  lower_joint_limits_.resize(joint_count_);
+  upper_joint_limits_.resize(joint_count_);
+  for (const std::string &joint_name : joint_names_) {
+    moveit::core::JointModel *jm = model->getJointModel(joint_name);
+    // we use getVariableBounds()[0] because there is only a single variable for all of our joints
+    lower_joint_limits_[joint_map_[joint_name]] = jm->getVariableBounds()[0].min_position_;
+    upper_joint_limits_[joint_map_[joint_name]] = jm->getVariableBounds()[0].max_position_;
+  }
+
   std::string control_mode;
   control_mode = nh_->get_parameter("servos.control_mode").as_string();
   RCLCPP_INFO(nh_->get_logger(), "Control mode: %s", control_mode.c_str());
@@ -88,10 +103,19 @@ void DynamixelServoHardwareInterface::commandCb(const bitbots_msgs::msg::JointCo
     RCLCPP_ERROR(nh_->get_logger(), "Dynamixel Controller got command with inconsistent array lengths.");
     return;
   }
-  // todo improve performance
   for (unsigned int i = 0; i < command_msg.joint_names.size(); i++) {
     int joint_id = joint_map_[command_msg.joint_names[i]];
-    goal_position_[joint_id] = command_msg.positions[i];
+    if (command_msg.positions[i] > upper_joint_limits_[joint_id] ||
+        command_msg.positions[i] < lower_joint_limits_[joint_id]) {
+      RCLCPP_WARN_STREAM(nh_->get_logger(), "Invalid position for " << command_msg.joint_names[i] << ": "
+                                                                    << command_msg.positions[i] << " not in ("
+                                                                    << lower_joint_limits_[joint_id] << ", "
+                                                                    << upper_joint_limits_[joint_id] << ")");
+      goal_position_[joint_id] =
+          std::clamp(command_msg.positions[i], lower_joint_limits_[joint_id], upper_joint_limits_[joint_id]);
+    } else {
+      goal_position_[joint_id] = command_msg.positions[i];
+    }
     goal_velocity_[joint_id] = command_msg.velocities[i];
     goal_acceleration_[joint_id] = command_msg.accelerations[i];
     goal_effort_[joint_id] = command_msg.max_currents[i];

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_bringup/launch/rosbag_record.launch.py

@@ -43,7 +43,7 @@
     "/goal_pose",
     "/head_mode",
     "/imu_head/data",
-    "/imu/data_raw",
+    "/imu/data",
     "/joint_states",
     "/motion_odometry",
     "/move_base/current_goal",