def createArea(frame):
poly = PolygonStamped()
poly.header.frame_id = frame
poly.header.stamp = rospy.Time.now()
poly.polygon = Polygon()
poly.polygon.points = exploration_area_vertices
return poly
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 visualize_region(self, pts_A, pts_B):
poly_A = PolygonStamped()
poly_A.header.frame_id = "/base_link"
poly_A.header.stamp = rospy.Time.now()
for i in xrange(len(pts_A)):
pt = Point32()
pt.x = pts_A[i][0]
pt.y = pts_A[i][1]
pt.z = 0.0
poly_A.polygon.points.append(pt)
poly_B = PolygonStamped()
poly_B.header.frame_id = "/base_link"
poly_B.header.stamp = rospy.Time.now()
for i in xrange(len(pts_B)):
pt = Point32()
pt.x = pts_B[i][0]
pt.y = pts_B[i][1]
pt.z = 0.0
poly_B.polygon.points.append(pt)
if (self.lifter_status == 0):
poly_B.polygon.points = []
elif (self.lifter_status == 1):
poly_A.polygon.points = []
self.viz_outer_region_pub.publish(poly_A)
self.viz_inner_region_pub.publish(poly_B)
def getting_cordi(A,B):
theta_increment=(2)*(math.pi)/360
re=PolygonArray()
re.header.frame_id="odom"
roar=PolygonStamped()
roar.header.frame_id="odom"
pt=[]
count=0
# print("This is A:")
# print(A)
for i in range(len(A)):
if str(A[i])!="inf":
count+=1
theta1=(i)*theta_increment
# print(A[i])
x=A[i]*math.sin(theta1)
y=A[i]*math.cos(theta1)
roar.polygon.points.append(Point32(x,y,0))
pt.append([x,y,theta1,i])
if i==len(A)-1 or str(A[i+1])=="inf":
for j in range(i,i-count,-1):
theta2=(j)*theta_increment
x=B[j]*math.sin(theta2)
y=B[j]*math.cos(theta2)
roar.polygon.points.append(Point32(x,y,0))
pt.append([x,y,theta2,j])
# print(pt)
pt=[]
count=0
# print("***************************************************************")
# print(poly.points)
re.polygons.append(roar)
roar=PolygonStamped()
roar.header.frame_id="odom"
return re
def main():
rospy.init_node('pr2_state_machine')
brain = PR2RobotBrain()
brain.getReady()
area_to_explore = PolygonStamped()
center_point = PointStamped()
now = rospy.Time.now()
area_to_explore.header.stamp = now
area_to_explore.header.frame_id = "map"
points = [Point32(-1.65, -1.56, 0.0),
Point32(5.41, -1.7, 0.0),
Point32(5.57, 4.44, 0.0),
Point32(-1.75, 4.37, 0.0)]
area_to_explore.polygon = Polygon(points)
center_point.header = area_to_explore.header
center_point.point.x = 1.83
center_point.point.y = 1.57
center_point.point.z = 0.0
brain = PR2RobotBrain(area_to_explore, center_point)
brain.loop()
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 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 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 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 read_field_file(self):
# Setup msgs
self.safety_msg = PolygonStamped()
self.boundry_msg = PolygonStamped()
self.cut_area_msg = PolygonStamped()
self.safety_msg.header.stamp = rospy.Time.now()
self.safety_msg.header.frame_id = self.field_frame_id
self.boundry_msg.header = self.safety_msg.header
self.cut_area_msg.header = self.safety_msg.header
# Parse out the points
polygon_points = []
polygon_points32 = []
point_count = 0
for point in self.field_polygon:
point_count += 1
if point['fix_type'] < 3:
rospy.logwarn('Point %i has a low quality fix type of %i'
% (point_count, point['fix_type']))
(easting, northing) = (point['easting'], point['northing'])
polygon_points.append((float(easting), float(northing)))
polygon_points32.append(Point32(float(easting), float(northing), 0))
# Put the points into the boundry_msg
self.boundry_msg.polygon = Polygon(polygon_points32)
# Expand and contract the field shape for safety buffer and cut area
safety_points = self.offset_polygon(polygon_points, 2)
cut_area_points = self.offset_polygon(polygon_points, -0.5)
self.safety_msg.polygon = Polygon(safety_points)
self.cut_area_msg.polygon = Polygon(cut_area_points)
return True
def getting_cordi(A,B,shu):
theta_increment=shu
re=PolygonArray()
re.header.frame_id="base_scan"
roar=PolygonStamped()
roar.header.frame_id="base_scan"
roar.header.stamp=rospy.Time.now()
pt=Exp_msg()
count=0
baby=Ipoly()
for i in range(len(A)):
if str(A[i])!="inf":
if count==0:
for t in range(3,1,-1):
theta1=(i-t)*theta_increment
x=A[i]*math.cos(theta1)
y=A[i]*math.sin(theta1)
roar.polygon.points.append(Point32(x,y,0))
pt.bliss.append(Cordi(x,y,0))
count+=1
theta1=(i)*theta_increment
x=A[i]*math.cos(theta1)
y=A[i]*math.sin(theta1)
roar.polygon.points.append(Point32(x,y,0))
pt.bliss.append(Cordi(x,y,0))
if i==len(A)-1 or str(A[i+1])=="inf" or abs(A[i+1]-A[i])>0.1:
for t in range(1,4):
theta1=(t+i)*theta_increment
x=A[i]*math.cos(theta1)
y=A[i]*math.sin(theta1)
roar.polygon.points.append(Point32(x,y,0))
pt.bliss.append(Cordi(x,y,0))
for t in range(3,1,-1):
theta1=(t+i)*theta_increment
x=B[i]*math.cos(theta1)
y=B[i]*math.sin(theta1)
roar.polygon.points.append(Point32(x,y,0))
pt.bliss.append(Cordi(x,y,0))
for j in range(i,i-count,-1):
theta2=(j)*theta_increment
x=B[j]*math.cos(theta2)
y=B[j]*math.sin(theta2)
roar.polygon.points.append(Point32(x,y,0))
pt.bliss.append(Cordi(x,y,0))
for t in range(-1,-4,-1):
theta2=(j+t)*theta_increment
x=B[j]*math.cos(theta2)
y=B[j]*math.sin(theta2)
roar.polygon.points.append(Point32(x,y,0))
pt.bliss.append(Cordi(x,y,0))
baby.eternal_bliss.append(pt)
pt=Exp_msg()
count=0
re.polygons.append(roar)
roar=PolygonStamped()
roar.header.frame_id="base_scan"
roar.header.stamp=rospy.Time.now()
pubf=rospy.Publisher("ol2",Ipoly,queue_size=0)
pubf.publish(baby)
return re
def main():
rospy.init_node('kobuki_exploration')
area_to_explore = PolygonStamped()
center_point = PointStamped()
now = rospy.Time.now()
area_to_explore.header.stamp = now
area_to_explore.header.frame_id = "map"
points = [
Point32(-2.65, -2.56, 0.0),
Point32(5.41, -2.7, 0.0),
Point32(5.57, 4.44, 0.0),
Point32(-2.75, 4.37, 0.0)
]
area_to_explore.polygon = Polygon(points)
center_point.header = area_to_explore.header
center_point.point.x = 1.83
center_point.point.y = 1.57
center_point.point.z = 0.0
brain = PR2RobotBrain(area_to_explore, center_point)
brain.getReady()
brain.loop()
def getting_cordi(A, B, shu):
theta_increment = shu
re = PolygonArray()
re.header.frame_id = "odom"
roar = PolygonStamped()
roar.header.frame_id = "odom"
roar.header.stamp = rospy.Time.now()
pt = Exp_msg()
count = 0
baby = Ipoly()
for i in range(len(A)):
if str(A[i]) != "inf":
if count == 0:
for t in range(10, 1, -1):
theta1 = (i - t) * theta_increment
x = A[i] * math.cos(theta1)
y = A[i] * math.sin(theta1)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
count += 1
theta1 = (i) * theta_increment
x = A[i] * math.cos(theta1)
y = A[i] * math.sin(theta1)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
if i == len(A) - 1 or abs(A[i + 1] - A[i]) > 0.15:
# ra=A[i]/math.cos(theta_increment)
for t in range(1, 10):
theta1 = (t + i) * theta_increment
# ra=ra/math.cos(theta_increment)
x = A[i] * math.cos(theta1)
y = A[i] * math.sin(theta1)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
for t in range(10, 1, -1):
theta1 = (t + i) * theta_increment
x = (B[i]) * math.cos(theta1)
y = (B[i]) * math.sin(theta1)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
for j in range(i, i - count, -1):
theta2 = (j) * theta_increment
x = B[j] * math.cos(theta2)
y = B[j] * math.sin(theta2)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
for t in range(-1, -10, -1):
theta2 = (j + t) * theta_increment
x = B[j] * math.cos(theta2)
y = B[j] * math.sin(theta2)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
baby.eternal_bliss.append(pt)
pt = Exp_msg()
count = 0
re.polygons.append(roar)
roar = PolygonStamped()
roar.header.frame_id = "odom"
roar.header.stamp = rospy.Time.now()
return re
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 pcb(event):
cv = []
for x in range(0,6):
cv.append([])
for y in range(0, 6):
cv[x].append(last_update.covariance[6*x+y])
cv = np.array(cv)
e = np.linalg.eigvals(cv)
e *= 0.05 #0.01
std = np.sqrt(e)
#print(std)
dx = BASE_LENGTH/2 + std[0] * N_SIGMA
dy = BASE_WIDTH/2 + std[1] * N_SIGMA
dth = std[2] * N_SIGMA
quaternion = [0, 0, dth, 1.0]
deuler = tf.transformations.euler_from_quaternion(quaternion)
dyaw = deuler[2]
dth = dyaw
p = PolygonStamped()
x = last_position.pose.position.x
y = last_position.pose.position.y
quaternion = [last_position.pose.orientation.x, last_position.pose.orientation.y, last_position.pose.orientation.z, last_position.pose.orientation.w]
euler = tf.transformations.euler_from_quaternion(quaternion)
yaw = euler[2]
th = yaw
#print((x, dx))
#print((y, dy))
#print((th, dth))
points = [
Point32(x + (dx * cos(th-dth) - dy * sin(th-dth)),y + (dx * sin(th - dth) + dy*cos(th-dth)), 0),
Point32(x + (dx * cos(th) - dy * sin(th)),y + (dx * sin(th) + dy*cos(th)), 0),
Point32(x + (dx * cos(th+dth) - dy * sin(th+dth)),y + (dx * sin(th+ dth) + dy*cos(th+dth)), 0),
Point32(x + (-dx * cos(th-dth) - dy * sin(th-dth)),y + (-dx * sin(th - dth) + dy*cos(th-dth)), 0),
Point32(x + (-dx * cos(th) - dy * sin(th)),y + (-dx * sin(th)+ dy*cos(th)), 0),
Point32(x + (-dx * cos(th+dth) - dy * sin(th+dth)),y + (-dx * sin(th + dth) + dy*cos(th+dth)), 0),
Point32(x + (-dx * cos(th-dth) + dy * sin(th-dth)),y + (-dx * sin(th - dth) - dy*cos(th-dth)), 0),
Point32(x + (-dx * cos(th)+ dy * sin(th)),y + (-dx * sin(th) - dy*cos(th)), 0),
Point32(x + (-dx * cos(th+dth) + dy * sin(th+dth)),y + (-dx * sin(th + dth) - dy*cos(th+dth)), 0),
Point32(x + (dx * cos(th-dth) + dy * sin(th-dth)),y + (dx * sin(th - dth) - dy*cos(th-dth)), 0),
Point32(x + (dx * cos(th) + dy * sin(th)),y + (dx * sin(th) - dy*cos(th)), 0),
Point32(x + (dx * cos(th+dth) + dy * sin(th+dth)),y + (dx * sin(th + dth) - dy*cos(th+dth)), 0)
]
p.polygon.points = points
p.header.stamp = rospy.Time.now()
p.header.frame_id="map"
ppub.publish(p)
ip = PolygonStamped()
ip.header.frame_id = "map"
ip.header.stamp = rospy.Time.now()
ip.polygon.points= [
Point32(x + (BASE_LENGTH/2 * cos(th) - BASE_WIDTH/2 * sin(th)),y + (BASE_LENGTH/2 * sin(th) + BASE_WIDTH/2*cos(th)), 0),
Point32(x + (-BASE_LENGTH/2 * cos(th) - BASE_WIDTH/2 * sin(th)),y + (-BASE_LENGTH/2 * sin(th)+ BASE_WIDTH/2*cos(th)), 0),
Point32(x + (-BASE_LENGTH/2 * cos(th)+ BASE_WIDTH/2 * sin(th)),y + (-BASE_LENGTH/2 * sin(th) - BASE_WIDTH/2*cos(th)), 0),
Point32(x + (BASE_LENGTH/2 * cos(th) + BASE_WIDTH/2 * sin(th)),y + (BASE_LENGTH/2 * sin(th) - BASE_WIDTH/2*cos(th)), 0)
]
ipPub.publish(ip)
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 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 getting_cordi(A,B,shu):
theta_increment=shu
re=PolygonArray()
re.header.frame_id="odom"
roar=PolygonStamped()
roar.header.frame_id="odom"
pt=[]
count=0
# print("This is A:")
# print(A)
for i in range(len(A)):
if str(A[i])!="inf":
if count==0:
for t in range(3,1,-1):
theta1=(i-t)*theta_increment
# print(A[i])
x=A[i]*math.cos(theta1)
y=A[i]*math.sin(theta1)
roar.polygon.points.append(Point32(x,y,0))
count+=1
theta1=(i)*theta_increment
# print(A[i])
x=A[i]*math.cos(theta1)
y=A[i]*math.sin(theta1)
roar.polygon.points.append(Point32(x,y,0))
pt.append([x,y,theta1,i])
if i==len(A)-1 or str(A[i+1])=="inf" or abs(A[i+1]-A[i])>0.5:
for t in range(1,5):
theta1=(t+i)*theta_increment
x=A[i]*math.cos(theta1)
y=A[i]*math.sin(theta1)
roar.polygon.points.append(Point32(x,y,0))
for t in range(3,0,-1):
theta1=(t+i)*theta_increment
x=B[i]*math.cos(theta1)
y=B[i]*math.sin(theta1)
roar.polygon.points.append(Point32(x,y,0))
for j in range(i,i-count,-1):
theta2=(j)*theta_increment
x=B[j]*math.cos(theta2)
y=B[j]*math.sin(theta2)
roar.polygon.points.append(Point32(x,y,0))
pt.append([x,y,theta2,j])
for t in range(-1,-3,-1):
theta2=(j+t)*theta_increment
x=B[j]*math.cos(theta2)
y=B[j]*math.sin(theta2)
roar.polygon.points.append(Point32(x,y,0))
# print(pt)
pt=[]
count=0
# print("***************************************************************")
# print(poly.points)
re.polygons.append(roar)
roar=PolygonStamped()
roar.header.frame_id="odom"
return re
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 publish_obstacle_msg():
pub = rospy.Publisher('/test_optim_node/obstacles',
ObstacleMsg,
queue_size=1)
#pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleMsg, queue_size=1)
rospy.init_node("test_obstacle_msg")
obstacle_msg = ObstacleMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
# Add point obstacle
obstacle_msg.obstacles.append(PolygonStamped())
obstacle_msg.obstacles[0].polygon.points = [Point32()]
obstacle_msg.obstacles[0].polygon.points[0].x = 1.5
obstacle_msg.obstacles[0].polygon.points[0].y = 0
obstacle_msg.obstacles[0].polygon.points[0].z = 0
# Add line obstacle
obstacle_msg.obstacles.append(PolygonStamped())
line_start = Point32()
line_start.x = -2.5
line_start.y = 0.5
#line_start.y = -3
line_end = Point32()
line_end.x = -2.5
line_end.y = 2
#line_end.y = -4
obstacle_msg.obstacles[1].polygon.points = [line_start, line_end]
# Add polygon obstacle
obstacle_msg.obstacles.append(PolygonStamped())
v1 = Point32()
v1.x = -1
v1.y = -1
v2 = Point32()
v2.x = -0.5
v2.y = -1.5
v3 = Point32()
v3.x = 0
v3.y = -1
obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3]
r = rospy.Rate(10) # 10hz
t = 0.0
while not rospy.is_shutdown():
# Vary y component of the point obstacle
obstacle_msg.obstacles[0].polygon.points[0].y = 1 * math.sin(t)
t = t + 0.1
pub.publish(obstacle_msg)
r.sleep()
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 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):
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 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 _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 spin(self):
# base convex hull
hull = self.build_base_hull(self.footprint)
# add points to the hull
now = rospy.Time.now()
try:
for target_frame in self.target_frames:
joint = PointStamped()
joint.header.frame_id = target_frame
joint.header.stamp = now
# transform
self.tf_listener.waitForTransform(target_frame,
self.footprint_frame, now,
rospy.Duration(1))
tf_joint = self.tf_listener.transformPoint(
self.footprint_frame, joint)
# append joint and its inflated version
r = self.inflation_radius
np_joint = np.array([
(tf_joint.point.x, tf_joint.point.y),
(tf_joint.point.x + r, tf_joint.point.y + r),
(tf_joint.point.x + r, tf_joint.point.y - r),
(tf_joint.point.x - r, tf_joint.point.y + r),
(tf_joint.point.x - r, tf_joint.point.y - r)
])
hull.add_points(np_joint)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
pass
except tf.Exception:
rospy.logwarn("TF Exception. Possible clock reset.")
hull.close()
# generate and publish Polygon, PolygonStamped
polygon_msg = self.build_polygon_msg(hull)
polygon_stamped_msg = PolygonStamped()
polygon_stamped_msg.header.frame_id = self.footprint_frame
polygon_stamped_msg.header.stamp = now
polygon_stamped_msg.polygon = polygon_msg
self.pub.publish(polygon_msg)
self.pub_stamped.publish(polygon_stamped_msg)
# publish base footprint
self.base_msg.header.stamp = now
self.pub_original_stamped.publish(self.base_msg)
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_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 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 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 _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 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
