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_pybullet_sim/package.xml b/bitbots_simulation/bitbots_pybullet_sim/package.xml
index f39988600..cbe9abf83 100644
--- a/bitbots_simulation/bitbots_pybullet_sim/package.xml
+++ b/bitbots_simulation/bitbots_pybullet_sim/package.xml
@@ -27,6 +27,7 @@
python3-numpy
+ ament_cmake
unknown
python
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 9b107b1e3..cce9a575f 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
@@ -10,7 +10,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
@@ -119,6 +119,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")
@@ -242,6 +245,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])