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
