def publish_marker(self, e):
# construct GoalRelative message
goal = GoalRelative()
goal.header.stamp = rospy.get_rostime()
goal.header.frame_id = "map"
goal.confidence = 1.0
# calculate the positions of the right and the left post
orientation = euler_from_quaternion(
(self.pose.orientation.x, self.pose.orientation.y,
self.pose.orientation.z, self.pose.orientation.w))
angle = orientation[2]
left_post = Point()
left_post.x = self.pose.position.x - math.sin(angle) * GOAL_WIDTH / 2
left_post.y = self.pose.position.y + math.cos(angle) * GOAL_WIDTH / 2
right_post = Point()
right_post.x = self.pose.position.x + math.sin(angle) * GOAL_WIDTH / 2
right_post.y = self.pose.position.y - math.cos(angle) * GOAL_WIDTH / 2
goal.left_post = left_post
goal.right_post = right_post
# publish the new goal position
if self.publish:
self.publisher.publish(goal)
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 goal_rel_msg(self, lx, ly, rx, ry, cx, cy, c):
msg = GoalRelative()
msg.header.stamp = rospy.Time.now()
msg.left_post.x = lx
msg.left_post.y = ly
msg.right_post.x = rx
msg.right_post.y = ry
msg.center_direction.x = cx
msg.center_direction.y = cy
msg.confidence = c
return msg
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 _callback_goal(self, msg):
if self.camera_info is None:
self.warn_camera_info()
return
field = self.get_plane(msg.header.stamp, 0.0, "base_footprint")
if field is None:
return
goal = GoalRelative()
goal.header.stamp = msg.header.stamp
goal.header.frame_id = self.publish_frame
transformed_left = self.transform(msg.left_post.foot_point, field,
msg.header.stamp)
if transformed_left is None:
rospy.logwarn_throttle(
5.0, "Got a left post with foot point (" +
str(msg.left_post.foot_point.x) +
str(msg.left_post.foot_point.y) + ") I could not transform.")
else:
goal.left_post = transformed_left
# Messages do not contain None values so the coordinates have to be checked
if msg.right_post.foot_point.x != 0 and msg.right_post.foot_point.y != 0:
transformed_right = self.transform(msg.right_post.foot_point,
field, msg.header.stamp)
if transformed_right is None:
rospy.logwarn_throttle(
5.0, "Got a left post with foot point (" +
str(msg.left_post.foot_point.x) +
str(msg.left_post.foot_point.y) +
") I could not transform.")
else:
goal.right_post = transformed_right
#TODO evaluate whether we need center direction
#goal.center_direction.x = goal.left_post.x + (goal.right_post.x - goal.left_post.x) / 2.0
#goal.center_direction.y = goal.left_post.y + (goal.right_post.y - goal.left_post.y) / 2.0
goal.confidence = msg.confidence
self.goal_relative_pub.publish(goal)
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 perform(self):
new_goal = GoalRelative()
new_goal.header.frame_id = 'odom'
new_goal.header.stamp = rospy.Time.now()
if self.goal_changing:
new_goal.right_post = Point(2 + random.random(),
1 + random.random(),
0 + random.random())
new_goal.left_post = Point(2 + random.random(),
1 + random.random(),
0 + random.random())
else:
new_goal.right_post = Point(2, 1, 0)
new_goal.left_post = Point(1.5, 0.7, 0.5)
new_goal.confidence = random.random()
self.goal_seen_pub.publish(new_goal)
def publish_goals():
global tf_buffer
global cam_info
global pubGoals #todo really necessary?!
detected_goals = []
goals_msg = GoalRelative()
goals = ["goalLB", "goalLT", "goalRB", "goalRT"]
for goal in goals:
try:
trans = tfBuffer.lookup_transform("camera_optical_frame",
str(goal), rospy.Time(0))
corner_stamped = PoseStamped()
corner_stamped.header.frame_id = "camera_optical_frame"
corner_stamped.pose.position.x = trans.transform.translation.x
corner_stamped.pose.position.y = trans.transform.translation.y
corner_stamped.pose.position.z = trans.transform.translation.z
corner_stamped.pose.orientation = trans.transform.rotation
p = [
corner_stamped.pose.position.x, corner_stamped.pose.position.y,
corner_stamped.pose.position.z
]
k = np.reshape(cam_info.K, (3, 3))
p_pixel = np.matmul(k, p)
if p_pixel[2] > 0:
p_pixel = p_pixel * (1 / p_pixel[2])
if p_pixel[0] > 0 and p_pixel[0] <= cam_info.width and p_pixel[
1] > 0 and p_pixel[1] <= cam_info.height:
if np.random.uniform(10) < 8 or True:
pose = tfBuffer.transform(
corner_stamped,
"base_footprint",
timeout=Duration(seconds=0.5))
point_msg = Point()
point_msg.x = add_gaussian_noise(pose.pose.position.x)
point_msg.y = add_gaussian_noise(pose.pose.position.y)
point_msg.z = pose.pose.position.z #todo set to 0?
detected_goals.append(point_msg)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
if len(detected_goals) == 1:
goals_msg.left_post = detected_goals[0]
goals_msg.right_post = detected_goals[0]
elif len(detected_goals) == 2:
goals_msg.left_post = detected_goals[0]
goals_msg.right_post = detected_goals[1]
goals_msg.header.stamp = self.get_clock().now()
pubGoals.publish(goals_msg)