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 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 test_node_name_replacement(self, node_name_replacement):
for attempt in range(self.ATTEMPTS):
if node_name_replacement in self.node.get_node_names():
break
time.sleep(self.TIME_BETWEEN_ATTEMPTS)
rclpy.spin_once(self.node, timeout_sec=0)
self.assertIn(node_name_replacement, self.node.get_node_names())
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):
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 service_loop():
global ros_node
global ros_thread_loop
if __ros_version__ == 2:
while ros_thread_loop :
rclpy.spin_once(ros_node, timeout_sec=1.0)
print("..Terminate RosThread")
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 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 test_service_replacement(self, service_replacement):
for attempt in range(self.ATTEMPTS):
if service_replacement in self.get_services():
break
time.sleep(self.TIME_BETWEEN_ATTEMPTS)
rclpy.spin_once(self.node, timeout_sec=0)
self.assertNotIn(service_replacement, self.get_topics())
self.assertIn(service_replacement, self.get_services())
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)
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 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):
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 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(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 func_zero_callback(args):
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)))
rclpy.spin_once(node, period / 2.)
node.destroy_timer(timer)
rclpy.shutdown()
assert len(callbacks) == 0, \
"shouldn't have received any callback, received %d" % len(callbacks)
return True
def main(args=None):
rclpy.init(args=args)
minimal_client = MinimalClientAsync()
minimal_client.send_open_request()
while rclpy.ok():
rclpy.spin_once(minimal_client)
if minimal_client.future.done():
try:
response = minimal_client.future.result()
except Exception as e:
minimal_client.get_logger().info('Service call failed %r' %
(e, ))
else:
if response.success == False:
minimal_client.send_close_request()
break
minimal_client.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_client_async')
minimal_client = MinimalClientAsync(node)
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(node)
node.destroy_node()
rclpy.shutdown()
def test_run_no_configure(self, pipeline_manager, proc_info, proc_output):
goal_msg = RunPipeline.Goal()
goal_msg.input = 0
future = self.run_client.send_goal_async(goal_msg)
future_count = 0
result = None
while rclpy.ok():
rclpy.spin_once(self.node)
if future.done():
future_count += 1
if future_count == 1:
future = future.result().get_result_async()
if future_count == 2:
result = future.result().result
break
if result.output == 1:
proc_output.assertWaitFor(
expected_output='Pipeline is not configured')
else:
self.fail('Should abort due to unconfigured pipeline')
def main(args=None):
rclpy.init(args=args)
client_vel = service_node_vel()
client_vel.client_request()
while rclpy.ok():
rclpy.spin_once(client_vel)
if client_vel.response.done():
try:
response = client_vel.response.result()
except Exception as e:
client_vel.get_logger().info('Service call failed %r' % (e, ))
else:
client_vel.get_logger().info(
' Two wheel speeds set to %d , %d ' %
(client_vel.value.left_speed,
client_vel.value.right_speed))
break
client_vel.destroy_node()
rclpy.shutdown()
def test_node_init_state(self):
"""
Test init state.
Expect fault_injection_node does not publish /diagnostics.status
while the talker does not to publish
"""
msg_buffer = []
self.test_node.create_subscription(DiagnosticArray, "/diagnostics",
lambda msg: msg_buffer.append(msg),
10)
# Wait until the talker transmits two messages over the ROS topic
end_time = time.time() + self.evaluation_time
while time.time() < end_time:
rclpy.spin_once(self.test_node, timeout_sec=0.1)
# Return False if no valid data is received
self.assertEqual(
self.get_num_valid_data(msg_buffer, DiagnosticStatus.ERROR), 0)
def call_service(self, srv_type, srv_name, model_name, model_xml=None, robot_namespace=None, initial_pose=None, reference_frame=None):
service_node = ServiceNode(srv_type,srv_name)
if srv_type == SpawnEntity:
service_node.send_spawn(model_name, model_xml,robot_namespace,initial_pose,reference_frame)
else:
service_node.send_delete(model_name)
rclpy.spin_once(service_node)
if service_node.future.done():
try:
response = service_node.future.result()
except Exception as e:
service_node.get_logger().info(
'Service call failed %r' % (e,))
else:
service_node.get_logger().info("Successfully executed service")
service_node.get_logger().info(
'Result of service: '+response.status_message)
service_node.destroy_node()
def take_observation(self):
"""
Take observation from the environment and return it.
:return: state.
"""
# # Take an observation
rclpy.spin_once(self.node)
obs_img = self._observation_img
# Check that the observation is not prior to the action
while obs_img is None:
rclpy.spin_once(self.node)
obs_img = self._observation_img
img = self.bridge.imgmsg_to_cv2(obs_img, "bgr8")
img = PIL_Image.fromarray(img)
obs_img = np.asarray(img, dtype="uint8")
self._observation_img = None
return obs_img
def random_positions(node: rclpy.node.Node) -> Optional[numpy.ndarray]:
if "client" not in random_positions.__dict__:
random_positions.client = node.create_client(RandomPositions,
'/random_positions')
if random_positions.client.wait_for_service(timeout_sec=5.0):
# node.get_logger().debug('Calling service /create_marker')
srv_call = random_positions.client.call_async(
RandomPositions.Request())
while rclpy.ok():
if srv_call.done():
break
rclpy.spin_once(node)
# Sort the joint and position pairs such that arm_1_joint is first, and arm_3_joint is last
joint_val_pairs = list(
zip(srv_call.result().joints,
srv_call.result().positions))
joint_val_pairs.sort()
positions = [v for _, v in joint_val_pairs]
return numpy.array(positions)
node.get_logger().error('Service /random_positions unavailable')
return None
def main(args=None):
rclpy.init(args=args)
cmd_vel_publisher = CmdVelPublisher()
start_time = cmd_vel_publisher.get_clock().now().to_msg().sec
clock_now = start_time
time_delta = 0
while (clock_now - start_time) < 5:
rclpy.spin_once(cmd_vel_publisher)
clock_now = cmd_vel_publisher.get_clock().now().to_msg().sec
time_delta = clock_now - start_time
cmd_vel_publisher.get_logger().info(f'{time_delta} seconds passed')
cmd_vel_publisher.stop_robot()
cmd_vel_publisher.get_logger().info('\n==== Stop Publishing ====')
cmd_vel_publisher.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 _delete_entity(self):
# Delete entity from gazebo on shutdown if bond flag enabled
self.get_logger().info('Deleting entity [{}]'.format(self.args.entity))
client = self.create_client(
DeleteEntity, '%s/delete_entity' % self.args.gazebo_namespace)
if client.wait_for_service(timeout_sec=5.0):
req = DeleteEntity.Request()
req.name = self.args.entity
self.get_logger().info('Calling service %s/delete_entity' %
self.args.gazebo_namespace)
srv_call = client.call_async(req)
while rclpy.ok():
if srv_call.done():
self.get_logger().info('Deleting status: %s' %
srv_call.result().status_message)
break
rclpy.spin_once(self)
else:
self.get_logger().error(
'Service %s/delete_entity unavailable. ' +
'Was Gazebo started with GazeboRosFactory?')
def test_run_comp_not_valid(self, pipeline_manager, proc_info, proc_output):
req = ConfigurePipeline.Request()
pipeline_type = PipelineType()
test_type = 'example'
pipeline_type.type = test_type
req.pipeline_type = pipeline_type
test_file_name = 'test_run_comp_not_valid'
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():
break
goal_msg = RunPipeline.Goal()
goal_msg.input = 0
future = self.run_client.send_goal_async(goal_msg)
future_count = 0
result = None
while rclpy.ok():
rclpy.spin_once(self.node)
if future.done():
future_count += 1
if future_count == 1:
future = future.result().get_result_async()
if future_count == 2:
result = future.result().result
break
if result.output == 1:
component = 'example::DoesNotExist'
proc_output.assertWaitFor(
expected_output='Failed to find class with the requested plugin name \'{}\' in the loaded library'.format(component))
proc_output.assertWaitFor(
expected_output='The component {} was not loaded successfully'.format(component))
proc_output.assertWaitFor(
expected_output='Aborting the pipeline')
else:
self.fail('Should abort from component not able to load')
def run(self):
"""
Runs the pipeline sequence
For each pipeline type in the sequence, configures the pipeline with the type,
then runs the pipeline and gets the result.
"""
for i in range(len(self.pipelines)):
pipeline = self.pipelines[i]
future = self._send_configure_request(pipeline)
success = False
# Configure pipeline
while rclpy.ok():
rclpy.spin_once(self)
if future.done():
success = future.result().success
break
if not success:
self.get_logger().error("Error while configuring pipeline")
break
# Run pipeline
future = self._run_current_pipeline()
while rclpy.ok():
rclpy.spin_once(self)
if future.done():
future = future.result().get_result_async()
break
# Get pipeline result
while rclpy.ok():
rclpy.spin_once(self)
if future.done():
result = future.result().result
break
def take_observation(self):
"""
Take observation from the environment and return it.
:return: state.
"""
# Take an observation
rclpy.spin_once(self.node)
obs_message = self._observation_msg
if obs_message is None:
print("Last observation is empty")
return None
# Collect the end effector points and velocities in cartesian coordinates for the process_observations state.
# Collect the present joint angles and velocities from ROS for the state.
last_observations = ut_mara.process_observations(obs_message, self.environment)
# Get Jacobians from present joint angles and KDL trees
# The Jacobians consist of a 6x6 matrix getting its from from
# (joint angles) x (len[x, y, z] + len[roll, pitch, yaw])
ee_link_jacobians = ut_mara.get_jacobians(last_observations, self.num_joints, self.jac_solver)
if self.environment['link_names'][-1] is None:
print("End link is empty!!")
return None
else:
translation, rot = general_utils.forward_kinematics(self.mara_chain,
self.environment['link_names'],
last_observations[:self.num_joints],
base_link=self.environment['link_names'][0], # make the table as the base to get the world coordinate system
end_link=self.environment['link_names'][-1])
current_ee_pos_tgt = np.ndarray.flatten(general_utils.get_ee_points(self.environment['end_effector_points'], translation, rot).T)
ee_pos_points = current_ee_pos_tgt - self.target_position
ee_velocities = ut_mara.get_ee_points_velocities(ee_link_jacobians, self.environment['end_effector_points'], rot, last_observations)
# Concatenate the information that defines the robot state
# vector, typically denoted asrobot_id 'x'.
state = np.r_[np.reshape(last_observations, -1),
np.reshape(ee_pos_points, -1),
np.reshape(ee_velocities, -1),]
return state
def _call_get_transitions(node, states, service_name):
clients = {}
futures = {}
# create clients
for node_name in states.keys():
clients[node_name] = node.create_client(GetAvailableTransitions,
f'{node_name}/{service_name}')
# wait until all clients have been called
while True:
for node_name in [n for n in states.keys() if n not in futures]:
# call as soon as ready
client = clients[node_name]
if client.service_is_ready():
request = GetAvailableTransitions.Request()
future = client.call_async(request)
futures[node_name] = future
if len(futures) == len(clients):
break
rclpy.spin_once(node, timeout_sec=1.0)
# wait for all responses
for future in futures.values():
rclpy.spin_until_future_complete(node, future)
# return transitions from current state or exception for each node
transitions = {}
for node_name, future in futures.items():
if future.result() is not None:
response = future.result()
transitions[node_name] = []
for transition_description in response.available_transitions:
if (states[node_name] is None
or transition_description.start_state
== states[node_name]):
transitions[node_name].append(transition_description)
else:
transitions[node_name] = future.exception()
return transitions
def main(args=None):
rclpy.init(args=args)
client_node = rclpy.create_node('minimal_action_client')
ac_list = []
action_client_1 = MinimalActionClient(node=client_node,
server_name='fibonacci_1')
action_client_2 = MinimalActionClient(node=client_node,
server_name='fibonacci_2')
ac_list.append(action_client_1)
ac_list.append(action_client_2)
# action_client_1.send_goal()
# action_client_2.send_goal()
while True:
for i in range(len(ac_list)):
if ac_list[i].goal_done():
print("I'm done")
print("result", ac_list[i].get_result())
ac_list[i].set_done(False)
ac_list[i].set_result(None)
ac_list[i].set_free(True)
print("apply policy")
else:
if ac_list[i].client_free():
ac_list[i].set_free(False)
if i == 0:
ac_list[i].send_goal(5)
else:
ac_list[i].send_goal(6)
print("sent goal")
else:
rclpy.spin_once(client_node)
print("spinning")
# rclpy.spin(client_node)
print("print executed")
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():
try:
response = minimal_client.future.result()
except Exception as e:
minimal_client.get_logger().info(
'Service call failed %r' % (e,))
else:
minimal_client.get_logger().info(
'Setting SPIKE motor to %d' %
(minimal_client.req.speed))
break
minimal_client.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
que_cli = QueueClient()
try:
while rclpy.ok():
rclpy.spin_once(que_cli, timeout_sec=1.0)
if que_cli.future is None:
que_cli.send_request()
else:
if que_cli.future.done():
rsp = que_cli.future.result()
que_cli.get_logger().info("Get Last Data: [{}]".format(
rsp.values[-1].string_value))
que_cli.future = None
except KeyboardInterrupt:
pass
que_cli.destroy_node()
rclpy.shutdown()
def main():
rclpy.init(args=sys.argv)
node = rclpy.create_node(NODE_NAME)
node.create_subscription(
AppCommand, # msg_type
Topics.SEND_APP_COMMAND.value, # topic
send_app_command_callback, # callback
qos_profile_sensor_data # QoS Profile
)
node.create_service(
GetAppData, # srv_type
Services.GET_APP_DATA.value, # srv_name
get_app_data_callback # callback
)
print(f"{NODE_NAME} is UP.")
while rclpy.ok():
rclpy.spin_once(node)
print(f"{NODE_NAME} is DOWN.")
def main(args=None):
env: LobotArmEnv = gym.make('LobotArmDiscrete-v1') # Discrete with noise
action_space: Type[MultiDiscrete] = env.action_space
rclpy.spin_once(env.node)
while True:
print("-------------Starting----------------")
for x in range(500):
# action = numpy.array([1, 0, 4])
action = action_space.sample()
action_do_nothing = numpy.array([2, 2, 2])
observation, reward, done, info = env.step(action_do_nothing)
# Type hints
observation: numpy.ndarray
reward: float
done: bool
info: dict
if done:
break
time.sleep(0.8)
print("-------------Resetting environment---------------")
env.reset()
print("-------------Reset finished----------------")
def subscriber(node, topic_name, message_type, callback):
if message_type is None:
topic_names_and_types = get_topic_names_and_types(node=node)
try:
expanded_name = expand_topic_name(topic_name, node.get_name(), node.get_namespace())
except ValueError as e:
raise RuntimeError(e)
try:
validate_full_topic_name(expanded_name)
except rclpy.exceptions.InvalidTopicNameException as e:
raise RuntimeError(e)
for n, t in topic_names_and_types:
if n == expanded_name:
if len(t) > 1:
raise RuntimeError(
"Cannot echo topic '%s', as it contains more than one type: [%s]" %
(topic_name, ', '.join(t))
)
message_type = t[0]
break
else:
raise RuntimeError(
'Could not determine the type for the passed topic')
# TODO(dirk-thomas) this logic should come from a rosidl related package
try:
package_name, message_name = message_type.split('/', 2)
if not package_name or not message_name:
raise ValueError()
except ValueError:
raise RuntimeError('The passed message type is invalid')
module = importlib.import_module(package_name + '.msg')
msg_module = getattr(module, message_name)
node.create_subscription(
msg_module, topic_name, callback, qos_profile=qos_profile_sensor_data)
while rclpy.ok():
rclpy.spin_once(node)
def test_set_io(self):
"""Test to set an IO and check whether it has been set."""
self.io_msg = None
io_states_sub = self.node.create_subscription(
IOStates,
"/io_and_status_controller/io_states",
self.io_msg_cb,
rclpy.qos.qos_profile_system_default,
)
pin = 0
set_io_req = SetIO.Request()
set_io_req.fun = 1
set_io_req.pin = pin
set_io_req.state = 1.0
self.call_service(self.set_io_client, set_io_req)
pin_state = False
end_time = time.time() + 5
while not pin_state and time.time() < end_time:
rclpy.spin_once(self.node, timeout_sec=0.1)
if self.io_msg is not None:
pin_state = self.io_msg.digital_out_states[pin].state
self.assertEqual(pin_state, 1)
set_io_req.state = 0.0
self.call_service(self.set_io_client, set_io_req)
end_time = time.time() + 5
while pin_state and time.time() < end_time:
rclpy.spin_once(self.node, timeout_sec=0.1)
if self.io_msg is not None:
pin_state = self.io_msg.digital_out_states[pin].state
self.assertEqual(pin_state, 0)
self.node.destroy_subscription(io_states_sub)
def run(self):
# Start main thread loop
while rclpy.ok() and not self.exitFlag:
rclpy.spin_once(self.node)
(setResponse, getResponse) = self.node.getRequestResponses()
if setResponse is not None:
self.regulatorSettingsSetDone.emit(setResponse.success,
setResponse.error_text)
if getResponse is not None:
self.regulatorSettingsGetDone.emit(getResponse)
sleep(0.01)
# Cleanup: disable motors
self.setEnabled(False)
# Cleanup: destroy timers (threads!)
self.node.destroyTimers()
self.node.destroy_node()
rclpy.shutdown()
def main():
init()
rclpy.init()
real = yf_node.YF_RealSpeed(nodeName["RealSpeed"], "RealSpeed", "pub")
soll = yf_node.YF_SollSpeed(nodeName["SollSpeed"], "SollSpeed", "sub")
rclpy.spin_once(soll.node, timeout_sec=0.1)
q_subNode = Queue(1)
q_pubNode = Queue(1)
q_subNode.put_nowait(soll)
q_pubNode.put_nowait(real)
t_sub = threading.Thread(target=subSpin, args=(q_subNode, ))
t_pub = threading.Thread(target=pubSpin, args=(q_pubNode, ))
t_sub.start()
t_pub.start()
Motor_serial = SerialThread("/dev/ttyUSB0")
t_read = threading.Thread(target=Motor_serial.read, daemon=False)
t_write = threading.Thread(target=Motor_serial.write, daemon=False)
Motor_serial.alive = True
t_read.start()
t_write.start()
try:
while True: #:
Motor_serial.control_data = soll.subMsg
real.publishMsg(Motor_serial.read_data)
time.sleep(0.25)
finally:
Motor_serial.control_data = [0.0, 0.0, 0.0, 0.0]
time.sleep(.7)
Motor_serial.stop()
time.sleep(.5)
t_read.join()
t_write.join()
time.sleep(.1)
def retrieve_urdf(self, timeout_sec: float = 5) -> None:
"""Retrieve the URDF file from the /robot_description topic.
Will raise an EnvironmentError if the topic is unavailable.
"""
qos_profile = QoSProfile(depth=1)
qos_profile.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL
self.urdf = None
def urdf_received(msg: String):
self.urdf = msg.data
self.create_subscription(
msg_type=String,
topic='/robot_description',
qos_profile=qos_profile,
callback=urdf_received,
)
rclpy.spin_once(self, timeout_sec=timeout_sec)
if self.urdf is None:
raise EnvironmentError('Could not retrieve the URDF!')
def spin(self):
""" Start event loop"""
if self.transport == 'ZMQ':
# Event Loop calback option
if self.sub_callback != None:
stream_sub = zmqstream.ZMQStream(self.sub_socket)
stream_sub.on_recv(self.__process_message)
ioloop.IOLoop.instance().start()
elif self.transport == 'ROS':
import rospy
"""Function used to spin ROS subscriber"""
rospy.spin()
elif self.transport == 'ROS2':
import rclpy
while rclpy.ok():
rclpy.spin_once(self.node_ros2)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
self.node_ros2.destroy_node()
rclpy.shutdown()
def run(self):
jointSub = self.node_.create_subscription(JointState, "/Joint_state",
self.jointCallback)
intSub = self.node_.create_subscription(Int16MultiArray, "/Int_Info",
self.intCallback)
floatSub = self.node_.create_subscription(Float32MultiArray,
"/Ioniq_info",
self.floatCallback)
self.accelPub = self.node_.create_publisher(Int16, '/dbw_cmd/Accel',
qos_profile_default)
self.brakePub = self.node_.create_publisher(Int16, '/dbw_cmd/Brake',
qos_profile_default)
self.steerPub = self.node_.create_publisher(Int16, '/dbw_cmd/Steer',
qos_profile_default)
self.cruise_speed_Pub = self.node_.create_publisher(
Int16, '/Cruise_speed', qos_profile_default)
while rclpy.ok():
rclpy.spin_once(self.node_)
time.sleep(0.002)
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():
try:
response = minimal_client.future.result()
except Exception as e:
minimal_client.get_logger().info('Service call failed %r' %
(e, ))
else:
minimal_client.get_logger().info(
'Result of add_two_ints: for %d + %d = %d' %
(minimal_client.req.a, minimal_client.req.b, response.sum))
break
minimal_client.destroy_node()
rclpy.shutdown()
def test_configure_valid_type_no_yaml(self, pipeline_manager, proc_info,
proc_output):
req = ConfigurePipeline.Request()
pipeline_type = PipelineType()
test_type = 'example'
pipeline_type.type = test_type
req.pipeline_type = pipeline_type
test_file_name = 'test_no_yaml'
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 find {}.yaml'.format(
test_file_name))
else:
self.fail('Expected configure pipeline service to fail'
' due to no yaml associated to pipeline type')
break
def main(args=None):
rclpy.init(args=args)
srv_cli = AsyncServiceClient()
try:
while rclpy.ok():
rclpy.spin_once(srv_cli, timeout_sec=0.1)
if srv_cli.future is None:
srv_cli.send_request()
else:
if srv_cli.future.done():
rsp = srv_cli.future.result()
srv_cli.get_logger().info(
'Result of add_two_ints: for %d + %d = %d' %
(srv_cli.req.a, srv_cli.req.b, rsp.sum))
srv_cli.future = None
except KeyboardInterrupt:
pass
srv_cli.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('PubAndSub.py')
# sub_key = node.create_subscription(String4, 'key', key_sub_callback, 10)
# pub_control = node.create_publisher(Carcontrol1, 'control', 10)
Control_Subscriber = ControlSubscriber()
Control_Publisher = ControlPublisher()
i = 0
while True:
rclpy.spin_once(Control_Subscriber)
i += 1
Control_Publisher.key_pub(Control_Subscriber.con_msg)
Control_Publisher.get_logger().info(
'%d Publishing: emergency:%d speed:%d steer:%d' %
(i, Control_Subscriber.con_msg.emergency,
Control_Subscriber.con_msg.speed,
Control_Subscriber.con_msg.steer))
rclpy.shutdown()
def test_talker_listener():
launch_desc = LaunchDescriptor()
launch_desc.add_process(
cmd=[sys.executable, os.path.join(this_dir, 'talker.py')],
name='test_talker_listener__talker'
)
launcher = DefaultLauncher()
launcher.add_launch_descriptor(launch_desc)
async_launcher = AsynchronousLauncher(launcher)
async_launcher.start()
import rclpy
rclpy.init([])
from rclpy.qos import qos_profile_default
from std_msgs.msg import String
assert String.__class__._TYPE_SUPPORT is not None
node = rclpy.create_node('listener')
received_messages = []
chatter_callback = functools.partial(
listener_callback, received_messages=received_messages)
# TODO(wjwwood): should the subscription object hang around?
node.create_subscription(String, 'chatter', chatter_callback, qos_profile_default)
spin_count = 0
while len(received_messages) == 0 and spin_count < 10 and launcher.is_launch_running():
rclpy.spin_once(node) # This will wait 1 second or until something has been done.
spin_count += 1
async_launcher.terminate()
async_launcher.join()
assert len(received_messages) > 0, "Should have received a message from talker"
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)
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 _run(self):
while not self._stop.is_set():
rclpy.spin_once(self._node, timeout_sec=self._time_between_spins)
