def main(): # We need to initalize ROS environment for Robot and camera to connect/communicate ROSEnvironment() # Initalize robot robot = Robot() # Start robot robot.start() robot.center() time.sleep(1) #TODO: change the values in left robot.left(0.2) time.sleep(1)#wait a second #TODO: change the values in right robot.right(0.2) time.sleep(1)
class BallTrackingSystem: def __init__(self): self.initParam() self.pan_speed = 0 self.tilt_speed = 0 # init self.robot = Robot() self.ball_detector = BallDetector() self.camera = Camera(self.imageCallback) # move robot head to center self.robot.center() # load timer rospy.Timer(rospy.Duration(1.0 / self.rate), self.robotUpdate) def robotUpdate(self, event): # self.robot.test() self.robot.move(self.pan_speed, self.tilt_speed) def imageCallback(self, frame): results = self.ball_detector.detect(frame, 640) frame = results[0] if results[1] != None: self.set_joint_cmd(results) return frame def set_joint_cmd(self, msg): ball = msg[1] frame = msg[0] (image_height, image_width) = frame.shape[:2] target_offset_x = ball[0] - image_width / 2 target_offset_y = ball[1] - image_height / 2 try: percent_offset_x = float(target_offset_x) / (float(image_width) / 2.0) percent_offset_y = float(target_offset_y) / (float(image_height) / 2.0) except: percent_offset_x = 0 percent_offset_y = 0 if abs(percent_offset_x) > self.pan_threshold: if target_offset_x > 0: self.pan_speed = min( self.max_joint_speed, max(0, self.gain_pan * abs(percent_offset_x))) else: self.pan_speed = -1 * min( self.max_joint_speed, max(0, self.gain_pan * abs(percent_offset_x))) else: self.pan_speed = 0 if abs(percent_offset_y) > self.tilt_threshold: if target_offset_y > 0: self.tilt_speed = min( self.max_joint_speed, max(0, self.gain_tilt * abs(percent_offset_y))) else: self.tilt_speed = -1 * min( self.max_joint_speed, max(0, self.gain_tilt * abs(percent_offset_y))) else: self.tilt_speed = 0 def initParam(self): self.rate = rospy.get_param("~rate", 20) # Joint speeds are given in radians per second self.max_joint_speed = rospy.get_param('~max_joint_speed', 0.1) # The pan/tilt thresholds indicate what percentage of the image window # the ROI needs to be off-center before we make a movement self.pan_threshold = rospy.get_param("~pan_threshold", 0.05) self.tilt_threshold = rospy.get_param("~tilt_threshold", 0.05) # The gain_pan and gain_tilt parameter determine how responsive the # servo movements are. If these are set too high, oscillation can result. self.gain_pan = rospy.get_param("~gain_pan", 1.0) self.gain_tilt = rospy.get_param("~gain_tilt", 1.0) self.max_pan_angle_radian = rospy.get_param("~max_pan_angle_radian", 1.0) self.max_tilt_angle_radian = rospy.get_param("~max_tilt_angle_radian", 1.0)