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()