def ScanMap(self):
if self.odom is None:
return
# Transform: odom frame -> map frame
poseStamped = PoseStamped()
poseStamped.header = self.odom.header
poseStamped.pose = self.odom.pose.pose
try:
poseStamped = self.tfBuffer.transform(poseStamped, self.map_frame, timeout=rospy.Duration(1.0 / self.rate))
except:
# rospy.logerr('Transform failed')
return
# Scan
yaw = tf.transformations.euler_from_quaternion([poseStamped.pose.orientation.x, poseStamped.pose.orientation.y,
poseStamped.pose.orientation.z, poseStamped.pose.orientation.w])[2]
R = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]])
T = np.array([poseStamped.pose.position.x, poseStamped.pose.position.y])
scan_buffer = (np.matmul(self.scan_buffer, R.T) + T)
scan_buffer = self.occGridParam.map2ImageTransform(scan_buffer)
scan_buffer = np.int32(np.round(scan_buffer))
scan_buffer[:, :, 0] = np.clip(scan_buffer[:, :, 0], 0, self.width - 1)
scan_buffer[:, :, 1] = np.clip(scan_buffer[:, :, 1], 0, self.height - 1)
pixel_values = self.occMap[scan_buffer[:, :, 1], scan_buffer[:, :, 0]]
mask = pixel_values > 0
indices = np.where(mask.any(axis=1), mask.argmax(axis=1), -1)
pointclouds = []
for index, scan_points in zip(indices, scan_buffer):
if index == -1:
pointcloud = [np.inf, np.inf, np.inf]
else:
pointcloud = [scan_points[index, 0], scan_points[index, 1], 0.0]
pointclouds.append(pointcloud)
pointclouds = np.asarray(pointclouds)
pointclouds[:, :2] = self.occGridParam.image2MapTransform(pointclouds[:, :2])
self.map_pointclouds = pointclouds
def ObstacleCallback(self, msg):
if self.odom is None:
return
# Transform
poseStamped = PoseStamped()
poseStamped.header = msg.header
poseStamped.pose = msg.pose.pose
try:
poseStamped = self.tfBuffer.transform(poseStamped, self.odom_frame, timeout=rospy.Duration(1.0 / self.rate))
except:
# rospy.logerr('Transform failed')
return
x, y, z = poseStamped.pose.position.x, poseStamped.pose.position.y, poseStamped.pose.position.z
dx, dy = x - self.odom.pose.pose.position.x, y - self.odom.pose.pose.position.y
dist = np.sqrt(dx ** 2 + dy ** 2)
if dist < self.min_range or dist > self.max_range:
self.obstacle_pointclouds = None
return
pointclouds = np.array([[x, y, z]])
self.obstacle_pointclouds = pointclouds
def state_update(state_msg):
global ball_pose
global goal_post_1_pose
global goal_post_2_pose
global tf_buffer
global cam_info
global ball_pub
global goal_posts_pub
if not cam_info:
return
ball_indices = []
for i, name in enumerate(state_msg.name):
if name == "teensize_ball":
ball_indices.append(i)
post_index1 = 0
post_index2 = 0
for name in state_msg.name:
if name == "teensize_goal":
if post_index1 == 0:
post_index1 = post_index2
else:
break
post_index2 += 1
ball_msgs = []
for ball_index in ball_indices:
ball_pose = state_msg.pose[ball_index]
ball_pose_stamped = PoseStamped()
ball_pose_stamped.header.stamp = rospy.Time.now()
ball_pose_stamped.header.frame_id = "map"
ball_pose_stamped.pose = ball_pose
ball_in_camera_optical_frame = tf_buffer.transform(
ball_pose_stamped,
cam_info.header.frame_id,
timeout=rospy.Duration(0.5))
# we only have to compute if the ball is inside the image, if the ball is in front of the camera
if ball_in_camera_optical_frame.pose.position.z >= 0:
p = [
ball_in_camera_optical_frame.pose.position.x,
ball_in_camera_optical_frame.pose.position.y,
ball_in_camera_optical_frame.pose.position.z
]
k = np.reshape(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.
# also z has to be positive to make sure that the ball is in front of the camera and not in the back
if 0 < p_pixel[0] <= cam_info.width and 0 < p_pixel[
1] <= cam_info.height:
ball_in_footprint_frame = tf_buffer.transform(
ball_in_camera_optical_frame,
"base_footprint",
timeout=rospy.Duration(0.5))
ball_relative = PoseWithCertainty()
ball_relative.pose.pose = ball_in_footprint_frame.pose.position
ball_relative.confidence = 1.0
ball_msgs.append(ball_relative)
balls_relative_msg = PoseWithCertaintyArray()
balls_relative_msg.header = ball_in_footprint_frame.header
balls_relative_msg.poses = ball_msgs
ball_pub.publish(balls_relative_msg)
goal = PoseWithCertaintyArray()
goal_post_1_pose = state_msg.pose[post_index1]
goal_post_2_pose = state_msg.pose[post_index2]
for gp in (goal_post_1_pose, goal_post_2_pose):
goal_post_stamped = PoseWithCertainty()
goal_post_stamped.pose.pose = gp
left_post = deepcopy(goal_post_stamped)
left_post.pose.pose.position.y += 1.35
left_post_in_camera_optical_frame = tf_buffer.transform(
left_post.pose,
cam_info.header.frame_id,
timeout=rospy.Duration(0.5))
if left_post_in_camera_optical_frame.pose.position.z >= 0:
p = [
left_post_in_camera_optical_frame.pose.position.x,
left_post_in_camera_optical_frame.pose.position.y,
left_post_in_camera_optical_frame.pose.position.z
]
k = np.reshape(cam_info.K, (3, 3))
p_pixel = np.matmul(k, p)
p_pixel = p_pixel * (1 / p_pixel[2])
if 0 < p_pixel[0] <= cam_info.width and 0 < p_pixel[
1] <= cam_info.height:
left_post = PoseWithCertainty()
left_post.confidence = 1
left_post.pose = tf_buffer.transform(
left_post.pose,
"base_footprint",
timeout=rospy.Duration(0.5)).pose.position
goal.poses.append(left_post)
right_post = goal_post_stamped
right_post.pose.position.y -= 1.35
right_post_in_camera_optical_frame = tf_buffer.transform(
right_post, cam_info.header.frame_id, timeout=rospy.Duration(0.5))
if right_post_in_camera_optical_frame.pose.position.z >= 0:
p = [
right_post_in_camera_optical_frame.pose.position.x,
right_post_in_camera_optical_frame.pose.position.y,
right_post_in_camera_optical_frame.pose.position.z
]
k = np.reshape(cam_info.K, (3, 3))
p_pixel = np.matmul(k, p)
p_pixel = p_pixel * (1 / p_pixel[2])
if 0 < p_pixel[0] <= cam_info.width and 0 < p_pixel[
1] <= cam_info.height:
right_post = PoseWithCertainty()
right_post.confidence = 1
right_post.pose = tf_buffer.transform(
right_post_in_camera_optical_frame,
"base_footprint",
timeout=rospy.Duration(0.5)).pose.position
goal.poses.append(right_post)
if len(goal.poses) > 0:
goal_posts_pub.publish(goal)
