def PointStamped(x=0, y=0, z=0, frame_id="/map", stamp=None, point=None):
if not stamp:
stamp = rospy.get_rostime()
if not point:
return gm.PointStamped(header=Header(frame_id, stamp), point=Point(x,y,z))
else:
return gm.PointStamped(header=Header(frame_id, stamp), point=point)
def to_PointStamped(*args, **kwargs):
'''
converts the input to a geometry_msgs/PointStamped 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:
- frame_id
- stamp
- x, y, z
'frame_id' and 'stamp' logic:
1. If frame_id or stamp are specified as keywords, they will be used,
otherwise,
2. if the passed in argument is a *Stamped type, the frame_id and stamp
will be taked from that, otherwise,
3. the rospy.BASE_FRAME and 0 time stamp will be used.
Note: if more than 1 argument then they should be in the order they appear
'''
# get point
pts = geometry_msgs.PointStamped()
pts.point = to_Point(*args, **kwargs)
if ('frame_id' in kwargs.keys()):
pts.header.frame_id = kwargs['frame_id']
elif ((len(args) == 1 and args[0] != None)
and (type(args[0]) == geometry_msgs.PoseStamped
or type(args[0]) == geometry_msgs.PointStamped
or type(args[0]) == geometry_msgs.TransformStamped
or type(args[0]) == geometry_msgs.Vector3Stamped)):
pts.header.frame_id = args[0].header.frame_id
if ('stamp' in kwargs.keys()):
pts.header.stamp = kwargs['stamp']
elif ((len(args) == 1 and args[0] != None)
and (type(args[0]) == geometry_msgs.PoseStamped
or type(args[0]) == geometry_msgs.PointStamped
or type(args[0]) == geometry_msgs.TransformStamped
or type(args[0]) == geometry_msgs.Vector3Stamped)):
pts.header.stamp = args[0].header.stamp
return pts
def execute(self, userdata):
#Listen for a point if we don't have one.
point_stamped = self.message
t_start = rospy.get_time()
r = rospy.Rate(10)
waitobj = WaitForMessage('/cloud_click_point', geo.PoseStamped)
pose_stamped = None
while point_stamped == None:
try:
pose_stamped = waitobj.get_message()
if pose_stamped != None:
point_stamped = geo.PointStamped()
point_stamped.header = pose_stamped.header
point_stamped.point = pose_stamped.pose.position
r.sleep()
except rospy.ROSException, e:
pass
if self.preempt_requested():
self.service_preempt()
return 'preempted'
if (self.time_out != None) and (
(rospy.get_time() - t_start) > self.time_out):
return 'timed_out'
def determinePointsOfInterest(self, e):
points = []
x = e.pose.position.x
y = e.pose.position.y
if len(e.convex_hull) == 0:
rospy.logerr('Entity: {0} has an empty convex hull'.format(e.id))
return []
''' Convert convex hull to map frame '''
center_pose = poseMsgToKdlFrame(e.pose)
ch = list()
for point in e.convex_hull:
p = pointMsgToKdlVector(point)
# p = center_pose * p
# p = p * center_pose
pf = kdl.Frame(kdl.Rotation(), p)
# pf = pf * center_pose # Original
pf = center_pose * pf # Test
p = pf.p
ch.append(copy.deepcopy(p)) # Needed to fix "RuntimeError: underlying C/C++ object has been deleted"
''' Loop over hulls '''
self.marker_array.markers = []
ch.append(ch[0])
for i in xrange(len(ch) - 1):
dx = ch[i+1].x() - ch[i].x()
dy = ch[i+1].y() - ch[i].y()
length = math.hypot(dx, dy)
d = self._edge_distance
while d < (length-self._edge_distance):
''' Point on edge '''
xs = ch[i].x() + d/length*dx
ys = ch[i].y() + d/length*dy
''' Shift point inwards and fill message'''
ps = gm.PointStamped()
ps.header.frame_id = "/map"
ps.point.x = xs - dy/length * self._edge_distance
ps.point.y = ys + dx/length * self._edge_distance
ps.point.z = e.pose.position.z + e.z_max
points.append(ps)
self.marker_array.markers.append(self.create_marker(ps.point.x, ps.point.y, ps.point.z))
# ToDo: check if still within hull???
d += self._spacing
self.marker_pub.publish(self.marker_array)
return points
def kdl_vector_stamped_to_point_stamped(vector_stamped):
"""Convert a VectorStamped to a PointStamped
:param vector_stamped the VectorStamped to be converted
:returns PointStamped"""
ps = gm.PointStamped()
ps.header.frame_id = vector_stamped.frame_id
ps.point = gm.Point(vector_stamped.vector.x(), vector_stamped.vector.y(),
vector_stamped.vector.z())
return ps
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 test_make_topic_strings_with_header(self):
strings = ez_model.make_topic_strings(geo_msgs.PointStamped(),
'/cmd_vel')
self.assertEqual(len(strings), 7)
self.assertTrue('/cmd_vel/header/seq' in strings)
self.assertTrue('/cmd_vel/header/stamp/secs' in strings)
self.assertTrue('/cmd_vel/header/stamp/nsecs' in strings)
self.assertTrue('/cmd_vel/header/frame_id' in strings)
self.assertTrue('/cmd_vel/point/x' in strings)
self.assertTrue('/cmd_vel/point/y' in strings)
self.assertTrue('/cmd_vel/point/z' in strings)
def create_3d_point(x, y, z, frame=None):
""" Create a geometry_msgs/Point or geometry_msgs/PointStamped
(if frame is provided) from 3D coordinates """
point = geometry_msgs.PointStamped()
point.point.x = x
point.point.y = y
point.point.z = z
if frame:
point.header.frame_id = frame
return point
else:
return point.point
def process_result(self, face_result):
face = face_result.face_positions[0]
point_stamped = geo.PointStamped()
point_stamped.header.frame_id = face.header.frame_id
point_stamped.point = face.pos
frame_id_T_frame_name = tfu.origin_to_frame(
point_stamped, self.orientation_frame, self.robot.tf_listener,
point_stamped.header.frame_id)
ps = stamp_pose(mat_to_pose(frame_id_T_frame_name),
point_stamped.header.frame_id)
ps = change_pose_stamped_frame(self.robot.tf_listener, ps,
'/base_link')
self.broadcast_transform_srv(self.frame_name, ps)
def _resolve(self):
# type is a reserved keyword. Maybe unpacking a dict as kwargs is
# cleaner
query = SimpleQueryRequest(id=self.entity_designator.resolve())
entities = self.ed(query).entities
if entities:
entity = entities[0]
pointstamped = gm.PointStamped(point=entity.pose.position,
header=std.Header(
entity.id, rospy.get_rostime())
) # ID is also the frame ID. Ed just works that way
self._current = pointstamped
return self.current
else:
return None
def get_smach_state(self):
if self.wait_for_msg:
return Point3DStateSmach(self.get_name(),
self.orientation_frame,
message=None,
time_out=self.time_out)
else:
ps = geo.PointStamped()
ps.header.frame_id = self.point_frame
ps.header.stamp = rospy.Time.now()
ps.point.x = self.point[0]
ps.point.y = self.point[1]
ps.point.z = self.point[2]
return Point3DStateSmach(self.get_name(),
self.orientation_frame,
message=ps,
time_out=None)
def _point_head(self,
p,
frame,
execute_timeout=rospy.Duration(3.0),
preempt_timeout=rospy.Duration(3.0)):
p = gm.Point(*p)
ps = gm.PointStamped(point=p)
ps.header.frame_id = frame
ps.header.stamp = rospy.Time.now()
goal = PointHeadGoal(target=ps)
goal.pointing_axis = gm.Vector3(0.0, 0.0, 1.0)
goal.pointing_frame = self.pointing_frame
goal.max_velocity = 1.0
rospy.loginfo('Sending goal to head and waiting for result.')
self._ac.send_goal_and_wait(goal,
execute_timeout=execute_timeout,
preempt_timeout=preempt_timeout)
agent_beta = []
#Counterclockwise angle
for name in agent_names:
if not agent_positions[name] is None and np.linalg.norm(
position - agent_positions[name]) <= rho:
agent_beta.append(Angle(position, agent_positions[name]))
# If there is only one agent beta=2*pi
beta = 0
if len(agent_beta) > 0:
beta = min(agent_beta)
#Control law
est_dist = np.linalg.norm(estimate - position)
vel = k_d * bearing_measurement * (
est_dist - DESIRED_DISTANCE) + k_fi * est_dist * phi_bar * (alpha +
beta)
#Velocity message
cmdvel_msg = gms.Vector(x=vel[0], y=vel[1])
#Beta message
beta_msg = gm.PointStamped()
beta_msg.header.seq = 0
beta_msg.header.stamp = rp.Time.now()
beta_msg.point.x = beta
beta_msg.point.y = 0
beta_msg.point.z = 0
LOCK.release()
# Publishing
cmdvel_pub.publish(cmdvel_msg)
beta_pub.publish(beta_msg)
RATE.sleep()
velocity = np.array([msg.x, msg.y])
LOCK.release()
rp.Subscriber(name='cmdvel',
data_class=gms.Vector,
callback=cmdvel_callback,
queue_size=10)
start = False
while not rp.is_shutdown() and not start:
LOCK.acquire()
if not velocity is None:
start = True
#Position message
y_msg = gm.PointStamped()
y_msg.header.seq = 0
y_msg.header.stamp = rp.Time.now()
y_msg.point.x = position[0]
y_msg.point.y = position[1]
y_msg.point.z = 0
LOCK.release()
#Initial position publishing
pub.publish(gms.Point(x=position[0], y=position[1]))
y_pub.publish(y_msg)
RATE.sleep()
while not rp.is_shutdown():
LOCK.acquire()
if stop_publish:
rp.signal_shutdown("agent vehicle removed")
#Integration
def linepub():
rospy.init_node("rviz_marker", anonymous=True)
arr = []
arr2 = []
sc = ScanSubscriber()
sc.testvalue = arr
sc.testorientation = arr2
rospy.Subscriber("/scan", senmsg.LaserScan, sc.scanCallBack)
pub_free = rospy.Publisher("free_space_polygon",
vismsg.Marker,
queue_size=1)
pub_text = rospy.Publisher("free_space_text", vismsg.Marker, queue_size=1)
pub_wayp = rospy.Publisher("free_waypoints", vismsg.Marker, queue_size=1)
pub_side = rospy.Publisher("side_points", vismsg.Marker, queue_size=1)
pub_poly = rospy.Publisher("smaller_poly", vismsg.Marker, queue_size=1)
pub_blockp = rospy.Publisher("wall", vismsg.Marker, queue_size=1)
pub_centerpointb = rospy.Publisher("goalpoint/xy",
geomsg.PointStamped,
queue_size=1)
#pub_centerpoint_map=rospy.Publisher("goalpoint/map", geomsg.PointStamped, queue_size=1)
rate = rospy.Rate(25)
#tfBuffer = tf2_ros.Buffer()
#listener = tf2_ros.TransformListener(tfBuffer)
# mark_f - free space marker
mark_f = vismsg.Marker()
mark_f.header.frame_id = "/laser"
mark_f.type = mark_f.LINE_STRIP
mark_f.action = mark_f.ADD
mark_f.scale.x = 0.2
mark_f.color.r = 0.1
mark_f.color.g = 0.4
mark_f.color.b = 0.9
mark_f.color.a = 0.9 # 90% visibility
mark_f.pose.orientation.x = mark_f.pose.orientation.y = mark_f.pose.orientation.z = 0.0
mark_f.pose.orientation.w = 1.0
mark_f.pose.position.x = mark_f.pose.position.y = mark_f.pose.position.z = 0.0
# mark_t - text marker
mark_t = vismsg.Marker()
mark_t.header.frame_id = "/laser"
mark_t.type = mark_t.TEXT_VIEW_FACING
mark_t.ns = "basic_shapes"
mark_t.color.r = mark_t.color.g = mark_t.color.b = 0.7
mark_t.color.a = 1.0 # 100% visibility
mark_t.scale.z = 1.0
mark_t.pose.orientation.w = 1.0
# mark_w - waypoint space marker
mark_w = vismsg.Marker()
mark_w.header.frame_id = "/laser"
mark_w.type = mark_w.LINE_STRIP
mark_w.action = mark_w.ADD
mark_w.scale.x = 0.2
mark_w.color.r = 0.9
mark_w.color.g = 0.2
mark_w.color.b = 0.1
mark_w.color.a = 0.9 # 90% visibility
mark_w.pose.orientation.x = mark_w.pose.orientation.y = mark_w.pose.orientation.z = 0.0
mark_w.pose.orientation.w = 1.0
mark_w.pose.position.x = mark_w.pose.position.y = mark_w.pose.position.z = 0.0
# mark_s - sidepoint space marker
mark_s = vismsg.Marker()
mark_s.header.frame_id = "/laser"
mark_s.type = mark_s.LINE_STRIP
mark_s.action = mark_s.ADD
mark_s.scale.x = 0.2
mark_s.color.r = 0.1
mark_s.color.g = 0.9
mark_s.color.b = 0.6
mark_s.color.a = 0.9 # 90% visibility
mark_s.pose.orientation.x = mark_s.pose.orientation.y = mark_s.pose.orientation.z = 0.0
mark_s.pose.orientation.w = 1.0
mark_s.pose.position.x = mark_s.pose.position.y = mark_s.pose.position.z = 0.0
# mark_sm - smaller polygon marker
mark_sm = vismsg.Marker()
mark_sm.header.frame_id = "/laser"
mark_sm.type = mark_sm.LINE_STRIP
mark_sm.action = mark_sm.ADD
mark_sm.scale.x = 0.2
mark_sm.color.r = 0.9
mark_sm.color.g = 1
mark_sm.color.b = 0.4
mark_sm.color.a = 0.9 # 90% visibility
mark_sm.pose.orientation.x = mark_sm.pose.orientation.y = mark_sm.pose.orientation.z = 0.0
mark_sm.pose.orientation.w = 1.0
mark_sm.pose.position.x = mark_sm.pose.position.y = mark_sm.pose.position.z = 0.0
# mark_e - endpoint space marker
mark_e = vismsg.Marker()
mark_e.header.frame_id = "/laser"
mark_e.type = mark_e.LINE_STRIP
mark_e.action = mark_e.ADD
mark_e.scale.x = 0.2
mark_e.color.r = 0.8
mark_e.color.g = 0.5
mark_e.color.b = 0
mark_e.color.a = 0.9 # 90% visibility
mark_e.pose.orientation.x = mark_e.pose.orientation.y = mark_e.pose.orientation.z = 0.0
mark_e.pose.orientation.w = 1.0
mark_e.pose.position.x = mark_e.pose.position.y = mark_e.pose.position.z = 0.0
while not rospy.is_shutdown():
if free_space is not None:
# marker line points
mark_f.points = []
pl1 = geomsg.Point()
pl1.x = -0.2
pl1.y = -1.0
pl1.z = 0.0 # x an y mismatch?
pl2 = geomsg.Point()
pl2.x = -0.2
pl2.y = 0.6
pl2.z = 0.0
mark_f.points.append(pl1)
mark_f.points.append(pl2)
try:
if type(free_space) is sg.polygon.Polygon:
#mark_t.text = str(np.shape(free_space.exterior.coords)[0] - 1) + "db"
mark_t.text = ("%.3f") % (closest_node)
for l in free_space.exterior.coords[
1:]: # the last point is the same as the first
p = geomsg.Point()
p.x = l[0]
p.y = l[1]
p.z = 0.0
mark_f.points.append(p)
except:
rospy.logwarn("exception")
mark_f.points.append(pl1)
# orient_line
mark_w.points = []
if orient_line is not None and orient_line is not []:
for w in orient_line.exterior.coords:
p = geomsg.Point()
p.x = w[0]
p.y = w[1]
p.z = 0.0
mark_w.points.append(p)
mark_s.points = []
if search_pol is not None and search_pol is not []:
for s in search_pol.exterior.coords:
p = geomsg.Point()
p.x = s[0]
p.y = s[1]
p.z = 0.0
mark_s.points.append(p)
mark_sm.points = []
mark_e.points = []
if id_or > 1:
if orient_mask[id_or - 2] == True or orient_mask[
id_or - 3] == False and orient_mask[id_or -
2] == False:
if smaller_polygon is not None and smaller_polygon is not [] and block_dist >-1:
for s in smaller_polygon.exterior.coords:
p = geomsg.Point()
p.x = s[0]
p.y = s[1]
p.z = 0.0
mark_sm.points.append(p)
if block_point is not None and block_point is not [] and line_length < block_length + block_length_tolerance and line_length > block_length - block_length_tolerance and block_dist > -1:
for e in range(2):
p = geomsg.Point()
p.x = xblock[e]
p.y = yblock[e]
p.z = 0.0
mark_e.points.append(p)
"""
else:
rospy.logwarn("no wall found")
"""
if ycenter is not None and xcenter is not None and block_point is not None and block_point is not [] and line_length<block_length+block_length_tolerance and line_length>block_length-block_length_tolerance and block_dist >-1:
mark_d = geomsg.PointStamped()
mark_d.header.frame_id = "laser"
mark_d.point.x = xcenter[0] - offset_from_block
mark_d.point.y = ycenter[0]
mark_d.point.z = 0.0
pub_centerpointb.publish(mark_d)
"""
else:
rospy.logwarn("no centerpoint")
try:
target_pt = tfBuffer.transform(mark_d, "map")
pub_centerpoint_map.publish(target_pt)
except:
continue
"""
# Publish the Markers
pub_free.publish(mark_f)
pub_text.publish(mark_t)
pub_wayp.publish(mark_w)
pub_side.publish(mark_s)
pub_poly.publish(mark_sm)
pub_blockp.publish(mark_e)
rate.sleep()
#rp.logwarn('waiting for position and measurements')
LOCK.release()
#Initial estimate publishing
estimate_pub.publish(gms.Point(x=estimate[0], y=estimate[1]))
RATE.sleep()
while not rp.is_shutdown() and not stop:
LOCK.acquire()
if stop_publish:
# rp.signal_shutdown("agent estimate removed")
stop = stop_publish
#Estimate algorithm
d_estimate = -estimate_gain * (np.eye(2) - np.outer(
bearing_measurement, bearing_measurement)).dot(estimate - position)
#Integration
estimate = estimate + d_estimate * TIME_STEP
# Error norm
if not target_position is None:
error = np.linalg.norm(target_position - estimate)
#Error norm publishing
error_msg = gm.PointStamped()
error_msg.header.seq = 0
error_msg.header.stamp = rp.Time.now()
error_msg.point.x = error
error_msg.point.y = 0
error_msg.point.z = 0
error_pub.publish(error_msg)
LOCK.release()
#Estimate publishing
estimate_pub.publish(gms.Point(x=estimate[0], y=estimate[1]))
RATE.sleep()
LOCK.acquire()
if all([not data is None for data in [ position ]]):
start = True
#else:
#rp.logwarn('waiting for measurements')
LOCK.release()
RATE.sleep()
while not rp.is_shutdown():
LOCK.acquire()
if stop_publish:
rp.signal_shutdown("agent sensor removed")
#Bearing vector (phi)
bearing = (TARGET_POSITION-position)/np.linalg.norm(TARGET_POSITION-position)
#rp.logwarn(bearing)
distance=np.linalg.norm(TARGET_POSITION-position)
#Distance publishing
distance_msg=gm.PointStamped()
distance_msg.header.seq=0
distance_msg.header.stamp = rp.Time.now()
distance_msg.point.x=distance
distance_msg.point.y=0
distance_msg.point.z=0
distance_pub.publish(distance_msg)
#Bearing vector publishing
bearing_msg = gms.Vector(*bearing)
bearing_pub.publish(bearing_msg)
LOCK.release()
RATE.sleep()
