def get_cleanup_pose(self, robot_pose):
pose = Pose2D()
pose.x = robot_pose.x
pose.y = robot_pose.y
poly_pt, dp = closest_point_on_zone(robot_pose, self.cleanup_zones)
if robot_pose.x == poly_pt.x:
pose.x = poly_pt.x
if robot_pose.y < poly_pt.y:
pose.y = poly_pt.y + self.goal_standoff_dist
else:
pose.y = poly_pt.y - self.goal_standoff_dist
else:
m = (robot_pose.y - poly_pt.y) / (robot_pose.x - poly_pt.x)
standoff_x = (self.goal_standoff_dist / dp *
abs(robot_pose.x - poly_pt.x))
if robot_pose.x < poly_pt.x:
pose.x = poly_pt.x + standoff_x
else:
pose.x = poly_pt.x - standoff_x
pose.y = m*pose.x - m*poly_pt.x + poly_pt.y
#pose.theta = atan2(pose.y - poly_pt.y, poly_pt.x - pose.x)
return pose
def get_object_visit_pose(self, pt):
"""
Method returns a pose such that the object pose given will be between
the returned pose and the closest point on the cleanup polygon
"""
pose = Pose2D()
pose.x = pt.x
pose.y = pt.y
poly_pt, dp = closest_point_on_zone(pt, self.cleanup_zones)
if pt.x == poly_pt.x:
pose.x = pt.x
if pt.y < poly_pt.y:
pose.y = pt.y - self.standoff_dist
else:
pose.y = pt.y + self.standoff_dist
else:
m = (pt.y - poly_pt.y) / (pt.x - poly_pt.x)
standoff_x = self.standoff_dist / dp * abs(pt.x - poly_pt.x)
if pt.x < poly_pt.x:
pose.x = pt.x - standoff_x
else:
pose.x = pt.x + standoff_x
pose.y = m*pose.x - m*pt.x + pt.y
pose.theta = atan2(pose.y - poly_pt.y, poly_pt.x - pose.x)
return pose
