diff --git a/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/.gitignore b/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/.gitignore deleted file mode 100644 index ab08e78d3..000000000 --- a/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -# Ignore generate parameter library build artifacts -bitbots_technical_challenge_vision_params.py diff --git a/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/__init__.py b/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision.py b/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision.py deleted file mode 100755 index 7c7957aad..000000000 --- a/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision.py +++ /dev/null @@ -1,183 +0,0 @@ -from typing import Optional, Tuple - -import cv2 -import numpy as np -import rclpy -import rclpy.logging -from ament_index_python.packages import get_package_share_directory -from cv_bridge import CvBridge -from rclpy.node import Node -from sensor_msgs.msg import Image -from soccer_vision_2d_msgs.msg import Robot, RobotArray - -from bitbots_technical_challenge_vision.bitbots_technical_challenge_vision_params import ( - bitbots_technical_challenge_vision, -) - - -class TechnicalChallengeVision(Node): - def __init__(self): - super().__init__("bitbots_technical_challenge_vision") - - self._package_path = get_package_share_directory("bitbots_technical_challenge_vision") - self._cv_bridge = CvBridge() - self._annotations_pub = self.create_publisher(RobotArray, "/robots_in_image", 10) - self._debug_img_pub = self.create_publisher(Image, "/bitbots_technical_challenge_vision_debug_img", 10) - self._debug_clrmp_pub_blue = self.create_publisher( - Image, "/bitbots_technical_challenge_vision_debug_clrmp_blue", 10 - ) - self._debug_clrmp_pub_red = self.create_publisher( - Image, "/bitbots_technical_challenge_vision_debug_clrmp_red", 10 - ) - self._img_sub = self.create_subscription(Image, "/camera/image_proc", self.image_callback, 10) - self._param_listener = bitbots_technical_challenge_vision.ParamListener(self) - self._params = self._param_listener.get_params() - - def create_robot_msg(self, x: int, y: int, w: int, h: int, t: int) -> Robot: - """ - Creates a Robot message from a robots bounding box data and its color. - - :param x: bb top left x - :param y: bb top left y - :param w: bb width - :param h: bb height - :param t: robot team - :return: robot message for that robot - """ - robot = Robot() - - robot.bb.center.position.x = float(x + (w / 2)) - robot.bb.center.position.y = float(y + (h / 2)) - robot.bb.size_x = float(w) - robot.bb.size_y = float(h) - robot.attributes.team = t - - return robot - - def process_image( - self, img: np.ndarray, debug_img: np.ndarray, arg - ) -> Tuple[list[Robot], list[Robot], np.ndarray, np.ndarray]: - """ - gets annotations from the camera image - - :param img: ndarray containing the camera image - :param debug_img: copy of img debug annotations should be drawn here - :param arg: __RosParameters object containing the dynamic parameters - :return: [[blue_robots], [red_robots], clrmp_blue, clrmp_red] - """ - # convert img to hsv to isolate colors better - img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) - - # get color maps - blue_map = cv2.inRange( - img, - (arg.blue_lower_h, arg.blue_lower_s, arg.blue_lower_v), - (arg.blue_upper_h, arg.blue_upper_s, arg.blue_upper_v), - ) - - red_map = cv2.inRange( - img, - (arg.red_lower_h, arg.red_lower_s, arg.red_lower_v), - (arg.red_upper_h, arg.red_upper_s, arg.red_upper_v), - ) - - # get contours in color maps - blue_contours, _ = cv2.findContours(blue_map, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) - red_contours, _ = cv2.findContours(red_map, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) - - # get lists of bounding boxes - blue_robots = [] - red_robots = [] - - def annotate(x, y, w, h, c) -> Optional[np.ndarray]: - if not arg.debug_mode: - return None - return cv2.rectangle( - debug_img, - (x, y), - (x + w, y + h), - c, - 2, - ) - - for cnt in blue_contours: - x, y, w, h = cv2.boundingRect(cnt) - if min(w, h) >= arg.min_size and max(h, w) <= arg.max_size: - # draw bb on debug img - annotate(x, y, w, h, (255, 0, 0)) - - # TODO I think 1 is for the blue team? - blue_robots.append(self.create_robot_msg(x, y, w, h, 1)) - - for cnt in red_contours: - x, y, w, h = cv2.boundingRect(cnt) - if min(w, h) >= arg.min_size and max(h, w) <= arg.max_size: - # draw bb on debug img - annotate(x, y, w, h, (0, 0, 255)) - - red_robots.append(self.create_robot_msg(x, y, w, h, 2)) - - return blue_robots, red_robots, blue_map, red_map, debug_img - - def image_callback(self, msg: Image): - # get dynamic parameters - if self._param_listener.is_old(self._params): - self._param_listener.refresh_dynamic_parameters() - self._params = self._param_listener.get_params() - - arg = self._params - - # set variables - img = self._cv_bridge.imgmsg_to_cv2(img_msg=msg, desired_encoding="bgr8") - header = msg.header - - if arg.debug_mode: - debug_img = np.copy(img) - else: - debug_img = None - - # get annotations - blue_robots, red_robots, blue_map, red_map, debug_img = self.process_image(img, debug_img, arg) - robots = [] - robots.extend(blue_robots) - robots.extend(red_robots) - - # make output message - robot_array_message = RobotArray() - robot_array_message.header = header - robot_array_message.robots = robots - - # publish output message - self._annotations_pub.publish(robot_array_message) - - if arg.debug_mode: - # make debug image message - debug_img_msg = self._cv_bridge.cv2_to_imgmsg(cvim=debug_img, encoding="bgr8", header=header) - - # publish debug image - self._debug_img_pub.publish(debug_img_msg) - - # make color map messages - clrmp_blue_img = cv2.cvtColor(blue_map, cv2.COLOR_GRAY2BGR) - clrmp_blue_msg = self._cv_bridge.cv2_to_imgmsg(cvim=clrmp_blue_img, encoding="bgr8", header=header) - - clrmp_red_img = cv2.cvtColor(red_map, cv2.COLOR_GRAY2BGR) - clrmp_red_msg = self._cv_bridge.cv2_to_imgmsg(clrmp_red_img, encoding="bgr8", header=header) - - # publish color map messages - self._debug_clrmp_pub_blue.publish(clrmp_blue_msg) - self._debug_clrmp_pub_red.publish(clrmp_red_msg) - - -def main(args=None): - rclpy.init(args=args) - node = TechnicalChallengeVision() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - node.destroy_node() - - -if __name__ == "__main__": - main() diff --git a/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml b/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml deleted file mode 100644 index 6716d55af..000000000 --- a/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml +++ /dev/null @@ -1,118 +0,0 @@ -bitbots_technical_challenge_vision: - blue_lower_h: { - type: int, - default_value: 92, - description: "hue value of the lower boundary for blue obstacles", - validation: { - bounds<>: [0, 179] - } - } - blue_upper_h: { - type: int, - default_value: 110, - description: "hue value of the upper boundary for blue obstacles", - validation: { - bounds<>: [0, 179] - } - } - blue_lower_s: { - type: int, - default_value: 90, - description: "separation value of the lower boundary for blue obstacles", - validation: { - bounds<>: [0, 255] - } - } - blue_upper_s: { - type: int, - default_value: 236, - description: "separation value of the upper boundary for blue obstacles", - validation: { - bounds<>: [0, 255] - } - } - blue_lower_v: { - type: int, - default_value: 0, - description: "value value of the lower boundary for blue obstacles", - validation: { - bounds<>: [0, 255] - } - } - blue_upper_v: { - type: int, - default_value: 255, - description: "value value of the upper boundary for blue obstacles", - validation: { - bounds<>: [0, 255] - } - } - red_lower_h: { - type: int, - default_value: 138, - description: "hue value of the lower boundary for red obstacles", - validation: { - bounds<>: [0, 179] - } - } - red_upper_h: { - type: int, - default_value: 179, - description: "hue value of the upper boundary for red obstacles", - validation: { - bounds<>: [0, 179] - } - } - red_lower_s: { - type: int, - default_value: 78, - description: "separation value of the lower boundary for red obstacles", - validation: { - bounds<>: [0, 255] - } - } - red_upper_s: { - type: int, - default_value: 255, - description: "separation value of the upper boundary for red obstacles", - validation: { - bounds<>: [0, 255] - } - } - red_lower_v: { - type: int, - default_value: 0, - description: "value value of the lower boundary for red obstacles", - validation: { - bounds<>: [0, 255] - } - } - red_upper_v: { - type: int, - default_value: 255, - description: "value value of the upper boundary for red obstacles", - validation: { - bounds<>: [0, 255] - } - } - min_size: { - type: int, - default_value: 20, - description: "minimum size of an obstacle to be considered", - validation: { - gt_eq<>: 0 - } - } - max_size: { - type: int, - default_value: 400, - description: "maximum size of an obstacle to be considered", - validation: { - gt_eq<>: 0 - } - } - debug_mode: { - type: bool, - default_value: false, - description: "set true if debug images should be drawn and published", - } diff --git a/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch b/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch deleted file mode 100644 index 1d36dc25f..000000000 --- a/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/bitbots_misc/bitbots_technical_challenge_vision/package.xml b/bitbots_misc/bitbots_technical_challenge_vision/package.xml deleted file mode 100644 index 9c0a53d6e..000000000 --- a/bitbots_misc/bitbots_technical_challenge_vision/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - bitbots_technical_challenge_vision - 0.0.0 - TODO: Package description - par - TODO: License declaration - - rclpy - python3-opencv - python3-numpy - cv_bridge - sensor_msgs - soccer_vision_2d_msgs - - generate_parameter_library - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/bitbots_misc/bitbots_technical_challenge_vision/resource/bitbots_technical_challenge_vision b/bitbots_misc/bitbots_technical_challenge_vision/resource/bitbots_technical_challenge_vision deleted file mode 100644 index e69de29bb..000000000 diff --git a/bitbots_misc/bitbots_technical_challenge_vision/setup.cfg b/bitbots_misc/bitbots_technical_challenge_vision/setup.cfg deleted file mode 100644 index 34ccaf1e9..000000000 --- a/bitbots_misc/bitbots_technical_challenge_vision/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/bitbots_technical_challenge_vision -[install] -install_scripts=$base/lib/bitbots_technical_challenge_vision diff --git a/bitbots_misc/bitbots_technical_challenge_vision/setup.py b/bitbots_misc/bitbots_technical_challenge_vision/setup.py deleted file mode 100644 index cfe0915aa..000000000 --- a/bitbots_misc/bitbots_technical_challenge_vision/setup.py +++ /dev/null @@ -1,35 +0,0 @@ -import glob - -from generate_parameter_library_py.setup_helper import generate_parameter_module -from setuptools import find_packages, setup - -package_name = "bitbots_technical_challenge_vision" - -setup( - name=package_name, - version="0.0.0", - packages=find_packages(exclude=["test"]), - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - ("share/" + package_name + "/config", glob.glob("config/*.yaml")), - ("share/" + package_name + "/launch", glob.glob("launch/*.launch")), - ], - install_requires=["setuptools"], - tests_require=["pytest"], - zip_safe=True, - maintainer="par", - maintainer_email="paer-wiegmann@gmx.de", - description="This Package provides a simple vision to detect the obstacles for the obstacle avoidance challenge.", - license="MIT", - entry_points={ - "console_scripts": [ - "bitbots_technical_challenge_vision = bitbots_technical_challenge_vision.bitbots_technical_challenge_vision:main", - ], - }, -) - -generate_parameter_module( - "bitbots_technical_challenge_vision_params", - "config/range.yaml", -) diff --git a/bitbots_misc/bitbots_teleop/scripts/teleop_keyboard.py b/bitbots_misc/bitbots_teleop/scripts/teleop_keyboard.py index 885df006e..341518df0 100755 --- a/bitbots_misc/bitbots_teleop/scripts/teleop_keyboard.py +++ b/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) diff --git a/bitbots_msgs/CMakeLists.txt b/bitbots_msgs/CMakeLists.txt index 61884419f..74c50a259 100644 --- a/bitbots_msgs/CMakeLists.txt +++ b/bitbots_msgs/CMakeLists.txt @@ -50,6 +50,7 @@ rosidl_generate_interfaces( "srv/SetObjectPose.srv" "srv/SetObjectPosition.srv" "srv/SetTeachingMode.srv" + "srv/SimulatorPush.srv" DEPENDENCIES action_msgs geometry_msgs diff --git a/bitbots_msgs/srv/SimulatorPush.srv b/bitbots_msgs/srv/SimulatorPush.srv new file mode 100644 index 000000000..db5f753f7 --- /dev/null +++ b/bitbots_msgs/srv/SimulatorPush.srv @@ -0,0 +1,4 @@ +geometry_msgs/Vector3 force +bool relative +string robot "amy" +--- diff --git a/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_supervisor_controller.py b/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_supervisor_controller.py index 40211b63d..e2b51cb70 100644 --- a/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_supervisor_controller.py +++ b/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_supervisor_controller.py @@ -8,7 +8,7 @@ from rosgraph_msgs.msg import Clock from std_srvs.srv import Empty -from bitbots_msgs.srv import SetObjectPose, SetObjectPosition +from bitbots_msgs.srv import SetObjectPose, SetObjectPosition, SimulatorPush G = 9.81 @@ -117,6 +117,9 @@ def __init__( self.set_ball_position_service = self.ros_node.create_service( SetObjectPosition, base_ns + "set_ball_position", self.ball_pos_callback ) + self.simulator_push_service = self.ros_node.create_service( + SimulatorPush, base_ns + "simulator_push", self.simulator_push + ) self.world_info = self.supervisor.getFromDef("world_info") self.ball = self.supervisor.getFromDef("ball") @@ -240,6 +243,10 @@ def robot_pose_callback(self, request=None, response=None): ) return response or SetObjectPose.Response() + def simulator_push(self, request=None, response=None): + self.robot_nodes[request.robot].addForce([request.force.x, request.force.y, request.force.z], request.relative) + return response or Empty.Response() + def reset_ball(self, request=None, response=None): self.ball.getField("translation").setSFVec3f([0, 0, 0.0772]) self.ball.getField("rotation").setSFRotation([0, 0, 1, 0]) diff --git a/sync_includes_wolfgang_nuc.yaml b/sync_includes_wolfgang_nuc.yaml index b07e22d3f..024bb81bb 100644 --- a/sync_includes_wolfgang_nuc.yaml +++ b/sync_includes_wolfgang_nuc.yaml @@ -14,7 +14,6 @@ include: - bitbots_ipm - bitbots_parameter_blackboard - bitbots_robot_description - - bitbots_technical_challenge_vision - bitbots_teleop - bitbots_tf_buffer - bitbots_tts