Add service to Webots sim interface for pushing robot and client to Teleop script

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

bitbots_msgs/srv/SimulatorPush.srv

@@ -0,0 +1,4 @@
+geometry_msgs/Vector3 force
+bool relative
+string robot "amy"
+---

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