def test_create_float_array_parameter(self):
p = Parameter('myparam', Parameter.Type.DOUBLE_ARRAY, [2.41, 6.28])
self.assertEqual(p.value, [2.41, 6.28])
p = Parameter('myparam', value=[2.41, 6.28])
self.assertEqual(p.type_, Parameter.Type.DOUBLE_ARRAY)
self.assertEqual(p.value, [2.41, 6.28])
def test_node_set_undeclared_parameters_atomically(self):
with self.assertRaises(ParameterNotDeclaredException):
self.node.set_parameters_atomically([
Parameter('foo', Parameter.Type.INTEGER, 42),
Parameter('bar', Parameter.Type.STRING, 'hello'),
Parameter('baz', Parameter.Type.DOUBLE, 2.41)
])
def test_create_integer_array_parameter(self):
p = Parameter('myparam', Parameter.Type.INTEGER_ARRAY, [1, 2, 3])
self.assertEqual(p.value, [1, 2, 3])
p = Parameter('myparam', value=[1, 2, 3])
self.assertEqual(p.type_, Parameter.Type.INTEGER_ARRAY)
self.assertEqual(p.value, [1, 2, 3])
def test_declare_qos_parameters_with_overrides():
node = Node('my_node',
parameter_overrides=[
Parameter('qos_overrides./my_topic.publisher.depth',
value=100),
Parameter('qos_overrides./my_topic.publisher.reliability',
value='best_effort'),
])
for i in range(
2
): # try twice, the second time the parameters will be get and not declared
_declare_qos_parameters(Publisher, node, '/my_topic',
QoSProfile(depth=10),
QoSOverridingOptions.with_default_policies())
qos_overrides = node.get_parameters_by_prefix('qos_overrides')
assert len(qos_overrides) == 3
expected_params = (
('/my_topic.publisher.depth', 100),
('/my_topic.publisher.history', 'keep_last'),
('/my_topic.publisher.reliability', 'best_effort'),
)
for actual, expected in zip(
sorted(qos_overrides.items(), key=lambda x: x[0]),
expected_params):
assert actual[0] == expected[0] # same param name
assert actual[1].value == expected[1] # same param value
def test_create_boolean_parameter(self):
p = Parameter('myparam', Parameter.Type.BOOL, True)
self.assertEqual(p.name, 'myparam')
self.assertEqual(p.value, True)
p = Parameter('myparam', value=True)
self.assertEqual(p.name, 'myparam')
self.assertEqual(p.value, True)
def test_create_string_array_parameter(self):
p = Parameter('myparam', Parameter.Type.STRING_ARRAY,
['hello', 'world'])
self.assertEqual(p.value, ['hello', 'world'])
p = Parameter('myparam', value=['hello', 'world'])
self.assertEqual(p.type_, Parameter.Type.STRING_ARRAY)
self.assertEqual(p.value, ['hello', 'world'])
def test_create_boolean_array_parameter(self):
p = Parameter('myparam', Parameter.Type.BOOL_ARRAY,
[True, False, True])
self.assertEqual(p.value, [True, False, True])
p = Parameter('myparam', value=[True, False, True])
self.assertEqual(p.type_, Parameter.Type.BOOL_ARRAY)
self.assertEqual(p.value, [True, False, True])
def test_create_string_parameter(self):
p = Parameter('myparam', Parameter.Type.STRING, 'pvalue')
self.assertEqual(p.name, 'myparam')
self.assertEqual(p.value, 'pvalue')
p = Parameter('myparam', value='pvalue')
self.assertEqual(p.name, 'myparam')
self.assertEqual(p.type_, Parameter.Type.STRING)
self.assertEqual(p.value, 'pvalue')
def test_error_on_illegal_value_type(self):
with self.assertRaises(TypeError):
Parameter('illegaltype', 'mytype', 'myvalue')
with self.assertRaises(TypeError):
Parameter('illegaltype', value=('invalid', 'type'))
with self.assertRaises(TypeError):
Parameter('illegaltype', value={'invalid': 'type'})
def test_node_set_parameters_atomically(self):
result = self.node.set_parameters_atomically([
Parameter('foo', Parameter.Type.INTEGER, 42),
Parameter('bar', Parameter.Type.STRING, 'hello'),
Parameter('baz', Parameter.Type.DOUBLE, 2.41)
])
self.assertEqual(self.node.get_parameter('foo').value, 42)
self.assertIsInstance(result, SetParametersResult)
self.assertTrue(result.successful)
def test_double_array(self):
double_array = array('d', [1.0, 2.0, 3.0])
self.assertEqual(Parameter.Type.DOUBLE_ARRAY,
Parameter.Type.from_parameter_value(double_array))
# test that it doesn't raise
Parameter.from_parameter_msg(
ParameterMsg(name='double_array',
value=ParameterValue(
type=8, double_array_value=[1.0, 2.0, 3.0])))
def test_integer_array(self):
int_array = array('i', [1, 2, 3])
self.assertEqual(Parameter.Type.INTEGER_ARRAY,
Parameter.Type.from_parameter_value(int_array))
# test that it doesn't raise
Parameter.from_parameter_msg(
ParameterMsg(name='int_array',
value=ParameterValue(type=7,
integer_array_value=[1, 2, 3])))
def test_create_integer_parameter(self):
p = Parameter('myparam', Parameter.Type.INTEGER, 42)
self.assertEqual(p.name, 'myparam')
self.assertEqual(p.value, 42)
p = Parameter('myparam', value=42)
self.assertEqual(p.name, 'myparam')
self.assertEqual(p.type_, Parameter.Type.INTEGER)
self.assertEqual(p.value, 42)
def test_create_float_parameter(self):
p = Parameter('myparam', Parameter.Type.DOUBLE, 2.41)
self.assertEqual(p.name, 'myparam')
self.assertEqual(p.value, 2.41)
p = Parameter('myparam', value=2.41)
self.assertEqual(p.name, 'myparam')
self.assertEqual(p.type_, Parameter.Type.DOUBLE)
self.assertEqual(p.value, 2.41)
def test_create_bytes_parameter(self):
p = Parameter('myparam', Parameter.Type.BYTE_ARRAY,
[b'p', b'v', b'a', b'l', b'u', b'e'])
self.assertEqual(p.name, 'myparam')
self.assertEqual(p.value, [b'p', b'v', b'a', b'l', b'u', b'e'])
p = Parameter('myparam', value=[b'p', b'v', b'a', b'l', b'u', b'e'])
self.assertEqual(p.name, 'myparam')
self.assertEqual(p.type_, Parameter.Type.BYTE_ARRAY)
self.assertEqual(p.value, [b'p', b'v', b'a', b'l', b'u', b'e'])
def test_create_not_set_parameter(self):
p = Parameter('myparam', Parameter.Type.NOT_SET)
self.assertIsNone(p.value)
p = Parameter('myparam')
self.assertIsNone(p.value)
self.assertEqual(p.type_, Parameter.Type.NOT_SET)
p = Parameter('myparam', value=None)
self.assertIsNone(p.value)
self.assertEqual(p.type_, Parameter.Type.NOT_SET)
def test_node_set_parameters(self):
results = self.node.set_parameters([
Parameter('foo', Parameter.Type.INTEGER, 42),
Parameter('bar', Parameter.Type.STRING, 'hello'),
Parameter('baz', Parameter.Type.DOUBLE, 2.41)
])
self.assertTrue(all(isinstance(result, SetParametersResult) for result in results))
self.assertTrue(all(result.successful for result in results))
self.assertEqual(self.node.get_parameter('foo').value, 42)
self.assertEqual(self.node.get_parameter('bar').value, 'hello')
self.assertEqual(self.node.get_parameter('baz').value, 2.41)
def setUp(self):
self.context = rclpy.context.Context()
rclpy.init(context=self.context)
self.node = rclpy.create_node(
TEST_NODE,
namespace=TEST_NAMESPACE,
context=self.context,
initial_parameters=[
Parameter('initial_foo', Parameter.Type.INTEGER, 4321),
Parameter('initial_bar', Parameter.Type.STRING, 'init_param'),
Parameter('initial_baz', Parameter.Type.DOUBLE, 3.14)
],
automatically_declare_initial_parameters=False)
def _on_parameter_event(self, event):
if event.node != self._remote_node_name:
return
if self._param_change_callback is not None:
self._param_change_callback([
Parameter.from_parameter_msg(p) for p in event.new_parameters
], [
Parameter.from_parameter_msg(p)
for p in event.changed_parameters
], [
Parameter.from_parameter_msg(p)
for p in event.deleted_parameters
])
def __init__(self, args, *, node_name=None):
timeout_reached = False
def timer_callback():
nonlocal timeout_reached
timeout_reached = True
argv = getattr(args, 'argv', [])
rclpy.init(args=argv)
node_name_suffix = getattr(args, 'node_name_suffix',
'_%d' % os.getpid())
start_parameter_services = getattr(args, 'start_parameter_services',
False)
use_sim_time = getattr(args, 'use_sim_time', False)
if node_name is None:
node_name = NODE_NAME_PREFIX + node_name_suffix
self.node = rclpy.create_node(
node_name,
start_parameter_services=start_parameter_services,
parameter_overrides=[
Parameter('use_sim_time', value=use_sim_time)
])
timeout = getattr(args, 'spin_time', DEFAULT_TIMEOUT)
timer = self.node.create_timer(timeout, timer_callback)
while not timeout_reached:
rclpy.spin_once(self.node)
self.node.destroy_timer(timer)
def __init__(self, joint_names: Optional[List[str]] = None):
super().__init__(NODE_NAME,
automatically_declare_parameters_from_overrides=True)
self.get_parameter_clients = {}
# If the given joint names are not None, use that value for the node
# Otherwise get the value in the parameters of the node
# If that value is an empty list, query the joint_names from the
# robot_state_publisher node
if joint_names is not None:
self.joint_names = joint_names
else:
joint_names = (self.get_parameter(
"joint_names").get_parameter_value().string_array_value)
if len(joint_names) > 0:
self.joint_names = sorted(joint_names)
else:
self.joint_names = self.query_joint_names()
# Make sure that node parameter is up to date
if not self.has_parameter("joint_names"):
self.declare_parameter("joint_names")
self.set_parameters([Parameter(name="joint_names", value=joint_names)])
self.create_service(GetJointNames, "robot_information/get_joint_names",
self.get_joint_names_cb)
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 prop_cb(self, element, prop, ros_param_name, ros_param_type):
value = element.get_property(prop.name)
if value != self.node.get_parameter(ros_param_name).value:
self.node.get_logger().debug('element callback ' + ros_param_name +
' changed')
self.node.set_parameters(
[Parameter(ros_param_name, ros_param_type, value)])
def __init__(self, argv=sys.argv):
parser = argparse.ArgumentParser()
parser.add_argument('-cs',
'--clean_start',
required=True,
type=str,
help='Clean start waypoint')
parser.add_argument('-st',
'--start_time',
help='Start time from now in secs, default: 0',
type=int,
default=0)
parser.add_argument("--use_sim_time",
action="store_true",
help='Use sim time, default: false')
self.args = parser.parse_args(argv[1:])
self.node = rclpy.create_node('task_requester')
self.submit_task_srv = self.node.create_client(SubmitTask,
'/submit_task')
# enable ros sim time
if self.args.use_sim_time:
self.node.get_logger().info("Using Sim Time")
param = Parameter("use_sim_time", Parameter.Type.BOOL, True)
self.node.set_parameters([param])
def __init__(self):
super().__init__('simple_api_server')
api_req_qos_profile = QoSProfile(history=History.KEEP_LAST,
depth=1,
reliability=Reliability.RELIABLE,
durability=Durability.TRANSIENT_LOCAL)
self.task_api_req_pub = self.create_publisher(ApiRequest,
'/task_api_requests',
api_req_qos_profile)
self.get_building_map_srv = self.create_client(GetBuildingMap,
'/get_building_map')
# to show robot states
self.fleet_state_subscription = self.create_subscription(
FleetState,
'fleet_states',
self.fleet_state_cb,
qos_profile=QoSProfile(depth=20))
self.fleet_states_dict = {}
# TODO remove this
sim_time_bool = Parameter('use_sim_time', Parameter.Type.BOOL, True)
self.set_parameters([sim_time_bool])
self.task_states_cache = {}
def __init__(self):
super().__init__('test_node')
#self.assertTrue(self.has_parameter(USE_SIM_TIME_NAME))
#self.assertFalse(self.get_parameter(USE_SIM_TIME_NAME).value)
# Set `use_sim_time` parameter enabled
results = self.set_parameters([
#Parameter(USE_SIM_TIME_NAME, Parameter.Type.BOOL, False)
Parameter(USE_SIM_TIME_NAME, Parameter.Type.BOOL, True)
])
#self.assertTrue(all(isinstance(result, SetParametersResult) for result in results))
#self.assertTrue(all(result.successful for result in results))
#self.assertEqual(self.get_parameter(USE_SIM_TIME_NAME).value, True)
def timer_callback():
self.get_logger().info('Timer expired!')
start_time = self.get_clock().now()
is_time_out = False
while not is_time_out:
time_now = self.get_clock().now()
spent_time = time_now - start_time
self.get_logger().info('Current time: {}'.format(
time_now.to_msg()))
time.sleep(0.1)
is_time_out = True if spent_time.nanoseconds > (10**
9) else False
# Set timer to expire in 2 seconds
timer = self.create_timer(2.0, timer_callback)
def get_parameters(self, names):
get_params_request = GetParameters.Request()
get_params_request.names = names
get_params_response = self._get_params_client.call(get_params_request)
return [
Parameter.from_parameter_msg(ParameterMsg(name=name, value=value))
for name, value in zip(names, get_params_response.values)
]
def _set_parameters_atomically_callback(self, request, response):
try:
response.result = self._node.set_parameters_atomically(
[Parameter.from_parameter_msg(p) for p in request.parameters])
except ParameterNotDeclaredException as e:
response.result = SetParametersResult(successful=False,
reason=str(e))
return response
def execute(self, context: LaunchContext):
"""Execute the action."""
node = get_ros_node(context)
param = Parameter('use_sim_time', Parameter.Type.BOOL, self.value)
node.set_parameters([param])
if not node.get_parameter(
'use_sim_time').get_parameter_value().bool_value:
raise RuntimeError('Failed to set use_sim_time parameter')
def set_parameter_defaults(self, params):
# If a parameter has not been set externally, set the value to a default.
# Passed a list of "(parameterName, parameterType, defaultValue)" tuples.
parameters_to_set = []
for (pparam, ptype, pdefault) in params:
if not self.has_parameter(pparam):
parameters_to_set.append(Parameter(pparam, ptype, pdefault))
if len(parameters_to_set) > 0:
self.set_parameters(parameters_to_set)
