def to_Point32(*args, **kwargs): ''' converts the input to a geometry_msgs/Point32 message supported input: - ROS Pose message (orientation ignored) - ROS PoseStamped message (orientation ignored) - ROS Point message - ROS PointStamped message - ROS Point32 message - ROS Transform message (orientation ignored) - ROS TransformStamped message (orientation ignored) - ROS Vector3 message - ROS Vector3Stamped message - kdl Frame (orientation ignored) - kdl Vector - tf transform (orientation ignored) - iterable of length 3 (list, tuple, np.array, ...) - individual x,y,z values - named x, y, z arguments. (minimum of 1 can be specified) - no arguments will result in the zero point keyword args: - x, y, z Note: if more than 1 argument then they should be in the order they appear ''' # get Point point = to_Point(*args, **kwargs) return geometry_msgs.Point32(point.x, point.y, point.z)
def to_geometry_msg(self) -> geometry_msgs.Point: """To ROS geometry_msg. Returns: This point as a geometry_msgs/Point """ return geometry_msgs.Point32(x=self.x, y=self.y, z=self.z)
def _to_ros_msg(self): msg = m.Polygon() for x, y, z in self.value: p2 = m.Point32() p2.x = x p2.y = y p2.z = z msg.points.append(p2) return msg
def generateTransparentObstacleMarkers(self): poly = msg.Polygon() try: transparent_obstacles = [e for e in self.getEntities() if isinstance(e, TranspanrentObstacle)] for to in transparent_obstacles: poly.points.extend([msg.Point32(p[0], p[1], 0.0) for p in to.getSegment().coords]) except (AttributeError, IndexError, ValueError): rospy.loginfo("Encountered problem getting transparent obstacles. Skipping...") return poly
def make_goal(self, ud, goal): """ Create a goal for the action """ room_number = ud['room_number'] segmented_map = ud['segmented_map'] # We need an image containing only the room to explore, so we create an empty image with the same # properties as the original map, set to white (\xFF) all pixels that correspond to the given room # number in the segmented map, and set as black (\x00) te rest goal.input_map = ud['map_image'] goal.input_map.data = b'' # segmented_map encoding is 32SC1, so 4 bytes, though just the first byte is enough up to 255 rooms for i in range(segmented_map.height): for j in range(segmented_map.width): idx = i * segmented_map.step + j * 4 val = segmented_map.data[idx:idx + 4] pixel = struct.unpack('<BBBB', val)[0] if pixel == room_number: goal.input_map.data += b'\xFF' else: goal.input_map.data += b'\x00' fov_points = rospy.get_param( '/exploration/room_exploration_server/field_of_view_points') goal.field_of_view_origin = TF2().transform_pose( None, 'kinect_rgb_frame', 'base_footprint').pose.position goal.field_of_view = [geometry_msgs.Point32(*pt) for pt in fov_points] goal.planning_mode = 2 # plan a path for coverage with the robot's field of view goal.starting_position = to_pose2d( ud['robot_pose']) # we need to convert to Pose2D msg # use room center as starting point; theta is ignored by room exploration room_info = ud['room_information_in_meter'][room_number - 1] start_point = geometry_msgs.PointStamped() start_point.header.frame_id = 'map' start_point.point = room_info.room_center goal.starting_position.x = room_info.room_center.x goal.starting_position.y = room_info.room_center.y goal.starting_position.theta = 0.0 # it's ignored self.start_pub.publish(start_point) # provide the starting pose so we can move there before starting exploring ud['start_pose'] = create_2d_pose(room_info.room_center.x, room_info.room_center.y, 0.0, 'map') # IDEA: can use room_info.room_min_max to avoid points colliding with the walls # visualize explore map and fov on RViz fov = geometry_msgs.PolygonStamped() fov.header.frame_id = 'kinect_rgb_frame' fov.polygon.points = goal.field_of_view rospy.Publisher('/exploration/img', sensor_msgs.Image, queue_size=1, latch=True).publish(goal.input_map) if not self.fov_pub_timer: self.fov_pub_timer = rospy.Timer( rospy.Duration(0.1), lambda e: self.fov_pub.publish(fov))
def _to_ros_msg(self): msg = m.PolygonStamped() for x, y, z in self.value: p2 = m.Point32() p2.x = x p2.y = y p2.z = z msg.polygon.points.append(p2) msg.header = self._get_header() return msg
def np_to_pointcloud(points_mat, frame): pc = sm.PointCloud() pc.header.stamp = rospy.get_rostime() pc.header.frame_id = frame for i in range(points_mat.shape[1]): p32 = gm.Point32() p32.x = points_mat[0, i] p32.y = points_mat[1, i] p32.z = points_mat[2, i] pc.points.append(p32) return pc
def to_geometry_msg(self): """To ROS geometry_msg. Returns: This polygon as a geometry_msgs.Polygon. """ msg = geometry_msgs.Polygon() msg.points = [ geometry_msgs.Point32(*p.to_numpy()) for p in self.get_points() ] return msg
def update_map(data): global numScans global map_ready global obstacles global obstacles_prev points = filter_laser_scan(data, 0, 0) scan_filtered = sensors.PointCloud() for i in range(0, len(points)): new_point = geometries.Point32() new_point.x = points[i][0] new_point.y = points[i][1] scan_filtered.points.append(new_point) scan_filtered_pub.publish(scan_filtered) if not map_ready: if numScans > 50: map_ready = True print "Map ready!" else: print "Number of scans: " + str(numScans) obstacles = distance_cluster(points) obstacle1 = geometries.Pose2D() obstacle1.theta = 0 obstacle1.x = obstacles[0][0] obstacle1.y = obstacles[0][1] obstacle1_pub.publish(obstacle1) obstacle2 = geometries.Pose2D() obstacle2.theta = 0 obstacle2.x = obstacles[1][0] obstacle2.y = obstacles[1][1] obstacle2_pub.publish(obstacle2) if not close_enough(obstacles_prev[0], obstacles[0]) and not close_enough( obstacles_prev[1], obstacles[1]): numScans = 0 obstacles_prev = obstacles if map_ready: #print "localization using map..." obstacle1 = geometries.Pose2D() obstacle1.theta = 0 obstacle1.x = obstacles[0][0] obstacle1.y = obstacles[0][1] obstacle1_pub.publish(obstacle1) obstacle2 = geometries.Pose2D() obstacle2.theta = 0 obstacle2.x = obstacles[1][0] obstacle2.y = obstacles[1][1] obstacle2_pub.publish(obstacle2) numScans = numScans + 1
def main(): #Read ROS parameter arguments, if present #phasespace_ip_addr="..." #framerate=50 # ros_params = rospy.get_param('~') # #Read command line options # parser = argparse.ArgumentParser() # parser.add_argument('server_ip') # parser.add_argument('-f', '--framerate', default=1, type=float, help='Desired framerate') # args = parser.parse_args() #Initialize the ROS node pub = rospy.Publisher('mocap_point_cloud', sensor_msgs.PointCloud) rospy.init_node('mocap_streamer') #Load the mocap stream ip = rospy.get_param('phasespace/ip', '192.168.0.6') with load_mocap.PhasespaceMocapSource(ip, num_points=10, framerate=480).get_stream() as mocap: #Play the points from the mocap stream #Loop until the node is killed with Ctrl+C frame_num = 0 while 1: frame = mocap.read(block=True)[0].squeeze() if rospy.is_shutdown(): break print('STREAMING: Frame ' + str(frame_num), end='\r') sys.stdout.flush() #Construct and publish the message message = sensor_msgs.PointCloud() message.header = std_msgs.Header() message.header.frame_id = 'world' #'mocap' #message.header.time = rospy.get_rostime() message.points = [] for i in range(frame.shape[0]): point = geometry_msgs.Point32() point.x = frame[i, 0] / 1000 point.y = -frame[i, 2] / 1000 point.z = frame[i, 1] / 1000 message.points.append(point) pub.publish(message) frame_num += 1 print('\rSTOPPED: Frame ' + str(frame_num))
def laser_scan_to_point_cloud(laser_scan_msg, density=100): """ Change type :param sensor_msgs.LaserScan laser_scan_msg: laser scans data (r, theta) type :param int intensity: laser scans data (r, theta) type """ point_cloud = sensor_msgs.PointCloud() point_cloud.header.frame_id = laser_scan_msg.header.frame_id for idx in range(0, len(laser_scan_msg.ranges), 100/density): point = geometry_msgs.Point32() a = laser_scan_msg.angle_min + laser_scan_msg.angle_increment * idx point.x = laser_scan_msg.ranges[idx] * cos(a) point.y = laser_scan_msg.ranges[idx] * sin(a) point_cloud.points.append(point) return point_cloud
def test_point_init(self): """Test if the point class can be initialize.""" # Basic p1 = Point(1, 3) self.assertListEqual([p1.x, p1.y, p1.z], [1, 3, 0]) examples = [ ([-1, -2], [-1, -2, 0]), (np.array([1, 2, 4]), [1, 2, 4]), (np.array([1, 2]), [1, 2, 0]), (g_msgs.Point32(1, 3, 4), [1, 3, 4]), ] for example in examples: # Point initialization point = Point(example[0]) self.assertListEqual([point.x, point.y, point.z], example[1]) self.assertEqual(Point(math.sqrt(2), math.sqrt(2)), Point(r=2, phi=math.pi / 4))
def point32_to_ros(p): return gm.Point32(x=p.x, y=p.y, z=p.z)
def test_point_extract(self): p1 = Point(1, 3, 2) self.assertEqual(p1.to_geometry_msg(), g_msgs.Point32(1, 3, 2))
def to_PointCloud(*args, **kwargs): ''' converts the input to a sensor_msgs/PointCloud message supported input: - ROS PointCloud messages - iterable of ROS Point messages - iterable of kdl Vectors - iterable of iterables - numpy array Nx3 or 3xN (if N == 3 then it is assumed Nx3) - no arguments will result in an empty PointCloud keyword args: - frame_id - stamp NOTE: mixing types is not supported ''' if (len(args) > 0 and type(args[0]) == sensor_msgs.PointCloud): return copy.deepcopy(args[0]) pc = sensor_msgs.PointCloud() if (len(args) > 0): pts = args[0] # numpy array if (type(pts) == np.ndarray): # 3xN (N > 3) if (max(pts.shape) > 3 and pts.shape[1] > pts.shape[0]): pts = pts.T # Nx3 or 3xN (N < 3) elif (pts.shape != (3, 3) and pts.shape[0] == 3): pts = pts.T pc.points = [geometry_msgs.Point32(p[0], p[1], p[2]) for p in pts] elif (hasattr(pts, '__iter__') and len(args[0]) > 0): # Point32 if (type(pts[0]) == geometry_msgs.Point32): pc.points = copy.deepcopy(pts) # Point elif (type(pts[0]) == geometry_msgs.Point): pc.points = [ geometry_msgs.Point32(p.x, p.y, p.z) for p in pts ] # kdl Vector elif (type(pts[0]) == kdl.Vector): pc.points = [ geometry_msgs.Point32(p.x(), p.y(), p.z()) for p in pts ] # iterable elif hasattr(pts[0], '__iter__'): pc.points = [ geometry_msgs.Point32(p[0], p[1], p[2]) for p in pts ] if ('frame_id' in kwargs.keys()): pc.header.frame_id = kwargs['frame_id'] if ('stamp' in kwargs.keys()): pc.header.stamp = kwargs['stamp'] return pc