def main(argv=sys.argv[1:]):
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
'--reliable', dest='reliable', action='store_true',
help='set qos profile to reliable')
parser.set_defaults(reliable=False)
parser.add_argument(
'-n', '--number_of_cycles', type=int, default=20,
help='number of sending attempts')
args = parser.parse_args(argv)
rclpy.init()
if args.reliable:
custom_qos_profile = qos_profile_default
print('Reliable publisher')
else:
custom_qos_profile = qos_profile_sensor_data
print('Best effort publisher')
node = rclpy.create_node('talker_qos')
chatter_pub = node.create_publisher(String, 'chatter_qos', custom_qos_profile)
msg = String()
cycle_count = 0
while rclpy.ok() and cycle_count < args.number_of_cycles:
msg.data = 'Hello World: {0}'.format(cycle_count)
print('Publishing: "{0}"'.format(msg.data))
chatter_pub.publish(msg)
cycle_count += 1
sleep(1)
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_client_async')
cli = node.create_client(AddTwoInts, 'add_two_ints')
req = AddTwoInts.Request()
req.a = 41
req.b = 1
while not cli.wait_for_service(timeout_sec=1.0):
node.get_logger().info('service not available, waiting again...')
future = cli.call_async(req)
while rclpy.ok():
rclpy.spin_once(node)
if future.done():
if future.result() is not None:
node.get_logger().info(
'Result of add_two_ints: for %d + %d = %d' %
(req.a, req.b, future.result().sum))
else:
node.get_logger().info('Service call failed %r' % (future.exception(),))
break
node.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_client_async')
cli = node.create_client(AddTwoInts, 'add_two_ints')
req = AddTwoInts.Request()
req.a = 41
req.b = 1
# TODO(mikaelarguedas) remove this once wait for service implemented
# wait for connection to be established
# (no wait for service in Python yet)
time.sleep(1)
cli.call(req)
while rclpy.ok():
# TODO(mikaelarguedas) This is not the final API, and this does not scale
# for multiple pending requests. This will change once an executor model is implemented
# In the future the response will not be stored in cli.response
if cli.response is not None:
print('Result of add_two_ints: for %d + %d = %d' %
(req.a, req.b, cli.response.sum))
break
rclpy.spin_once(node)
node.destroy_node()
rclpy.shutdown()
def execute_goal(goal_handle):
goal = goal_handle.request
feedback = NestedMessage.Feedback()
feedback.nested_different_pkg.sec = 2 * goal.nested_field_no_pkg.duration_value.sec
result = NestedMessage.Result()
result.nested_field.int32_value = 4 * goal.nested_field_no_pkg.duration_value.sec
num_feedback = 10
for i in range(0, num_feedback):
if not rclpy.ok():
goal_handle.abort()
return NestedMessage.Result()
# Check if the goal was canceled
if goal_handle.is_cancel_requested:
goal_handle.canceled()
print('Goal was canceled')
return result
# Publish feedback
goal_handle.publish_feedback(feedback)
print('Publishing feedback')
# 10 Hz update rate
time.sleep(0.1)
# Send final result
goal_handle.succeed()
print('Goal succeeded')
return result
def main(args=None):
rclpy.init(args)
node = rclpy.create_node('minimal_publisher')
publisher = node.create_publisher(String, 'topic')
msg = String()
i = 0
def timer_callback():
nonlocal i
msg.data = 'Hello World: %d' % i
i += 1
print('Publishing: "%s"' % msg.data)
publisher.publish(msg)
timer_period = 0.5 # seconds
timer = node.create_timer(timer_period, timer_callback)
while rclpy.ok():
rclpy.spin_once(node)
# Destroy the timer attached to the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
node.destroy_timer(timer)
node.destroy_node()
rclpy.shutdown()
def execute_goal(goal_handle):
goal = goal_handle.request
feedback = Fibonacci.Feedback()
feedback.sequence = [0, 1]
for i in range(1, goal.order):
if not rclpy.ok():
goal_handle.abort()
return Fibonacci.Result()
# Check if the goal was canceled
if goal_handle.is_cancel_requested:
goal_handle.canceled()
result = Fibonacci.Result()
result.sequence = feedback.sequence
print('Goal was canceled')
return result
# Update the sequence.
feedback.sequence.append(feedback.sequence[i] + feedback.sequence[i-1])
# Publish feedback
goal_handle.publish_feedback(feedback)
print('Publishing feedback')
# 10 Hz update rate
time.sleep(0.1)
# Send final result
result = Fibonacci.Result()
result.sequence = feedback.sequence
goal_handle.succeed()
print('Goal succeeded')
return result
def main(args = None):
#Initialzing the ros client with no arguments
rclpy.init(args = args)
#Creating a client object by client node
Reset_client_obj = client_node()
#Sending the request to server
Reset_client_obj.send_request()
#While the ros client is available
while rclpy.ok():
#Spin once
rclpy.spin_once(Reset_client_obj)
#If the request is sent
if(Reset_client_obj.futureReq.done()):
#Counter reset success message
Reset_client_obj.get_logger().info("Counter reset successfully")
#Break from while loop
break
#Shutdown the ros client
rclpy.shutdown()
def replier(service_name, number_of_cycles):
from service_fixtures import get_test_srv
import rclpy
service_pkg = 'test_communication'
module = importlib.import_module(service_pkg + '.srv')
srv_mod = getattr(module, service_name)
rclpy.init(args=[])
node = rclpy.create_node('replier')
srv_fixtures = get_test_srv(service_name)
chatter_callback = functools.partial(replier_callback,
srv_fixtures=srv_fixtures)
node.create_service(srv_mod, 'test_service_' + service_name,
chatter_callback)
spin_count = 1
print('replier: beginning loop')
while rclpy.ok() and spin_count < number_of_cycles:
rclpy.spin_once(node, timeout_sec=2)
spin_count += 1
print('spin_count: ' + str(spin_count))
node.destroy_node()
rclpy.shutdown()
def main(argv=sys.argv[1:]):
parser = argparse.ArgumentParser(
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--reliable',
dest='reliable',
action='store_true',
help='set qos profile to reliable')
parser.set_defaults(reliable=False)
parser.add_argument('-n',
'--number_of_cycles',
type=int,
default=20,
help='max number of spin_once iterations')
parser.add_argument('argv',
nargs=argparse.REMAINDER,
help='Pass arbitrary arguments to the executable')
args = parser.parse_args(argv)
rclpy.init(args=args.argv)
if args.reliable:
custom_qos_profile = QoSProfile(depth=10,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_RELIABLE)
else:
custom_qos_profile = qos_profile_sensor_data
node = ListenerQos(custom_qos_profile)
cycle_count = 0
while rclpy.ok() and cycle_count < args.number_of_cycles:
rclpy.spin_once(node)
cycle_count += 1
node.destroy_node()
rclpy.shutdown()
def main(argv=sys.argv[1:]):
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
'--reliable', dest='reliable', action='store_true',
help='set qos profile to reliable')
parser.set_defaults(reliable=False)
parser.add_argument(
'-n', '--number_of_cycles', type=int, default=20,
help='max number of spin_once iterations')
args = parser.parse_args(argv)
rclpy.init()
if args.reliable:
custom_qos_profile = qos_profile_default
print('Reliable listener')
else:
custom_qos_profile = qos_profile_sensor_data
print('Best effort listener')
node = rclpy.create_node('listener_qos')
sub = node.create_subscription(String, 'chatter_qos', chatter_callback, custom_qos_profile)
assert sub # prevent unused warning
cycle_count = 0
while rclpy.ok() and cycle_count < args.number_of_cycles:
rclpy.spin_once(node)
cycle_count += 1
def replier(service_name, number_of_cycles, namespace):
from test_msgs.service_fixtures import get_test_srv
import rclpy
service_pkg = 'test_msgs'
module = importlib.import_module(service_pkg + '.srv')
srv_mod = getattr(module, service_name)
rclpy.init(args=[])
node = rclpy.create_node('replier', namespace=namespace)
srv_fixtures = get_test_srv(service_name)
chatter_callback = functools.partial(
replier_callback, srv_fixtures=srv_fixtures)
node.create_service(
srv_mod, 'test/service/' + service_name, chatter_callback)
spin_count = 1
print('replier: beginning loop')
while rclpy.ok() and spin_count < number_of_cycles:
rclpy.spin_once(node, timeout_sec=2)
spin_count += 1
print('spin_count: ' + str(spin_count))
node.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args)
node = rclpy.create_node("minimal_client_async")
cli = node.create_client(AddTwoInts, "add_two_ints")
req = AddTwoInts.Request()
req.a = 41
req.b = 1
# TODO(mikaelarguedas) remove this once wait for service implemented
# wait for connection to be established
# (no wait for service in Python yet)
time.sleep(1)
cli.call(req)
while rclpy.ok():
# TODO(mikaelarguedas) This is not the final API, and this does not scale
# for multiple pending requests. This will change once an executor model is implemented
# In the future the response will not be stored in cli.response
if cli.response is not None:
print("Result of add_two_ints: for %d + %d = %d" % (req.a, req.b, cli.response.sum))
break
rclpy.spin_once(node)
rclpy.shutdown()
def main(args=None):
#initial ROS2
rclpy.init(args=args)
#initial turtle client
cli_obj = TurtleClient()
cli_obj.get_logger().info('Turtlebot Client Started!')
# call the service
cli_obj.color_srvcall()
while rclpy.ok():
rclpy.spin_once(cli_obj)
if cli_obj.service_future.done() and cli_obj.server_call:
cli_obj.server_call = False
try:
response = cli_obj.service_future.result()
except Exception as e:
cli_obj.get_logger().info('Server call failed')
else:
cli_obj.get_logger().info('Server call success: %d' % (response.ret))
break
# Destory the node explicitly
cli_obj.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_publisher')
publisher = node.create_publisher(String, 'topic')
msg = String()
i = 0
def timer_callback():
nonlocal i
msg.data = 'Hello World: %d' % i
i += 1
print('Publishing: "%s"' % msg.data)
publisher.publish(msg)
timer_period = 0.5 # seconds
timer = node.create_timer(timer_period, timer_callback)
while rclpy.ok():
rclpy.spin_once(node)
# Destroy the timer attached to the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
node.destroy_timer(timer)
node.destroy_node()
rclpy.shutdown()
def __init__(self,
parent_frame_id: str = "world",
child_frame_id: str = "unknown_child_id",
use_sim_time: bool = True,
node_name: str = 'drl_grasping_camera_tf_broadcaster'):
try:
rclpy.init()
except:
if not rclpy.ok():
import sys
sys.exit("ROS 2 could not be initialised")
Node.__init__(self, node_name)
self.set_parameters([
Parameter('use_sim_time',
type_=Parameter.Type.BOOL,
value=use_sim_time)
])
qos = QoSProfile(durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.RELIABLE,
history=QoSHistoryPolicy.KEEP_ALL)
self._tf2_broadcaster = StaticTransformBroadcaster(self, qos=qos)
self._transform_stamped = TransformStamped()
self.set_parent_frame_id(parent_frame_id)
self.set_child_frame_id(child_frame_id)
def listener():
rclpy.init()
node = rclpy.create_node('ioniq_sub_example')
node.create_subscription(Float32MultiArray, 'Ioniq_info', callback)
while rclpy.ok():
rclpy.spin(node)
def test_configure_valid_type_yaml_component_package_mismatch(
self, pipeline_manager, proc_info, proc_output):
compnum = 1
packnum = 2
req = ConfigurePipeline.Request()
pipeline_type = PipelineType()
test_type = 'example'
pipeline_type.type = test_type
req.pipeline_type = pipeline_type
test_file_name = 'test_comp_pack_mismatch'
req.config_file_name = test_file_name
future = self.configure_client.call_async(req)
while rclpy.ok():
rclpy.spin_once(self.node)
if future.done():
res = future.result()
if res.success is False:
proc_output.assertWaitFor(
expected_output=
'Number of components and package names do not match, {} and {} respectively'
.format(compnum, packnum))
else:
self.fail('Expected configure pipeline service to fail'
' due to nonmatching component and'
' package name numbers')
break
def func_number_callbacks(args):
period = float(args[0])
rclpy.init()
node = rclpy.create_node('test_timer_no_callback')
executor = SingleThreadedExecutor()
executor.add_node(node)
# The first spin_once() takes slighly longer, just long enough for 1ms timer tests to fail
executor.spin_once(timeout_sec=0)
callbacks = []
timer = node.create_timer(period, lambda: callbacks.append(len(callbacks)))
begin_time = time.time()
while rclpy.ok() and time.time() - begin_time < 4.5 * period:
executor.spin_once(timeout_sec=period / 10)
node.destroy_timer(timer)
executor.shutdown()
rclpy.shutdown()
assert len(
callbacks
) == 4, 'should have received 4 callbacks, received %s' % str(callbacks)
return True
def run():
if missing_modules == True:
common.output(
'You\'re missing one or more modules to run this script, you must have: numpy, cv2 and rclpy. Check syncity documentation or open this file (syncity/tools/ros2disk) for more information.',
'ERROR')
sys.exit(1)
if settings.topic == None:
common.output('No topic defined', 'ERROR')
sys.exit(1)
settings._zid = 0
common.output('RCL Init...')
rclpy.init()
common.output('Subscriber node init...')
settings._node = rclpy.create_node('minimal_subscriber')
common.output('Subscribing to {}...'.format(settings.topic))
subscription = settings._node.create_subscription(Image, settings.topic,
topic_cb)
subscription # prevent unused variable warning
common.output('Entering Spin loop...', 'DEBUG')
while rclpy.ok():
common.output('Spin', 'DEBUG')
rclpy.spin(settings._node)
common.output('Destroying...')
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
settings._node.destroy_node()
rclpy.shutdown()
def talker(message_name, number_of_cycles):
from message_fixtures import get_test_msg
import rclpy
message_pkg = 'test_communication'
module = importlib.import_module(message_pkg + '.msg')
msg_mod = getattr(module, message_name)
rclpy.init(args=[])
node = rclpy.create_node('talker')
chatter_pub = node.create_publisher(
msg_mod, 'test_message_' + message_name)
cycle_count = 0
print('talker: beginning loop')
msgs = get_test_msg(message_name)
while rclpy.ok() and cycle_count < number_of_cycles:
msg_count = 0
for msg in msgs:
chatter_pub.publish(msg)
msg_count += 1
print('publishing message #%d' % msg_count)
time.sleep(0.01)
cycle_count += 1
time.sleep(0.1)
node.destroy_node()
rclpy.shutdown()
def _rostopic_delay(node, topic, window_size=DEFAULT_WINDOW_SIZE):
"""
Periodically print the publishing delay of a topic to console until shutdown.
:param topic: topic name, ``str``
:param window_size: number of messages to average over, ``unsigned_int``
:param blocking: pause delay until topic is published, ``bool``
"""
# pause hz until topic is published
msg_class = get_msg_class(node,
topic,
blocking=True,
include_hidden_topics=True)
if msg_class is None:
node.destroy_node()
return
rt = ROSTopicDelay(node, window_size)
node.create_subscription(msg_class, topic, rt.callback_delay,
qos_profile_sensor_data)
timer = node.create_timer(1, rt.print_delay)
while rclpy.ok():
rclpy.spin_once(node)
node.destroy_timer(timer)
node.destroy_node()
rclpy.shutdown()
def rotate_forever(self):
twist = Twist()
while rclpy.ok():
twist.angular.z = 0.1
self._cmd_pub.publish(twist)
self.get_logger().info('Rotating robot: {}'.format(twist))
time.sleep(0.1)
def main(args=None):
rclpy.init(args=args)
operator = Operator()
future = operator.send_request()
user_trigger = True
try:
while rclpy.ok():
if user_trigger is True:
rclpy.spin_once(operator)
if future.done():
try:
service_response = future.result()
except Exception as e: # noqa: B902
operator.get_logger().warn(
'Service call failed: {}'.format(str(e)))
else:
operator.get_logger().info('Result: {}'.format(
service_response.arithmetic_result))
user_trigger = False
else:
input('Press Enter for next service call.')
future = operator.send_request()
user_trigger = True
except KeyboardInterrupt:
operator.get_logger().info('Keyboard Interrupt (SIGINT)')
operator.destroy_node()
rclpy.shutdown()
def talker(message_name, number_of_cycles):
from message_fixtures import get_test_msg
import rclpy
from rclpy.impl.rmw_implementation_tools import select_rmw_implementation
from rclpy.qos import qos_profile_default
message_pkg = 'test_communication'
module = importlib.import_module(message_pkg + '.msg')
msg_mod = getattr(module, message_name)
select_rmw_implementation(os.environ['RCLPY_IMPLEMENTATION'])
rclpy.init([])
node = rclpy.create_node('talker')
chatter_pub = node.create_publisher(msg_mod,
'test_message_' + message_name,
qos_profile_default)
cycle_count = 0
print('talker: beginning loop')
msgs = get_test_msg(message_name)
while rclpy.ok() and cycle_count < number_of_cycles:
msg_count = 0
for msg in msgs:
chatter_pub.publish(msg)
msg_count += 1
print('publishing message #%d' % msg_count)
time.sleep(0.01)
cycle_count += 1
time.sleep(0.1)
rclpy.shutdown()
def replier(service_pkg, service_name, number_of_cycles, namespace):
import rclpy
module = importlib.import_module(service_pkg + '.srv')
srv_mod = getattr(module, service_name)
rclpy.init(args=[])
node = rclpy.create_node('replier', namespace=namespace)
req = srv_mod.Request()
resp = srv_mod.Response()
srv_fixtures = [[req, resp]]
chatter_callback = functools.partial(replier_callback,
srv_fixtures=srv_fixtures)
node.create_service(srv_mod, 'test/service/' + service_name,
chatter_callback)
spin_count = 0
print('replier: beginning loop')
while rclpy.ok() and spin_count < number_of_cycles:
rclpy.spin_once(node, timeout_sec=2)
spin_count += 1
# print('spin_count: ' + str(spin_count))
node.destroy_node()
rclpy.shutdown()
def listener(message_name):
from message_fixtures import get_test_msg
import rclpy
message_pkg = 'test_communication'
module = importlib.import_module(message_pkg + '.msg')
msg_mod = getattr(module, message_name)
rclpy.init(args=[])
node = rclpy.create_node('listener')
received_messages = []
expected_msgs = get_test_msg(message_name)
chatter_callback = functools.partial(listener_cb,
received_messages=received_messages,
expected_msgs=expected_msgs)
node.create_subscription(msg_mod, 'test_message_' + message_name,
chatter_callback)
spin_count = 1
print('subscriber: beginning loop')
while (rclpy.ok() and len(received_messages) != len(expected_msgs)):
rclpy.spin_once(node)
spin_count += 1
print('spin_count: ' + str(spin_count))
node.destroy_node()
rclpy.shutdown()
assert len(received_messages) == len(expected_msgs),\
'Should have received {} {} messages from talker'.format(len(expected_msgs), message_name)
def main(args=None):
rclpy.init(args=args)
node = Node("task_power_rune2019_client")
cli = node.create_client(SetMode, "task_power_rune/set_mode")
while not cli.wait_for_service(timeout_sec=1.0):
node.get_logger().info('service not available, waiting again...')
print(msg)
old_attr = termios.tcgetattr(sys.stdin)
tty.setcbreak(sys.stdin.fileno())
while rclpy.ok():
if select.select([sys.stdin], [], [], 0)[0] == [sys.stdin]:
key=sys.stdin.read(1)
print(key)
if (key == 'q' or key == 'Q'):
set_mode_req(cli,0x00)
elif(key=='w' or key=='W'):
set_mode_req(cli,0x01)
elif(key=='e' or key=='E'):
set_mode_req(cli,0x02)
elif(key=='r' or key=='R'):
set_mode_req(cli,0x03)
elif(key=='a' or key=='A'):
set_mode_req(cli,0x10)
elif(key=='s' or key=='S'):
set_mode_req(cli,0x11)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_attr)
node.destroy_node()
rclpy.shutdown()
def main(argv=sys.argv[1:]):
parser = argparse.ArgumentParser(
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--reliable',
dest='reliable',
action='store_true',
help='set qos profile to reliable')
parser.set_defaults(reliable=False)
parser.add_argument('-n',
'--number_of_cycles',
type=int,
default=20,
help='max number of spin_once iterations')
args = parser.parse_args(argv)
rclpy.init()
if args.reliable:
custom_qos_profile = qos_profile_default
print('Reliable listener')
else:
custom_qos_profile = qos_profile_sensor_data
print('Best effort listener')
node = ListenerQos(custom_qos_profile)
cycle_count = 0
while rclpy.ok() and cycle_count < args.number_of_cycles:
rclpy.spin_once(node)
cycle_count += 1
node.destroy_node()
rclpy.shutdown()
def mover(self):
try:
# initialize variable to write elapsed time to file
# contourCheck = 1
# find direction with the largest distance from the Lidar,
# rotate to that direction, and start moving
self.pick_direction()
while rclpy.ok():
if self.laser_range.size != 0:
# check distances in front of TurtleBot and find values less
# than stop_distance
lri = (self.laser_range[front_angles]<float(stop_distance)).nonzero()
# self.get_logger().info('Distances: %s' % str(lri))
# if the list is not empty
if(len(lri[0])>0):
# stop moving
self.stopbot()
# find direction with the largest distance from the Lidar
# rotate to that direction
# start moving
self.pick_direction()
# allow the callback functions to run
rclpy.spin_once(self)
except Exception as e:
print(e)
# Ctrl-c detected
finally:
# stop moving
self.stopbot()
def main():
rclpy.init()
node = RaportGen()
while rclpy.ok():
rclpy.spin_once(node, timeout_sec=0.1)
rclpy.shutdown()
node.destroy_node()
def run_topic_listening(node, topic_monitor, options):
"""Subscribe to relevant topics and manage the data received from susbcriptions."""
while rclpy.ok():
# Check if there is a new topic online
# TODO(dhood): use graph events rather than polling
topic_names_and_types = node.get_topic_names_and_types()
it = zip(topic_names_and_types.topic_names, topic_names_and_types.type_names)
for topic_name, type_name in it:
if not topic_monitor.is_supported_type(type_name):
continue
# Infer the appropriate QoS profile from the topic name
topic_info = topic_monitor.get_topic_info(topic_name)
if topic_info is None:
# The topic is not for being monitored
continue
is_new_topic = topic_name and topic_name not in topic_monitor.monitored_topics
if is_new_topic:
# Register new topic with the monitor
qos_profile = qos_profile_default
qos_profile.depth = QOS_DEPTH
if topic_info['reliability'] == 'best_effort':
qos_profile.reliability = \
QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE
topic_monitor.add_monitored_topic(
Header, topic_name, node, qos_profile,
options.expected_period, options.allowed_latency, options.stale_time)
if topic_monitor.monitored_topics:
# Wait for messages with a timeout, otherwise this thread will block any other threads
# until a message is received
rclpy.spin_once(node, timeout_sec=0.05)
def main(args=None):
if args is None:
args = sys.argv
rclpy.init(args=args)
node = rclpy.create_node("Source")
node_logger = node.get_logger()
topic = args[1]
filenames = args[2:]
pub_img = node.create_publisher(sensor_msgs.msg.Image, topic)
pub_img_compressed = node.create_publisher(sensor_msgs.msg.CompressedImage,
topic + "/compressed")
time.sleep(1.0)
cvb = CvBridge()
while rclpy.ok():
try:
cvim = cv2.imread(filenames[0])
pub_img.publish(cvb.cv2_to_imgmsg(cvim))
pub_img_compressed.publish(cvb.cv2_to_compressed_imgmsg(cvim))
filenames = filenames[1:] + [filenames[0]]
time.sleep(1)
except KeyboardInterrupt:
node_logger.info("shutting down: keyboard interrupt")
break
node_logger.info("test_completed")
node.destroy_node()
rclpy.shutdown()
def _spawn_entity(self, entity_xml, initial_pose):
self.get_logger().info('Waiting for service %s/spawn_entity' %
self.args.gazebo_namespace)
client = self.create_client(
SpawnEntity, '%s/spawn_entity' % self.args.gazebo_namespace)
if client.wait_for_service(timeout_sec=5.0):
req = SpawnEntity.Request()
req.name = self.args.entity
req.xml = str(entity_xml, 'utf-8')
req.robot_namespace = self.args.robot_namespace
req.initial_pose = initial_pose
req.reference_frame = self.args.reference_frame
self.get_logger().info('Calling service %s/spawn_entity' %
self.args.gazebo_namespace)
srv_call = client.call_async(req)
while rclpy.ok():
if srv_call.done():
self.get_logger().info('Spawn status: %s' %
srv_call.result().status_message)
break
rclpy.spin_once(self)
return srv_call.result().success
self.get_logger().error(
'Service %s/spawn_entity unavailable. Was Gazebo started with GazeboRosFactory?'
)
return False
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_client_async')
cli = node.create_client(AddTwoInts, 'add_two_ints')
req = AddTwoInts.Request()
req.a = 41
req.b = 1
while not cli.wait_for_service(timeout_sec=1.0):
node.get_logger().info('service not available, waiting again...')
future = cli.call_async(req)
while rclpy.ok():
rclpy.spin_once(node)
if future.done():
try:
result = future.result()
except Exception as e:
node.get_logger().info('Service call failed %r' % (e, ))
else:
node.get_logger().info(
'Result of add_two_ints: for %d + %d = %d' %
(req.a, req.b, result.sum))
break
node.destroy_node()
rclpy.shutdown()
def test_configure_valid_type_param_mismatch(self, pipeline_manager,
proc_info, proc_output):
param = 'components'
req = ConfigurePipeline.Request()
pipeline_type = PipelineType()
test_type = 'example'
pipeline_type.type = test_type
req.pipeline_type = pipeline_type
test_file_name = 'test_param_mismatch'
req.config_file_name = test_file_name
future = self.configure_client.call_async(req)
while rclpy.ok():
rclpy.spin_once(self.node)
if future.done():
res = future.result()
if res.success is False:
proc_output.assertWaitFor(
expected_output=
'Could not get the pipeline parameter "{}", wrong type'
.format(param))
else:
self.fail('Expected configure pipeline service to fail'
' due to no parameter for yaml item'
' associated to pipeline type')
break
def main():
p.connect(p.GUI)
body = p.loadURDF(args.filename, useFixedBase=1)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
bb = BulletRoboy(body)
rclpy.init()
publisher_node = JointPublisher(body)
spin_thread = Thread(target=rclpy.spin, args=(publisher_node,))
spin_thread.start()
while rclpy.ok():
try:
t = time.time()
pos = [0.2 * math.cos(t)+0.2, -0.4, 0. + 0.2 * math.sin(t) + 0.7]
threshold = 0.001
maxIter = 100
bb.accurateCalculateInverseKinematics(pos, threshold, maxIter)
bb.drawDebugLines(pos)
except KeyboardInterrupt:
publisher_node.destroy_node()
rclpy.shutdown()
publisher_node.destroy_node()
rclpy.shutdown()
def main(argv=sys.argv[1:]):
parser = argparse.ArgumentParser(
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--reliable',
dest='reliable',
action='store_true',
help='set qos profile to reliable')
parser.set_defaults(reliable=False)
parser.add_argument('-n',
'--number_of_cycles',
type=int,
default=20,
help='max number of spin_once iterations')
args = parser.parse_args(argv)
rclpy.init()
if args.reliable:
custom_qos_profile = qos_profile_default
print('Reliable listener')
else:
custom_qos_profile = qos_profile_sensor_data
print('Best effort listener')
node = rclpy.create_node('listener_qos')
sub = node.create_subscription(String, 'chatter_qos', chatter_callback,
custom_qos_profile)
assert sub # prevent unused warning
cycle_count = 0
while rclpy.ok() and cycle_count < args.number_of_cycles:
rclpy.spin_once(node)
cycle_count += 1
def spin(self):
"""Check if the service calls resolved after each spin."""
while rclpy.ok():
rclpy.spin_once(self)
# Necessary to call the services after the spin so they can resolve.
# If they don't, then they are added to a queue and tried the next time.
incomplete_futures = []
for f in self.client_futures:
if f.done():
try:
res = f.result()
except Exception as e:
self.get_logger().warn(
'Service Call to ChangeLocomotionMode failed {}.'.
format(e))
else:
self.parse_future_result(res)
else:
incomplete_futures.append(f)
if len(incomplete_futures) > 0:
self.get_logger().debug('{} incomplete futures.'.format(
len(incomplete_futures)))
self.client_futures = incomplete_futures
def main(args=None):
rclpy.init(args=args)
#robot_name = ''
robot_name = sys.argv[1]
total_robot_num = sys.argv[2]
#peer_robot_name = sys.argv[2]
explore_node = MultiExploreNode(robot_name, total_robot_num)
executor = MultiThreadedExecutor(32)
executor.add_node(explore_node)
executor.add_node(explore_node.r_interface_)
# executor.add_node(explore_node.peer_interface_)
# executor.add_node(explore_node.group_coordinator_)
spin_thread = Thread(target=executor.spin)
# spin_thread = Thread(target=rclpy.spin, args=(explore_node,))
spin_thread.start()
# input('({})press to continue'.format(robot_name))
while rclpy.ok():
state = explore_node.updateMultiHierarchicalCoordination()
if state == explore_node.e_util.SYSTEM_SHUTDOWN:
break
explore_node.get_logger().info('system shutdown...')
# rclpy.spin(node)
rclpy.shutdown()
def talker(message_name, rmw_implementation, number_of_cycles):
import rclpy
from rclpy.qos import qos_profile_default
from rclpy.impl.rmw_implementation_tools import select_rmw_implementation
from message_fixtures import get_test_msg
message_pkg = 'test_communication'
module = importlib.import_module(message_pkg + '.msg')
msg_mod = getattr(module, message_name)
select_rmw_implementation(rmw_implementation)
rclpy.init([])
node = rclpy.create_node('talker')
chatter_pub = node.create_publisher(
msg_mod, 'test_message_' + message_name, qos_profile_default)
cycle_count = 0
print('talker: beginning loop')
msgs = get_test_msg(message_name)
while rclpy.ok() and cycle_count < number_of_cycles:
cycle_count += 1
msg_count = 0
for msg in msgs:
chatter_pub.publish(msg)
msg_count += 1
print('publishing message #%d' % msg_count)
time.sleep(0.1)
time.sleep(1)
rclpy.shutdown()
def talker(message_name, number_of_cycles, namespace):
from test_msgs.message_fixtures import get_test_msg
import rclpy
message_pkg = 'test_msgs'
module = importlib.import_module(message_pkg + '.msg')
msg_mod = getattr(module, message_name)
rclpy.init(args=[])
node = rclpy.create_node('talker', namespace=namespace)
chatter_pub = node.create_publisher(
msg_mod, 'test/message/' + message_name)
cycle_count = 0
print('talker: beginning loop')
msgs = get_test_msg(message_name)
while rclpy.ok() and cycle_count < number_of_cycles:
msg_count = 0
for msg in msgs:
chatter_pub.publish(msg)
msg_count += 1
print('publishing message #%d' % msg_count)
time.sleep(0.01)
cycle_count += 1
time.sleep(0.1)
node.destroy_node()
rclpy.shutdown()
def _rostopic_echo_test(topic):
rclpy.init(None)
node = rclpy.create_node('rostopic')
# TODO FIXME no path resolving for topics yet implemented thereby the initial "/" is
# omited.
sub = node.create_subscription(String, topic[1:], chatter_callback, qos_profile_default)
while rclpy.ok():
rclpy.spin_once(node)
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_client')
# Node's default callback group is mutually exclusive. This would prevent the client response
# from being processed until the timer callback finished, but the timer callback in this
# example is waiting for the client response
cb_group = ReentrantCallbackGroup()
cli = node.create_client(AddTwoInts, 'add_two_ints', callback_group=cb_group)
did_run = False
did_get_result = False
async def call_service():
nonlocal cli, node, did_run, did_get_result
did_run = True
try:
req = AddTwoInts.Request()
req.a = 41
req.b = 1
future = cli.call_async(req)
result = await future
if result is not None:
node.get_logger().info(
'Result of add_two_ints: for %d + %d = %d' %
(req.a, req.b, result.sum))
else:
node.get_logger().info('Service call failed %r' % (future.exception(),))
finally:
did_get_result = True
while not cli.wait_for_service(timeout_sec=1.0):
node.get_logger().info('service not available, waiting again...')
timer = node.create_timer(0.5, call_service, callback_group=cb_group)
while rclpy.ok() and not did_run:
rclpy.spin_once(node)
if did_run:
# call timer callback only once
timer.cancel()
while rclpy.ok() and not did_get_result:
rclpy.spin_once(node)
node.destroy_node()
rclpy.shutdown()
def func_cancel_reset_timer(args):
import time
import rclpy
period = float(args[0])
rclpy.init()
node = rclpy.create_node('test_timer_no_callback')
callbacks = []
timer = node.create_timer(period, lambda: callbacks.append(len(callbacks)))
begin_time = time.time()
assert not timer.is_canceled()
while rclpy.ok() and time.time() - begin_time < 2.5 * period:
rclpy.spin_once(node, period / 10)
assert len(callbacks) == 2, 'should have received 2 callbacks, received %d' % len(callbacks)
assert not timer.is_canceled()
timer.cancel()
assert timer.is_canceled()
callbacks = []
begin_time = time.time()
while rclpy.ok() and time.time() - begin_time < 2.5 * period:
rclpy.spin_once(node, period / 10)
assert timer.is_canceled()
assert [] == callbacks, \
"shouldn't have received any callback, received %d" % len(callbacks)
timer.reset()
assert not timer.is_canceled()
begin_time = time.time()
while rclpy.ok() and time.time() - begin_time < 2.5 * period:
rclpy.spin_once(node, period / 10)
assert not timer.is_canceled()
assert len(callbacks) == 2, 'should have received 2 callbacks, received %d' % len(callbacks)
node.destroy_timer(timer)
rclpy.shutdown()
return True
def main(args=None):
rclpy.init(args)
node = rclpy.create_node('add_two_ints_server')
minimal_service = MinimalService(node)
minimal_service # prevent unused variable warning
while rclpy.ok():
rclpy.spin_once(node)
rclpy.shutdown()
def main(args=None):
if args is None:
args = sys.argv
rclpy.init(args)
node = rclpy.create_node('listener')
sub = node.create_subscription(String, 'chatter', chatter_callback, qos_profile_default)
assert sub # prevent unused warning
while rclpy.ok():
rclpy.spin_once(node)
def send_goals(node, action_type, tests):
import rclpy
from rclpy.action import ActionClient
action_client = ActionClient(node, action_type, 'test/action/' + action_type.__name__)
if not action_client.wait_for_server(20):
print('Action server not available after waiting', file=sys.stderr)
return 1
test_index = 0
while rclpy.ok() and test_index < len(tests):
print('Sending goal for test number {}'.format(test_index), file=sys.stderr)
test = tests[test_index]
invalid_feedback = False
# On feedback, check the feedback is valid
def feedback_callback(feedback):
nonlocal invalid_feedback
if not test.is_feedback_valid(feedback):
invalid_feedback = True
goal_handle_future = action_client.send_goal_async(
test.goal,
feedback_callback=feedback_callback)
rclpy.spin_until_future_complete(node, goal_handle_future)
goal_handle = goal_handle_future.result()
if not goal_handle.accepted:
print('Goal rejected', file=sys.stderr)
return 1
get_result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, get_result_future)
result = get_result_future.result()
if not test.is_result_valid(result):
return 1
if invalid_feedback:
return 1
test_index += 1
return 0
def main(args=None):
rclpy.init(args)
node = rclpy.create_node('add_two_ints_server')
srv = node.create_service(AddTwoInts, 'add_two_ints', add_two_ints_callback)
while rclpy.ok():
rclpy.spin_once(node)
# Destroy the service attached to the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
node.destroy_service(srv)
rclpy.shutdown()
def main(args=None):
rclpy.init(args)
node = rclpy.create_node('minimal_publisher')
minimal_subscriber = MinimalSubscriber(node)
minimal_subscriber # prevent unused variable warning
while rclpy.ok():
rclpy.spin_once(node)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
node.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_subscriber')
subscription = node.create_subscription(String, 'topic', chatter_callback)
subscription # prevent unused variable warning
while rclpy.ok():
rclpy.spin_once(node)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
node.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args)
node = rclpy.create_node("minimal_subscriber")
subscription = node.create_subscription(String, "topic", lambda msg: print("I heard: [%s]" % msg.data))
subscription # prevent unused variable warning
while rclpy.ok():
rclpy.spin_once(node)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
node.destroy_node()
rclpy.shutdown()
def main(args=None):
if args is None:
args = sys.argv
rclpy.init(args)
# TODO(wjwwood): move this import back to the top of the file when
# it is possible to import the messages before rclpy.init().
from std_msgs.msg import String
node = rclpy.create_node('listener')
sub = node.create_subscription(String, 'chatter', chatter_callback, qos_profile_default)
assert sub # prevent unused warning
while rclpy.ok():
rclpy.spin_once(node)
def main(service_type, lifecycle_node, change_state_args='', args=None):
rclpy.init(args)
if not rclpy.ok():
print("Something is wrong with rclpy init")
return
if service_type == 'change_state':
change_state(lifecycle_node, change_state_args)
if service_type == 'get_state':
get_state(lifecycle_node)
if service_type == 'get_available_states':
get_available_states(lifecycle_node)
if service_type == 'get_available_transitions':
get_available_transitions(lifecycle_node)
def main(args=None):
rclpy.init(args=args)
minimal_client = MinimalClientAsync()
minimal_client.send_request()
while rclpy.ok():
# TODO(mikaelarguedas) This is not the final API, and this does not scale
# for multiple pending requests. This will change once an executor model is implemented
# In the future the response will not be stored in cli.response
if minimal_client.cli.response is not None:
print(
'Result of add_two_ints: for %d + %d = %d' %
(minimal_client.req.a, minimal_client.req.b, minimal_client.cli.response.sum))
break
rclpy.spin_once(minimal_client)
minimal_client.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
minimal_client = MinimalClientAsync()
minimal_client.send_request()
while rclpy.ok():
rclpy.spin_once(minimal_client)
if minimal_client.future.done():
if minimal_client.future.result() is not None:
response = minimal_client.future.result()
minimal_client.get_logger().info(
'Result of add_two_ints: for %d + %d = %d' %
(minimal_client.req.a, minimal_client.req.b, response.sum))
else:
minimal_client.get_logger().info(
'Service call failed %r' % (minimal_client.future.exception(),))
break
minimal_client.destroy_node()
rclpy.shutdown()
def func_number_callbacks(args):
import time
import rclpy
period = float(args[0])
rclpy.init()
node = rclpy.create_node('test_timer_no_callback')
callbacks = []
timer = node.create_timer(period, lambda: callbacks.append(len(callbacks)))
begin_time = time.time()
while rclpy.ok() and time.time() - begin_time < 4.5 * period:
rclpy.spin_once(node, period / 10)
node.destroy_timer(timer)
rclpy.shutdown()
assert len(callbacks) == 4, 'should have received 4 callbacks, received %s' % str(callbacks)
return True
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_publisher')
publisher = node.create_publisher(String, 'topic')
msg = String()
i = 0
while rclpy.ok():
msg.data = 'Hello World: %d' % i
i += 1
print('Publishing: "%s"' % msg.data)
publisher.publish(msg)
sleep(0.5) # seconds
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
node.destroy_node()
rclpy.shutdown()
def requester(service_name, namespace):
import rclpy
from test_msgs.service_fixtures import get_test_srv
# Import the service
service_pkg = 'test_msgs'
module = importlib.import_module(service_pkg + '.srv')
srv_mod = getattr(module, service_name)
srv_fixtures = get_test_srv(service_name)
service_name = 'test/service/' + service_name
rclpy.init(args=[])
try:
node = rclpy.create_node('requester', namespace=namespace)
try:
# wait for the service to be available
client = node.create_client(srv_mod, service_name)
tries = 15
while rclpy.ok() and not client.wait_for_service(timeout_sec=1.0) and tries > 0:
print('service not available, waiting again...')
tries -= 1
assert tries > 0, 'service still not available, aborting test'
print('requester: beginning request')
# Make one call to that service
for req, resp in srv_fixtures:
future = client.call_async(req)
rclpy.spin_until_future_complete(node, future)
assert repr(future.result()) == repr(resp), \
'unexpected response %r\n\nwas expecting %r' % (future.result(), resp)
print('received reply #%d of %d' % (
srv_fixtures.index([req, resp]) + 1, len(srv_fixtures)))
finally:
node.destroy_node()
finally:
rclpy.shutdown()
def listener(message_name, rmw_implementation, number_of_cycles):
import rclpy
from rclpy.qos import qos_profile_default
from rclpy.impl.rmw_implementation_tools import select_rmw_implementation
from message_fixtures import get_test_msg
message_pkg = 'test_communication'
module = importlib.import_module(message_pkg + '.msg')
msg_mod = getattr(module, message_name)
select_rmw_implementation(rmw_implementation)
rclpy.init([])
node = rclpy.create_node('listener')
received_messages = []
expected_msgs = get_test_msg(message_name)
chatter_callback = functools.partial(
listener_cb, received_messages=received_messages, expected_msgs=expected_msgs)
node.create_subscription(
msg_mod, 'test_message_' + message_name, chatter_callback,
qos_profile_default)
spin_count = 1
print('subscriber: beginning loop')
while (rclpy.ok() and spin_count < number_of_cycles and
len(received_messages) != len(expected_msgs)):
rclpy.spin_once(node)
spin_count += 1
rclpy.shutdown()
assert len(received_messages) == len(expected_msgs),\
'Should have received {} {} messages from talker'.format(len(expected_msgs), message_name)