def unitTest():
print "Unit Test running..."
req = PathToPoseRequest()
# Build a sample border for this test
req.border = PolygonStamped()
center = Point32()
center.x = 0.0
center.y = 0.0
center.z = 0.0
buildShapeH(req.border.polygon, center)
req.start = PoseStamped()
req.start.pose.position.x = -3.0
req.start.pose.position.y = 3.0
req.start.pose.position.z = 0.0
req.goal = PoseStamped()
req.goal.pose.position.x = 3.0
req.goal.pose.position.y = -3.0
req.goal.pose.position.z = 0.0
edges = buildCollisionEdges(req)
for edge in edges:
drawLine(edge[0], edge[1], white)
path = astarFindPath(req)
startPoint = path[0]
for p in path[1::]:
drawLine(Point(startPoint[0], startPoint[1], 0), Point(p[0], p[1], 0),
red)
startPoint = p
result = PathToPose()
result.path = Path()
for p in path:
pose = Pose()
pose.position.x = p[0]
pose.position.y = p[1]
pose.position.z = 0
result.path.poses.append(pose)
print "Unit Test completed..."
while True:
for e in pygame.event.get():
if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE):
sys.exit("Leaving because you requested it.")
def handle_msg(msg):
goal_tf = "goal%d_ring" % msg.data
now = rospy.Time.now()
try:
listener.waitForTransform("base_link", goal_tf, now,
rospy.Duration(1.5))
(goal_pos, _) = listener.lookupTransform("map", goal_tf, now)
(robot_trans, rot) = listener.lookupTransform("base_link", goal_tf,
now)
rot = tf.transformations.euler_from_quaternion(rot)
a = math.atan2(robot_trans[1], robot_trans[0])
angle_base.publish(Float32(a))
(launcher_pos, _) = listener.lookupTransform("map", "launcher_base",
now)
v0, phi, t = launcher.findvelocityandangle(launcher_pos[0],
launcher_pos[1],
launcher_pos[2],
Point32(*goal_pos))
angle_up.publish(Float32(-phi))
msg = PolygonStamped()
msg.header.frame_id = 'map'
msg.header.stamp = now
now = rospy.Time.now()
listener.waitForTransform("map", "launcher_tip", now,
rospy.Duration(1.5))
(launcher_pos, rot) = listener.lookupTransform("map", "launcher_tip",
now)
(_, phi, a) = tf.transformations.euler_from_quaternion(rot)
x0 = np.array(launcher_pos)
f0 = np.array([math.cos(a), math.sin(a), math.sin(-phi)]) * v0
for t in np.arange(0, 2, 0.01):
xt = f0 * t + launcher_pos
xt[2] -= 0.5 * t * t * 9.81
msg.polygon.points.append(Point32(*xt))
ball_trajectory.publish(msg)
except Exception as e:
rospy.logerr(e)
pass
def __init__(self):
self.polygon1 = PolygonStamped()
self.polygon1.header.frame_id = "map"
points1 = [[0, 0], [6, 0], [6, 2], [2, 2], [2, 4], [6, 4], [6, 6],
[0, 6]]
for point in points1:
new_point = Point32()
new_point.x = point[0]
new_point.y = point[1]
self.polygon1.polygon.points.append(new_point)
self.polygon2 = PolygonStamped()
self.polygon2.header.frame_id = "map"
points2 = [[0, 0], [6, 0], [6, 6], [0, 6], [0, 4], [5, 4], [5, 2],
[0, 2]]
for point in points2:
new_point = Point32()
new_point.x = point[0]
new_point.y = point[1]
self.polygon2.polygon.points.append(new_point)
self.pose = Odometry()
self.pose.header.frame_id = "map"
self.pose.pose.pose.position.x = 5.5
self.pose.pose.pose.position.y = 1
self.pose2 = Odometry()
self.pose2.header.frame_id = "map"
self.pose2.pose.pose.position.x = 4.9
self.pose2.pose.pose.position.y = 1.1
self.setpoint = PoseStamped()
self.setpoint.header.frame_id = "map"
self.setpoint.pose.position.x = 5.5
self.setpoint.pose.position.y = 5
self.ticker = 0
def convert_shapely_as_ros_polygon(shapely_polygon):
polygon_stamped = PolygonStamped()
polygon_stamped.header.seq = 1
polygon_stamped.header.stamp = rospy.Time.now()
polygon_stamped.header.frame_id = "/map"
ros_polygon = RosPolygon()
coords = list(shapely_polygon.exterior.coords)
for coord in coords:
point = Point32()
point.x = coord[0]
point.y = coord[1]
ros_polygon.points.append(point)
polygon_stamped.polygon = ros_polygon
return polygon_stamped
def DynamicPolygon(self, header, p_list):
"""
Dynamicly changing poligon
"""
p = PolygonStamped()
p.header = header
# Minimum 3, otherwise the normals can't be calculated and gives error.
for i in p_list:
point_object = Point32(x=i[0], y=i[1])
p.polygon.points.append(point_object)
return p
def publish_visible_area(self, t, o):
visible_area = self.cleaningManager.mapManager.get_visible_area_corners(
t, o)
points = []
for a_point in visible_area:
point = Point32()
point.x = a_point[0]
point.y = a_point[1]
points.append(point)
rect = PolygonStamped()
rect.header.frame_id = "/map"
rect.polygon.points = points
if len(points) == 4:
self.visible_area_pub.publish(rect)
def path_to_polygon(self, path):
path_msg = PolygonStamped()
path_msg.header.frame_id = "/map"
path_msg.header.stamp = rospy.Time.now()
list_of_poses = []
for path_index in range(len(path)):
temp_pose = Point32()
temp_pose.x = path[path_index][0]
temp_pose.y = path[path_index][1]
temp_pose.z = -1
list_of_poses.append(temp_pose)
path_msg.polygon.points = list_of_poses
return path_msg
def execute_cb(self, goal):
# Create polygon
p = PolygonStamped()
p.header.frame_id = goal.target_pose.header.frame_id
pose = goal.target_pose.pose
gx = pose.position.x
gy = pose.position.y
rpy = euler_from_quaternion([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
gt = rpy[2]
for i in range(4):
angle = i * pi / 2 + gt + pi / 4
x = gx + FOOTPRINT_RADIUS * cos(angle)
y = gy + FOOTPRINT_RADIUS * sin(angle)
p.polygon.points.append(Point32(x,y,0))
# front point
x = gx + FOOTPRINT_RADIUS * cos(gt)
y = gy + FOOTPRINT_RADIUS * sin(gt)
p.polygon.points.append(Point32(x,y,0))
rate = rospy.Rate(5)
while not rospy.is_shutdown():
if self._as.is_preempt_requested():
self._as.set_preempted()
return
self.pub.publish(p)
t, pose = get_time_and_pose(self.tf, p.header.frame_id, self.target_frame)
if pose is not None:
if close(pose, gx, gy, gt):
break
rate.sleep()
self._as.set_succeeded()
p.polygon.points = []
N = 10
for i in range(N):
angle = i * PIx2 / N
x = gx + FOOTPRINT_RADIUS * cos(angle)
y = gy + FOOTPRINT_RADIUS * sin(angle)
p.polygon.points.append(Point32(x,y,0))
for i in range(5):
self.pub.publish(p)
rate.sleep()
def publish_obstacle_msg():
rospy.init_node("test_obstacle_msg")
obstacle_list = []; #list of obstacles
pub_list = []; #list of publishers
topic_list = []; #for debugging
file = open(os.path.join(os.path.dirname(__file__),'./files','obstacles.txt'), 'r');
for line in file:
#extract line from file
obstacle_msg = line.strip('\n').split(',')
#prepare the publisher for a certain polygon topic
topic = obstacle_msg[0]
topic_list.append(topic)
pub = rospy.Publisher('/'+topic, PolygonStamped, queue_size=1)
point_list = [];
for i in range(1, len(obstacle_msg)):
point_i = Point32();
point_i.x = float(obstacle_msg[i].strip().split(' ')[0])
point_i.y = float(obstacle_msg[i].strip().split(' ')[1])
point_list.append(point_i)
obstacle = PolygonStamped()
obstacle.header.stamp = rospy.Time.now()
obstacle.header.frame_id = "map" # CHANGE HERE: odom/map
obstacle.polygon.points = point_list
obstacle_list.append(obstacle)
pub_list.append(pub)
file.close();
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
for i in range(0, len(obstacle_list)):
pub_list[i].publish(obstacle_list[i])
print(topic_list[i])
print(obstacle_list[i])
r.sleep()
def __init__(self):
"""Fake publisher for cart joint states
Arguments:
- `self`:
"""
self._joint_state_pub = rospy.Publisher("cart_joint_states",
JointState,
queue_size=10)
self.cart_footprint_pub = rospy.Publisher("/cart_footprint",
PolygonStamped,
queue_size=10)
self.tb = tf.TransformBroadcaster()
# Make the message just once since nothing is changing...
self.jointstate = JointState()
self.jointstate.header.stamp = rospy.Time.now()
self.jointstate.name = [
"lf_caster_joint", "lf_wheel_joint", "rf_caster_joint",
"rf_wheel_joint", "lb_caster_joint", "lb_wheel_joint",
"rb_caster_joint", "rb_wheel_joint"
]
self.jointstate.position = [0.0] * len(self.jointstate.name)
# Cart footprint message
self.cart_footprint = PolygonStamped()
self.cart_footprint.header.stamp = rospy.Time.now()
self.cart_footprint.header.frame_id = "cart_base_link"
self.cart_footprint.polygon.points = [
Point(0.225, 0.225, 0.0),
Point(0.225, -0.225, 0.0),
Point(-0.225, -0.225, 0.0),
Point(-0.225, 0.225, 0.0)
]
# Set a transform from the "base" link in the cart urdf to the "cart" frame
self.base_pose = PoseStamped(
pose=Pose(position=Point(1.15, 0.0, 0.0),
orientation=Quaternion(0.0, 0.0, 0.0, 1.0)))
self.base_pose.header.stamp = rospy.Time.now()
self.base_pose_tup = self.pose_msg_to_tuple(self.base_pose.pose)
# set up thread
self.thread = threading.Thread(target=self._pub_thread, args=(10, ))
self.thread.setDaemon(1)
self.alive = threading.Event()
self.alive.set()
self.thread.start()
def run_detector(self, image, header=None):
result = self.detect(image)
rois = result['rois']
masks = result['masks']
class_ids = result['class_ids']
scores = result['scores']
rect_array = RectArray()
for m in range(masks.shape[2]):
if scores[m] > self.__prob_thresh:
object_name, color = self.__objects_meta[class_ids[m]]
y1, x1, y2, x2 = rois[m]
poly = PolygonStamped()
poly.polygon.points.append(Point32(x=x1, y=y1))
poly.polygon.points.append(Point32(x=x2, y=y2))
if header is not None:
poly.header = header
rect_array.polygon.append(poly)
rect_array.labels.append(int(class_ids[m]))
rect_array.likelihood.append(scores[m])
rect_array.names.append(object_name)
im_mask = np.zeros(image.shape[:2], np.int64)
im_mask[masks[:,:,m]==True] = np.int64(class_ids[m])
## temp
for j in range(im_mask.shape[0]):
for i in range(im_mask.shape[1]):
rect_array.indices.append(im_mask[j, i])
if self.__debug:
im_plot = self.plot_detection(image, result, self.__labels)
cv_result = np.zeros(shape=im_plot.shape, dtype=np.uint8)
cv.convertScaleAbs(im_plot, cv_result)
print ('Scores: {} {} {}'.format(scores, class_ids, image.shape))
wname = 'image'
cv.namedWindow(wname, cv.WINDOW_NORMAL)
cv.imshow(wname, cv_result)
if cv.waitKey(3) == 27:
cv.destroyAllWindows()
sys.exit()
return rect_array
def make_polys(lines, tf):
obs = [PolygonStamped() for _ in range(len(lines))]
obs_centers_areas = list()
lines = parallel(lines, 0.3)
for i, line in enumerate(lines):
poly = obs[i].polygon
poly.points = [Point32() for _ in range(4)]
# we need to make sure that we "extend" the square out in the correct
# direction. first, we place the opposite edge of the square:
line_len = norm(line)
line1 = shift(line, line_len, dir=1)
line2 = shift(line, line_len, dir=-1)
if dist_to_robot(line1, tf) > dist_to_robot(line2, tf):
opp_line = line1
else:
opp_line = line2
# now we know the four points, so we can finish this polygon:
p0, p1 = line
p2, p3 = opp_line
obs_x = 0
obs_y = 0
for j, pt in enumerate((p0, p1, p3, p2)):
ppt = poly.points[j]
x, y = pt
ppt.x = x
ppt.y = y
obst = Obstacle(obs[i])
#check if similar to previous objects
flag = False
for data in obs_centers_areas:
flag = obst.sim_check(data)
if not flag:
obs_centers_areas.append(obst)
ret = Polygons()
ret.polygons = obs
#print("NUMBER OF LINES DETECTED", len(obs_centers_areas))
return ret, obs_centers_areas
def toPolygon(self):
poly = PolygonStamped()
poly.header = self.make_header("/map")
use_speed_profile = len(self.speed_profile) == len(self.points)
for i in xrange(len(self.points)):
p = self.points[i]
pt = Point32()
pt.x = p[0]
pt.y = p[1]
if use_speed_profile:
pt.z = self.speed_profile[i]
else:
pt.z = -1
poly.polygon.points.append(pt)
return poly
def publish_object_found_area(self, px, py, pz):
# Publishes a circle around the area where the object is placed
msg = PolygonStamped()
msg.header.frame_id = "/map"
r = 3.0
marker = Marker(type=Marker.CUBE,
id=0,
lifetime=rospy.Duration(1.0),
pose=Pose(Point(px, py, pz), Quaternion(0, 0, 0, 1)),
scale=Vector3(r, r, 2.0),
header=Header(frame_id='map'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8),
text="text")
self.pub_object_found_area.publish(marker)
def callback(data):
blue_coords, yellow_coords = find_cone_coords(data)
middle_x, middle_y = find_middle_points(blue_coords, yellow_coords)
middle_points_interpolated = interpolate_middle_points(middle_x, middle_y)
if ENABLE_PLOTTING:
plt.axes().set_aspect('equal')
plt.grid(True)
plt.show()
trajectory_polygon = PolygonStamped()
trajectory_polygon.polygon.points = middle_points_interpolated
trajectory_polygon.header.frame_id = "map"
trajectory_polygon.header.stamp = rospy.get_rostime()
publisher.publish(trajectory_polygon)
def publishPolygons(self, event=None):
pmsg = PolygonArray()
pmsg.header.stamp = rospy.Time.now()
cmsg = ModelCoefficientsArray()
cmsg.header.stamp = pmsg.header.stamp
for i in range(10):
pmsg.polygons.append(PolygonStamped())
pmsg.polygons[i].header.stamp = pmsg.header.stamp
pmsg.polygons[i].header.frame_id = str(i)
cmsg.coefficients.append(ModelCoefficients())
cmsg.coefficients[i].header.stamp = cmsg.header.stamp
cmsg.coefficients[i].header.frame_id = str(i)
pmsg.likelihood = [1.0, 2.0, 3.0, 4.0, 5.0, 4.0, 3.0, 2.0, 1.0, 0.0]
self.pub_polygon.publish(pmsg)
self.pub_coefficients.publish(cmsg)
def robot_tracking(self, TH=0.3):
'''
mu_t = np.mean(self.PI_t[:,0:2], axis = 0)
cov_t = np.cov(self.PI_t[:,0:2])
max_x = mu_t [0] + np.sqrt(cov_t[0,0]) * self.Sm
min_x = mu_t [0] - np.sqrt(cov_t[0,0]) * self.Sm
max_y = mu_t [1] + np.sqrt(cov_t[1,1]) * self.Sm
min_y = mu_t [1] - np.sqrt(cov_t[1,1]) * self.Sm
'''
max_x = np.max(self.PI_t[:, 0])
min_x = np.min(self.PI_t[:, 0])
max_y = np.max(self.PI_t[:, 1])
min_y = np.min(self.PI_t[:, 1])
poly = PolygonStamped()
poly.header.frame_id = "map"
poly.polygon.points = []
x = np.array([max_x, max_x, min_x, min_x])
y = np.array([max_y, min_y, min_y, max_y])
#print x, y
for i in range(4):
points = Point32()
points.x = x[i]
points.y = y[i]
points.z = 0
poly.polygon.points.append(points)
self.scope_pub.publish(poly)
mu_s = np.mean(self.PI_s, axis=0)
z = self.scan.scan2cart(mu_s).T * self.scan.map.map.info.resolution
z = z[z[:, 0] < max_x]
z = z[z[:, 0] > min_x]
z = z[z[:, 1] < max_y]
z = z[z[:, 1] > min_y]
if len(z) > 4:
ob = self.scan.obs()
_, indices = self.nbrs.kneighbors(z)
z_n = ob[indices].reshape(z.shape)
z = z[np.linalg.norm(z - z_n, axis=1) > TH]
if len(z) > 4:
self.mu_t_hat = np.mean(z, axis=0)
cov_t_hat = np.cov(z)
self.send_robot_estimated(self.mu_t_hat)
self.send_other_robot_observation(z)
def DynamicPolygon(header):
"""
Dynamicly changing poligon
"""
p = PolygonStamped()
p.header = header
# Minimum 3, otherwise the normals can't be calculated and gives error.
random_number_edges = np.random.randint(low=3, high=15, size=1)[0]
for i in range(random_number_edges):
point_object = Point32(x=np.random.ranf(), y=np.random.ranf())
p.polygon.points.append(point_object)
rospy.loginfo(p)
return p
def calculate_goalpoint_one_box(laser_data):
p0 = get_lidar_point_at_angle(laser_data, -80)
p1 = get_lidar_point_at_angle(laser_data, -70)
p2 = get_lidar_point_at_angle(laser_data, 70)
p3 = get_lidar_point_at_angle(laser_data, 80)
poly = PolygonStamped()
poly.header = Header(stamp=rospy.Time.now(), frame_id="laser")
poly.polygon.points = [Point32(x=p0[0], y=p0[1]),
Point32(x=p1[0], y=p1[1]),
Point32(x=p2[0], y=p2[1]),
Point32(x=p3[0], y=p3[1])]
pub_poly1.publish(poly)
p = (p0 + p1 + p2 + p3) / 4
return lidar_to_rear_axle_coords(p)
def __init__(self):
self.detected = []
self.people_pub = rospy.Publisher('people',
AggregatedPoseTwist,
queue_size=1)
self.tf_listener = tf.TransformListener()
self.footprint = PolygonStamped()
num_points = 32
for i in xrange(num_points):
p = Point32()
p.x = radius * cos((2. * pi * i) / num_points)
p.y = radius * sin((2. * pi * i) / num_points)
self.footprint.polygon.points.append(p)
rospy.sleep(0.5)
rospy.Subscriber('tracked_persons', TrackedPersons,
self.detected_persons)
def StarPolygon(header):
p = PolygonStamped()
p.header = header
p.polygon.points = [
Point32(x=.0000, y=1.0000 + 3.0),
Point32(x=.2245, y=.3090 + 3.0),
Point32(x=.9511, y=.3090 + 3.0),
Point32(x=.3633, y=-.1180 + 3.0),
Point32(x=.5878, y=-.8090 + 3.0),
Point32(x=.0000, y=-.3820 + 3.0),
Point32(x=-.5878, y=-.8090 + 3.0),
Point32(x=-.3633, y=-.1180 + 3.0),
Point32(x=-.9511, y=.3090 + 3.0),
Point32(x=-.2245, y=.3090 + 3.0)
]
return p
def pose_callback(self, msg: PoseStamped):
# transform the footprint into the pose
original = np.matrix(self.footprint).T
rotated = get_rot_matrix(msg) * original + get_trans_vector(msg)
# transform so we can iterate simpler
rotated = rotated.T
# generate the polygon
out = PolygonStamped()
out.header.frame_id = msg.header.frame_id
out.polygon.points = [
Point32(x=col.item(0), y=col.item(1)) for col in rotated
]
# publish the result
self.pub.publish(out)
def _convert(self, msg):
polys_msg = PolygonArray()
polys_msg.header = msg.header
for rect in msg.rects:
poly_msg = PolygonStamped()
poly_msg.header = msg.header
pt0 = Point32(x=rect.x, y=rect.y)
pt1 = Point32(x=rect.x, y=rect.y + rect.height)
pt2 = Point32(x=rect.x + rect.width, y=rect.y + rect.height)
pt3 = Point32(x=rect.x + rect.width, y=rect.y)
poly_msg.polygon.points.append(pt0)
poly_msg.polygon.points.append(pt1)
poly_msg.polygon.points.append(pt2)
poly_msg.polygon.points.append(pt3)
polys_msg.polygons.append(poly_msg)
self.pub.publish(polys_msg)
def publish(self):
"""Publish zone information"""
# last zone that the receiver was in
last_zone = None
while not rospy.is_shutdown():
# get the current zone
zone = self.positioning.get_zone(
self.msg_buffer) if self.msg_buffer else None
# check if zone change occurred
if zone != last_zone:
# publish zone change event
event = StringStamped()
event.header.stamp = self.last_time
# only zone leave
if zone is None:
event.data = last_zone.name
self.zone_leave_pub.publish(event)
# only zone enter
elif last_zone is None:
event.data = zone.name
self.zone_enter_pub.publish(event)
# leave on zone and enter another
else:
event.data = last_zone.name
self.zone_leave_pub.publish(event)
event.data = zone.name
self.zone_enter_pub.publish(event)
if zone is not None:
# publish zone name
name = StringStamped()
name.header.stamp = self.last_time
name.header.frame_id = zone.frame_id
name.data = zone.name
self.zone_name_pub.publish(name)
# publish zone polygon
polygon = PolygonStamped()
polygon.header.stamp = self.last_time
polygon.header.frame_id = zone.frame_id
points = []
for p in zone.polygon:
points.append(Point32(p[0], p[1], p[2]))
polygon.polygon.points = points
self.zone_polygon_pub.publish(polygon)
# set current zone to last zone
last_zone = zone
# wait to start next iteration
self.rate.sleep()
def __init__(self):
# TODO : non-hardcoded footprint (possibly load from .yaml files given as a rosparam)
self.fpt_ = np.asarray(
[[0.25649121, -0.17213032], [0.17213032, -0.17468672],
[0.0562406, -0.17468672], [-0.00596491, -0.14315788],
[-0.05027569, -0.09117794], [-0.06987468, -0.00426065],
[-0.05027569, 0.07243107], [0.00170426, 0.13804510],
[0.05624060, 0.16616541], [0.16957393, 0.16957393],
[0.25478697, 0.16786967]],
dtype=np.float32)
self.fpt_msg_ = PolygonStamped()
self.fpt_msg_.header.stamp = rospy.Time.now()
self.fpt_msg_.header.frame_id = 'base_link'
self.fpt_msg_.polygon = Polygon(
[Point32(x, y, 0) for (x, y) in self.fpt_])
self.fpt_pub_ = rospy.Publisher('fpt', PolygonStamped, queue_size=2)
def path_rosvis(path, topic_base_name="static_path_"):
pts_all = []
pose_all = []
path_all = []
for segment in path:
if type(segment) == Line2D:
pts_1seg = segment.to_ros(output_type=Point32)
poses_1seg = segment.to_ros(output_type=Pose)
path_1seg = segment.to_ros(output_type=PoseStamped)
elif type(segment) == Arc2D:
pts_1seg = segment.to_ros_div_len(0.5, output_type=Point32)
poses_1seg = segment.to_ros_div_len(0.5, output_type=Pose)
path_1seg = segment.to_ros_div_len(0.5, output_type=PoseStamped)
pts_all = pts_all + pts_1seg
pose_all = pose_all + poses_1seg
path_all = path_all + path_1seg
header = Header()
header.seq = 0
header.stamp = rospy.Time.now()
# rviz内部でfloat32を使っているらしくUTM座標系の表示は桁落ちでパスがガタガタになる
# 32bitのraspi用の ubuntu Mate だけでなく64bitPC版のUbuntuでも同様の現象になる
# header.frame_id = UTM_FRAME_NAME
header.frame_id = WORK_ORIGIN_FRAME_NAME
static_path_poly = PolygonStamped()
static_path_poly.header = header
static_path_poly.polygon.points = pts_all
pub_poly=rospy.Publisher(topic_base_name+"_poly", \
PolygonStamped, queue_size=2, latch=True)
pub_poly.publish(static_path_poly)
static_path_parr = PoseArray()
static_path_parr.header = header
static_path_parr.poses = pose_all
pub_parr=rospy.Publisher(topic_base_name+"_parr", \
PoseArray, queue_size=2, latch=True)
pub_parr.publish(static_path_parr)
static_path = Path()
static_path.header = header
static_path.poses = path_all
pub_path = rospy.Publisher(topic_base_name, Path, queue_size=2, latch=True)
pub_path.publish(static_path)
def __init__(self):
# ros interface
self.pub = rospy.Publisher('footprint', Polygon, queue_size=10)
self.pub_stamped = rospy.Publisher('footprint_stamped',
PolygonStamped,
queue_size=10)
self.pub_original_stamped = rospy.Publisher('footprint_base',
PolygonStamped,
queue_size=10)
self.tf_listener = tf.TransformListener()
# params
self.footprint = rospy.get_param("~footprint")
self.footprint_frame = rospy.get_param("~footprint_frame")
self.target_frames = rospy.get_param("~target_frames")
self.inflation_radius = rospy.get_param("~inflation_radius", 0.1)
# build footprint (for assertion purposes and base footprint generation)
base_hull = self.build_base_hull(self.footprint)
self.base_msg = PolygonStamped()
self.base_msg.header.frame_id = self.footprint_frame
self.base_msg.polygon = self.build_polygon_msg(base_hull)
# setup
rospy.loginfo("Waiting for initial transform from frames '" +
str(self.target_frames) + "' to '" +
self.footprint_frame + "'")
while not rospy.is_shutdown():
success = True
for target_frame in self.target_frames:
try:
self.tf_listener.waitForTransform(target_frame,
self.footprint_frame,
rospy.Time(),
rospy.Duration(2))
except tf.Exception:
rospy.logwarn("Unavailable transform from frame '" +
str(target_frame) + "' to '" +
self.footprint_frame + "'. Retrying ...")
success = False
if success:
break
rospy.loginfo("Ready to work ...")
def createExploreTaskGoal(points):
polygon = PolygonStamped()
polygon.header.stamp = rospy.Time.now()
polygon.header.frame_id = mapFrame
polygon.polygon.points = []
# Make explore corner points into a correct ROS message
for point in points:
x, y = point
polygon.polygon.points.append(Point32(x=x, y=y))
# Add start point for exploration
startPoint = PointStamped()
startPoint.header.stamp = rospy.Time.now()
startPoint.header.frame_id = mapFrame
startPoint.point = Point(x=0, y=0)
return ExploreTaskGoal(explore_boundary=polygon, explore_center=startPoint)
def rectangle(header, width, height, center):
poly = PolygonStamped()
poly.header = deepcopy(header)
x, y, z = center.x, center.y, center.z
# this is for doing it in image coordinates
# poly.polygon.points.append(Point(x,y-width/2,z-height/2))
# poly.polygon.points.append(Point(x,y+width/2,z-height/2))
# poly.polygon.points.append(Point(x,y+width/2,z+height/2))
# poly.polygon.points.append(Point(x,y-width/2,z+height/2))
# normal frame alignment
poly.polygon.points.append(Point(x - width / 2, y - height / 2, z))
poly.polygon.points.append(Point(x + width / 2, y - height / 2, z))
poly.polygon.points.append(Point(x + width / 2, y + height / 2, z))
poly.polygon.points.append(Point(x - width / 2, y + height / 2, z))
return poly
def paint(self, fieldOfView_, parent_frame_, publisher_) :
poly = PolygonStamped()
poly.header.frame_id=parent_frame_
poly.header.stamp = rospy.Time.now()
poly.polygon.points = [Point32() for i in range(4)]
poly.polygon.points[0].x = fieldOfView_.x_low
poly.polygon.points[0].y = fieldOfView_.y_low
poly.polygon.points[0].z = 1.0
poly.polygon.points[1].x = fieldOfView_.x_low
poly.polygon.points[1].y = fieldOfView_.y_high
poly.polygon.points[1].z = 1.0
poly.polygon.points[2].x = fieldOfView_.x_high
poly.polygon.points[2].y = fieldOfView_.y_high
poly.polygon.points[2].z = 1.0
poly.polygon.points[3].x = fieldOfView_.x_high
poly.polygon.points[3].y = fieldOfView_.y_low
poly.polygon.points[3].z = 1.0
publisher_.publish(poly)
