Example #1
0
 def get_all_markers(self, frame_id="map"):
     if len(self.points) > 0:
         wx, wy, wz = getDims(self.points[0], self.points[1])
     else:
         wx, wy, wz = 0, 0, 0
     marker = Marker()
     clr_ = create_color(1.0, 0, 0, 1.0)
     pts = [
         center(mn, mx)
         for mn, mx in zip(self.points[0::2], self.points[1::2])
     ]
     colors = [clr_ for x in pts]
     marker.type = marker.CUBE_LIST
     marker.action = marker.ADD
     marker.scale.x = abs(wx)
     marker.scale.y = abs(wy)
     marker.scale.z = abs(wz)
     marker.color.a = 1.0
     marker.color.g = 1.0
     marker.pose.position.x = 0
     marker.pose.position.y = 0
     marker.pose.position.z = 0
     marker.pose.orientation.w = 1.0
     marker.points = pts
     marker.colors = colors
     marker.frame_locked = True
     marker.header.frame_id = frame_id
     return [marker]
 def draw_curve(
     self,
     pose_array,
     id=None,
     rgba=(0, 1, 0, 1),
     width=.01,
     ns='default_ns',
     duration=0,
     type=Marker.LINE_STRIP,
     ):
     if id is None:
         id = self.get_unused_id()
     marker = Marker(type=type, action=Marker.ADD)
     marker.header = pose_array.header
     marker.points = [pose.position for pose in pose_array.poses]
     marker.lifetime = rospy.Duration(0)
     if isinstance(rgba, list):
         assert len(rgba) == len(pose_array.poses)
         marker.colors = [stdm.ColorRGBA(*c) for c in rgba]
     else:
         marker.color = stdm.ColorRGBA(*rgba)
     marker.scale = gm.Vector3(width, width, width)
     marker.id = id
     marker.ns = ns
     self.pub.publish(marker)
     self.ids.add(id)
     return MarkerHandle(marker, self.pub)
Example #3
0
    def _regenerateMesh(self):
        self.mesh = mesh_creation.create_cut_cylinder(radius=self.radius,
                                                      height=self.height,
                                                      cutting_planes=[
                                                          ([0., 0.,
                                                            0.], [1., 0., 0.])
                                                      ],
                                                      sections=10)

        mesh_marker = Marker()
        mesh_marker.type = Marker.TRIANGLE_LIST
        mesh_marker.color.r = self.color[0]
        mesh_marker.color.g = self.color[1]
        mesh_marker.color.b = self.color[2]
        if len(self.color) > 3:
            mesh_marker.color.a = self.color[3]
        else:
            mesh_marker.color.a = 1.
        tris = self.mesh.faces.ravel()
        verts = self.mesh.vertices[tris, :]
        mesh_marker.points = ros_utils.arrayToPointMsgs(verts.T)
        mesh_marker.colors = [
            std_msgs.msg.ColorRGBA(self.color[0], self.color[1], self.color[2],
                                   self.color[3])
        ] * tris.shape[0]

        if self.mesh_control is not None:
            self.im_marker.controls.remove(self.mesh_control)
        self.mesh_control = InteractiveMarkerControl()
        self.mesh_control.always_visible = True
        self.mesh_control.markers.append(mesh_marker)
        self.im_marker.controls.append(self.mesh_control)
def make_wire_marker(mat, scale = 1):
	marker = Marker()
	marker.header.frame_id = "base"
	marker.header.stamp = rospy.Time.now()
	marker.ns = "z_wire_phantom"
	marker.id = 0
	marker.action = Marker.MODIFY
	marker.type = Marker.LINE_LIST
	marker.pose.position.x = mat[0,3] * scale
	marker.pose.position.y = mat[1,3] * scale
	marker.pose.position.z = mat[2,3] * scale
	q = t3d.quaternions.mat2quat(mat[0:3,0:3])
	marker.pose.orientation.x = q[1]
	marker.pose.orientation.y = q[2]
	marker.pose.orientation.z = q[3]
	marker.pose.orientation.w = q[0]
	marker.color.a = 1.0
	marker.color.r = 0.5
	marker.color.g = 0.5
	marker.color.b = 1.0
	marker.scale.x = 0.001 * scale
	wires = _wires * scale
	marker.points = [Point(p[0], p[1], p[2]) for p in wires]
	marker.colors = [marker.color for c in range(len(marker.points))]
	return marker
Example #5
0
    def publish_wall(self, xy_valid):
        ps = self.wall_finder_(xy_valid)
        if ps is None:
            return
        rospy.loginfo_throttle(1.0, '# Walls Detected : {}'.format(len(ps)))
        #r, t = rt

        ### points
        #po = np.stack([r * np.cos(t), r * np.sin(t)],axis=-1).reshape(-1,1,2) # (n,1,2)

        ### vectors
        #u = np.stack([np.sin(t), -np.cos(t)], axis=-1).reshape(-1,1,2)
        #ps = po + ((3.0*u) * np.reshape([-1.0, 1.0], (1,2,1)) )
        ps = np.asarray(ps, dtype=np.float32)

        col = ColorRGBA(1.0, 0.0, 1.0, 1.0)

        msg = Marker()
        msg.lifetime = rospy.Duration(1.0)
        msg.header.frame_id = 'base_link'
        msg.header.stamp = self.scan_t_  #rospy.Time.now()
        msg.type = msg.LINE_LIST
        msg.action = msg.ADD
        msg.points = []
        msg.pose.position = Point()
        msg.pose.orientation = Quaternion(0, 0, 0, 1)

        for (p0, p1) in ps:
            msg.points.extend(
                [Point(x=p0[0], y=p0[1]),
                 Point(x=p1[0], y=p1[1])])
        msg.scale.x = msg.scale.y = msg.scale.z = 0.1
        msg.colors = [col for _ in msg.points]
        self.wall_pub_.publish(msg)
Example #6
0
    def publish_quad(self, range, angle):
        """ Publish four points front,back,left,right to the robot """
        msg = Marker()
        msg.lifetime = rospy.Duration(1.0)
        msg.header.frame_id = 'base_link'

        fwd = range[np.argmin(np.abs(U.adiff(angle, 0.0 * np.pi)))]
        fwd = Point(x=fwd)

        lft = range[np.argmin(np.abs(U.adiff(angle, 0.5 * np.pi)))]
        lidx = np.argmin(np.abs(U.adiff(angle, 0.5 * np.pi)))
        print 'lft', lft, lidx
        lft = Point(y=lft)

        bwd = range[np.argmin(np.abs(U.adiff(angle, 1.0 * np.pi)))]
        bwd = Point(x=-bwd)

        rgt = range[np.argmin(np.abs(U.adiff(angle, -0.5 * np.pi)))]
        rgt = Point(y=-rgt)

        col = ColorRGBA(1.0, 1.0, 1.0, 1.0)

        msg.type = msg.POINTS
        msg.action = msg.ADD
        msg.points = [fwd, lft, bwd, rgt]
        msg.colors = [col, col, col, col]
        msg.scale.x = msg.scale.y = msg.scale.z = 0.2

        self.viz_pub_.publish(msg)
def make_pose_marker(mat, scale = 1):
	marker = Marker()
	marker.header.frame_id = "base"
	marker.header.stamp = rospy.Time.now()
	marker.ns = "z_wire"
	marker.id = 0
	marker.action = Marker.MODIFY
	marker.type = Marker.LINE_LIST
	marker.pose.position.x = mat[0,3] * scale
	marker.pose.position.y = mat[1,3] * scale
	marker.pose.position.z = mat[2,3] * scale
	q = t3d.quaternions.mat2quat(mat[0:3,0:3])
	marker.pose.orientation.x = q[1]
	marker.pose.orientation.y = q[2]
	marker.pose.orientation.z = q[3]
	marker.pose.orientation.w = q[0]
	marker.color.a = 1.0
	marker.color.r = 0.0
	marker.color.g = 1.0
	marker.color.b = 0.0
	marker.scale.x = 0.001 * scale
	marker.points = [Point() for p in range(6)]
	marker.points[1].x = 0.005 * scale
	marker.points[3].y = 0.005 * scale
	marker.points[5].z = 0.005 * scale
	marker.colors = [ColorRGBA(1,0,0,1), ColorRGBA(1,0,0,1),
					 ColorRGBA(0,1,0,1), ColorRGBA(0,1,0,1),
					 ColorRGBA(0,0,1,1), ColorRGBA(0,0,1,1)]
	return marker
    def add_leader_plot(self, msg, height=0.25):
        print("Adding leader.")
        leader_marker = Marker()
        leader_marker.id = self.current_id
        leader_marker.action = Marker.ADD
        leader_marker.header.frame_id = 'map'
        leader_marker.type = Marker.LINE_STRIP
        leader_marker.ns = "points"
        leader_marker.scale.x = 0.25
        leader_marker.colors = []

        leader_marker.points = []
        leader_marker.points.append(
            Point(msg.leader.initial[0], msg.leader.initial[1], height))
        leader_marker.points.append(
            Point(msg.leader.line_start[0], msg.leader.line_start[1], height))
        leader_marker.points.append(
            Point(msg.leader.line_end[0], msg.leader.line_end[1], height))
        leader_marker.points.append(
            Point(msg.leader.goal[0], msg.leader.goal[1], height))
        leader_marker.colors.append(ColorRGBA(1.0, 0.0, 0.0, 1.0))
        leader_marker.colors.append(ColorRGBA(1.0, 0.0, 0.0, 1.0))
        leader_marker.colors.append(ColorRGBA(1.0, 0.0, 0.0, 1.0))
        leader_marker.colors.append(ColorRGBA(1.0, 0.0, 0.0, 1.0))
        leader_marker.lifetime = rospy.Duration(self.refresh_rate)

        self.current_id = self.current_id + 1
        self.Markers.markers.append(leader_marker)
Example #9
0
 def write_dict_to_topic(self):
     if not self.detected_items:
         return
     msg_list = TrackedObjectList()
     for key in self.detected_items:
         msg = TrackedObject()
         msg.name = key
         msg.x = self.detected_items[key].x
         msg.y = self.detected_items[key].y
         msg.th = self.detected_items[key].th
         msg.robot_pose = self.detected_items[key].pose
         msg.confidence = self.detected_items[key].confidence
         msg_list.ob_msgs.append(msg)
     self.pub.publish(msg_list)
     # Publish Markers
     point_list = []
     color_list = []
     for key in self.detected_items:
         point_list.append(
             Point(self.detected_items[key].x, self.detected_items[key].y,
                   0.5))
         if key[0:len(key) - 1] == "stop_sign":
             color_list.append(ColorRGBA(1.0, 0, 0, 1.0))
         else:
             color_list.append(ColorRGBA(0, 1.0, 0.1, 1.0))
     m = Marker()
     m.type = Marker.CUBE_LIST
     m.action = Marker.ADD
     m.header.frame_id = "map"
     m.scale.x = 0.07
     m.scale.y = 0.07
     m.scale.z = 1.5
     m.points = point_list
     m.colors = color_list
     self.pub_marker.publish(m)
def copy_marker_attributes(sample):
    marker = Marker()
    marker.header.frame_id = sample.header.frame_id
    marker.header.stamp = sample.header.stamp
    marker.id = sample.id
    marker.type = sample.type
    marker.action = sample.action
    marker.scale.x = sample.scale.x
    marker.scale.y = sample.scale.y
    marker.scale.z = sample.scale.z
    marker.pose.position.x = sample.pose.position.x
    marker.pose.position.y = sample.pose.position.y
    marker.pose.position.z = sample.pose.position.z
    marker.pose.orientation.x = sample.pose.orientation.x
    marker.pose.orientation.y = sample.pose.orientation.y
    marker.pose.orientation.z = sample.pose.orientation.z
    marker.pose.orientation.w = sample.pose.orientation.w
    marker.color.a = sample.color.a
    marker.color.r = sample.color.r
    marker.color.g = sample.color.g
    marker.color.b = sample.color.b
    marker.text = sample.text
    marker.mesh_resource = sample.mesh_resource
    marker.mesh_use_embedded_materials = sample.mesh_use_embedded_materials
    marker.points = sample.points
    marker.colors = sample.colors

    return marker
Example #11
0
 def create_point_list(cls,
                       ref_pose,
                       points,
                       colors,
                       size=0.01,
                       namespace='point_list'):
     """
     Creates a point list referenced to a pose
     Args:
     ref_pose (PoseStamped): This is used for the header and the ref_points
     points (list): List of Point objects with the positions relative to ref_pose
     colors (list): The colors for the points
     size (float): size of the points in meters
     """
     marker = Marker()
     marker.type = Marker.POINTS
     marker.header = ref_pose.header
     marker.pose = ref_pose.pose
     marker.ns = namespace
     marker.action = Marker.ADD
     marker.scale.x = size
     marker.scale.y = size
     if isinstance(colors[0], list):
         # colors defined for every element
         marker.colors = [cls.rgba_from_list(color) for color in colors]
     else:
         marker.color = cls.rgba_from_list(colors)
     marker.points = points
     return marker
    def add_follower_plot(self, msg, height=0.25):
        print("Adding follower")
        follower_marker = Marker()
        follower_marker.id = self.current_id
        follower_marker.action = Marker.ADD
        follower_marker.header.frame_id = 'map'
        follower_marker.type = Marker.LINE_STRIP
        follower_marker.ns = "points"
        follower_marker.scale.x = 0.25
        follower_marker.colors = []

        follower_marker.points = []
        follower_marker.points.append(
            Point(msg.follower.initial[0], msg.follower.initial[1], height))
        follower_marker.points.append(
            Point(msg.follower.line_start[0], msg.follower.line_start[1],
                  height))
        follower_marker.points.append(
            Point(msg.follower.line_end[0], msg.follower.line_end[1], height))
        follower_marker.points.append(
            Point(msg.follower.goal[0], msg.follower.goal[1], height))
        follower_marker.colors.append(ColorRGBA(0.0, 0.0, 1.0, 1.0))
        follower_marker.colors.append(ColorRGBA(0.0, 0.0, 1.0, 1.0))
        follower_marker.colors.append(ColorRGBA(0.0, 0.0, 1.0, 1.0))
        follower_marker.colors.append(ColorRGBA(0.0, 0.0, 1.0, 1.0))
        follower_marker.lifetime = rospy.Duration(self.refresh_rate)
        self.Markers.markers.append(follower_marker)
        self.current_id = self.current_id + 1
Example #13
0
    def publish(self, suc, info):
        # area ...
        # self.ar_msg_.header.stamp = rospy.Time.now()
        # self.ar_msg_.header.frame_id = 'base_link'
        # self.ar_msg_.polygon = Polygon([Point32(x,y,0) for (x,y) in self.ar_])
        # self.ar_pub_.publish(self.ar_msg_)

        # detections ...
        if not suc:
            return

        p0, p1 = info
        p0x,p0y = p0 # np.mean(c0,axis=0) # centroid
        p1x,p1y = p1 # np.mean(c1,axis=0) # centroid

        p0 = Point(x=p0x,y=p0y)
        p1 = Point(x=p1x,y=p1y)
        col = ColorRGBA(1.0,1.0,1.0,1.0)

        msg = Marker()
        msg.lifetime = rospy.Duration(1.0)
        msg.header.frame_id = 'odom'

        msg.type = msg.POINTS
        msg.action = msg.ADD
        msg.points = [p0, p1]
        msg.colors = [col,col]
        msg.scale.x = msg.scale.y = msg.scale.z = 0.2

        self.viz_pub_.publish(msg)
Example #14
0
    def _draw_sphere_list(self,
                          uid,
                          namespace,
                          scale,
                          points,
                          color=None,
                          colors=None):
        # type: (int, str, Vector3, list, ColorRGBA, list) -> None
        marker = Marker()
        # Header
        marker.header.frame_id = "base_link"
        marker.header.stamp = rospy.Time()

        # Body
        marker.ns = namespace
        marker.id = uid
        marker.type = Marker.SPHERE_LIST
        marker.action = Marker.ADD
        marker.scale = scale
        marker.pose.orientation = Quaternion(0, 0, 0, 1)
        marker.points = points
        if color is not None:
            marker.color = color
        if colors is not None:
            marker.colors = colors

        self._visualization_publisher.publish(marker)
Example #15
0
    def draw_risk_grid(self, risk_grid):
        if not rospy.is_shutdown():
            p_list = list()
            c_list = list()
            x_gen = xrange(0, self.x_dim, 1)
            y_gen = xrange(0, self.y_dim, 1)

            for i in x_gen:
                for j in y_gen:
                    risk = risk_grid[i, j]
                    pnt = Point(i, j, 0)
                    r, g, b, a = self.sm.to_rgba(risk)
                    clr = ColorRGBA(r, g, b, a)
                    p_list.append(pnt)
                    c_list.append(clr)

            marker = Marker()
            marker.header.frame_id = "/my_frame"
            marker.lifetime = rospy.Duration(10000000)
            marker.type = marker.POINTS
            marker.scale.x = 1
            marker.scale.y = 1
            marker.action = marker.ADD
            marker.points = p_list
            marker.colors = c_list
            marker.id = -1
            self.pub.publish(marker)
        return self
Example #16
0
    def line(self, frame_id, p, r, t=[0.0, 0.0], key=None):
        """
        line: t r + p

        This will be drawn for t[0] .. t[1]
        """

        r = np.array(r)
        p = np.array(p)

        m = Marker()
        m.header.frame_id = frame_id
        m.ns = key or 'lines'
        m.id = 0 if key else len(self.lines)
        m.type = Marker.LINE_STRIP
        m.action = Marker.MODIFY
        m.pose = Pose(Point(0,0,0), Quaternion(0,0,0,1))

        m.scale = Vector3(0.005, 0.005, 0.005)
        m.color = ColorRGBA(0,0.8,0.8,1)

        m.points = [Point(*(p+r*t)) for t in np.linspace(t[0], t[1], 10)]
        m.colors = [m.color] * 10

        key = key or element_key(m)

        with self.mod_lock:
            self.lines[key] = m

        return key
Example #17
0
def publish_marker(points, header, marker_id):
    marker = Marker()
    marker.header = header
    marker.ns = 'sensfloor'
    marker.type = Marker.POINTS
    marker.lifetime = RVIZ_MARKER_LIFETIME
    marker.pose.orientation.w = 1
    marker.scale.x = RVIZ_MARKER_SCALE
    marker.scale.y = RVIZ_MARKER_SCALE

    marker.id = marker_id

    marker.points = map(lambda x: Point(x=x.x, y=x.y), points)
    marker.colors = map(
        lambda x: ColorRGBA(
            r=max(
                0.01,
                min(1.0,
                    float(x.c - NEUTRAL_SENSOR_VALUE) / SENSOR_COLOUR_RANGE)),
            g=0.0,
            b=0.0,
            a=max(
                0.01,
                min(1.0,
                    float(x.c - NEUTRAL_SENSOR_VALUE) / SENSOR_COLOUR_RANGE))),
        points)

    # print marker.colors

    markerPub.publish(marker)
Example #18
0
    def make_Marker(slef, markerP, id):

        marker = Marker()
        marker.type = Marker.LINE_STRIP
        marker.header.frame_id = "map"
        marker.header.stamp = rospy.Time.now()
        marker.id = id
        marker.action = marker.ADD

        marker.pose.position.x = 0
        marker.pose.position.y = 0
        marker.pose.position.z = 0
        marker.pose.orientation.x = 0.0
        marker.pose.orientation.y = 0.0
        marker.pose.orientation.z = 0.0
        marker.pose.orientation.w = 1.0
        marker.scale.x = 0.1
        marker.scale.y = 0
        marker.scale.z = 0
        marker.color.a = 1.0
        marker.color.r = 1.0
        marker.color.g = 1.0
        marker.color.b = 1.0

        marker.points = markerP
        marker.colors = [ColorRGBA(1, 0, 0, 1) for i in markerP]
        return marker
Example #19
0
    def marker(self, position=None, orientation=None, points=None, colors=None, scale=1.0):
        mark = Marker()
        mark.header.stamp = rospy.Time.now()
        mark.header.frame_id = self.frame_id
        mark.ns = self.ns
        mark.type = self.type
        mark.id = self.counter
        mark.action = self.action
        mark.scale.x = self.scale[0]
        mark.scale.y = self.scale[1]
        mark.scale.z = self.scale[2]
        mark.color.r = self.color[0]
        mark.color.g = self.color[1]
        mark.color.b = self.color[2]
        mark.color.a = self.color[3]
        mark.lifetime = rospy.Duration(self.lifetime)

        if points is not None:
            mark.points = []
            for point in points:
                mark.points.append(get_point(point, scale))
        if colors is not None:
            mark.colors = colors

        if position is not None or orientation is not None:
            mark.pose.position = get_point(position, scale)
            mark.pose.orientation = get_quat(orientation)

           

        self.counter+=1
        return mark
Example #20
0
    def markers_for_skeleton(self,
                             skeleton,
                             index=0,
                             sphere_color=None,
                             line_color=None):
        sphere_color = ColorRGBA(1, 0, 0,
                                 1) if not sphere_color else sphere_color
        line_color = ColorRGBA(0, 0, 1, 1) if not line_color else line_color

        joints_marker = Marker()
        joints_marker.id = index
        # joints_marker.lifetime = rospy.Duration(30)
        joints_marker.type = Marker.SPHERE_LIST
        joints_marker.scale.x, joints_marker.scale.y, joints_marker.scale.z = 0.05, 0.05, 0.05
        joints_marker.action = Marker.ADD
        joints_marker.ns = "skeleton_spheres"
        joints_marker.header.frame_id = skeleton.bodyparts.values(
        )[0].header.frame_id  # Just take the first one
        joints_marker.header.stamp = rospy.Time.now()
        joints_marker.points = [
            joint_ps.point for joint_name, joint_ps in skeleton.items()
        ]
        joints_marker.colors = [sphere_color for _, _ in skeleton.items()]
        rospy.loginfo("Added {} joints for skeleton {}".format(
            len(joints_marker.points), skeleton))

        links_marker = Marker()
        links_marker.id = index
        # links_marker.lifetime = rospy.Duration(30)
        links_marker.type = Marker.LINE_LIST
        links_marker.ns = "skeleton_lines"
        links_marker.scale.x = 0.02
        links_marker.action = Marker.ADD
        links_marker.header.frame_id = skeleton.bodyparts.values(
        )[0].header.frame_id  # Just take the first one
        links_marker.header.stamp = rospy.Time.now()
        links_marker.points = list(skeleton.generate_links())
        links_marker.colors = [
            line_color for _ in links_marker.points
        ]  # TODO: not in sync, whould iterate over pairs of points here

        rospy.loginfo("Added {} links for skeleton {}".format(
            len(links_marker.points) / 2,
            skeleton))  # /2 because lines are pairs of points

        return [joints_marker, links_marker]
Example #21
0
    def resizeWithResolution(self, map, resolution):
        marker_container = Marker()
        marker_container.id = 2
        marker_container.type = Marker.CUBE_LIST
        marker_container.points = []
        marker_container.colors = []
        marker_container.header.frame_id = "map"
        marker_container.ns = "wall"
        marker_container.scale.x = (0.5 / float(10)) * resolution
        marker_container.scale.y = (0.5 / float(10)) * resolution
        marker_container.header.stamp = rospy.Time.now()
        marker_container.pose.orientation.w = 1

        resizedMapArray = [[0 for x in range(self.map_height / resolution)]
                           for x in range(self.map_width / resolution)]
        i = 0
        j = 0
        while i < len(map[0]):
            j = 0
            while j < len(map):
                if (i == 0):
                    new_i = 0
                else:
                    new_i = i / resolution

                if (j == 0):
                    new_j = 0
                else:
                    new_j = j / resolution

                # if(j>=0 and j<self.map_width/resolution and i>=0 and i<self.map_height/resolution):
                if self.isObstacle(map, i, j, resolution):
                    resizedMapArray[new_i][new_j] = self.MAP_OBSTACLE_VALUE
                    current_point = Point()
                    current_color = ColorRGBA()

                    current_color.a = 0.5
                    current_color.r = 0
                    current_color.g = 1
                    current_color.b = 1

                    current_point.z = 0.20 / float(10)

                    current_point.x = ((new_j / float(2)) /
                                       (float(10) / resolution)) + 0.2
                    current_point.y = ((new_i / float(2)) /
                                       (float(10) / resolution)) + 0.2

                    marker_container.points.append(current_point)
                    marker_container.points.append(current_color)
                else:
                    # print 'i:'+str(i)+"--> obstacle"
                    # print 'j:'+str(j)
                    resizedMapArray[new_i][new_j] = 0
                j = j + resolution
            i = i + resolution
        self.pub_marker.publish(marker_container)
        return resizedMapArray
def test():
    rospy.init_node('intersect_plane_test')
    marker_pub = rospy.Publisher('table_marker', Marker)
    pose_pub = rospy.Publisher('pose', PoseStamped)
    int_pub = rospy.Publisher('intersected_points', PointCloud2)
    tfl = tf.TransformListener()
    
    # Test table
    table = Table()
    table.pose.header.frame_id = 'base_link'
    table.pose.pose.orientation.x, table.pose.pose.orientation.y, table.pose.pose.orientation.z, table.pose.pose.orientation.w = (0,0,0,1)
    table.x_min = -0.5
    table.x_max =  0.5
    table.y_min = -0.5
    table.y_max =  0.5

    # A marker for that table
    marker = Marker()
    marker.header.frame_id = table.pose.header.frame_id
    marker.id = 1
    marker.type = Marker.LINE_STRIP
    marker.action = 0
    marker.pose = table.pose.pose
    marker.scale.x, marker.scale.y, marker.scale.z = (0.005,0.005,0.005)
    marker.color.r, marker.color.g, marker.color.b, marker.color.a = (0.0,1.0,1.0,1.0) 
    marker.frame_locked = False
    marker.ns = 'table'
    marker.points = [
        Point(table.x_min,table.y_min, table.pose.pose.position.z),
        Point(table.x_min,table.y_max, table.pose.pose.position.z),
        Point(table.x_max,table.y_max, table.pose.pose.position.z),
        Point(table.x_max,table.y_min, table.pose.pose.position.z),
        Point(table.x_min,table.y_min, table.pose.pose.position.z),
    ]
    marker.colors = []
    marker.text = ''
    # marker.mesh_resource = ''
    marker.mesh_use_embedded_materials = False
    marker.header.stamp = rospy.Time.now()

    # A test pose
    pose = PoseStamped()
    pose.header = table.pose.header
    pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = (0,0,0.5)
    pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(0,-pi/5,pi/5)
    
    intersection = cast_ray(pose, table, tfl)
    cloud = xyz_array_to_pointcloud2(np.array([[intersection.point.x, intersection.point.y, intersection.point.z]]))
    cloud.header = pose.header
    
    while not rospy.is_shutdown():
        marker_pub.publish(marker)
        pose_pub.publish(pose)
        int_pub.publish(cloud)
        rospy.loginfo('published')
        rospy.sleep(0.1)
Example #23
0
    def publish_marker(self):
        if self.points is not None or not self.locked:

            del self.actualPoints[:]
            for x in self.points:

                if not self.dict.has_key((x.x, x.y)) and x.c > 137:
                    self.isEvent = True
                    self.dict.update({(x.x, x.y): x.c})

                if x.c <= 137 and self.dict.has_key((x.x, x.y)):
                    self.dict.pop((x.x, x.y), None)

            if self.isEvent is True:
                event_time = int(round(time.time() * 1000))
                #print "Event-Sensfloor: " + str(event_time)
                logger.debug(event_time)
                footpressEvent.publish(
                    SensfloorEventMsg(x=x.x, y=x.y, c=x.c, ms=event_time))
                self.isEvent = False

            for key, value in self.dict.iteritems():
                self.actualPoints.append(
                    CapacitancePoint(x=key[0], y=key[1], c=value))

            marker = Marker()
            marker.header = self.header
            marker.ns = 'sensfloor'
            marker.type = Marker.POINTS
            marker.lifetime = RVIZ_MARKER_LIFETIME
            marker.pose.orientation.w = 1
            marker.scale.x = RVIZ_MARKER_SCALE
            marker.scale.y = RVIZ_MARKER_SCALE

            marker.id = 0
            marker.points = map(lambda x: Point(x=x.x, y=x.y),
                                self.actualPoints)
            marker.colors = map(
                lambda x: ColorRGBA(r=max(
                    0.01,
                    min(
                        1.0,
                        float(x.c - NEUTRAL_SENSOR_VALUE) / SENSOR_COLOUR_RANGE
                    )),
                                    g=0.0,
                                    b=0.0,
                                    a=max(
                                        0.01,
                                        min(
                                            1.0,
                                            float(x.c - NEUTRAL_SENSOR_VALUE) /
                                            SENSOR_COLOUR_RANGE))),
                self.actualPoints)

            markerPub.publish(marker)
            self.locked = True
Example #24
0
    def create_trajectory_marker(self, traj):
        # create an interactive marker for our server
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "/map"
        int_marker.name = traj.uuid
        int_marker.description = traj.uuid
        pose = Pose()
        pose.position.x = traj.pose[0]['position']['x']
        pose.position.y = traj.pose[0]['position']['y']
        int_marker.pose = pose

        line_marker = Marker()
        line_marker.type = Marker.LINE_STRIP
        line_marker.scale.x = 0.05

        # random.seed(traj.uuid)
        # val = random.random()
        # line_marker.color.r = r_func(val)
        # line_marker.color.g = g_func(val)
        # line_marker.color.b = b_func(val)
        # line_marker.color.a = 1.0

        line_marker.points = []

        MOD  = 1
        for i, point in enumerate(traj.pose):
            if i % MOD == 0:
                x = point['position']['x']
                y = point['position']['y']
                p = Point()
                p.x = x - int_marker.pose.position.x  
                p.y = y - int_marker.pose.position.y
                line_marker.points.append(p)

        line_marker.colors = []
        for i, vel in enumerate(traj.vel):
            if i % MOD == 0:
                color = ColorRGBA()
                val = vel / traj.max_vel
                color.r = r_func(val)
                color.g = g_func(val)
                color.b = b_func(val)
                color.a = 1.0
                line_marker.colors.append(color)
                

                
        # create a control which will move the box
        # this control does not contain any markers,
        # which will cause RViz to insert two arrows
        control = InteractiveMarkerControl()
        control.markers.append(line_marker) 
        int_marker.controls.append(control)
        
        return int_marker
Example #25
0
    def plane(self, frame_id, p, n, r=1.0, key=None, draw_triad=False):
        """
        plane: n . ( X - p ) = 0

        This will be drawn as a disc of radius r about p.
        """

        n = np.array(n)
        p = np.array(p)

        m = Marker()
        m.header.frame_id = frame_id
        m.ns = key or 'planes'
        m.id = 0 if key else len(self.planes)
        m.type = Marker.TRIANGLE_LIST
        m.action = Marker.MODIFY

        # Compute plane rotation
        nz = n / np.linalg.norm(n)
        nx = np.cross(nz, np.array([0,0,1])); nx = nx / np.linalg.norm(nx)
        ny = np.cross(nz, nx); ny = ny / np.linalg.norm(ny)
        R = kdl.Rotation(
            kdl.Vector(*nx),
            kdl.Vector(*ny),
            kdl.Vector(*nz))

        # define the pose of the plane marker 
        m.pose = Pose(Point(*(p)), Quaternion(*R.GetQuaternion()))
        m.scale = Vector3(1, 1, 1)

        # flatten the triangle list
        for triangle in self.mesh_t:
            m.points.extend([
                Point(r*self.mesh_p[t][0],r*self.mesh_p[t][1],0)
                for t in triangle])

        m.color = ColorRGBA(0.8,0,0.8,0.9)
        m.colors = [m.color] * len(m.points)

        if draw_triad:
            self.line(frame_id, p, nx, t=[0.0,r],key=key+'-x')
            self.line(frame_id, p, ny, t=[0.0,r],key=key+'-y')
            self.line(frame_id, p, nz, t=[0.0,r],key=key+'-z')

            self.point(frame_id, p, 0.005, key=key+'-center-point')


        key = key or element_key(m)

        with self.mod_lock:
            self.planes[key] = m

        return key
    def makeViz(self, reachSets):
        pointSets = [[tuple(p.p) for p in rs.set] for rs in reachSets.sets]
        triangleSets = [tripy.earclip(ps) for ps in pointSets]

        header = Header()
        header.stamp = rospy.Time.now()
        header.frame_id = reachSets.header.frame_id

        origin = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1))

        lineMarkers = MarkerArray()
        lineMarkerArray = []
        for ii in range(len(pointSets)):
            m = Marker()
            m.header = header
            m.id = self.outline_marker_id + ii
            m.action = 0
            m.pose = origin
            m.type = 4  #LINE_STRIP
            m.color = self.colors[ii % len(self.colors)]
            m.points = [Point(p[0], p[1], 0) for p in pointSets[ii]]
            m.scale = Vector3(.05, 0, 0)
            lineMarkerArray.append(m)

        lineMarkers.markers = lineMarkerArray

        self.outlinePub.publish(lineMarkers)

        triPoints = [xy for tri in triangleSets for xy in tri]

        triMarker = Marker()
        triMarker.header = header
        triMarker.id = self.tri_marker_id
        triMarker.type = 11  #TRIANGLE_LIST
        triMarker.action = 0
        triMarker.pose = origin
        triMarker.color = ColorRGBA(1, 1, 1, 1)
        triMarker.scale = Vector3(1, 1, 1)
        triMarker.points = [
            Point(p[0], p[1], 0) for tri in triPoints for p in tri
        ]

        #expand color array to cover all verts for all tris in each set with same color
        triFrequency = [len(ps) for ps in pointSets]
        triColors = [
            self.colors[ii % len(self.colors)] for ii in range(len(pointSets))
        ]
        triMarker.colors = [
            c for cidx in range(len(triColors))
            for c in repeat(triColors[cidx], triFrequency[cidx])
        ]

        self.trisPub.publish(triMarker)
 def _create_marker_container(self):
     marker_container = Marker()
     marker_container.id = 1
     marker_container.type = Marker.CUBE_LIST
     marker_container.points = []
     marker_container.colors = []
     marker_container.header.frame_id = "map"
     marker_container.header.stamp = rospy.Time.now()
     marker_container.scale.x = (0.5 / float(10)) * self.RESOLUTION
     marker_container.scale.y = (0.5 / float(10)) * self.RESOLUTION
     marker_container.pose.orientation.w = 1
     return marker_container
Example #28
0
    def create_plan_marker(self, plan, plan_values):
        # create an interactive marker for our server
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "/map"
        int_marker.name = plan.ID
        int_marker.description = plan.ID
        pose = Pose()
        #pose.position.x = traj.pose[0]['position']['x']
        #pose.position.y = traj.pose[0]['position']['y']
        int_marker.pose = pose
        
        line_marker = Marker()
        line_marker.type = Marker.LINE_STRIP
        line_marker.scale.x = 0.1

        # random.seed(float(plan.ID))
        # val = random.random()
        # line_marker.color.r = r_func(val)
        # line_marker.color.g = g_func(val)
        # line_marker.color.b = b_func(val)
        # line_marker.color.a = 1.0

        line_marker.points = []
        for view in plan.views:
            x = view.get_ptu_pose().position.x
            y = view.get_ptu_pose().position.y
            z = 0.0 # float(plan.ID) / 10
            p = Point()
            p.x = x - int_marker.pose.position.x  
            p.y = y - int_marker.pose.position.y
            p.z = z - int_marker.pose.position.z
            line_marker.points.append(p)

            line_marker.colors = []
            for i, view in enumerate(plan.views):
                color = ColorRGBA()
                val = float(i) / len(plan.views)
                color.r = r_func(val)
                color.g = g_func(val)
                color.b = b_func(val)
                color.a = 1.0
                line_marker.colors.append(color)
                

        # create a control which will move the box
        # this control does not contain any markers,
        # which will cause RViz to insert two arrows
        control = InteractiveMarkerControl()
        control.markers.append(line_marker) 
        int_marker.controls.append(control)
        
        return int_marker
Example #29
0
    def transformStampedArrayToLabeledLineStripMarker(cls, tsdata_lst, fmt="%Y-%m-%d %H:%M:%S", zoffset=0.05, label_downsample=1, discrete=False):
        "[[transformStamped, meta],...] -> LineStrip / String"
        res = []
        # make line strip
        points = []
        colors = []
        t_first = tsdata_lst[0][0]
        prev_t = t_first.transform.translation
        for ts, _ in tsdata_lst:
            t = ts.transform.translation
            dist = distanceOfVector3(prev_t, t) * 0.65
            rgb = colorsys.hsv_to_rgb(dist, 1.0, 1.0)
            points.append(Point(x=t.x, y=t.y, z=t.z))
            colors.append(ColorRGBA(rgb[0],rgb[1],rgb[2],1.0))
            prev_t = t

        h = Header()
        h.stamp = rospy.Time(0) #rospy.Time.now()
        h.frame_id = "eng2" #t_first.child_frame_id
        if discrete:
            m_type = Marker.POINTS
        else:
            m_type = Marker.LINE_STRIP
        m = Marker(type=m_type,
                   action=Marker.ADD,
                   header=h,
                   id=cls.marker_id)
        cls.marker_id += 1
        m.scale.x = 0.1
        m.scale.y = 0.1
        m.points = points
        m.colors = colors
        m.ns = "labeled_line"
        m.lifetime = rospy.Time(3000)
        res.append(m)

        for t, t_meta in tsdata_lst[::label_downsample]:
            text = Marker(type=Marker.TEXT_VIEW_FACING,
                          action=Marker.ADD,
                          header=h,
                          id=cls.marker_id)
            cls.marker_id += 1
            text.scale.z = 0.1
            text.pose = T.poseFromTransform(t.transform)
            text.pose.position.z += zoffset
            text.color = ColorRGBA(0.0,0.0,1.0,1.0)
            text.text = t_meta["inserted_at"].strftime(fmt)
            text.ns = "labeled_line_text"
            text.lifetime = rospy.Time(3000)
            res.append(text)
        return res
Example #30
0
    def create_plan_marker(self, plan, plan_values):
        # create an interactive marker for our server
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "/map"
        int_marker.name = plan.ID
        int_marker.description = plan.ID
        pose = Pose()
        #pose.position.x = traj.pose[0]['position']['x']
        #pose.position.y = traj.pose[0]['position']['y']
        int_marker.pose = pose

        line_marker = Marker()
        line_marker.type = Marker.LINE_STRIP
        line_marker.scale.x = 0.1

        # random.seed(float(plan.ID))
        # val = random.random()
        # line_marker.color.r = r_func(val)
        # line_marker.color.g = g_func(val)
        # line_marker.color.b = b_func(val)
        # line_marker.color.a = 1.0

        line_marker.points = []
        for view in plan.views:
            x = view.get_ptu_pose().position.x
            y = view.get_ptu_pose().position.y
            z = 0.0  # float(plan.ID) / 10
            p = Point()
            p.x = x - int_marker.pose.position.x
            p.y = y - int_marker.pose.position.y
            p.z = z - int_marker.pose.position.z
            line_marker.points.append(p)

            line_marker.colors = []
            for i, view in enumerate(plan.views):
                color = ColorRGBA()
                val = float(i) / len(plan.views)
                color.r = r_func(val)
                color.g = g_func(val)
                color.b = b_func(val)
                color.a = 1.0
                line_marker.colors.append(color)

        # create a control which will move the box
        # this control does not contain any markers,
        # which will cause RViz to insert two arrows
        control = InteractiveMarkerControl()
        control.markers.append(line_marker)
        int_marker.controls.append(control)

        return int_marker
Example #31
0
 def make_match_marker(key, match, time, lifetime):
     marker = Marker(ns="Match:" + key, id=match.id, type=3,
                     text=key, pose=match.pose, lifetime=lifetime)
     marker.header.frame_id = "map"
     marker.header.stamp = time
     marker.scale.z = match.estimated_height * 0.5
     marker.scale.x = 0.1
     marker.scale.y = 0.1
     marker.color.a = 1.0
     marker.color.r = 0.25
     marker.color.g = 0.75
     marker.color.b = 0.25
     marker.colors = [marker.color]
     return marker
Example #32
0
    def echoes_callback(echo):
        echo['data'] = echo['data'][np.bitwise_and(
            echo['data']['flags'],
            0x01).astype(np.bool)]  #keep valid echoes only
        indices, flags, distances, amplitudes = [
            echo['data'][x]
            for x in ['indices', 'flags', 'distances', 'amplitudes']
        ]
        stamp = rospy.Time.now()

        if pub_raw.get_num_connections() > 0:
            pub_raw.publish(
                ros_numpy.msgify(PointCloud2, echo['data'], stamp, frame_id))

        if pub_cloud.get_num_connections() > 0:
            struct_cloud = np.empty(indices.size,
                                    dtype=[('x', np.float32),
                                           ('y', np.float32),
                                           ('z', np.float32),
                                           ('intensity', np.float32)])

            points = clouds.to_point_cloud(indices, distances, directions,
                                           np.float32)

            struct_cloud['x'] = points[:, 0]
            struct_cloud['y'] = points[:, 1]
            struct_cloud['z'] = points[:, 2]

            struct_cloud['intensity'] = amplitudes
            pub_cloud.publish(
                ros_numpy.msgify(PointCloud2, struct_cloud, stamp, frame_id))

        if pub_triangles.get_num_connections() > 0:
            points, quad_amplitudes, quad_indices = clouds.to_quad_cloud(
                indices, distances, amplitudes, quad_directions, specs.v,
                specs.h, np.float32)

            stl_points = points[quad_indices]
            stl_amplitudes = quad_amplitudes[quad_indices]

            marker = Marker()
            marker.header.stamp = stamp
            marker.header.frame_id = frame_id
            marker.type = Marker.TRIANGLE_LIST
            marker.scale = Vector3(1, 1, 1)
            marker.points = [Point(x[0], x[1], x[2])
                             for x in stl_points]  #TODO: optimize me
            marker.colors = [ColorRGBA(a, a, a, 1)
                             for a in stl_amplitudes]  #TODO: optimize me
            pub_triangles.publish(marker)
Example #33
0
    def cls_viz(self, cls):
        col = ColorRGBA(0.0, 0.0, 1.0, 1.0)

        msg = Marker()
        msg.lifetime = rospy.Duration(1.0)
        msg.header.frame_id = 'base_link'

        msg.type = msg.POINTS
        msg.action = msg.ADD
        msg.points = [Point(x=x, y=y) for (x, y) in cls]
        msg.colors = [col for _ in msg.points]
        msg.scale.x = msg.scale.y = msg.scale.z = 0.1

        self.cls_viz_pub_.publish(msg)
Example #34
0
    def calculate_error(self, opt_all_vec):
        opt_param_vec, full_pose_arr = self.split_all(opt_all_vec)

        full_param_vec = self.calculate_full_param_vec(opt_param_vec)

        # Update the primitives with the new set of parameters
        self._robot_params.inflate(full_param_vec)

        # Update all the blocks' configs
        for multisensor in self._multisensors:
            multisensor.update_config(self._robot_params)

        r_list = []
        for multisensor, cb_pose_vec in zip(self._multisensors, list(full_pose_arr)):
            # Process cb pose
            cb_points = SingleTransform(cb_pose_vec).transform * self._robot_params.checkerboards[multisensor.checkerboard].generate_points()
            if (self._use_cov):
                r_list.append(multisensor.compute_residual_scaled(cb_points))
            else:
                r_list.append(multisensor.compute_residual(cb_points))

            cb_points_msgs = [ geometry_msgs.msg.Point(cur_pt[0,0], cur_pt[0,1], cur_pt[0,2]) for cur_pt in cb_points.T]
            cb_colors_msgs = [ ColorRGBA(1,0,1,1) for cur_pt in cb_points.T]
            cb_colors_msgs[0] = ColorRGBA(0,1,0,1)
            cb_colors_msgs[1] = ColorRGBA(0,1,0,1)
            
            m = Marker()
            m.header.frame_id = self._robot_params.base_link 
            m.pose.orientation.w = 1;
            m.ns = "points_3d"
            m.id = id
            m.type = Marker.SPHERE_LIST
            m.action = Marker.MODIFY
            m.points = cb_points_msgs
            m.colors = cb_colors_msgs
            m.scale.x = 0.02
            m.scale.y = 0.02
            m.scale.z = 0.02
            self.marker_pub.publish(m)
            id += 1
                
        r_vec = concatenate(r_list)

        rms_error = numpy.sqrt( numpy.mean(r_vec**2) )
        
        print "\t\t\t\t\tRMS error: %.3f    \r" % rms_error,
        sys.stdout.flush()

        return array(r_vec)
Example #35
0
    def init_marker(self, centres):
        marker = Marker()
        marker.header = self.header
        marker.ns = 'sensfloor_cluster'
        marker.type = Marker.POINTS
        marker.lifetime = RVIZ_MARKER_LIFETIME
        marker.pose.orientation.w = 1
        marker.scale.x = RVIZ_MARKER_SCALE
        marker.scale.y = RVIZ_MARKER_SCALE
        marker.id = 0
        marker.points = map(lambda x: Point(x=x[0], y=x[1] - 1), centres)
        marker.colors = map(lambda x: ColorRGBA(r=0, g=100, b=0, a=1), centres)
        marker.text = str(centres)

        return marker
 def lineMarker(self, p, d):
   marker = Marker()
   header = Header()
   header.stamp = rospy.Time.now()
   header.frame_id = self.base_frame
   marker.header = header
   marker.ns = "rays"
   marker.id = self.recent_next
   marker.type = Marker.LINE_LIST
   marker.action = Marker.ADD
   marker.scale = Vector3(*[0.02 for i in range(3)])
   marker.points = toPoints(np.column_stack([p, (p+2*d/np.linalg.norm(d))]))
   color = self.recent_color
   marker.color = color
   marker.colors = [color for i in range(len(marker.points))]
   return marker
    def publish(self, event):
        """GVG를 출력한다"""

        gvg_node = Marker()  # GVG 노드 마커를 생성한다.
        gvg_node.header.stamp = rospy.Time.now()
        gvg_node.header.frame_id = 'map'
        gvg_node.id = 0
        gvg_node.type = Marker.POINTS
        gvg_node.action = Marker.ADD
        gvg_node.scale.x = 0.5 * self.map.info.resolution
        gvg_node.scale.y = 0.5 * self.map.info.resolution
        gvg_node.points = []
        gvg_node.colors = []
        for n in self.graph.nodes:
            c = ColorRGBA()
            c.a = 0.5
            c.r = 1
            gvg_node.colors.append(c)
            p = Point()
            p.x = self.graph.nodes[n]['pos'][0]
            p.y = self.graph.nodes[n]['pos'][1]
            p.z = 0.01
            gvg_node.points.append(p)

        gvg_edge = Marker()  # GVG 엣지 마커를 생성한다.
        gvg_edge.header.stamp = rospy.Time.now()
        gvg_edge.header.frame_id = 'map'
        gvg_edge.id = 1
        gvg_edge.type = Marker.LINE_LIST
        gvg_edge.action = Marker.ADD
        gvg_edge.scale.x = 0.2 * self.map.info.resolution
        gvg_edge.points = []
        gvg_edge.color.a = 0.7
        for e in self.graph.edges:
            p1 = Point()
            p1.x = self.graph.nodes[e[0]]['pos'][0]
            p1.y = self.graph.nodes[e[0]]['pos'][1]
            p1.z = 0.01
            gvg_edge.points.append(p1)
            p2 = Point()
            p2.x = self.graph.nodes[e[1]]['pos'][0]
            p2.y = self.graph.nodes[e[1]]['pos'][1]
            p2.z = 0.01
            gvg_edge.points.append(p2)

        self.publisher.publish([gvg_node, gvg_edge])  # GVG 마커를 출력한다.
Example #38
0
 def on_lead_obstacle(self, msg):
     marker = Marker()
     marker.header.frame_id = "middle_radar_link"
     if rospy_compat.use_ros_1:
         marker.header.stamp = rospy_compat.rospy.Time.now()
     marker.ns = "lead_obstacle"
     marker.id = 1
     marker.type = Marker.POINTS
     marker.action = Marker.MODIFY
     marker.scale.x = 0.5
     marker.scale.y = 0.5
     marker.scale.z = 0.1
     marker.color.r = 1.0
     marker.color.a = 1.0
     msg.point.y *= -1
     marker.points = [msg.point]
     marker.colors = [ColorRGBA(0.,0.,1.0,1.0)]
     self.radar_rviz_pub.publish(marker)
 def toMarker(self, points, observation_type):
   marker = Marker()
   header = Header()
   header.stamp = rospy.Time.now()
   header.frame_id = self.base_frame
   marker.header = header
   marker.ns = "object_tracker"
   marker.id = observation_type
   marker.type = Marker.SPHERE_LIST
   marker.action = Marker.ADD
   marker.scale = Vector3(*[self.sigma_observation for i in range(3)])
   marker.points = toPoints(points)
   color = None
   if observation_type == ObjectTracker.RECENT:
     color = self.recent_color
   elif observation_type == ObjectTracker.ALL:
     color = self.all_color
   else:
     raise ValueError
   marker.color = color
   marker.colors = [color for i in range(len(marker.points))]
   return marker
Example #40
0
    def point(self, frame_id, p, s, key=None):
        """ point: p
        size: s"""

        m = Marker()
        m.header.frame_id = frame_id
        m.ns = key or 'points'
        m.id = 0 if key else len(self.points)
        m.type = Marker.POINTS
        m.action = Marker.MODIFY
        m.pose = Pose(Point(0,0,0), Quaternion(0,0,0,1))

        m.scale = Vector3(s,s,1.0)

        m.points = [Point(*p)]
        m.colors = [ColorRGBA(0.8,0.8,0,1)]

        key = key or element_key(m)

        with self.mod_lock:
            self.points[key] = m

        return key
Example #41
0
def marker_maker():
    pub = rospy.Publisher("markers",Marker,queue_size=10)
    rospy.init_node("marker_maker")
    r = rospy.Rate(1)
    q = 0
    i = 1
    j = 0
    while not rospy.is_shutdown():
      msg = Marker()
      msg.ns = "markers"
      msg.type=Marker.SPHERE
      msg.id=q
      msg.action = 0
      msg.color.r = .75
      msg.color.g = 0
      msg.color.b = .5
      msg.color.a = 1
      msg.colors = [ColorRGBA(1,1,1,1),ColorRGBA(0,0,0,0)]
      msg.pose.position.x = j
      msg.pose.position.y = j
      msg.pose.position.z = j
      msg.pose.orientation.x = 0
      msg.pose.orientation.y = 0
      msg.pose.orientation.z = 0
      msg.pose.orientation.w = 1
      msg.scale.x = 1
      msg.scale.y = 1
      msg.scale.z = 1
      msg.lifetime = rospy.Duration()
      msg.header.frame_id = "my_frame"
      msg.header.stamp = rospy.Time.now()
      pub.publish(msg)
      q += 1
      i /= 1.1
      j += 1
      r.sleep()
def callback(data):
	global imagePub
	global markerPub
	
	#Convert image to CV image
	bridge=CvBridge()
	cv_image = bridge.imgmsg_to_cv(data, "mono8")
	
	# Convert the image to a Numpy array since most cv2 functionsrequire Numpy arrays.
	searched = np.array(cv_image, dtype=np.uint8)
	
	#Create a copy for sobel comparison
	searchedCopy=copy.copy(searched)
	searchedCopy[searchedCopy==255]=0
	
	#Take Sobel Derivatives
	sobel_x=np.uint8(np.absolute(cv2.Sobel(searched,cv2.CV_16S,1,0,ksize=1)))
	sobel_y=np.uint8(np.absolute(cv2.Sobel(searched,cv2.CV_16S,0,1,ksize=1)))
	sobel_xy=cv2.addWeighted(sobel_x,0.5,sobel_y,0.5,0)
	sobel_x_base=np.uint8(np.absolute(cv2.Sobel(searchedCopy,cv2.CV_16S,1,0,ksize=1)))
	sobel_y_base=np.uint8(np.absolute(cv2.Sobel(searchedCopy,cv2.CV_16S,0,1,ksize=1)))
	sobel_xy_base=cv2.addWeighted(sobel_x_base,0.5,sobel_y_base,0.5,0)
	ret,sobel_xy_thres = cv2.threshold(sobel_xy,0,255,cv2.THRESH_BINARY)
	ret,sobel_xy_base_thres = cv2.threshold(sobel_xy_base,0,255,cv2.THRESH_BINARY)
	
	#Subtract Comparisons for frontiers only
	sobelCombined=sobel_xy_base_thres-sobel_xy_thres
	ret,sobelCombined_thresh = cv2.threshold(sobelCombined,0,255,cv2.THRESH_BINARY)

	#Dialate Combined
	dialate=cv2.dilate(sobelCombined_thresh,np.ones((3,3),'uint8'))
	
	#Find Contour Centorids
	dialateCopy=copy.copy(dialate)
	contours,hier=cv2.findContours(dialateCopy,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)
	centroids = []
	colors = []
	for i in contours:
		if len(i) > 10:
			moments=cv2.moments(i)
			cx = int(moments['m10']/moments['m00'])
			cy = int(moments['m01']/moments['m00'])
			centroidPoint = Point()
			centroidColor = ColorRGBA()
			centroidPoint.x = cx/20.0-20
			centroidPoint.y = cy/20.0-32.8
			centroidPoint.z = 0
			#centroidColor = (0,0,1,1)
			centroidColor.r = 0
			centroidColor.g = 0
			centroidColor.b = 1
			centroidColor.a = 1
			centroids.append(centroidPoint)
			colors.append(centroidColor)

	#code.interact(local=locals())
	#Display Image in PLT Window
	'''plt.subplot(1,5,1),plt.imshow(searched,cmap='gray'),plt.title('Searched Space'),plt.xticks([]), plt.yticks([])
	plt.subplot(1,5,2),plt.imshow(sobel_xy_thres,cmap='gray'),plt.title('Sobel All'),plt.xticks([]), plt.yticks([])
	plt.subplot(1,5,3),plt.imshow(sobel_xy_base_thres,cmap='gray'),plt.title('Sobel All'),plt.xticks([]), plt.yticks([])
	plt.subplot(1,5,4),plt.imshow(sobelCombined,cmap='gray'),plt.title('Frontiers Map'),plt.xticks([]), plt.yticks([])
	plt.subplot(1,5,5),plt.imshow(dialate,cmap='gray'),plt.title('Dialate'),plt.xticks([]), plt.yticks([])
	plt.draw()
	plt.pause(0.001)'''
	
	#Convert the image back to mat format and publish as ROS image
	frontiers = cv.fromarray(dialate)
	imagePub.publish(bridge.cv_to_imgmsg(frontiers, "mono8"))

	#Publish centorids of frontiers as marker
	markerMsg = Marker()
	markerMsg.header.frame_id = "/map"
	markerMsg.header.stamp = rospy.Time.now()
	markerMsg.ns = ""
	markerMsg.id = 0
	markerMsg.type = 7			#Sphere List
	markerMsg.action = 0		#Add
	markerMsg.scale.x = 0.5
	markerMsg.scale.y = 0.5
	markerMsg.scale.z = 0.5
	markerMsg.points = centroids
	markerMsg.colors = colors
	markerPub.publish(markerMsg)
	print 'yes'
Example #43
0
	def imageCallback(self,data):
		#Convert image to CV2 supported numpy array
		bridge=CvBridge()
		cv_image = bridge.imgmsg_to_cv(data, "mono8")
		searched = np.array(cv_image, dtype=np.uint8)
		
		#Create copy and clear searched space in copy (leaving map only)
		searchedCopy=copy.copy(searched)
		searchedCopy[searchedCopy==255]=0
		
		#Take Sobel Derivatives of searched and map only
		sobel_x=np.uint8(np.absolute(cv2.Sobel(searched,cv2.CV_16S,1,0,ksize=1)))
		sobel_y=np.uint8(np.absolute(cv2.Sobel(searched,cv2.CV_16S,0,1,ksize=1)))
		sobel_xy=cv2.addWeighted(sobel_x,0.5,sobel_y,0.5,0)
		sobel_x_base=np.uint8(np.absolute(cv2.Sobel(searchedCopy,cv2.CV_16S,1,0,ksize=1)))
		sobel_y_base=np.uint8(np.absolute(cv2.Sobel(searchedCopy,cv2.CV_16S,0,1,ksize=1)))
		sobel_xy_base=cv2.addWeighted(sobel_x_base,0.5,sobel_y_base,0.5,0)
		ret,sobel_xy_thres = cv2.threshold(sobel_xy,0,255,cv2.THRESH_BINARY)
		ret,sobel_xy_base_thres = cv2.threshold(sobel_xy_base,0,255,cv2.THRESH_BINARY)
		
		#Subtract Sobel Derivatives (Leaves Frontiers Only)
		sobelCombined=sobel_xy_base_thres-sobel_xy_thres
		ret,sobelCombined_thresh = cv2.threshold(sobelCombined,0,255,cv2.THRESH_BINARY)
		
		#Dialate Frontiers To Form Continuous Contours
		dialate=cv2.dilate(sobelCombined_thresh,np.ones((3,3),'uint8'))
		
		#Find Contours (make copy since dialate destorys original image)
		dialateCopy=copy.copy(dialate)
		contours,hier=cv2.findContours(dialateCopy,cv2.RETR_LIST,cv2.CHAIN_APPROX_NONE)

		#Convert the image back to mat format for publishing as ROS image
		frontiers = cv.fromarray(dialate)
		
		#Create List Data for Marker Message
		centroids = []
		colors = []
		#Filter Frontier Contour by number of pixels
		for i in contours:
			if len(i) > 50:
				moments=cv2.moments(i)
				cx = int(moments['m10']/moments['m00'])
				cy = int(moments['m01']/moments['m00'])
				centroidPoint = Point()
				centroidColor = ColorRGBA()
				centroidPoint.x = cx*self.mapRes+self.mapOrigin.position.x
				centroidPoint.y = cy*self.mapRes+self.mapOrigin.position.y
				centroidPoint.z = 0; 
				centroidColor.r = 0 
				centroidColor.g = 0 
				centroidColor.b = 1 
				centroidColor.a = 1
				centroids.append(centroidPoint)
				colors.append(centroidColor)
				
		#Pack Marker Message
		markerMsg = Marker()
		markerMsg.header.frame_id = "/map"
		markerMsg.header.stamp = rospy.Time.now()
		markerMsg.ns = ""
		markerMsg.id = 0
		markerMsg.type = 7			#Sphere List Type
		markerMsg.action = 0		#Add Mode
		markerMsg.scale.x = 0.5
		markerMsg.scale.y = 0.5
		markerMsg.scale.z = 0.5
		markerMsg.points = centroids
		markerMsg.colors = colors
	
		#Publish Marker and Image messages
		self.imagePub.publish(bridge.cv_to_imgmsg(frontiers, "mono8"))
		self.markerPub.publish(markerMsg)