def read_field_file(self): # Setup msgs self.safety_msg = PolygonStamped() self.boundry_msg = PolygonStamped() self.cut_area_msg = PolygonStamped() self.safety_msg.header.stamp = rospy.Time.now() self.safety_msg.header.frame_id = self.field_frame_id self.boundry_msg.header = self.safety_msg.header self.cut_area_msg.header = self.safety_msg.header # Parse out the points polygon_points = [] polygon_points32 = [] point_count = 0 for point in self.field_polygon: point_count += 1 if point['fix_type'] < 3: rospy.logwarn('Point %i has a low quality fix type of %i' % (point_count, point['fix_type'])) (easting, northing) = (point['easting'], point['northing']) polygon_points.append((float(easting), float(northing))) polygon_points32.append(Point32(float(easting), float(northing), 0)) # Put the points into the boundry_msg self.boundry_msg.polygon = Polygon(polygon_points32) # Expand and contract the field shape for safety buffer and cut area safety_points = self.offset_polygon(polygon_points, 2) cut_area_points = self.offset_polygon(polygon_points, -0.5) self.safety_msg.polygon = Polygon(safety_points) self.cut_area_msg.polygon = Polygon(cut_area_points) return True
def getPersonPts(cloud): #find out which points are in front of the robot: # print cloud.header robotframe = "/base_footprint" globalframe = cloud.header.frame_id listener.waitForTransform(robotframe, globalframe, cloud.header.stamp, rospy.Duration(.20)) tcloud = listener.transformPointCloud(robotframe, cloud) legs = [] for pt in tcloud.points: if pt.x < boxback and pt.x > boxfront and abs(pt.y) < boxwidth: legs.append(pt) if len(legs) == 1: # make a leg next to the current leg pt = copy.copy(legs[0]) pt.y += .1 legs.append(pt) if len(legs) == 0: return Polygon(), Polygon(), 0, 0, 0 #now find point perpendicular to legs: center = Point32() center.x = (legs[0].x + legs[1].x) / 2.0 center.y = (legs[0].y + legs[1].y) / 2.0 center.z = legs[0].z slope = -(legs[1].x - legs[0].x) / (legs[1].y - legs[0].y) # slope=max(0.0,slope) target = copy.copy(center) norm = 1.0 / sqrt(1.0 + slope * slope) target.x -= 1.0 * norm target.y -= slope * .8 * norm if target.x < 0.0: target.y *= .2 legs.append(center) legs.append(target) #make region display: region = Polygon() for x, y in [(boxback, -boxwidth), (boxback, boxwidth), (boxfront, boxwidth), (boxfront, -boxwidth)]: pt = Point32() pt.x = x pt.y = y region.points.append(copy.copy(pt)) region.points.append(copy.copy(region.points[0])) plegs = Polygon() plegs.points = legs glegs = transformPoly(globalframe, plegs, tcloud.header) gregion = transformPoly(globalframe, region, tcloud.header) return glegs, gregion, slope / 2.0, target.x, target.y
def test_polygon(self): """Test passing and getting a Polygon message""" interface_name = 'polygon_' + self.interface_type getter_service = 'lama_interfaces/GetPolygon' setter_service = 'lama_interfaces/SetPolygon' # Set up node as well as getter and setter services. iface = self.interface_factory(interface_name, getter_service, setter_service) get_srv = rospy.ServiceProxy(iface.getter_service_name, iface.getter_service_class) set_srv = rospy.ServiceProxy(iface.setter_service_name, iface.setter_service_class) polygon = Polygon() descriptor_from_setter = set_srv(polygon) # descriptor_from_setter cannot be passed to get_srv because of # type incompatibility, "transform" it to a ..._getRequest() descriptor_to_getter = GetPolygonRequest() descriptor_to_getter.id = descriptor_from_setter.id response = get_srv(descriptor_to_getter) self.assertIsNot(polygon, response.descriptor) for point_out, point_in in zip(polygon.points, response.descriptor.points): self.assertAlmostEqual(point_out.x, point_in.x, places=5) self.assertAlmostEqual(point_out.y, point_in.y, places=5) self.assertAlmostEqual(point_out.z, point_in.z, places=5) polygon = Polygon() polygon.points.append(Point32()) polygon.points[0].x = 45.6 polygon.points[0].y = -34.0 polygon.points[0].z = -26.4 polygon.points.append(Point32()) polygon.points[1].x = -5.6 polygon.points[1].y = -3.0 polygon.points[1].z = 6.4 descriptor_from_setter = set_srv(polygon) # descriptor_from_setter cannot be passed to get_srv because of # type incompatibility, "transform" it to a ..._getRequest() descriptor_to_getter = GetPolygonRequest() descriptor_to_getter.id = descriptor_from_setter.id response = get_srv(descriptor_to_getter) self.assertIsNot(polygon, response.descriptor) for point_out, point_in in zip(polygon.points, response.descriptor.points): self.assertAlmostEqual(point_out.x, point_in.x, places=5) self.assertAlmostEqual(point_out.y, point_in.y, places=5) self.assertAlmostEqual(point_out.z, point_in.z, places=5)
def process_image(self, data): if self.no_one_listening(): return begin_processing = rospy.Time.now() if begin_processing - self.last_detection < self.detection_interval: return # Unpackage image from ros message cv_im = self.unpack_image(data) rectangles = self.detector.apply_frame(cv_im) # Use Super Polygon super_polygon = [] for rect in rectangles: super_polygon.append(Point32(x=rect[0][0], y=rect[0][1], z=0)) super_polygon.append(Point32(x=rect[1][0], y=rect[1][1], z=0)) super_polygon.append(Point32(x=rect[2][0], y=rect[2][1], z=0)) super_polygon.append(Point32(x=rect[3][0], y=rect[3][1], z=0)) self.people_pub.publish(Polygon(super_polygon)) # Publish to teleop_husky node self.publish_viz(rectangles, cv_im) return
def format_msg(src, Obj, s=2): assert type(Obj)==type and 'msg' in str(Obj) from sensor_msgs.msg import RegionOfInterest from geometry_msgs.msg import Point32, Polygon hd = hasattr(src,'header') if type(src)==str: im = cv2.imread(src) elif type(src)==np.ndarray: im = src.copy() elif hd: im = CVB.compressed_imgmsg_to_cv2(src) obj = Obj(header=src.header) if hd else Obj() im, res = get_instances(im, HSV_Set, {}, s) if not res: return obj, im, res if hasattr(Obj,'class_ids'): obj.class_ids = res['cid'] if hasattr(Obj,'class_names'): obj.class_names = res['cls'] if hasattr(Obj,'scores'): obj.scores = [1.0]*len(res['cls']) if hasattr(Obj,'boxes'): obj.boxes = [RegionOfInterest(x_offset=x, y_offset=y, width=w, height=h) for (x,y,w,h) in res['box']] if hasattr(Obj,'contours'): obj.contours = [Polygon([Point32( x=x, y=y) for (x,y) in c.transpose()]) for c in res['cnt']] elif hasattr(Obj,'masks'): # for masks for mk in res['mask']: # get masks m = CVB.cv2_to_imgmsg(mk.astype('uint8')*255, 'mono8') m.header = obj.header; obj.masks.append(m) return obj, im, res
def map_callback(map): global pub print("map received, building contours...") grid = np.array(map.data) grid = np.resize(grid, (map.info.height, map.info.width)) contours = measure.find_contours(grid, 0.8) print("done making contours!") array = PolygonArray() for n, contour in enumerate(contours): new_s = contour.copy() appr_s = approximate_polygon(new_s, tolerance=5) #coords = [(c[1], c[0]) for c in contour] #print(coords) #poly = shapely.geometry.Polygon(coords) #plt.plot(appr_s[:, 1], appr_s[:, 0], linewidth=1) #plt.plot(contour[:, 1], contour[:, 0], linewidth=2) poly = Polygon() poly.points = [Point(a[1], a[0], 0) for a in appr_s] if len(poly.points) > 3: array.polygons.append(poly) pub.publish(array)
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 get_roi_to_observe(self, roi_id, roi_config): """Get the roi to observe. Select objects to observe based upon this region - i.e. the recommended interesting roi """ observe_polygon = None if roi_id != "": # for (roi, meta) in self.soma_roi_store.query(SOMAROIObject._type): query = SOMAQueryROIsRequest(query_type=0, roiids=[roi_id], roiconfigs=[roi_config], returnmostrecent=True) response = self.soma_query(query) for roi in response.rois: if roi.map_name != self.soma_map: continue if roi.config != roi_config: continue if roi.id != roi_id: continue #if roi.geotype != "Polygon": continue observe_polygon = Polygon([(p.position.x, p.position.y) for p in roi.posearray.poses]) rospy.loginfo("Observe ROI: %s" % roi.type) if observe_polygon == None: rospy.logwarn("ROI given to observe not found") else: rospy.logwarn("No ROI given to observe - use robot ROI") observe_polygon = self.robot_polygon return observe_polygon
def get_robot_roi(self): """Find the roi of the robot at it's waypoint - in case no target ROI is passed Make sure the randomly generated viewpoints are all in this roi also - i.e. this room """ self.robot_polygon = None # for (roi, meta) in self.soma_roi_store.query(SOMAROIObject._type): # OLD SOMA query = SOMAQueryROIsRequest(query_type=0, roiconfigs=[self.soma_config], returnmostrecent=True) for roi in self.soma_query(query).rois: if roi.map_name != self.soma_map: continue if roi.config != self.soma_config: continue #if roi.geotype != "Polygon": continue polygon = Polygon([(p.position.x, p.position.y) for p in roi.posearray.poses]) if polygon.contains( Point([ self.robot_pose.position.x, self.robot_pose.position.y ])): rospy.loginfo("Robot (%s,%s) in ROI: %s" % (self.robot_pose.position.x, self.robot_pose.position.y, roi.type)) self.robot_polygon = polygon return True rospy.logwarn("This waypoint is not defined in a ROI") return False
def __init__(self): # Get ROS parameters self.robot_ = Polygon() for vertex in rospy.get_param('~robot'): p = Point32() p.x = vertex[0] p.y = vertex[1] self.robot_.points.append(p) self.allow_unknown_ = rospy.get_param('~allow_unknown', True) res_deg = rospy.get_param('~theta_resolution_deg', 1.0) self.theta_resolution_ = res_deg * math.pi / 180 self.occupancy_threshold_ = rospy.get_param('~occupancy_threshold', 10) self.set_config() self.service_ = rospy.ServiceProxy('collision_check', Check) self.pub_footprint_ = rospy.Publisher('footprint', OccupancyGrid, queue_size=1) self.pub_robot_ = rospy.Publisher('robot', MarkerArray, queue_size=1) rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.cb_pose, queue_size=1)
def update(self): while not rospy.is_shutdown(): if self.state == 'init': rospy.loginfo("state is {}".format(self.state)) if self.pose_goal_set == False or self.pose_start_set == False or self.node_start == None or self.node_goal == None: rospy.logwarn("Start or goal point not set yet") else: self.nodes = [self.node_start] self.state = 'build tree' elif self.state == 'build tree': rospy.loginfo("state is {}".format(self.state)) self.state, self.nodes = self.build_tree() elif self.state == 'complete': rospy.loginfo("state is {}".format(self.state)) node_current = self.nodes[-1] path = [] while node_current.parent != None: # pose_current = Point32( # node_current.point[0], node_current.point[1], 0.0) pose_parrent = Point32(node_current.parent.point[0], node_current.parent.point[1], 0.0) path.append(pose_parrent) # path.append(pose_current) self.pub_path.publish(Polygon(path)) node_current = node_current.parent self.rate.sleep() self.state = 'done' elif self.state == 'incomplete': rospy.loginfo("state is {}".format(self.state)) rospy.loginfo("Cannot find path!") else: break self.rate.sleep() if rospy.is_shutdown(): del self.nodes[:]
def find_footprint(self, points): """This function will take in an numpy.array of points in 2D and create a convex polygon that encapsilates every point provided.\ Arguments to pass in: points = 2D array of points numpy.array([:,:]) Returns a Polygon""" footprint = Polygon() if self.halo_density: buffed_points = self.halo_buffer(points, self.halo_radius, self.halo_density) else: buffed_points = points if not points.size: pass else: hull = spatial.ConvexHull(buffed_points) for vertex in hull.vertices: point = Point32() point.x = buffed_points[vertex, 0] point.y = buffed_points[vertex, 1] point.z = 0.0 footprint.points.append(point) return footprint
def main(): rospy.init_node('kobuki_exploration') area_to_explore = PolygonStamped() center_point = PointStamped() now = rospy.Time.now() area_to_explore.header.stamp = now area_to_explore.header.frame_id = "map" points = [ Point32(-2.65, -2.56, 0.0), Point32(5.41, -2.7, 0.0), Point32(5.57, 4.44, 0.0), Point32(-2.75, 4.37, 0.0) ] area_to_explore.polygon = Polygon(points) center_point.header = area_to_explore.header center_point.point.x = 1.83 center_point.point.y = 1.57 center_point.point.z = 0.0 brain = PR2RobotBrain(area_to_explore, center_point) brain.getReady() brain.loop()
def to_obsarray(): array = ObstacleArrayMsg() array.header.frame_id = 'field' oid = [0] def next_id(): oid[0] += 1 return oid[0] - 1 red_line = [to_point((0, 4.1)), to_point((1.9, 6))] blue_line = [to_point((4.1, 0)), to_point((6, 1.9))] for l in (blue_line, red_line): obs = ObstacleMsg(polygon=Polygon(l)) obs.id = next_id() array.obstacles.append(obs) for stationary in ((2, 4), (4, 2)): obs = ObstacleMsg(polygon=to_square(stationary, 0.127)) obs.id = next_id() array.obstacles.append(obs) for color, pos in world.goals: obs = ObstacleMsg(polygon=to_square(pos, 0.127)) obs.id = next_id() array.obstacles.append(obs) for pos in world.cones: obs = ObstacleMsg(polygon=to_square(pos, 0.076)) obs.id = next_id() array.obstacles.append(obs) return array
def callback_marker(self,marker): if self.npoint: self.npoint=False point = Point32() self.msg_poly.header.stamp = rospy.Time.now() self.msg_poly.header.frame_id = "/mocap" point.x = marker.pose.position.x point.y = marker.pose.position.y point.z = marker.pose.position.z print('appending point') self.polygon.points.append(point) self.msg_poly.polygon = self.polygon elif self.new: self.new = False self.polygon = Polygon() self.msg_poly = PolygonStamped() self.pc += 1 print('add new polygon') elif self.save: self.save = False df = pu.ros_to_pd(self.polygon) df = pu.interpolate(df, 0.1) #self.old_poly = pu.pandas_to_ros(df) pu.save(df, 'polygons/poly_' + str(self.sc) + '_' + str(self.pc) + '.csv') print('ploygon saved') pub.publish(self.msg_poly)
def createArea(frame): poly = PolygonStamped() poly.header.frame_id = frame poly.header.stamp = rospy.Time.now() poly.polygon = Polygon() poly.polygon.points = exploration_area_vertices return poly
def build_polygon_msg(hull): msg = Polygon() for vertex_idx in hull.vertices: msg.points.append( Point32(hull.points[vertex_idx, 0], hull.points[vertex_idx, 1], 0)) return msg
def mock_road_obstacle(): pub = rospy.Publisher('/obstacles', PolygonArray, queue_size=10) pub_r = rospy.Publisher('/road_markings', RoadMarkings, queue_size=10) sub = rospy.Subscriber('/setpoint', Float32, callback) rospy.init_node('mock_road_obstacle', anonymous=True) marking = RoadMarkings marking.left_line = [2, 0, 0] marking.center_line = [0.59, 0, 0] marking.right_line = [-1.5, 0, 0] print('RoadMarkings sent') pub_r.publish(marking) point = Point32(x=0.4, y=-0.15, z=0) polygon = Polygon() polygon.points.append(point) point.y = 0.15 polygon.points.append(point) point.x = 0.45 polygon.points.append(point) point.y = -0.15 polygon.points.append(point) polygons = PolygonArray() polygons.polygons.append(polygon) pub.publish(polygons) print('polygon sent') rospy.spin()
def handle_current_area_service(self, req): """ Will handle the get_current_map_area service call. This function will get the XY coordinates of a point in parameter and determine in which room this point is located. Returns a response containing the room name. :param req: input parameters of the service :type action: CurrentArea """ x_robot = req.coord_x y_robot = req.coord_y point_to_check = Point(x_robot, y_robot) for room in self.coord_IMs.keys(): list_points = [] for i in range(0, len(self.coord_IMs[room].keys())): list_points.append((self.coord_IMs[room][str(i)]['x'], self.coord_IMs[room][str(i)]['y'])) self.polygon_room = Polygon(list_points) validation = self.polygon_room.contains(point_to_check) if validation == True: return CurrentAreaResponse(room) return CurrentAreaResponse('')
def handle_image(self, data): if self.no_one_listening(): return begin_processing = rospy.Time.now() if begin_processing - self.last_detection < self.detection_interval: return # don't do anything unless enough time has elapsed # convert from ros message to openCV image cv2_image = self.ros_msg_to_cv2(data) rectangles = self.detector.apply_frame(cv2_image) # publish super_polygon super_polygon = [] for rect in rectangles: super_polygon.append(Point32(x=rect[0][0], y=rect[0][1], z=0)) super_polygon.append(Point32(x=rect[1][0], y=rect[1][1], z=0)) super_polygon.append(Point32(x=rect[2][0], y=rect[2][1], z=0)) super_polygon.append(Point32(x=rect[3][0], y=rect[3][1], z=0)) self.people_publisher.publish(Polygon(super_polygon)) self.publish_viz(rectangles, cv2_image) elapsed = rospy.Time.now() - begin_processing # adjust frame processing rate to match detector rate, # plus a small margin self.detection_interval = rospy.Duration(elapsed.to_sec() + 0.1)
def __init__(self, **kwargs): assert all(['_' + key in self.__slots__ for key in kwargs.keys()]), \ "Invalid arguments passed to constructor: %r" % kwargs.keys() from std_msgs.msg import Header self.header = kwargs.get('header', Header()) from geometry_msgs.msg import Polygon self.polygon = kwargs.get('polygon', Polygon())
def generate_road_poly_msg(self): # update road poly # generate road poly list road_poly_list = [] for pose in self.env.env.road_poly: p1_l = [pose[0][0][0], pose[0][0][1]] p1_r = [pose[0][1][0], pose[0][1][1]] p2_r = [pose[0][2][0], pose[0][2][1]] p2_l = [pose[0][3][0], pose[0][3][1]] if pose[1] != (1, 1, 1) and pose[1] != ( 1, 0, 0): # get rid of corner markers road_poly_list.append([p1_l, p1_r, p2_r, p2_l]) # inflate road poly road_poly_inflated = inflate(road_poly_list, self.inflation_radius) # create road poly message road_poly_array = [] for poly in road_poly_inflated: p1_l = Point32(poly[0][0], poly[0][1], 0) # z is always 0 p1_r = Point32(poly[1][0], poly[1][1], 0) p2_r = Point32(poly[2][0], poly[2][1], 0) p2_l = Point32(poly[3][0], poly[3][1], 0) road_poly = Polygon([ p1_l, p1_r, p2_r, p2_l, p1_l ]) # add first again so that it works with point in poly check road_poly_array.append(road_poly) road_poly_msg = RoadPoly(road_poly_array) # send road poly message time.sleep(1.0) self.road_poly_pub.publish(road_poly_msg) time.sleep(1.0)
def setup(self): self.inflation_radius = float( rospy.get_param('inflation_radius', '0.0')) #points = rospy.get_param('roi', '[]') roi = rospy.get_param('roi', []) points = [] for point in roi: rospy.loginfo('Point: %s', point) points.append(Point32(float(point[0]), float(point[1]), 0)) polygon = Polygon(points) self.roi = polygon self.views_at_pose = int(rospy.get_param('views_at_pose', '8')) self.min_pan = float(rospy.get_param('min_pan', '-2.09')) self.max_pan = float(rospy.get_param('max_pan', '2.09')) self.min_tilt = float( rospy.get_param('min_tilt', '-0.22') ) # self.min_tilt = float(rospy.get_param('min_tilt', '-0.22')) self.max_tilt = float(rospy.get_param('max_tilt', '0.52')) self.velocity = float(rospy.get_param('velocity', '1.0')) rospy.loginfo("Wait for nav_goals") rospy.wait_for_service('nav_goals') rospy.loginfo("Done (nav_goals)") try: self.nav_goals = rospy.ServiceProxy('nav_goals', NavGoals) except rospy.ServiceException, e: rospy.logerr("Service call failed: %s" % e)
def publish_obstacle_msg(): rospy.init_node("test_obstacle_msg") pub = rospy.Publisher('/transparent_obstacle', Polygon, queue_size=1) obstacle_msg = Polygon() # Add polygon obstacle v1 = Point32() v1.x = -3 v1.y = 3 v2 = Point32() v2.x = -4 v2.y = 4 v3 = Point32() v3.x = -6 v3.y = 3 v4 = Point32() v4.x = -7 v4.y = 4 obstacle_msg.points = [v1, v2, v3, v4] # obstacle_msg.points = [v1, v2] pub.publish(obstacle_msg) r = rospy.Rate(10) # 10hz # t = 0.0 while not rospy.is_shutdown(): # # Vary y component of the point obstacle # obstacle_msg.points[0].y = 1*math.sin(t) # t = t + 0.1 pub.publish(obstacle_msg) r.sleep()
def pose_callback(self, pose_msg): ''' Uses a pose message to generate an odometry and state message. :param pose_msg: (geometry_msgs/PoseStamped) pose message ''' new_pose_msg, trans, rot = self.tf_to_pose("LO01_base_link", "map", pose_msg.header) if not self.use_amcl: pose_msg = new_pose_msg self.new_pose_pub.publish( pose_msg ) # if using AMCL, this will be the same as the original pose message if trans and rot: # if not getting tf, trans and rot will be None footprint = PolygonStamped() footprint.header = pose_msg.header # has same frame_id (map) and time stamp loomo_points = np.array([[0.16, -0.31], [0.16, 0.31], [-0.16, 0.31], [-0.16, -0.31]]) roll, pitch, yaw = tf.transformations.euler_from_quaternion(rot) rot = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]]) rotated_points = np.matmul(rot, loomo_points.T) # 2x4 array rot_and_trans = rotated_points + np.array([[trans[0]], [trans[1]]]) polygon = Polygon( points=[Point32(x=x, y=y, z=0) for x, y in rot_and_trans.T]) footprint.polygon = polygon self.footprint_pub.publish(footprint) odom_msg = Odometry() odom_msg.pose.pose = pose_msg.pose odom_msg.header = pose_msg.header self.odom_pub.publish(odom_msg) state_msg = State() state_msg.header = pose_msg.header state_msg.state_stamp = pose_msg.header.stamp state_msg.pos.x = pose_msg.pose.position.x state_msg.pos.y = pose_msg.pose.position.y state_msg.pos.z = pose_msg.pose.position.z state_msg.quat.x = pose_msg.pose.orientation.x state_msg.quat.y = pose_msg.pose.orientation.y state_msg.quat.z = pose_msg.pose.orientation.z state_msg.quat.w = pose_msg.pose.orientation.w if self.last_state_time and self.last_state: dt = pose_msg.header.stamp.nsecs - self.last_state_time state_msg.vel.x = (state_msg.pos.x - self.last_state.pos.x) / (float(dt) / 10**9) state_msg.vel.y = (state_msg.pos.y - self.last_state.pos.y) / (float(dt) / 10**9) state_msg.vel.z = 0 # ground robot not traveling in z direction self.last_state_time = pose_msg.header.stamp.nsecs self.last_state = state_msg self.state_pub.publish(state_msg)
def __init__(self): self.npoint = False self.save = False self.new = False self.polygon = Polygon() self.msg_poly = PolygonStamped() self.pc = 0 self.sc = 12
def __init__(self): print 'status init' self.depth = PointCloud() self.image = Image() self.roi = Polygon() self.humans = Humans() self.objects = Objects() self.robots = Robots()
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from std_msgs.msg import Header self.header = kwargs.get('header', Header()) from geometry_msgs.msg import Polygon self.polygon = kwargs.get('polygon', Polygon())
def kinematicCalculation(self, objMsg, Js, rconfig=None): H = self.__createHomoCam(Js, rconfig) self.worldCoors = [] self.labels = [] self.rects = [] for l, rect in zip(objMsg.labels, objMsg.rects): cnt, center = self.__rect2CntAndCenter(rect) temp = self.calculate3DCoor(cnt, HCamera=H) # r = [self.calculate2DCoor(i[1], i[0], HCamera=H) for i in temp if i[0] is not None] # print temp poly = Polygon() for n, p in temp: # print p if n is None: continue p2D = self.calculate2DCoor([p], n, HCamera=H)[0] if p2D is not None: poly.points.append(Point32(x=p2D[0], y=p2D[1])) center = self.calculate3DCoor([center], HCamera=H)[0] fromBase = None if center[0] is None: continue elif center[0] == "ground": # center[1] = np.hstack((center[1],1)) center_aux = np.hstack((center[1], 1)).T fromBase = np.matmul(self.H1_ground, center_aux)[:-1] / 10.0 elif center[0] == "right": center_aux = np.hstack((center[1], 1)).T fromBase = np.matmul(self.H1_right, center_aux)[:-1] / 10.0 elif center[0] == "left": center_aux = np.hstack((center[1], 1)).T fromBase = np.matmul(self.H1_left, center_aux)[:-1] / 10.0 self.worldCoors.append( Point32(x=fromBase[0], y=fromBase[1], z=fromBase[2])) # print self.H1_right # print center_aux # print fromBase self.labels.append(l) self.rects.append(poly) self.point2D1 = self.calculate2DCoor(self.points, "ground", HCamera=H) self.point2D1 = np.array(self.point2D1) self.point2D2 = self.calculate2DCoor(self.points, "left", HCamera=H) self.point2D2 = np.array(self.point2D2) self.point2D3 = self.calculate2DCoor(self.points, "right", HCamera=H) self.point2D3 = np.array(self.point2D3) msg = suchartPostDictMsg(rects=objMsg.rects, labels=objMsg.labels, world_coor=self.worldCoors) msg.header.stamp = rospy.Time.now() return msg
def toPolygon3D(geometry, is_text=False): if not is_text: geometry = db().execute(ST_AsText(geometry)).scalar() polygon = geometry.strip('POLYGON Z((').strip('))').split(',') ros = Polygon() for point in polygon[0:len(polygon) - 1]: values = [float(x) for x in point.split(' ')] ros.points.append(Point32(values[0], values[1], values[2])) return ros