def process_touch_event(self, button_id, status):
if (button_id is 4 or button_id is 3) and status is 1:
if button_id is 4:
button_id = std_msgs.String('right')
elif button_id is 3:
button_id = std_msgs.String('left')
self.publishers['TouchSensorEvent'].publish(button_id)
def publish_blackboard(self, unused_tree=None):
"""
Lazy string publishing of the blackboard (the contents of
the blackboard can be of many and varied types, so string form is the only
way to transmit this across a ros message) on the **~blackboard** topic.
Typically you would call this from a tree custodian (e.g.
:class:`py_trees_ros.trees.BehaviourTree`) after each and every tick.
.. note:: Lazy: it will only do the relevant string processing if there are subscribers present.
Args:
unused_tree (:obj:`any`): if used as a post_tick_handler, needs the argument, but nonetheless, gets unused
"""
if self.publisher is None:
self.node.get_logger().error(
"Blackboard Exchange: no publishers [hint: call setup() on the exchange]"
)
return
# publish blackboard
if self.node.count_subscribers("~/blackboard") > 0:
if self._is_changed():
msg = std_msgs.String()
msg.data = "{0}".format(self.blackboard)
self.publisher.publish(msg)
# publish watchers
if len(self.views) > 0:
for view in self.views:
if self.node.count_publishers(view.topic_name) > 0:
if view._is_changed():
msg = std_msgs.String()
msg.data = "{0}".format(view)
view.publisher.publish(msg)
def _publish_tree_modifications(self, tree):
"""
Publishes updates when the whole tree has been modified.
This function is passed in as a visitor to the underlying behaviour tree and triggered
when there has been a change.
"""
if self.publishers is None:
rospy.logerr("BehaviourTree: call setup() on this tree to initialise the ros components")
return
self.publishers.ascii_tree.publish(std_msgs.String(py_trees.display.ascii_tree(self.root)))
self.publishers.dot_tree.publish(std_msgs.String(py_trees.display.stringify_dot_tree(self.root)))
def publish(self, *_):
"""Publishes to any required topics, only if conditions are met"""
del _
# DEBUG TODO DELETE
if self._debug_publish:
self._pub_am.publish(
std_msgs.String(data=pickle.dumps(self._abstract_map)))
# Only proceed publishing if the network has recently changed from
# being settled to unsettled (or vice versa)
settled = self._abstract_map._spatial_layout.isSettled()
if settled == self._last_settled:
return
# Update the paused status
if settled:
self._abstract_map._spatial_layout._paused = True
# Publish the abstract map as a pickled byte stream
self._pub_am.publish(
std_msgs.String(data=pickle.dumps(self._abstract_map)))
# Publish an updated goal if the running in goal mode
if (self._abstract_map._spatial_layout.isObserved(self._goal)
and settled):
self._goal_complete = True
rospy.loginfo("MISSION ACCOMPLISHED! %s was found." % (self._goal))
elif settled and self._pub_goal is not None:
# Get the suggested pose for the goal from the abstract map
goal_pos = self._abstract_map.getToponymLocation(self._goal)
# Send the message (only if we found a suggested pose for the goal)
if goal_pos is not None:
self._pub_goal.publish(
geometry_msgs.PoseStamped(
header=std_msgs.Header(stamp=rospy.Time.now(),
frame_id='map'),
pose=geometry_msgs.Pose(
position=geometry_msgs.Vector3(
goal_pos[0], goal_pos[1], 0),
orientation=tools.yawToQuaternionMsg(0))))
# Update the last_settled state
self._last_settled = settled
if settled:
self._debug_lock = True
# pudb.set_trace()
self._debug_lock = False
def cancel_flashing(self, last_uuid):
with self.lock:
if self.last_uuid == last_uuid:
# We're still relevant, publish and make us irrelevant
self.display_publisher.publish(std_msgs.String(data=""))
self.last_text = ""
self.last_uuid = uuid.uuid4()
def debug_publish(messege):
try:
msg_str = msg.String(data=messege)
debug_pub.publish(msg_str)
rospy.loginfo("Sent on debug: " + str(msg_str.data))
except rospy.ROSInterruptException:
pass
def __init__(self,
node,
publishers,
introspection_topic_name="publishers"):
resolved_names = []
publisher_details = []
for info in publishers:
if len(info) == 3:
publisher_details.append(
(basename(info[0]), info[0], info[1], info[2]))
else:
# naively assume the user got it right and added exactly 5 fields
publisher_details.append(info)
# TODO: handle latched, queue size
for (name, topic_name, publisher_type, latched) in publisher_details:
if latched:
self.__dict__[name] = node.create_publisher(
msg_type=publisher_type,
topic=topic_name,
qos_profile=qos_profile_latched_topic())
else:
self.__dict__[name] = node.create_publisher(
msg_type=publisher_type, topic=topic_name)
resolved_names.append(resolve_name(node, topic_name))
# TODO: handle latched, queue size
self.introspection_publisher = node.create_publisher(
msg_type=std_msgs.String,
topic="~/introspection/" + introspection_topic_name,
qos_profile=qos_profile_latched_topic())
s = console.bold + "\nPublishers\n\n" + console.reset
for name in resolved_names:
s += console.yellow + " " + name + "\n" + console.reset
self.introspection_publisher.publish(std_msgs.String(data=s))
def _publish_tree_snapshots(self, tree):
"""
Callback that runs on a :class:`BehaviourTree <py_trees.trees.BehaviourTree>` after
it has ticked.
:param tree: the behaviour tree
:type tree: :py:class:`BehaviourTree <py_trees.trees.BehaviourTree>`
"""
if self.publishers is None:
rospy.logerr("BehaviourTree: call setup() on this tree to initialise the ros components")
return
snapshot = "\n\n%s" % py_trees.display.ascii_tree(self.root, snapshot_information=self.snapshot_visitor)
self.publishers.ascii_snapshot.publish(std_msgs.String(snapshot))
for behaviour in self.logging_visitor.tree.behaviours:
behaviour.is_active = True if unique_id.fromMsg(behaviour.own_id) in self.snapshot_visitor.nodes else False
# We're not interested in sending every single tree - only send a
# message when the tree changes.
if self.logging_visitor.tree.behaviours != self.last_tree.behaviours:
if self.root.tip() is None:
rospy.logerr("Behaviours: your tree is returning in an INVALID state (should always be FAILURE, RUNNING or SUCCESS)")
return
self.publishers.tip.publish(conversions.behaviour_to_msg(self.root.tip()))
self.publishers.log_tree.publish(self.logging_visitor.tree)
with self.lock:
if not self._bag_closed:
self.bag.write(self.publishers.log_tree.name, self.logging_visitor.tree)
self.last_tree = self.logging_visitor.tree
def main():
# Parse arguments
parser = argparse.ArgumentParser(description='Delete frames being broadcast by a multi_static_transform_publisher')
parser.add_argument('--node-name',
metavar=('node_name'),
default=['multi_tf_pub'],
type=str, nargs=1,
help='The name of the multi publisher that should publish this transform. Default is \'multi_tf_pub\'')
parser.add_argument('frame_ids',
metavar=('frame_id'),
type=str, nargs='+',
help='The frame_ids of each frame to stop broadcasting.')
myargv = rospy.myargv(argv=sys.argv)
args = parser.parse_args(args=myargv[1:])
rospy.init_node('multi_static')
topic_name = args.node_name[0]+'/del_frame'
# Publish the transform
pub = rospy.Publisher(topic_name, std_msgs.String, latch=True)
for frame_id in args.frame_ids:
pub.publish(std_msgs.String(frame_id))
# Wait for sigint
rospy.spin()
def command_callback(self, msg: std_msgs.String):
"""
If the requested state is different from the existing state, update and
restart a periodic timer to affect the flashing effect.
Args:
msg (:class:`std_msgs.msg.String`): incoming command message
"""
with self.lock:
text = self.generate_led_text(msg.data)
# don't bother publishing if nothing changed.
if self.last_text != text:
self.node.get_logger().info("{}".format(text))
self.last_text = text
self.last_uuid = uuid.uuid4()
self.display_publisher.publish(std_msgs.String(data=msg.data))
if self.flashing_timer is not None:
self.flashing_timer.cancel()
self.node.destroy_timer(self.flashing_timer)
# TODO: convert this to a one-shot once rclpy has the capability
# Without oneshot, it will keep triggering, but do nothing while
# it has the uuid check
self.flashing_timer = self.node.create_timer(
timer_period_sec=self.duration_sec,
callback=functools.partial(
self.cancel_flashing,
this_uuid=self.last_uuid
)
)
def _writeDescription(self, Bag, Info):
# Write description of the bag.
# Only 1 information message is allowed.
if Info != "":
metadata = rosmsg.String()
metadata.data = self.Info
Bag.write("info", metadata)
def __init__(self):
super(ConcertClient, self).__init__()
# parameters
self.concert_parameters = ConcertParameters()
# working with the gateway
try:
self._match_robot_name_to_gateway_name()
self._set_gateway_flip_rules()
except GatewayNotFoundException:
return # error messages already in the underlying functions
# ros api
latched = True
queue_size_five = 5
self.concert_publishers = rocon_python_comms.utils.Publishers(
[
('~introspection/concert_parameters', std_msgs.String, latched, queue_size_five),
]
)
# let the publishers come up
rospy.rostime.wallsleep(0.5)
self.concert_publishers.concert_parameters.publish(std_msgs.String("%s" % self.concert_parameters))
def publish_resolved_names(publisher, ros_communication_handles):
"""
Worker that provides a string representation of all the resolved names
and publishes it so we can use it as an introspection topic in runtime.
"""
s = console.bold + "\nResolved Names\n\n" + console.reset
for handle in ros_communication_handles:
s += console.yellow + " " + handle.resolved_name + "\n" + console.reset
publisher.publish(std_msgs.String("%s" % s))
def terminate(self, new_status):
"""
Shoot off a clearing command to the led strip.
Args:
new_status (:class:`~py_trees.common.Status`): the behaviour is transitioning to this new status
"""
self.publisher.publish(std_msgs.String(""))
self.feedback_message = "cleared"
def publish_blackboard(self, unused_tree=None):
"""
Lazy string publishing of the blackboard (the contents of
the blackboard can be of many and varied types, so string form is the only
way to transmit this across a ros message) on the **~blackboard** topic.
Typically you would call this from a tree custodian (e.g.
:class:`py_trees_ros.trees.BehaviourTree`) after each and every tick.
.. note:: Lazy: it will only do the relevant string processing if there are subscribers present.
Args:
unused_tree (:obj:`any`): if used as a post_tick_handler, needs the argument, but nonetheless, gets unused
"""
if self.publisher is None:
self.node.get_logger().error(
"Blackboard Exchange: no publishers [hint: call setup() on the exchange]"
)
return
# publish blackboard
# the blackboard watcher doesn't actually use the lazy publisher, but
# if latching and string formatting in ROS gets up to speed, it will be a
# more useful thing to have around - easy to detect and rostopic echo the contents
if self.node.count_subscribers(self.publisher.topic) > 0:
if self._is_changed():
msg = std_msgs.String()
msg.data = "{0}".format(self.blackboard)
self.publisher.publish(msg)
# publish watchers
# print("DJS: publish_blackboard()")
# print(" DJS: # views = %s" % len(self.views))
if len(self.views) > 0:
for view in self.views:
# print(" DJS: view publisher topic: %s" % (view.publisher.topic))
# print(" DJS: # view publishers = %s" % self.node.count_publishers(view.topic_name))
# print(" DJS: # view subscribers = %s" % self.node.count_subscribers(view.topic_name))
if self.node.count_subscribers(view.topic_name) > 0:
if view.is_changed():
# print(" DJS: # printing on: %s" % view.topic_name)
msg = std_msgs.String()
msg.data = "{0}".format(view)
view.publisher.publish(msg)
def chat():
pub = rospy.Publisher('chatter', Chat, queue_size=10)
rospy.Subscriber('chatter', Chat, callback)
name = raw_input('What is your username? ')
rospy.init_node(name)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
hello_str = raw_input('Type new message below: \n')
header = msg.Header()
header.stamp = rospy.Time.now()
source_id = msg.String(rospy.get_name())
message = msg.String(hello_str)
c = Chat(header, source_id, message)
push_data(log, c)
pub.publish(c)
print_log(log)
rate.sleep()
def update(self):
"""
Annoy the led strip to keep firing every time it ticks over (the led strip will clear itself
if no command is forthcoming within a certain period of time). This behaviour will only finish if it
is terminated or interrupted from above.
"""
self.logger.debug("%s.update()" % self.__class__.__name__)
self.publisher.publish(std_msgs.String(self.colour))
self.feedback_message = "flashing {0}".format(self.colour)
return py_trees.common.Status.RUNNING
def command_callback(self, msg):
with self.lock:
text = self.generate_led_text(msg.data)
# don't bother publishing if nothing changed.
if self.last_text != text:
self.display_publisher.publish(std_msgs.String(text))
if self.flashing_timer is not None:
self.flashing_timer.shutdown()
self.flashing_timer = rospy.Timer(period=self.duration,
callback=self.cancel_flashing,
oneshot=True)
self.last_text = text
def _ros_publish_paired_interactions(self):
"""
For debugging purposes only we publish the currently running pairing interactions.
"""
# Disabled for now
pass
s = console.bold + console.white + "\nRuntime Pairings\n" + console.reset
for signature in self.active_paired_interactions:
s += " %s\n" % signature
rospy.logdebug("Interactions : updated paired interactions list\n%s" %
s)
self.publishers.paired_interactions.publish(std_msgs.String("%s" % s))
def run(self):
tf_publisher = rospy.Publisher('tf', tf_msgs.tfMessage)
status_publisher = rospy.Publisher('status', std_msgs.String)
rospy.Subscriber('tf', tf_msgs.tfMessage, self.callback)
while not rospy.is_shutdown():
tf_publisher.publish(tf_msgs.tfMessage())
if (rospy.Time.now() - self.time) > rospy.Duration(5):
status_publisher.publish(
std_msgs.String('%.2f Hz' % (next(self.count) / 5.0)))
with self.lock:
self.count = itertools.count()
self.time = rospy.Time.now()
def terminate(self, new_status: py_trees.common.Status):
"""
Shoot off a clearing command to the led strip.
Args:
new_status: the behaviour is transitioning to this new status
"""
self.logger.debug("{}.terminate({})".format(
self.qualified_name, "{}->{}".format(self.status, new_status)
if self.status != new_status else "{}".format(new_status)))
self.publisher.publish(std_msgs.String(data=""))
self.feedback_message = "cleared"
def publish_resolved_names(publisher, ros_communication_handles):
"""
Worker that provides a string representation of all the resolved names
and publishes it so we can use it as an introspection topic in runtime.
Args:
publisher (:obj:`rospy.Publisher`): use this object to publish with
ros_communication_handles ([]): list of handles with their resolved names to to publish
"""
s = console.bold + "\nResolved Names\n\n" + console.reset
for handle in ros_communication_handles:
s += console.yellow + " " + handle.resolved_name + "\n" + console.reset
publisher.publish(std_msgs.String("%s" % s))
def _init_private_publishers(self):
'''
Generate some private publishers for internal nodes to introspect on (these do not sit on the
shifting namespace used for the remote controller and never get advertised/flipped).
'''
private_publishers = {}
# this might be worth upgrading to a rocon_app_manager_msgs.srv.Status-like publisher at some point in the future
private_publishers['remote_controller'] = rospy.Publisher(
'~remote_controller', std_msgs.String, latch=True)
# initialise some of the bastards
private_publishers['remote_controller'].publish(
std_msgs.String(rapp_manager_msgs.Constants.NO_REMOTE_CONTROLLER))
return private_publishers
def cancel_flashing(self, this_uuid: uuid.UUID):
"""
If the notification identified by the given uuid is still relevant (i.e.
new command requests haven't come in) then publish an update with an
empty display message.
Args:
this_uuid: the uuid of the notification to cancel
"""
with self.lock:
if self.last_uuid == this_uuid:
# We're still relevant, publish and make us irrelevant
self.display_publisher.publish(std_msgs.String(data=""))
self.last_text = ""
self.last_uuid = uuid.uuid4()
def test_sent_message_gets_received_over_bridge(self):
# setup
send_topic = rospy.get_param("/udp_bridge/topics")[0]
receive_topic = f"{gethostname()}/{send_topic}".replace("//", "/")
subscriber = MockSubscriber(receive_topic, msg.String)
# execution
publisher = rospy.Publisher(send_topic,
msg.String,
latch=True,
queue_size=1)
publisher.publish(msg.String("Hello World"))
# verification
self.with_assertion_grace_period(subscriber.assertOneMessageReceived,
1000 * 5)
def monitor_update(self,id):
"""
updates Monitor interface
:return:
rostopic pub -1 /ged/listener std_msgs/String "cursor_move(0,510.123456,50,true);"
rostopic pub -1 /ged/listener std_msgs/String "cursor_rotate(0,25.3);"
rostopic pub -1 /ged/listener std_msgs/String "trail_points(0,'');"
rostopic pub -1 /ged/listener std_msgs/String "trail_points(0,'0 0 50 50 1000 0');"
"""
msgs=cursors_collection[id].move(self.pose)
pubWebMsg=smsg.String()
for msg in msgs:
pubWebMsg.data=msg
self.pubWeb.publish(pubWebMsg)
def test_topic_gets_published_and_sent(self):
# setup makeshift receiver
port = rospy.get_param("/udp_bridge/port")
sock = socket.socket(type=socket.SOCK_DGRAM)
sock.bind(("127.0.0.1", port))
sock.settimeout(1.0)
# publish a test message
topic = rospy.get_param("/udp_bridge/topics")[0]
publisher = rospy.Publisher(topic,
msg.String,
latch=True,
queue_size=1)
publisher.publish(msg.String("Hello World"))
# assert that a message is sent by trying to receive it
self.with_assertion_grace_period(
lambda: self.assertIsNotNone(sock.recv(10240)), 1000 * 5)
def command_callback(self, msg):
with self.lock:
text = self.generate_led_text(msg.data)
# don't bother publishing if nothing changed.
if self.last_text != text:
print("{}".format(text))
self.last_text = text
self.last_uuid = uuid.uuid4()
self.display_publisher.publish(std_msgs.String(data=text))
if self.flashing_timer is not None:
self.flashing_timer.cancel()
self.node.destroy_timer(self.flashing_timer)
# TODO: convert this to a one-shot once rclpy has the capability
# Without oneshot, it will keep triggering, but do nothing while
# it has the uuid check
self.flashing_timer = self.node.create_timer(
timer_period_sec=self.duration_sec,
callback=functools.partial(self.cancel_flashing,
last_uuid=self.last_uuid))
def post_tick_handler(self, visited_client_ids: typing.List[uuid.UUID]=None):
"""
Update blackboard watcher views, publish changes and
clear the activity stream. Publishing is lazy, depending
on blackboard watcher connections.
Typically you would call this from a tree custodian (e.g.
:class:`py_trees_ros.trees.BehaviourTree`) after each and every tick.
Args:
visited_client_ids: blackboard client unique identifiers
"""
# update watcher views and publish
if len(self.views) > 0:
for view in self.views:
if self.node.count_subscribers(view.topic_name) > 0:
if view.is_changed(visited_client_ids): # update in here
msg = std_msgs.String()
if view.with_activity_stream:
msg.data = console.green + "Blackboard Data\n" + console.reset
msg.data += "{}\n".format(view.sub_blackboard)
msg.data += py_trees.display.unicode_blackboard_activity_stream(
activity_stream=view.sub_activity_stream.data
)
else:
msg.data = "{}".format(view.sub_blackboard)
view.publisher.publish(msg)
# manage the activity stream
if py_trees.blackboard.Blackboard.activity_stream is not None:
if self.activity_stream_clients == 0:
py_trees.blackboard.Blackboard.disable_activity_stream()
else:
py_trees.blackboard.Blackboard.activity_stream.clear()
elif self.activity_stream_clients > 0:
py_trees.blackboard.Blackboard.enable_activity_stream()
def test_String_custom():
msg = std_msgs.String("teststr")
msgconv.populate_instance({"data": "teststr2"},
msg) # we shouldnt need "data" here
val = msgconv.extract_values(msg)
assert val["data"] == "teststr2"