Here’s our python implementation of Visual Servoing Control Node:
import numpy as np
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Point
class VisualServoNode(Node):
def __init__(self, node_name="visual_servo_node"):
super().__init__(node_name=node_name)
self.paddle_z_offset = 0.19 # Distance from center of paddle to EE tip is 19cm
self.paddle_y = -1.65 # Half-table length is 137cm, but we add a bit extra
# to avoid collision with the table
self.prev_targets = [] # The low pass filter is a moving average with
self.moving_average_size = 5 # size 5.
self.bse_sub = self.create_subscription( # Subscribes to the ball position estimate.
Point, "/ball_state_estimate", self.on_bse, 10
)
self.target_pub = self.create_publisher( # Publishes a target position.
Point, "/target_prediction", 10
)
def on_bse(self, ball):
target = Point()
target.x = ball.x / 100 # Convert x from cm to m.
target.y = self.paddle_y # Keeps the y coordinate of the paddle constant.
target.z = (ball.z / 100) - self.paddle_z_offset # Convert z from cm to m, and shift by an offset.
target.x = max(min(target.x, 0.6), -0.6) # Keeps x coordinate of paddle between -0.6 and +0.6.
target.z = max(min(target.z, 0.85), -0.1) # Keeps z coordinate of paddle between 0.35 and 1.3
self.prev_targets.append(target) # (the coordinates are in the world frame).
if len(self.prev_targets) < self.moving_average_size:
return
if len(self.prev_targets) > self.moving_average_size:
self.prev_targets.pop(0)
final_target = Point() # Computes the moving average over the last 5 targets.
final_target.x = sum(t.x for t in self.prev_targets) / self.moving_average_size
final_target.y = sum(t.y for t in self.prev_targets) / self.moving_average_size
final_target.z = sum(t.z for t in self.prev_targets) / self.moving_average_size
self.target_pub.publish(final_target)
def main(args=None):
rclpy.init(args=args)
visual_servo_node = VisualServoNode()
rclpy.spin(visual_servo_node)
rclpy.shutdown()