def publish(self, msg: Union[MsgType, bytes]) -> None:
"""
Publish a message if the lifecycle publisher is enabled.
See rclpy.publisher.Publisher.publish() for more details.
"""
Publisher.publish(self, msg)
def destroy_publisher(self, publisher: Publisher) -> bool:
"""
Destroy a publisher created by the node.
:return: ``True`` if successful, ``False`` otherwise.
"""
if publisher in self.__publishers:
self.__publishers.remove(publisher)
try:
publisher.destroy()
except InvalidHandle:
return False
return True
return False
def create_publisher(
self,
msg_type,
topic: str,
*,
qos_profile: QoSProfile = qos_profile_default) -> Publisher:
"""
Create a new publisher.
:param msg_type: The type of ROS messages the publisher will publish.
:param topic: The name of the topic the publisher will publish to.
:param qos_profile: The quality of service profile to apply to the publisher.
:return: The new publisher.
"""
# this line imports the typesupport for the message module if not already done
check_for_type_support(msg_type)
failed = False
try:
with self.handle as node_capsule:
publisher_capsule = _rclpy.rclpy_create_publisher(
node_capsule, msg_type, topic,
qos_profile.get_c_qos_profile())
except ValueError:
failed = True
if failed:
self._validate_topic_or_service_name(topic)
publisher_handle = Handle(publisher_capsule)
publisher_handle.requires(self.handle)
publisher = Publisher(publisher_handle, msg_type, topic, qos_profile)
self.__publishers.append(publisher)
return publisher
def create_publisher(self, msg_type, topic, qos_profile=qos_profile_default):
# this line imports the typesupport for the message module if not already done
if msg_type.__class__._TYPE_SUPPORT is None:
msg_type.__class__.__import_type_support__()
if msg_type.__class__._TYPE_SUPPORT is None:
raise NoTypeSupportImportedException
publisher_handle = _rclpy.rclpy_create_publisher(
self.handle, msg_type, topic, qos_profile.get_c_qos_profile())
publisher = Publisher(publisher_handle, msg_type, topic, qos_profile, self.handle)
self.publishers.append(publisher)
return publisher
def create_publisher(self, msg_type, topic, *, qos_profile=qos_profile_default):
# this line imports the typesupport for the message module if not already done
check_for_type_support(msg_type)
failed = False
try:
publisher_handle = _rclpy.rclpy_create_publisher(
self.handle, msg_type, topic, qos_profile.get_c_qos_profile())
except ValueError:
failed = True
if failed:
self._validate_topic_or_service_name(topic)
publisher = Publisher(publisher_handle, msg_type, topic, qos_profile, self.handle)
self.publishers.append(publisher)
return publisher
def visualize(
publisher: Publisher,
clock: Clock,
path_id: int,
path: Union[np.ndarray, Iterable[List[float]]], # n x 2
weights: Optional[Union[List[float], np.ndarray]] = None,
weight_max: Optional[float] = 1.0,
frame_id: Optional[str] = "/map",
ns: Optional[str] = "paths",
scale: Optional[float] = 0.05,
color=ColorRGBA(r=0.0, g=0.0, b=1.0, a=1.0)
) -> None:
"""
Generates a visualization_msgs::msg::Marker from a set of points
Can colorize points on a scale based on another set of values ('weights')
"""
# Use point cloud instead?
m = Marker(header=Header(frame_id=frame_id, stamp=clock.now().to_msg()),
ns=ns,
action=Marker.ADD,
id=path_id,
type=Marker.LINE_STRIP if len(path) > 1 else Marker.POINTS,
scale=Vector3(x=scale, y=scale),
color=color)
m.points.extend(Point(x=1.0 * pt[0], y=1.0 * pt[1]) for pt in path)
if weights is not None:
assert len(path) == len(weights)
m.colors.extend(
# TODO: Better color range generation
ColorRGBA(r=1.0,
g=1.0 - min(max(1.0 * wt / weight_max, 0.0), 1.0),
b=0.0,
a=1.0) for wt in weights)
publisher.publish(m)
sleep(0.01)
def __init__(self, *args, **kwargs):
SimpleManagedEntity.__init__(self)
Publisher.__init__(self, *args, **kwargs)
