def LargeSquarePolygon(header):
p = PolygonStamped()
p.header = header
p.polygon.points = [
Point32(x=-10.0, y=10.0),
Point32(x=-6.0, y=10.0),
Point32(x=-2.0, y=10.0),
Point32(x=2.0, y=10.0),
Point32(x=6.0, y=10.0),
Point32(x=10.0, y=10.0),
Point32(x=10.0, y=6.0),
Point32(x=10.0, y=2.0),
Point32(x=10.0, y=-2.0),
Point32(x=10.0, y=-6.0),
Point32(x=10.0, y=-10.0),
Point32(x=6.0, y=-10.0),
Point32(x=2.0, y=-10.0),
Point32(x=-2.0, y=-10.0),
Point32(x=-6.0, y=-10.0),
Point32(x=-10.0, y=-10.0),
Point32(x=-10.0, y=-6.0),
Point32(x=-10.0, y=-2.0),
Point32(x=-10.0, y=2.0),
Point32(x=-10.0, y=6.0)
]
return p
def _create_initial_frontier_goal(self):
frontier_gh = ExploreTaskGoal()
curr_ts = rospy.Time.now()
curr_header = Header()
curr_header.frame_id = "map"
curr_header.seq = 1
curr_header.stamp = curr_ts
# Point Stamped
pt_s = PointStamped()
pt_s.header = curr_header
pt_s.point.x = 1.0
# Polygon Stamped
# Note that polygon is not defined so it's an unbounded exploration
pg_s = PolygonStamped()
pg_s.header = curr_header
vertices = [(5.0, 5.0), (-5.0, 5.0), (-5.0, -5.0), (5.0, -5.0)]
for vertex in vertices:
pg_s.polygon.points.append(Point(x=vertex[0], y=vertex[1]))
#frontier_gh.header = curr_header
#frontier_gh.goal_id.stamp = curr_ts
#frontier_gh.goal_id.id = 'initial_frontier_marco'
frontier_gh.explore_boundary = pg_s
frontier_gh.explore_center = pt_s
rospy.loginfo(frontier_gh)
return frontier_gh
def publishMap(self):
'''
Takes a mapPath [(x1, y1), (x2, y2), ...] in map coordinates and
outputs a PolygonStmped of the points in vesc (world) coordinates
'''
poly = PolygonStamped()
poly.header = utils_TA.make_header("/map")
scale = 0.0504
for i in xrange(len(self.pathTuples)):
p = self.pathTuples[i]
x = 25.90 - p[0] * scale
y = -16.50 + p[1] * scale
pt = Point32()
pt.x = x
pt.y = y
pt.z = -1
poly.polygon.points.append(pt)
self.path_pub.publish(poly)
print poly
print "traj pubbed"
def MovingSquare(header):
"""
Dynamicly moving Square
"""
p = PolygonStamped()
p.header = header
#print header.stamp.nsecs/100000000.0
dynamic_position_val = sin(header.stamp.nsecs / 1000000000.0)
dynamic_position_cos = cos(header.stamp.nsecs / 1000000000.0)
#print dynamic_position_val
p.polygon.points = [
Point32(x=dynamic_position_val + 1.0,
y=dynamic_position_val + 1.0,
z=dynamic_position_val),
Point32(x=dynamic_position_val - 1.0,
y=dynamic_position_val + 1.0,
z=dynamic_position_cos),
Point32(x=dynamic_position_val - 1.0,
y=dynamic_position_val - 1.0,
z=dynamic_position_cos),
Point32(x=dynamic_position_val + 1.0,
y=dynamic_position_val - 1.0,
z=dynamic_position_val)
]
return p
def main(pub_path):
global m_pose_seen, m_pose_stamped
r = rospy.Rate(2)
m_polygon = PolygonStamped()
header = Header()
header.frame_id = "odom"
header.stamp = rospy.Time.now()
m_polygon.header = header
m_polygon = LargeSquarePolygon(header)
"""
m_polygon = [SquarePolygon(header),
RectanglePolygon(header),
CirclePolygon(header),
StarPolygon(header)][3]
"""
while not m_pose_seen:
rospy.loginfo("NOT pose seen.")
r.sleep()
rospy.loginfo("pose seen.")
res = do_action(m_polygon, m_pose_stamped)
rospy.loginfo("len(res.plan.points) == " + str(len(res.plan.points)))
rospy.loginfo("do_action(..) seen.")
mpath = convert_striping_plan_to_path_client(res)
rospy.loginfo("convert_striping_plan_to_path_client(..) seen.")
rospy.loginfo(str(mpath))
while not rospy.is_shutdown():
pub_path.publish(mpath.path)
r.sleep()
def get_rightwall_endpoint(laser_data):
p1 = get_lidar_point_at_angle(laser_data, -90)
p2 = get_lidar_point_at_angle(laser_data, -90 + 20)
delta_y = p2[1] - p1[1]
delta_x = p2[0] - p1[0]
denominator = math.sqrt(delta_y**2 + delta_x**2)
numerator_const_term = p2[0] * p1[1] - p2[1] * p1[0]
wall_at_infinity = p1 + (p2 - p1) * 40
rightwall = PolygonStamped()
rightwall.header = Header(stamp=rospy.Time.now(), frame_id="laser")
rightwall.polygon.points = [
Point32(x=p1[0], y=p1[1]),
Point32(x=wall_at_infinity[0], y=wall_at_infinity[1])
]
pub_rightwall.publish(rightwall)
angle = -90 + 20
endpoint = get_lidar_point_at_angle(laser_data, angle)
for i in range(20):
angle += 5
candid = get_lidar_point_at_angle(laser_data, angle)
distance = abs(delta_y * candid[0] - delta_x * candid[1] +
numerator_const_term) / denominator
if distance < 0.5:
endpoint = candid
else:
break
endpoint_msg = PointStamped()
endpoint_msg.header = Header(stamp=rospy.Time.now(), frame_id="laser")
endpoint_msg.point = Point(endpoint[0], endpoint[1], 0)
pub_rightwall_endpoint.publish(endpoint_msg)
return endpoint
def pose_callback(self, pose_msg):
'''
Uses a pose message to generate an odometry and state message.
:param pose_msg: (geometry_msgs/PoseStamped) pose message
'''
new_pose_msg, trans, rot = self.tf_to_pose("LO01_base_link", "map",
pose_msg.header)
if not self.use_amcl:
pose_msg = new_pose_msg
self.new_pose_pub.publish(
pose_msg
) # if using AMCL, this will be the same as the original pose message
if trans and rot: # if not getting tf, trans and rot will be None
footprint = PolygonStamped()
footprint.header = pose_msg.header # has same frame_id (map) and time stamp
loomo_points = np.array([[0.16, -0.31], [0.16, 0.31],
[-0.16, 0.31], [-0.16, -0.31]])
roll, pitch, yaw = tf.transformations.euler_from_quaternion(rot)
rot = np.array([[np.cos(yaw), -np.sin(yaw)],
[np.sin(yaw), np.cos(yaw)]])
rotated_points = np.matmul(rot, loomo_points.T) # 2x4 array
rot_and_trans = rotated_points + np.array([[trans[0]], [trans[1]]])
polygon = Polygon(
points=[Point32(x=x, y=y, z=0) for x, y in rot_and_trans.T])
footprint.polygon = polygon
self.footprint_pub.publish(footprint)
odom_msg = Odometry()
odom_msg.pose.pose = pose_msg.pose
odom_msg.header = pose_msg.header
self.odom_pub.publish(odom_msg)
state_msg = State()
state_msg.header = pose_msg.header
state_msg.state_stamp = pose_msg.header.stamp
state_msg.pos.x = pose_msg.pose.position.x
state_msg.pos.y = pose_msg.pose.position.y
state_msg.pos.z = pose_msg.pose.position.z
state_msg.quat.x = pose_msg.pose.orientation.x
state_msg.quat.y = pose_msg.pose.orientation.y
state_msg.quat.z = pose_msg.pose.orientation.z
state_msg.quat.w = pose_msg.pose.orientation.w
if self.last_state_time and self.last_state:
dt = pose_msg.header.stamp.nsecs - self.last_state_time
state_msg.vel.x = (state_msg.pos.x -
self.last_state.pos.x) / (float(dt) / 10**9)
state_msg.vel.y = (state_msg.pos.y -
self.last_state.pos.y) / (float(dt) / 10**9)
state_msg.vel.z = 0 # ground robot not traveling in z direction
self.last_state_time = pose_msg.header.stamp.nsecs
self.last_state = state_msg
self.state_pub.publish(state_msg)
def polygon_cb(msg_var, data_var):
global polygon_voronoi_publisher, header
idx = data_var
polygon_msg = PolygonStamped()
polygon_msg.header = header
polygon_msg.polygon = msg_var
#print data_var,msg_var
polygon_voronoi_publisher[idx].publish(polygon_msg)
def RectanglePolygon(header):
p = PolygonStamped()
p.header = header
p.polygon.points = [Point32(x=-1.0, y=1.0, z=1.0),
Point32(x=-2.0, y=1.0, z=1.0),
Point32(x=-2.0, y=-1.0, z=1.0),
Point32(x=-1.0, y=-1.0, z=1.0)]
return p
def main_loop(self): rate = rospy.Rate(self.frequency) while not rospy.is_shutdown(): if self.current_velocity is not None and self.current_heading is not None and self.nearest_pedestrian is not None: x = self.nearest_pedestrian[0]/100 y = self.nearest_pedestrian[1]/100 v = self.current_velocity if v > 20: v = 20 h = self.current_heading state = [x,y,v,h] vi, ci, ii = self.check_reachability(state) critical_polygon = PolygonStamped() header = Header() header.stamp = rospy.Time.now() header.frame_id = 'velodyne' critical_polygon.header = header for i in range(len(self.critical_reachability[vi][ci][0])): x = self.critical_reachability[vi][ci][0][i] y = self.critical_reachability[vi][ci][1][i] xr = x*np.cos(h)-y*np.sin(h) yr = x*np.sin(h)+y*np.cos(h) z = 0 critical_polygon.polygon.points.append(Point32(x=xr, y=yr, z=z)) self.critical_polygon_pub.publish(critical_polygon) imminent_polygon = PolygonStamped() header = Header() header.stamp = rospy.Time.now() header.frame_id = 'velodyne' imminent_polygon.header = header for i in range(len(self.imminent_reachability[vi][ii][0])): x = self.imminent_reachability[vi][ii][0][i] y = self.imminent_reachability[vi][ii][1][i] xr = x*np.cos(h)-y*np.sin(h) yr = x*np.sin(h)+y*np.cos(h) z = 0 imminent_polygon.polygon.points.append(Point32(x=xr, y=yr, z=z)) self.imminent_polygon_pub.publish(imminent_polygon) rate.sleep()
def SquarePolygon(header):
p = PolygonStamped()
p.header = header
p.polygon.points = [Point32(x=1.0, y=1.0),
Point32(x=-1.0, y=1.0),
Point32(x=-1.0, y=-1.0),
Point32(x=1.0, y=-1.0)]
return p
def CirclePolygon(header):
p = PolygonStamped()
p.header = header
for i in range(100):
theta = i / 100.0 * 2.0 * pi
x = 1.0 * cos(theta) + 3.0
y = 1.0 * sin(theta)
p.polygon.points.append(Point32(x=x, y=y))
return p
def CirclePolygon(header):
p = PolygonStamped()
p.header = header
for i in range(100):
theta = i / 100.0 * 2.0 * pi
x = 1.0 * cos(theta) + 3.0
y = 1.0 * sin(theta)
p.polygon.points.append(Point32(x=x, y=y))
return p
def RectanglePolygon(header):
p = PolygonStamped()
p.header = header
p.polygon.points = [
Point32(x=-1.0, y=1.0, z=1.0),
Point32(x=-2.0, y=1.0, z=1.0),
Point32(x=-2.0, y=-1.0, z=1.0),
Point32(x=-1.0, y=-1.0, z=1.0)
]
return p
def SquarePolygon(header):
p = PolygonStamped()
p.header = header
p.polygon.points = [
Point32(x=1.0, y=1.0),
Point32(x=-1.0, y=1.0),
Point32(x=-1.0, y=-1.0),
Point32(x=1.0, y=-1.0)
]
return p
def calculate_goalpoint_two_boxes(laser_data):
poly = PolygonStamped()
center = PointStamped()
p1 = get_lidar_point_at_angle(laser_data, -80)
p2 = get_lidar_point_at_angle(laser_data, -70)
p3 = get_lidar_point_at_angle(laser_data, 70)
p4 = get_lidar_point_at_angle(laser_data, 80)
poly.header = Header(stamp=rospy.Time.now(), frame_id="laser")
poly.polygon.points = [Point32(x=p1[0], y=p1[1]),
Point32(x=p2[0], y=p2[1]),
Point32(x=p3[0], y=p3[1]),
Point32(x=p4[0], y=p4[1])]
pub_poly1.publish(poly)
center1 = (p1 + p2 + p3 + p4) / 4
center.header = Header(stamp=rospy.Time.now(), frame_id="laser")
center.point = Point(center1[0], center1[1], 0)
pub_point1.publish(center)
p5 = get_lidar_point_at_angle(laser_data, -50)
p6 = get_lidar_point_at_angle(laser_data, -40)
p7 = get_lidar_point_at_angle(laser_data, 40)
p8 = get_lidar_point_at_angle(laser_data, 50)
poly.header = Header(stamp=rospy.Time.now(), frame_id="laser")
poly.polygon.points = [Point32(x=p5[0], y=p5[1]),
Point32(x=p6[0], y=p6[1]),
Point32(x=p7[0], y=p7[1]),
Point32(x=p8[0], y=p8[1])]
pub_poly2.publish(poly)
center2 = (p5 + p6 + p7 + p8) / 4
center.header = Header(stamp=rospy.Time.now(), frame_id="laser")
center.point = Point(center2[0], center2[1], 0)
pub_point2.publish(center)
centerboxes = (center1+center2)/2
return lidar_to_rear_axle_coords(centerboxes)
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 + rect.width, y=rect.y + rect.height)
poly_msg.polygon.points.append(pt0)
poly_msg.polygon.points.append(pt1)
polys_msg.polygons.append(poly_msg)
self.pub.publish(polys_msg)
def convert_rect_to_polygon(rect, header):
poly_msg = PolygonStamped()
poly_msg.header = 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)
return poly_msg
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 + rect.width, y=rect.y + rect.height)
poly_msg.polygon.points.append(pt0)
poly_msg.polygon.points.append(pt1)
polys_msg.polygons.append(poly_msg)
self.pub.publish(polys_msg)
def publish(self):
polygon_msg = PolygonStamped()
pointcloud_msg = PointCloud()
pointcloud_msg.header.frame_id = self.publish_frame
pointcloud_msg.header.stamp = rospy.Time.now()
pointcloud_msg.points = self.corner_points_transformed
self.corner_points_pub.publish(pointcloud_msg)
polygon_msg.header = pointcloud_msg.header
polygon_msg.polygon.points = self.corner_points_transformed
self.foot_state_msg.foot_state.polygon.points = self.corner_points_transformed
self.polygon_pub.publish(polygon_msg)
self.foot_state_pub.publish(self.foot_state_msg)
def _convert(self, msg):
polygon_msg = PolygonStamped()
polygon_msg.header = msg.header
if self.index < 0:
return
elif self.index < len(msg.polygons):
polygon_msg = msg.polygons[self.index]
self.pub.publish(polygon_msg)
else:
logerr_throttle(
10, 'Invalid index {} is specified '
'for polygon array whose size is {}'.format(
self.index, len(msg.polygons)))
def generate_plane(self, header,
xmin=-0.0, xmax=1.0,
ymin=-1.0, ymax=1.0):
msg = PolygonArray()
msg.header = header
p = PolygonStamped()
p.header = header
p.polygon.points = [Point32(x=xmin, y=ymin),
Point32(x=xmax, y=ymin),
Point32(x=xmax, y=ymax),
Point32(x=xmin, y=ymax)]
msg.polygons.append(p)
return msg
def publish_triangle_polygon(self, pose):
pts = []
pts.append(tfx.pose((0,0.0079,0)))
pts.append(tfx.pose((0,-0.0079,0)))
pts.append(tfx.pose((0,0, -0.013)))
for i in range(len(pts)):
# IPython.embed()
pts[i] = (pose.as_tf()*pts[i]).position.msg.Point()
polygon = Polygon()
polygon_stamped = PolygonStamped()
polygon.points = pts
polygon_stamped.polygon = polygon
polygon_stamped.header = pose.msg.Header()
self.triangle_polygon_publisher.publish(polygon_stamped)
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 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 _convert(self, msg):
polygon_msg = PolygonStamped()
polygon_msg.header = msg.header
if self.index < 0:
return
elif self.index < len(msg.polygons):
polygon_msg = msg.polygons[self.index]
self.pub.publish(polygon_msg)
else:
logerr_throttle(
10,
"Invalid index {} is specified "
"for polygon array whose size is {}".format(self.index, len(msg.polygons)),
)
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 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 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 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 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 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 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 zones_to_polygons(self, zones):
polygons = []
for zone in zones.zones:
# Expand to a PolygonStamped of 3d points
polygon = PolygonStamped()
polygon.header = zone.header # copy both tf frame and timestamp
# Permute!
point = Point32()
x, y, z = tuple(dim/2.0 for dim in zone.extents.dimensions) # sides -> half-sides
for point.x in [-1*x, x]:
for point.y in [-1*y, y]:
for point.z in [-1*z, z]:
polygon.polygon.points.append(copy.deepcopy(point))
polygons.append(polygon)
return polygons
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 _callback_field_boundary(self, msg):
field = self.get_plane(msg.header.stamp, 0.0)
if field is None:
return
field_boundary = PolygonStamped()
field_boundary.header = msg.header
field_boundary.header.frame_id = self._publish_frame
for p in msg.polygon.points:
p_relative = self._transform_point(p, field, msg.header.stamp)
if p_relative is not None:
field_boundary.polygon.points.append(p_relative)
else:
rospy.logwarn_throttle(
5.0,
rospy.get_name() +
": At least one point of the Field Boundary could not be transformed,"
+ " dropping message")
return
self._field_boundary_pub.publish(field_boundary)
def callback_objsBoxs(self, objsBoxs_msg):
objsPints = PolygonStamped()
objsPints.header = objsBoxs_msg.header
for i in range(len(objsBoxs_msg.objects_vector)):
x_offset = objsBoxs_msg.objects_vector[i].roi.x_offset
y_offset = objsBoxs_msg.objects_vector[i].roi.y_offset
height = objsBoxs_msg.objects_vector[i].roi.height
width = objsBoxs_msg.objects_vector[i].roi.width
# u_center <-- x + w*1/2
# v_center <-- y + h*1/2
u_center = x_offset + width * 1 / 2
v_center = y_offset + height * 1 / 2
d = self.cv_dipth_img[v_center, u_center]
if math.isnan(d):
rospy.logwarn("=== d is nan ============")
else:
z = d / self.camera_factor
x = (u_center - self.camera_cx) * z / self.camera_fx
y = (v_center - self.camera_cy) * z / self.camera_fy
objPint = Point32()
objPint.x = x
objPint.y = y
objPint.z = z
objsPints.polygon.points.append(objPint)
if objsPints.polygon.points:
self._pub_objsPoints.publish(objsPints)
# rospy.loginfo(objsPints)
else:
self._pub_objsPoints.publish(objsPints)
pass #
def deflect_path(self):
# use the nearest colliding point to deflect the path accordingly
if self.closest_pt_index is not None:
dist = self.laser_ranges[self.closest_pt_index]
angle = normalize_angle(
self.real_laser_angles[self.closest_pt_index] +
self.FORWARD_ANGLE)
angle_rel_car_pose = self.laser_angles[self.closest_pt_index]
print("angle", angle)
print("best", self.best_pt_angle)
deflection_dir = 1 if (sign(self.best_pt_angle) < sign(angle) or
(sign(self.best_pt_angle) == sign(angle) and
self.best_pt_angle - angle > 0)) else -1
deflection_factor = deflection_dir * min(
(self.MAX_TURN_ANGLE) / (max(dist, 1.0)**2) *
abs(abs(angle_rel_car_pose) - math.pi / 4) /
(math.pi / 4), self.MAX_TURN_ANGLE)
print("def factor", deflection_factor)
new_angle = normalize_angle(self.best_pt_angle + deflection_factor)
print("new angle", new_angle)
self.deflected_pt = (self.cur_pose.position.x +
self.best_pt_dist * math.cos(new_angle),
self.cur_pose.position.y +
self.best_pt_dist * math.sin(new_angle))
else:
self.deflected_pt = self.best_pt
path = PolygonStamped()
path.header = Utils.make_header("map")
path.polygon = Polygon([
Point(self.cur_pose.position.x, self.cur_pose.position.y, 0),
Point(self.deflected_pt[0], self.deflected_pt[1], 0)
])
self.path_command_pub.publish(path)
def publish_freespace(Publisher, LF, Header, FrameId):
"""
Function that publishes a Polygon message showing the current local freespace
Input:
1) Publisher: PolygonStamped ROS publisher
2) LF: Local freespace polygon
3) Header: ROS header to be appended to Polygon message
4) FrameId: String defining the frame_id of the Polygon message
"""
# Publish freespace polygon
polygon_msg = GeometryMsgsPolygon()
polygon_msg.header = Header
polygon_msg.header.frame_id = FrameId
if LF.any() and ShapelyPolygon(LF).is_valid:
numvertex = LF.shape[0]
for i in range(0, numvertex):
polygon_msg.polygon.points.append(
Point32(x=LF[i][0], y=LF[i][1], z=0.))
else:
polygon_msg.polygon.points.append(Point32(x=0., y=0., z=0.))
Publisher.publish(polygon_msg)
return
