def process_scan(self, m):
pcloud = PointCloud()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = "map"
pcloud.header = header
lidar_range_rad = np.deg2rad(270)
step = 8
theta = np.deg2rad(self.current_angle)
for i in range(0, len(m.ranges), step):
d = m.ranges[i]
phi = np.deg2rad(Node.translate(i, 0.0, float(len(m.ranges)), 0.0, 270)) - 0.7843
xyz = Node.spherical_to_cartesian(d, theta, phi)
pcloud.points.append(Point32(xyz[0], xyz[1], xyz[2]))
if self.print_once:
if not self.printed:
time.sleep(2)
xyzs = []
xs = []
ys = []
zs = []
phis = []
for i in range(len(m.ranges)):
phi = np.deg2rad(Node.translate(i, 0.0, float(len(m.ranges)), 0.0, 270)) - 0.7843
phis.append(np.rad2deg(phi))
d = m.ranges[i]
xyz = Node.spherical_to_cartesian(d, theta, phi)
xyzs.append(xyz)
xs.append(xyz[0])
ys.append(xyz[1])
zs.append(xyz[2])
f, axarr = plt.subplots(2, 2)
axarr[0, 0].plot(phis, m.ranges)
axarr[0, 0].set_title("phi vs distance")
axarr[0, 1].plot(phis, xs)
axarr[0, 1].set_title("phi vs X values")
# axarr[0,1].text(0,0,"x = r * np.sin(theta) * np.cos(phi)",fontsize=12)
axarr[1, 0].plot(phis, zs)
axarr[1, 0].set_title("phi vs Z values")
plt.show()
self.printed = True
print("done plotting")
# print Node.pp_list(m.ranges)
try:
self.publisher.publish(pcloud)
except ROSException:
pass
def publish_pointcloud(self, cloud, frame_out, cloud_publisher):
cloud_out = PointCloud()
cloud_out.header.frame_id = frame_out
cloud_out.header.stamp = rospy.Time.now()
for p in cloud:
p_out = Point32()
p_out.x = p.item(0)
p_out.y = p.item(1)
p_out.z = p.item(2)
cloud_out.points.append(p_out)
cloud_publisher.publish(cloud_out)
def getAllNode(self):
all_node = PointCloud()
all_node.header.frame_id = 'map'
for node_idx in self.nodes:
tmp_point = Point32()
tmp_point.x = self.nodes[node_idx].point[0]
tmp_point.y = self.nodes[node_idx].point[1]
tmp_point.z = 0
all_node.points.append(tmp_point)
return all_node
def set_polygon(self, pts):
msg = self.get_current_item()
del msg.polygon[0].polygon.points[:]
for pt in pts:
msg.polygon[0].polygon.points.append(Point32(pt[0], pt[1], 0))
self._update_item()
def PointCloudGen(d): GenPointCloud = PointCloud() GenPointCloud.header.frame_id = "head" for i in range(len(d)): point = Point32() point.x = d[i][2] point.y = -d[i][0]+0.03 #Needed for aligntment (idk why) point.z = -d[i][1] GenPointCloud.points.append(point) t.sleep(0.05) Choosen.publish(GenPointCloud)
def callback_odom(self, msg):
t0 = time.time()
if self.code_ready and self.cloud_ready:
t = [self.odom_msg.header.stamp.to_sec(), self.cloud_stamp.to_sec(), msg.header.stamp.to_sec()]
p = [self.odom_msg.pose.pose.position, Point32(), msg.pose.pose.position]
q = [self.odom_msg.pose.pose.orientation, Quaternion(), msg.pose.pose.orientation]
o = [quat_to_rpy(q[0]), Point32(), quat_to_rpy(q[2])]
k = ((t[1]-t[0])/(t[2]-t[0]))
if abs(o[2].x - o[0].x) > pi: o[2].x += -sign(o[2].x)*2*pi
if abs(o[2].y - o[0].y) > pi: o[2].y += -sign(o[2].y)*2*pi
if abs(o[2].z - o[0].z) > pi: o[2].z += -sign(o[2].z)*2*pi
p[1].x = p[0].x + k * (p[2].x - p[0].x)
p[1].y = p[0].y + k * (p[2].y - p[0].y)
p[1].z = p[0].z + k * (p[2].z - p[0].z)
o[1].x = o[0].x + k * (o[2].x - o[0].x)
o[1].y = o[0].y + k * (o[2].y - o[0].y)
o[1].z = o[0].z + k * (o[2].z - o[0].z)
if abs(o[1].x) > pi: o[1].x += -sign(o[1].x)*2*pi
if abs(o[1].y) > pi: o[1].y += -sign(o[1].y)*2*pi
if abs(o[1].z) > pi: o[1].z += -sign(o[1].z)*2*pi
q[1] = rpy_to_quat(o[1])
self.odom_child_frame = msg.header.frame_id
self.odom_position = p[1]
self.odom_orientation = q[1]
self.cloud_ready = False
self.interp_ready = True
self.odom_msg = msg
self.odom_ready = True
self.time[0][1] = 1e3*(time.time()-t0)
def set_place_grid(self, pts):
msg = self.get_current_item()
assert len(msg.polygon) > 0
del msg.polygon[0].polygon.points[:]
for pt in pts:
msg.polygon[0].polygon.points.append(Point32(pt[0], pt[1], 0))
self._update_item()
def pointToPointcloud(
self, x, y, z
): # FROM -> https://answers.ros.org/question/207071/how-to-fill-up-a-pointcloud-message-with-data-in-python/?answer=218951#post-id-218951
my_awesome_pointcloud = PointCloud() # declaring pointcloud
#filling pointcloud header
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'map'
my_awesome_pointcloud.header = header # assign header to pointcloud
my_awesome_pointcloud.points.append(Point32(x, y,
z)) #filling some points
self.pointcloud_publisher.publish(my_awesome_pointcloud) # publish
def __route2resp(self, route):
resp = CoveragePathResponse()
resp.header.stamp = rospy.get_rostime()
for point in route.coords:
ppoint = Point32()
ppoint.x = point[0]
ppoint.y = point[1]
resp.path.append(ppoint)
# resp.response = resp
return resp
def callback(odom):
position = odom.pose.pose.position
mean = np.array([position.x, position.y])
cov_raw = np.array(odom.pose.covariance)
cov_raw = np.reshape(cov_raw, (6, 6))
cov = cov_raw[0:2, 0:2]
samples = np.random.multivariate_normal(mean, cov, num_samples)
pub_cloud.publish(
PointCloud(header=odom.header,
points=[Point32(x, y, position.z) for (x, y) in samples]))
def Points(self, ns_):
_pc = PointCloud()
_pc.header.frame_id = "map"
for idx in ns_:
_p = Point32()
_p.x = ns_[idx].point[0]
_p.y = ns_[idx].point[1]
_p.z = ns_[idx].point[2]
_pc.points.append(_p)
# print(_pc)
self.point_pub.publish(_pc)
def e3poseCb(self, msg):
i = 2
lat = msg.latitude
lon = msg.longitude
# convert gps to grid position in ENU
x_enu, y_enu = hp.LLA_local_deltaxy(self.lat0, self.lon0, lat, lon)
p = Point32(x_enu, y_enu, 0.0)
self.e_msg.enemy_position[i] = p
# get sector
self.e_msg.enemy_sectors[i] = self.enu2sector(x_enu, y_enu, 0.0)
def __init__(self):
# number of agents, ROS parameters
self.Nd = rospy.get_param('N_defenders', 3)
self.Ne = rospy.get_param('N_attackers', 2)
# GPS coords of grid zero position
self.lat0 = rospy.get_param('lat0', 47.397742)
self.lon0 = rospy.get_param('long0', 8.5455933)
# gazebo model state
self.gz_msg = ModelStates()
# capture distance, [m]
self.cap_dist = rospy.get_param('capture_distance', 1.0)
# game time, [sec]
self.Tgame = 120
self.nRB = 0
# msgs
self.d_msg = DefendersState()
self.e_msg = EnemyState()
self.master_msg = MasterCommand()
# msg initialization
self.d_msg.header.stamp = rospy.Time.now()
self.d_msg.defenders_count = self.Nd
#self.d_msg.defenders_position = []
#self.d_msg.defenders_sectors = []
p = Point32(0.0, 0.0, 0.0)
for i in range(self.Nd):
self.d_msg.defenders_position.append(p)
self.d_msg.defenders_sectors.append(i)
self.e_msg.header.stamp = rospy.Time.now()
self.e_msg.enemy_count = self.Ne
self.e_msg.enemy_position = []
self.e_msg.enemy_sectors = []
self.e_msg.is_captured = []
for i in range(self.Ne):
self.e_msg.enemy_position.append(p)
self.e_msg.enemy_sectors.append(i + self.Nd)
self.e_msg.is_captured.append(False)
self.master_msg.allCaptured = False
self.master_msg.gameEnd = False
# loop rate
self.rate = rospy.Rate(50)
def pozyx_positioning_pub():
pub = rospy.Publisher('pozyx_positioning', Point32, queue_size=100)
rospy.init_node('positioning_pub')
try:
pozyx = pypozyx.PozyxSerial(str(comports()[0]).split(' ')[0])
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
pozyx.doPositioning(coords, remote_id=remote_id)
pub.publish(Point32(coords.x, coords.y, coords.z))
def generatePointCloud(obstacleCart):
global cloud
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = "laser"
numberOfPoints = len(obstacleCart)
cloud = PointCloud()
cloud.header = h
point = []
for i in range(0,numberOfPoints):
cloud.points.append(Point32( obstacleCart[i][0], obstacleCart[i][1], 0 ))
return cloud
def publish_zone_shape(self, task):
zone = PolygonStamped()
zone.header.frame_id = "/map"
points = []
for corner in task.zone.corners:
p = Point32()
p.x = corner.position.x
p.y = corner.position.y
p.z = corner.position.z
points.append(p)
zone.polygon.points = points
self.zone_publisher.publish(zone)
def ranges_callback(self, msg: RangeArray):
if self.pos is None:
# We have still not received the first pose from the FC
return
ranges = np.array(list(map(lambda x: x.range, msg.ranges)))
worldmap = self.mapper.update(ranges, self.pos, self.rot)
points = list(map(lambda x: Point32(x[0], x[1], x[2]), worldmap))
header = Header()
header.stamp = msg.header.stamp
header.frame_id = "map" # Needs to correspond to the settings in rviz.
msg = PointCloud(header, points, [])
self.pub.publish(msg)
def getMsg_dynamic(lidar_data):
gen = point_cloud2.read_points(lidar_data, skip_nans=True)
points_list = []
for p in gen:
if p[4] == 7:
points_list.append([p[0], p[1], p[2], p[3]])
pcl_data = pcl.PointCloud_PointXYZRGB()
pcl_data.from_list(points_list)
# downsampling 실행 코드 부분
print("Input :", pcl_data)
LEAF_SIZE = 0.1
cloud = do_voxel_grid_downssampling(pcl_data, LEAF_SIZE)
print("Output :", cloud)
# ROI 실행 코드 부분
filter_axis = 'x'
axis_min = 0.1
axis_max = 7.0
cloud = do_passthrough(cloud, filter_axis, axis_min, axis_max)
filter_axis = 'y'
axis_min = -4.0
axis_max = 4.0
cloud = do_passthrough(cloud, filter_axis, axis_min, axis_max)
filter_axis = 'z'
axis_min = 0.0
axis_max = 2.0
cloud = do_passthrough(cloud, filter_axis, axis_min, axis_max)
test = PointCloud()
get_in = ChannelFloat32()
get_in.name = 'VLP_intensity'
test.points = []
for p in cloud:
# if p[1] > 0:
park = Point32()
park.x = p[0]
park.y = p[1]
park.z = 0
get_in.values.append(p[3])
test.points.append(park)
#print("Input :", cnt)
test.channels.append(get_in)
test.header.frame_id = 'world'
test.header.stamp = rospy.Time.now()
pub.publish(test)
def heaps2cubes(self):
cubes = []
for i in range(len(self.heaps)):
if self.picked[i]:
continue
for j in range(self.n_cubes_in_heap):
cube = ObstacleMsg()
cube.header.stamp = rospy.Time.now()
cube.header.frame_id = "map"
cube.polygon.points = [Point32(*self.heaps[i][j])]
cubes.append(cube)
return cubes
def e1poseCb(self, msg):
i = 0
lat = msg.latitude
lon = msg.longitude
# convert gps to grid position in ENU
x_enu, y_enu = self.uti.global2local_ENU(lat, lon)
p = Point32(x_enu, y_enu, 0.0)
self.e_msg.enemy_position[i] = p
# get sector
self.e_msg.enemy_sectors[i] = self.enu2sector(x_enu, y_enu, 0.0)
def publish_point_cloud_messages(data_publishers, measured_distances):
point_32_msg = Point32()
point_cloud_msg = PointCloud()
for index, measured_distance in enumerate(measured_distances):
point_32_msg.x = measured_distance
point_cloud_msg.points = [point_32_msg]
point_cloud_msg.header.frame_id = range_sensors_frame_ids[index]
point_cloud_msg.header.stamp = rospy.Time.now()
data_publishers[index].publish(point_cloud_msg)
def set_smaller(pub_local, pub_global):
polygon_stamped = PolygonStamped()
ps = ((-0.01, -0.01), (-0.01, 0.01), (0.01, 0.01), (0.01, -0.01))
polygon = Polygon()
for p in ps:
point = Point32()
point.x, point.y, point.z = p[0], p[1], 0.0
polygon.points.append(point)
polygon_stamped.header.frame_id = "base_link"
pub_local.publish(polygon)
pub_global.publish(polygon)
time.sleep(0.5)
def publishDLO(self):
cloudpoints = self.estimate
cloud_msg = PointCloud()
cloud_msg.header.stamp = rospy.Time.now()
cloud_msg.header.frame_id = "yumi_base_link"
for i in range(cloudpoints.shape[0]):
cloud_msg.points.append(
Point32(cloudpoints[i, 0], cloudpoints[i, 1], cloudpoints[i,
2]))
# Change to camera frame
self.DLOPub.publish(cloud_msg)
def sub_cb(self, msg):
pnt = PointCloud()
pnt.header = msg.header
angle_step = 1.0 / (msg.range * 100.0)
angle = -msg.spread_angle
if msg.range < msg.range_max and msg.range > msg.range_min:
while angle < msg.spread_angle:
pnt.points.append(
Point32(msg.range * cos(angle), msg.range * sin(angle), 0))
angle += angle_step
print(pnt)
self.pub.publish(pnt)
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 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 callback_cmt(self, data):
print "received data: ", data
if data.points[0].z > 20:
width = max(abs(data.points[1].x - data.points[0].x),
abs(data.points[2].x - data.points[0].x),
abs(data.points[3].x - data.points[0].x))
height = max(abs(data.points[1].y - data.points[0].y),
abs(data.points[2].y - data.points[0].y),
abs(data.points[3].y - data.points[0].y))
center_x = min(data.points[0].x, data.points[1].x,
data.points[2].x, data.points[3].x) + 0.5 * width
center_y = min(data.points[0].y, data.points[1].y,
data.points[2].y, data.points[3].y) + 0.5 * height
Z = np.array([[np.float32(center_x)], [np.float32(center_y)],
[np.float32(width)], [np.float32(height)]])
self.kalman_filter.update(Z)
print("kalman filter gain:")
print self.kalman_filter.kalman.gain
new_data = Polygon()
new_data.points = [
Point32(),
Point32(),
Point32(),
Point32(),
Point32(),
Point32()
]
new_data.points[0].x = self.kalman_filter.current_prediction[
0] # center_x
new_data.points[0].y = self.kalman_filter.current_prediction[
1] # center_y
new_data.points[1].x = self.kalman_filter.current_prediction[
2] # width
new_data.points[1].y = self.kalman_filter.current_prediction[
3] # height
new_data.points[
2].x = new_data.points[0].x + 0.5 * new_data.points[1].x
new_data.points[
2].y = new_data.points[0].y + 0.5 * new_data.points[1].y
new_data.points[
3].x = new_data.points[0].x + 0.5 * new_data.points[1].x
new_data.points[
3].y = new_data.points[0].y - 0.5 * new_data.points[1].y
new_data.points[
4].x = new_data.points[0].x - 0.5 * new_data.points[1].x
new_data.points[
4].y = new_data.points[0].y - 0.5 * new_data.points[1].y
new_data.points[
5].x = new_data.points[0].x - 0.5 * new_data.points[1].x
new_data.points[
5].y = new_data.points[0].y + 0.5 * new_data.points[1].y
self.pub_kalman.publish(new_data)
def construct_scene_msg(self, segmentation_result, cluster_information):
scene = SceneMsg()
# table
table = segmentation_result.table
t_center = table.pose.pose.position
t_min = Point32(
-(t_center.y + table.y_max), # x in robot space is -y is planar
t_center.x + table.x_min,
t_center.z)
t_max = Point32(-(t_center.y + table.y_min), t_center.x + table.x_max,
t_center.z)
scene.bboxes.append(PolygonMsg([t_min, t_max]))
scene.names.append('table')
scene.colors.append('')
scene.categories.append('')
for idx, cluster in enumerate(segmentation_result.clusters):
# find cluster with index idx and grab its bounding box
#print cluster_information
#print idx
bbox = cluster_information[[ci[0] for ci in cluster_information
].index(idx)][4]
o_center = bbox.pose.pose.position
o_min = Point32(-(o_center.y + bbox.box_dims.y / 2.0),
o_center.x - bbox.box_dims.x / 2.0, o_center.z)
o_max = Point32(-(o_center.y - bbox.box_dims.y / 2.0),
o_center.x + bbox.box_dims.x / 2.0, o_center.z)
scene.bboxes.append(PolygonMsg([o_min, o_max]))
scene.names.append(str(idx))
scene.colors.append('')
scene.categories.append('')
return scene
def publish_obstacle_msg():
pub = rospy.Publisher('/test_optim_node/obstacles',
ObstacleArrayMsg,
queue_size=1)
#pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
rospy.init_node("test_obstacle_msg")
y_0 = -3.0
vel_x = 0.0
vel_y = 0.3
range_y = 6.0
obstacle_msg = ObstacleArrayMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "map" # CHANGE HERE: odom/map
# Add point obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[0].id = 99
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
yaw = math.atan2(vel_y, vel_x)
q = tf.transformations.quaternion_from_euler(0, 0, yaw)
obstacle_msg.obstacles[0].orientation = Quaternion(*q)
obstacle_msg.obstacles[0].velocities.twist.linear.x = vel_x
obstacle_msg.obstacles[0].velocities.twist.linear.y = vel_y
obstacle_msg.obstacles[0].velocities.twist.linear.z = 0
obstacle_msg.obstacles[0].velocities.twist.angular.x = 0
obstacle_msg.obstacles[0].velocities.twist.angular.y = 0
obstacle_msg.obstacles[0].velocities.twist.angular.z = 0
r = rospy.Rate(10) # 10hz
t = 0.0
while not rospy.is_shutdown():
# Vary y component of the point obstacle
if (vel_y >= 0):
obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y *
t) % range_y
else:
obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (
vel_y * t) % range_y - range_y
t = t + 0.1
pub.publish(obstacle_msg)
r.sleep()
def to_ros_div_N(self,
n,
output_type=PoseStamped,
frame_id=WORK_ORIGIN_FRAME_NAME):
#output_type:
# Point32 (for geometry_msg/PolygonStamped)
# Pose (for geometry_msgs/PoseArray)
# PoseStamped (for nav_msgs/Path)
if n <= 0:
n = 1
drad = self.crad / n
plist = []
for i in range(n + 1):
if i == n:
theta = self.rad_start + self.crad
else:
theta = self.rad_start + drad * i
x = self.cx + self.R * cos(theta)
y = self.cy + self.R * sin(theta)
if self.crad > 0: #direction of tangent line
direc = theta + pi / 2.0
else:
direc = theta - pi / 2.0
if output_type == Point32:
p = Point32(x=x, y=y, z=0)
elif output_type == Pose:
p = Pose()
p.position.x = x
p.position.y = y
p.position.z = 0
q = tf_conversions.transformations.quaternion_from_euler(
0, 0, direc)
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]
elif output_type == PoseStamped:
p = PoseStamped()
p.header.seq = 0
p.header.stamp = rospy.Time(0)
p.header.frame_id = frame_id
p.pose.position.x = x
p.pose.position.y = y
p.pose.position.z = 0
q = tf_conversions.transformations.quaternion_from_euler(
0, 0, direc)
p.pose.orientation.x = q[0]
p.pose.orientation.y = q[1]
p.pose.orientation.z = q[2]
p.pose.orientation.w = q[3]
plist.append(p)
return plist
