def __init__(self, field_length, field_width, goal_width):
self.position = Position2D()
self.tf_buffer = tf2.Buffer(cache_time=rospy.Duration(30))
self.tf_listener = tf2.TransformListener(self.tf_buffer)
self.ball = PointStamped() # The ball in the base footprint frame
self.goal = GoalRelative()
self.goal_odom = GoalRelative()
self.goal_odom.header.stamp = rospy.Time.now()
self.goal_odom.header.frame_id = 'odom'
self.obstacles = ObstaclesRelative()
self.my_data = dict()
self.counter = 0
self.ball_seen_time = rospy.Time(0)
self.goal_seen_time = rospy.Time(0)
self.ball_seen = False
self.field_length = field_length
self.field_width = field_width
self.goal_width = goal_width
# Publisher for Visualisation in RViZ
self.ball_publisher = rospy.Publisher('/debug/viz_ball',
PointStamped,
queue_size=1)
self.goal_publisher = rospy.Publisher('/debug/viz_goal',
GoalRelative,
queue_size=1)
def position_callback(self, pos):
# Convert PositionWithCovarianceStamped to Position2D
position2d = Position2D()
position2d.header = pos.header
position2d.pose.x = pos.pose.pose.position.x
position2d.pose.y = pos.pose.pose.position.y
rotation = pos.pose.pose.orientation
position2d.pose.theta = euler_from_quaternion(
[rotation.x, rotation.y, rotation.z, rotation.w])[2]
self.position = position2d
def __init__(self, config):
self.position = Position2D()
self.tf_buffer = tf2.Buffer(cache_time=rospy.Duration(5.0))
self.tf_listener = tf2.TransformListener(self.tf_buffer)
self.ball = PointStamped() # The ball in the base footprint frame
self.goal = GoalRelative()
self.obstacles = ObstaclesRelative()
self.my_data = dict()
self.counter = 0
self.field_length = config["Body"]["Common"]["Field"]["length"]
self.goal_width = config["Body"]["Common"]["Field"]["length"]
def add_robot(self, name):
self.robots_pos[name] = Position2D()
# define rpos to shorten the code
rpos = self.robots_pos[name]
# randomlize the robots_pos
rpos.header.stamp = rospy.Time.now()
rpos.header.frame_id = "map"
rpos.pose.x = 9.0 * random.random() - 4.5
rpos.pose.y = 6.0 * random.random() - 3.0
rpos.pose.theta = 2 * PI * random.random()
rpos.confidence = 1.0
def __init__(self):
self.robots_pos = dict()
self.ball_pos = Position2D()
# both the messages are JSON ecoded msg
self.rpos_pub = rospy.Publisher("/robots_pos", String, queue_size=2)
self.bpos_pub = rospy.Publisher("/ball_pos", String, queue_size=2)
# randomlize the ball_pos
self.ball_pos.header.stamp = rospy.Time.now()
self.ball_pos.header.frame_id = "map"
self.ball_pos.pose.x = 9.0 * random.random() - 4.5
self.ball_pos.pose.y = 6.0 * random.random() - 3.0
self.ball_pos.confidence = 1.0
def __init__(self, field_length, field_width):
self.position = Position2D()
self.tf_buffer = tf2.Buffer(cache_time=rospy.Duration(30))
self.tf_listener = tf2.TransformListener(self.tf_buffer)
self.ball = PointStamped() # The ball in the base footprint frame
self.goal = GoalRelative()
self.goal_odom = GoalRelative()
self.goal_odom.header.stamp = rospy.Time.now()
self.goal_odom.header.frame_id = 'odom'
self.obstacles = ObstaclesRelative()
self.my_data = dict()
self.counter = 0
self.ball_seen_time = rospy.Time(0)
self.goal_seen_time = rospy.Time(0)
self.field_length = field_length
self.field_width = field_width
def position_msg(self, x, y):
msg = Position2D()
msg.pose.x = x
msg.pose.y = y
return msg
def __init__(self):
# sorry for use BallRelative in this place, but it is the
self.cached_ballpos = Position2D()
#this bpos_pub publishes whether the robot think it sees the ball and the relative position of the ball
self.bpos_pub = rospy.Publisher('/ball_relative', BallRelative, queue_size = 2)
rospy.Subscriber('/ball_pos', String, self.ballpos_callback)