def _callback_obstacles(self, msg: ObstacleInImageArray):
field = self.get_plane(msg.header.stamp, 0.0)
if field is None:
return
obstacles = ObstacleRelativeArray()
obstacles.header = msg.header
obstacles.header.frame_id = self._publish_frame
for o in msg.obstacles:
obstacle = ObstacleRelative()
obstacle.playerNumber = o.playerNumber
obstacle.pose.confidence = o.confidence
obstacle.type = o.type
point = Point()
point.x = o.top_left.x + o.width / 2
point.y = o.top_left.y + o.height
# Check if obstacle is not going out of the image at the bottom
if not self._object_at_bottom_of_image(
point.y, self._obstacle_footpoint_out_of_image_threshold):
position = self._transform_point(point, field,
msg.header.stamp)
if position is not None:
obstacle.pose.pose.pose.position = position
obstacles.obstacles.append(obstacle)
else:
rospy.logwarn_throttle(
5.0,
rospy.get_name() +
": Got an obstacle I could not transform")
self._obstacle_relative_pub.publish(obstacles)
def get_relative_msg(self):
# check if obstacle is also visible for the robot and only then return
if self.cam_info:
try:
obstacle_point_stamped = PointStamped()
obstacle_point_stamped.header.stamp = rospy.Time.now()
obstacle_point_stamped.header.frame_id = "map"
obstacle_point_stamped.point = self.pose.position
obstacle_in_camera_optical_frame = tf_buffer.transform(
obstacle_point_stamped,
self.cam_info["frame_id"],
timeout=rospy.Duration(0.5))
if obstacle_in_camera_optical_frame.point.z >= 0:
p = [
obstacle_in_camera_optical_frame.point.x,
obstacle_in_camera_optical_frame.point.y,
obstacle_in_camera_optical_frame.point.z
]
k = np.reshape(self.cam_info["K"], (3, 3))
p_pixel = np.matmul(k, p)
p_pixel = p_pixel * (1 / p_pixel[2])
# make sure that the transformed pixel is inside the resolution and positive.
if 0 < p_pixel[0] <= self.cam_info["width"] and 0 < p_pixel[
1] <= self.cam_info["height"]:
obstacle_relative = ObstacleRelative()
ball_in_footprint_frame = tf_buffer.transform(
obstacle_in_camera_optical_frame,
"base_footprint",
timeout=rospy.Duration(0.5))
obstacle_relative.pose.pose.pose.position = ball_in_footprint_frame.point
obstacle_relative.pose.confidence = self.confidence
obstacle_relative.playerNumber = self.player_number
obstacle_relative.type = self.type
return obstacle_relative
except tf2_ros.LookupException as ex:
rospy.logwarn_throttle(10.0, rospy.get_name() + ": " + str(ex))
return None
except tf2_ros.ExtrapolationException as ex:
rospy.logwarn_throttle(10.0, rospy.get_name() + ": " + str(ex))
return None
return None
def _callback_obstacles(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
obstacles = ObstaclesRelative()
obstacles.header = msg.header
obstacles.header.frame_id = self.publish_frame
for o in msg.obstacles:
obstacle = ObstacleRelative()
obstacle.playerNumber = o.playerNumber
obstacle.confidence = o.confidence
obstacle.color = o.color
point = Point()
point.x = o.top_left.x + o.height
point.y = o.top_left.y + o.width / 2
obstacle.position = self.transform(point, field, msg.header.stamp)
obstacles.obstacles.append(obstacle)
self.obstacle_relative_pub.publish(obstacles)
def obstacle_rel_msg(self, x, y, w, h, t, c):
msg = ObstacleRelative()
msg.position.x = x
msg.position.y = y
msg.width = w
msg.height = h
msg.color = t
msg.confidence = c
return msg
def get_absolute_message(self):
msg = ObstacleRelative()
msg.pose.pose.pose.position = self.pose.position
msg.height = OBSTACLE_HEIGT
msg.width = OBSTACLE_DIAMETER
msg.type = self.type
msg.pose.confidence = self.confidence
msg.playerNumber = self.player_number
return msg
def publisher_main():
#initiieren des publishers
rospy.init_node('publisher')
print('started publisher node')
pub = rospy.Publisher('ball_relative', BallRelative, queue_size=10)
pubRobo = rospy.Publisher('amcl_pose',
PoseWithCovarianceStamped,
queue_size=10)
pubTeam = rospy.Publisher('obstacles_relative',
ObstaclesRelative,
queue_size=10)
pubStrategy = rospy.Publisher('strategy', Strategy, queue_size=10)
pubGame = rospy.Publisher('gamestate', GameState, queue_size=10)
pubState = rospy.Publisher('robot_state', RobotControlState, queue_size=10)
pubTarget = rospy.Publisher('move_base_simple/goal', Pose2D, queue_size=10)
rate = rospy.Rate(10)
timeCounter = 30
roboActionCounter = 30
firsthalf = True
durationHalfGame = 60
# Coordinates from pathMaker ========================================================================================
# robo1 with pathmaker
robo1 = getCoordinates("robo4.yaml")
robo1Length = len(robo1)
robo1Counter = 1
# teammates with pathmaker
teammate1 = getCoordinates('TeamClubMate1.yaml')
team1Length = len(teammate1) #anzahl eintraege
team1Counter = 1
teammate2 = getCoordinates('TeamClubMate2.yaml')
team2Length = len(teammate2)
team2Counter = 1
# opponents with pathmaker
opponent1 = getCoordinates('SuperScaryOpponent.yaml')
op1Length = len(opponent1)
op1Counter = 1
# opponents with pathmaker
undef = getCoordinates('undef.yaml')
undefLength = len(opponent1)
undefCounter = 1
# ball with pathmaker
ball = getCoordinates('HeartBall.yaml')
ballLength = len(ball)
ballCounter = 1
#teammate1[0 % length ].get('x') # fuer 0 ein counter, dann entsteht loop
#teammate1[1].get('x') # an der ersten Stelle x-wert
while not rospy.is_shutdown():
# Ball with pathmaker
msgBall = BallRelative()
msgBall.header.stamp = rospy.Time.now()
msgBall.header.frame_id = "base_link"
msgBall.ball_relative.y = ball[ballCounter % ballLength].get('x')
msgBall.ball_relative.x = ball[ballCounter % ballLength].get('y')
msgBall.confidence = 1.0
pub.publish(msgBall)
ballCounter += 1
# Robo1 with pathmaker
msgRobo = PoseWithCovarianceStamped()
msgRobo.header.stamp = rospy.Time.now()
msgRobo.pose.pose.position.x = robo1[int(robo1Counter) %
robo1Length].get('x')
msgRobo.pose.pose.position.y = robo1[int(robo1Counter) %
robo1Length].get('y')
# Angle of robot in quaternions
angle = robo1[int(robo1Counter) % robo1Length].get('ang')
quaternion = tf.transformations.quaternion_from_euler(
0, 0, float(angle))
msgRobo.pose.pose.orientation.x = quaternion[0]
msgRobo.pose.pose.orientation.y = quaternion[1]
msgRobo.pose.pose.orientation.z = quaternion[2]
msgRobo.pose.pose.orientation.w = quaternion[3]
pubRobo.publish(msgRobo)
# Role of Robo1, gets information from pathMaker
msgStrategy = Strategy()
msgRoleString = robo1[int(robo1Counter) % robo1Length].get('action')
msgStrategy.role = actionDecoder.get(
msgRoleString
) #actiondecoder gleicht den string ab mit dictonary und gibt int zurueck
# Action of Robo1, changes after short time (roboActionCounter)
if roboActionCounter == 0:
msgStrategy.action = 3 # TRYING_TO_SCORE
else:
msgStrategy.action = 2 # GOING_TO_BALL
pubStrategy.publish(msgStrategy)
roboActionCounter -= 1
roboActionCounter = max(roboActionCounter, 0)
robo1Counter += 1
# Teammates with pathmaker, contains list of teammates
msgTeam = ObstaclesRelative()
msgTeam1 = ObstacleRelative()
msgTeam1.color = 2 # magenta
msgTeam1.position.x = teammate1[int(team1Counter) %
team1Length].get('x')
msgTeam1.position.y = teammate1[int(team1Counter) %
team1Length].get('y')
msgTeam2 = ObstacleRelative()
msgTeam2.color = 2 # magenta
msgTeam2.position.x = teammate2[int(team2Counter) %
team2Length].get('x')
msgTeam2.position.y = teammate2[int(team2Counter) %
team2Length].get('y')
# Opponents with pathmaker, contains list of opponents
msgOp = ObstaclesRelative()
msgUndef = ObstacleRelative()
msgUndef.color = 1 # undef
msgUndef.position.x = undef[int(undefCounter) % undefLength].get('x')
msgUndef.position.y = undef[int(undefCounter) % undefLength].get('y')
msgOp1 = ObstacleRelative()
msgOp1.color = 3 # cyan
msgOp1.position.x = opponent1[int(op1Counter) % op1Length].get('x')
msgOp1.position.y = opponent1[int(op1Counter) % op1Length].get('y')
# Publish all obstacles
msgTeam.obstacles = [msgTeam1, msgTeam2, msgOp1, msgUndef]
pubTeam.publish(msgTeam)
team1Counter += 1
team2Counter += 1
op1Counter += 1
undefCounter += 1
# GameState msgs ===========================================================================================
# Penalty: Seconds till unpenalized and boolean
msgGame = GameState()
msgBall.header.stamp = rospy.Time.now()
msgGame.secondsTillUnpenalized = timeCounter
# Penalty boolean
msgGame.penalized = timeCounter > 0
# Sets halftime and rest secs
msgGame.firstHalf = firsthalf
msgGame.secondsRemaining = durationHalfGame
# Sets Score
msgGame.ownScore = 7
msgGame.rivalScore = 1
# team colors
msgGame.teamColor = 1 # magenta
pubGame.publish(msgGame)
timeCounter -= 1
timeCounter = max(timeCounter, 0)
durationHalfGame -= 1
if durationHalfGame == 0:
durationHalfGame = 60
firsthalf = False
# Sets hardware state
msgState = RobotControlState()
msgState.state = 10
pubState.publish(msgState)
# Target
msgTarget = Pose2D()
if firsthalf:
msgTarget.x = 3.5
msgTarget.y = 2.0
else:
msgTarget.x = 2.0
msgTarget.y = 1.0
pubTarget.publish(msgTarget)
rate.sleep()
import rospy
from geometry_msgs.msg import PoseWithCovariance
from humanoid_league_msgs.msg import ObstacleRelativeArray, ObstacleRelative, PoseWithCertaintyArray, PoseWithCertainty
if __name__ == '__main__':
rospy.init_node("TeamCommTest")
ball_pub = rospy.Publisher("balls_relative", PoseWithCertaintyArray, queue_size=1)
position_pub = rospy.Publisher("pose_with_certainty", PoseWithCertainty, queue_size=1)
obstacle_pub = rospy.Publisher("obstacles_relative", ObstacleRelativeArray, queue_size=1)
position_msg = PoseWithCertainty()
position_msg.pose.pose.position.x = 2
position_msg.confidence = 0.7
obstacle_msg = ObstacleRelativeArray()
obstacle = ObstacleRelative()
obstacle.pose.pose.pose.position.x = 4
obstacle.type = 2
obstacle_msg.obstacles.append(obstacle)
obstacle2 = ObstacleRelative()
obstacle2.pose.pose.pose.position.x = 2
obstacle2.type = 2
obstacle_msg.obstacles.append(obstacle2)
ball_msg = PoseWithCertaintyArray()
ball = PoseWithCertainty()
ball.confidence = 1.0
ball_position = PoseWithCovariance()
ball_position.pose.position.x = 3
ball.pose = ball_position
ball_msg.poses.append(ball)
def handle_message(self, msg):
def covariance_proto_to_ros(fmat3, ros_covariance):
# ROS covariance is row-major 36 x float, protobuf covariance is column-major 9 x float [x, y, θ]
ros_covariance[0] = fmat3.x.x
ros_covariance[1] = fmat3.y.x
ros_covariance[5] = fmat3.z.x
ros_covariance[6] = fmat3.x.y
ros_covariance[7] = fmat3.y.y
ros_covariance[11] = fmat3.z.y
ros_covariance[30] = fmat3.x.z
ros_covariance[31] = fmat3.y.z
ros_covariance[35] = fmat3.z.z
def pose_proto_to_ros(robot, pose):
pose.pose.position.x = robot.position.x
pose.pose.position.y = robot.position.y
quat = transforms3d.euler.euler2quat(
0.0, 0.0, robot.position.z) # wxyz -> ros: xyzw
pose.pose.orientation.x = quat[1]
pose.pose.orientation.y = quat[2]
pose.pose.orientation.z = quat[3]
pose.pose.orientation.w = quat[0]
if pose.covariance:
covariance_proto_to_ros(robot.covariance, pose.covariance)
message = robocup_extension_pb2.Message()
message.ParseFromString(msg)
player_id = message.current_pose.player_id
team_id = message.current_pose.team
if player_id == self.player_id or team_id != self.team_id:
# Skip information from ourselves or from the other team
return
team_data = TeamData()
header = Header()
# The robots' times can differ, therefore use our own time here
header.stamp = rospy.Time.now()
header.frame_id = self.map_frame
# Handle timestamp
##################
team_data.header = header
# Handle robot ID
#################
team_data.robot_id = player_id
# Handle state
##############
team_data.state = message.state
# Handle pose of current player
###############################
pose_proto_to_ros(message.current_pose, team_data.robot_position)
# Handle ball
#############
team_data.ball_absolute.pose.position.x = message.ball.position.x
team_data.ball_absolute.pose.position.y = message.ball.position.y
team_data.ball_absolute.pose.position.z = message.ball.position.z
if message.ball.covariance:
covariance_proto_to_ros(message.ball.covariance,
team_data.ball_absolute.covariance)
# Handle obstacles
##################
obstacle_relative_array = ObstacleRelativeArray()
obstacle_relative_array.header = header
for index, robot in enumerate(message.others):
obstacle = ObstacleRelative()
# Obstacle type
team_to_obstacle_type = dict(self.team_mapping)
obstacle.type = team_to_obstacle_type[robot.team]
obstacle.playerNumber = robot.player_id
pose_proto_to_ros(robot, obstacle.pose.pose)
if hasattr(message, "other_robot_confidence") and index < len(
message.other_robot_confidence):
obstacle.pose.confidence = message.other_robot_confidence[
index]
team_data.obstacles.obstacles.append(obstacle)
# Handle time to position at ball
#################################
if hasattr(message, "time_to_ball"):
team_data.time_to_position_at_ball = message.time_to_ball
# Handle strategy
#################
if hasattr(message, "role"):
role_mapping = dict(self.role_mapping)
team_data.strategy.role = role_mapping[message.role]
if hasattr(message, "action"):
action_mapping = dict(self.action_mapping)
team_data.strategy.action = action_mapping[message.action]
if hasattr(message, "offensive_side"):
offensive_side_mapping = dict(self.side_mapping)
team_data.strategy.offensive_side = offensive_side_mapping[
message.offensive_side]
self.pub_team_data.publish(team_data)