Python Marker.CYLINDER屬性代碼示例

# 需要導入模塊: from visualization_msgs.msg import Marker [as 別名]
# 或者: from visualization_msgs.msg.Marker import CYLINDER [as 別名]
def marker_from_circle(circle, index=0, linewidth=0.1, color=ColorRGBA(1, 0, 0, 1), z=0., lifetime=10.0):
	marker = Marker()
	marker.header = make_header("/map")

	marker.ns = "Markers_NS"
	marker.id = index
	marker.type = Marker.CYLINDER
	marker.action = 0 # action=0 add/modify object
	# marker.color.r = 1.0
	# marker.color.g = 0.0
	# marker.color.b = 0.0
	# marker.color.a = 0.4
	marker.color = color
	marker.lifetime = rospy.Duration.from_sec(lifetime)

	marker.pose = Pose()
	marker.pose.position.z = z
	marker.pose.position.x = circle.center[0]
	marker.pose.position.y = circle.center[1]

	marker.scale = Vector3(circle.radius*2.0, circle.radius*2.0, 0)
	return marker 

# 需要導入模塊: from visualization_msgs.msg import Marker [as 別名]
# 或者: from visualization_msgs.msg.Marker import CYLINDER [as 別名]
def marker_from_point_radius(point, radius, index=0, linewidth=0.1, color=ColorRGBA(1, 0, 0, 1), z=0., lifetime=10.0):
	marker = Marker()
	marker.header = make_header("/map")

	marker.ns = "Speed_NS"
	marker.id = index
	marker.type = Marker.CYLINDER
	marker.action = 0 # action=0 add/modify object
	# marker.color.r = 1.0
	# marker.color.g = 0.0
	# marker.color.b = 0.0
	# marker.color.a = 0.4
	marker.color = color
	marker.lifetime = rospy.Duration.from_sec(lifetime)

	marker.pose = Pose()
	marker.pose.position.z = z
	marker.pose.position.x = point[0]
	marker.pose.position.y = point[1]

	marker.scale = Vector3(radius*2.0, radius*2.0, 0.001)
	return marker 

# 需要導入模塊: from visualization_msgs.msg import Marker [as 別名]
# 或者: from visualization_msgs.msg.Marker import CYLINDER [as 別名]
def make_interactive_marker(name, pose, frame='base', color=(0.5, 0.5, 0.5)):
    rospy.loginfo("Creating imarker...")
    int_marker = interactive_marker_server.InteractiveMarker()
    int_marker.header.frame_id = frame
    int_marker.header.stamp = rospy.Time.now()

    int_marker.scale = 0.25
    int_marker.name = name
    int_marker.description = "interactive marker"

    cylinder_marker = Marker()
    cylinder_marker.type = Marker.CYLINDER
    cylinder_marker.scale.x = 0.25
    cylinder_marker.scale.y = 0.25
    cylinder_marker.scale.z = 0.25
    cylinder_marker.color.r = color[0]
    cylinder_marker.color.g = color[1]
    cylinder_marker.color.b = color[2]
    cylinder_marker.color.a = 0.5

    cylinder_control = InteractiveMarkerControl()
    cylinder_control.always_visible = True
    cylinder_control.markers.append(cylinder_marker)
    int_marker.controls.append(cylinder_control)

    for axis, orientation in zip(['x', 'y', 'z'],
                                 [(1, 1, 0, 0), (1, 0, 1, 0), (1, 0, 0, 1)]):
        control = InteractiveMarkerControl()
        for attr, val in zip(['w', 'x', 'y', 'z'],
                             orientation):
            setattr(control.orientation, attr, val)
        control.name = "rotate_{}".format(axis)
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        int_marker.controls.append(control)
        control.name = "move_{}".format(axis)
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        int_marker.controls.append(control)

    int_marker.pose = pose
    rospy.loginfo("Imarker created")
    return int_marker 

# 需要導入模塊: from visualization_msgs.msg import Marker [as 別名]
# 或者: from visualization_msgs.msg.Marker import CYLINDER [as 別名]
def run(self):
        while self.is_looping():
            navigation_tf_msg = TFMessage()
            try:
                motion_to_robot = almath.Pose2D(self.motion.getRobotPosition(True))
                localization = self.navigation.getRobotPositionInMap()
                exploration_path = self.navigation.getExplorationPath()
            except Exception as e:
                navigation_tf_msg.transforms.append(self.get_navigation_tf(almath.Pose2D()))
                self.publishers["map_tf"].publish(navigation_tf_msg)
                self.rate.sleep()
                continue
            if len(localization) > 0 and len(localization[0]) == 3:
                navigation_to_robot = almath.Pose2D(localization[0][0], localization[0][1], localization[0][2])
                navigation_to_motion = navigation_to_robot * almath.pinv(motion_to_robot)
                navigation_tf_msg.transforms.append(self.get_navigation_tf(navigation_to_motion))
            self.publishers["map_tf"].publish(navigation_tf_msg)
            if len(localization) > 0:
                if self.publishers["uncertainty"].get_num_connections() > 0:
                    uncertainty = Marker()
                    uncertainty.header.frame_id = "/base_footprint"
                    uncertainty.header.stamp = rospy.Time.now()
                    uncertainty.type = Marker.CYLINDER
                    uncertainty.scale = Vector3(localization[1][0], localization[1][1], 0.2)
                    uncertainty.pose.position = Point(0, 0, 0)
                    uncertainty.pose.orientation = self.get_ros_quaternion(
                        almath.Quaternion_fromAngleAndAxisRotation(0, 0, 0, 1))
                    uncertainty.color = ColorRGBA(1, 0.5, 0.5, 0.5)
                    self.publishers["uncertainty"].publish(uncertainty)
            if self.publishers["map"].get_num_connections() > 0:
                aggregated_map = None
                try:
                    aggregated_map = self.navigation.getMetricalMap()
                except Exception as e:
                    print("error " + str(e))
                if aggregated_map is not None:
                    map_marker = OccupancyGrid()
                    map_marker.header.stamp = rospy.Time.now()
                    map_marker.header.frame_id = "/map"
                    map_marker.info.resolution = aggregated_map[0]
                    map_marker.info.width = aggregated_map[1]
                    map_marker.info.height = aggregated_map[2]
                    map_marker.info.origin.orientation = self.get_ros_quaternion(
                        almath.Quaternion_fromAngleAndAxisRotation(-1.57, 0, 0, 1))
                    map_marker.info.origin.position.x = aggregated_map[3][0]
                    map_marker.info.origin.position.y = aggregated_map[3][1]
                    map_marker.data = aggregated_map[4]
                    self.publishers["map"].publish(map_marker)
            if self.publishers["exploration_path"].get_num_connections() > 0:
                path = Path()
                path.header.stamp = rospy.Time.now()
                path.header.frame_id = "/map"
                for node in exploration_path:
                    current_node = PoseStamped()
                    current_node.pose.position.x = node[0]
                    current_node.pose.position.y = node[1]
                    path.poses.append(current_node)
                self.publishers["exploration_path"].publish(path)
            self.rate.sleep() 

# 需要導入模塊: from visualization_msgs.msg import Marker [as 別名]
# 或者: from visualization_msgs.msg.Marker import CYLINDER [as 別名]
def _img_callback(self, msg):
        frame_index = self.timestamp_map[msg.header.stamp.to_nsec()]
        for i, f in enumerate(self.frame_map[frame_index]):
            trans = f.trans
            rotq = f.rotq
            h, w, l = f.size
            if f.gt:
                # This is a bit hackish but will show one (the first) gt with fixed color
                # and will show multiple predicted objects, but colors per object will change
                # if the number of objects predicted changes.
                bid = 16
                oid = 17
                color = kelly_colors_dict['very_light_blue']
            else:
                bid = i * 2
                oid = i * 2 + 1
                color = kelly_colors_list[i % len(kelly_colors_list)]

            if self.sphere:
                s = max(l, w, h)
                self._publish_marker(
                    Marker.SPHERE, bid, msg.header.stamp,
                    trans, rotq, [s, s, s],
                    color)
                self._publish_marker(
                    Marker.ARROW, oid, msg.header.stamp,
                    trans, rotq, [l/2, w/2, h/2],
                    color)    
            else:
                #FIXME change Marker types based on object type (ie car vs pedestrian)
                if f.object_type == 'Pedestrian':
                    self._publish_marker(
                        Marker.CYLINDER, bid, msg.header.stamp,
                        trans, rotq, [w, w, h],
                        color)
                    self._publish_marker(
                        Marker.ARROW, oid, msg.header.stamp,
                        trans, rotq, [w/2, w/2, w/2],
                        color)
                else:
                    self._publish_marker(
                        Marker.CUBE, bid, msg.header.stamp,
                        trans, rotq, [l, w, h],
                        color)
                    self._publish_marker(
                        Marker.ARROW, oid, msg.header.stamp,
                        trans, rotq, [l/2, w/2, h/2],
                        color)

            self._send_transform(trans, rotq, i)