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