def __init__(self):
super().__init__('odrive_node')
self.declare_parameter('connection.timeout', 15, ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER, description='ODrive connection timeout in seconds'))
self.declare_parameter('battery.max_voltage', 4.2 * 6, ParameterDescriptor(
type=ParameterType.PARAMETER_DOUBLE, description='Max battery voltage'))
self.declare_parameter('battery.min_voltage', 3.2 * 6, ParameterDescriptor(
type=ParameterType.PARAMETER_DOUBLE, description='Min battery voltage'))
self.declare_parameter('battery.topic', 'barrery_percentage', ParameterDescriptor(
type=ParameterType.PARAMETER_STRING, description='Battery percentage publisher topic'))
self.declare_parameter('joint_state.topic', 'joint_state', ParameterDescriptor(
type=ParameterType.PARAMETER_STRING, description='Joint State publisher topic'))
self.connect_odrive_service = self.create_service(
Trigger,
'connect_odrive',
self.connect_odrive_callback
)
self.request_state_service = self.create_service(
AxisState,
'request_state',
self.request_state_callback
)
self.position_cmd_service = self.create_service(
PositionControl,
'position_cmd',
self.position_cmd_callback
)
self.velocity_cmd_service = self.create_service(
VelocityControl,
'velocity_cmd',
self.velocity_cmd_callback
)
self.battery_percentage_publisher_ = self.create_publisher(
Float32,
self.get_parameter(
'battery.topic').get_parameter_value().string_value,
1
)
self.battery_percentage_publisher_timer = self.create_timer(
10,
self.battery_percentage_publisher_callback
)
self.joint_state_publisher_ = self.create_publisher(
JointState,
self.get_parameter(
'joint_state.topic').get_parameter_value().string_value,
10
)
self.joint_state_publisher_timer = self.create_timer(
0.1,
self.joint_state_publisher_callback
)
self.driver: odrive.fibre.remote_object.RemoteObject = None
def __init__(self):
super().__init__('openpose_node')
# ros params
is_debug_mode_descriptor = ParameterDescriptor(
type=ParameterType.PARAMETER_BOOL,
description='If true, run debug mode.')
self.declare_parameter('is_debug_mode', False,
is_debug_mode_descriptor)
self.is_debug_mode: bool = self.get_parameter(
"is_debug_mode").get_parameter_value().bool_value
openpose_descriptor = ParameterDescriptor(
type=ParameterType.PARAMETER_STRING,
description='The path of openpose project root.')
self.declare_parameter('openpose_root', '/openpose',
openpose_descriptor)
openpose_root: str = self.get_parameter(
"openpose_root").get_parameter_value().string_value
is_image_compressed_descriptor = ParameterDescriptor(
type=ParameterType.PARAMETER_BOOL,
description='Is input image compressed?')
self.declare_parameter('is_image_compressed', False,
is_image_compressed_descriptor)
is_image_compressed: bool = self.get_parameter(
"is_image_compressed").get_parameter_value().bool_value
image_node_descriptor = ParameterDescriptor(
type=ParameterType.PARAMETER_STRING,
description='The node name of input image.')
self.declare_parameter('image_node', '/image', image_node_descriptor)
image_node: str = self.get_parameter(
"image_node").get_parameter_value().string_value
self.openpose_wrapper = OpenPoseWrapper(openpose_root)
self.bridge = CvBridge()
# show info
self.get_logger().info('IsDebugMode : ' + str(self.is_debug_mode))
self.get_logger().info('OpenposeRoot : ' + openpose_root)
self.get_logger().info('ImageNode : ' + image_node)
self.get_logger().info('IsImageCompressed : ' +
str(is_image_compressed))
if self.is_debug_mode:
self._publisher = self.create_publisher(Image, '/openpose/preview',
10)
self._publisher_compressed = self.create_publisher(
CompressedImage, '/openpose/preview/compressed', 10)
self._pose_publisher = self.create_publisher(
PoseKeyPointsList, '/openpose/pose_key_points', 10)
if is_image_compressed:
self.subscription = self.create_subscription(
CompressedImage, image_node, self.get_img_compressed_callback,
10)
else:
self.subscription = self.create_subscription(
Image, image_node, self.get_img_callback, 10)
def __init__(self, ns="/l2"):
super().__init__('l2_vr', namespace=ns)
self.get_logger().info("Init")
self.declare_parameter('extra_names', [], ParameterDescriptor())
self.declare_parameter('verbose', False, ParameterDescriptor())
T0 = Time(clock_type=self.get_clock().clock_type)
self.verbose = self.get_parameter('verbose').value
self.ignore_names = set(['ground_plane'])
self.extra_names = self.get_parameter('extra_names').value
if self.extra_names is None:
self.extra_names = set()
else:
self.extra_names = set(self.extra_names)
self.get_logger().info("Ignoring %s, appending %s" % (self.ignore_names, self.extra_names))
self.sim_state = Simulation() # TODO support multiple simulations
self.vr_object3d = Object3DArray()
self.last_sim_msg = T0
self.last_vr_msg = T0
self.last_status = None
# 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 = rclpy.callback_groups.ReentrantCallbackGroup()
self.executor = rclpy.executors.MultiThreadedExecutor()
self.future_deadlines = []
self.logtmr = self.create_timer(10.0, self.log_status, callback_group=cb_group)
self.resolvetmr = self.create_timer(10.0, self.resolve_diffs, callback_group=cb_group)
self.rosstatetmr = self.create_timer(10.0, self.publish_ros_state,
callback_group=cb_group)
self.getcli = self.create_client(GetObject3D, 'storage/get_object3d', callback_group=cb_group)
self.putfilecli = self.create_client(PutFile, 'storage/put_file', callback_group=cb_group)
self.spawncli = self.create_client(SpawnObject3D, 'vr/SpawnObject3D', callback_group=cb_group)
self.rmcli = self.create_client(RemoveObject3D, 'vr/RemoveObject3D', callback_group=cb_group)
self.seqcli = ActionClient(self, L2SequenceAction, 'sequence')
self.vr_missing_pub = self.create_publisher(Object3DArray, "vr/missing_object3d", 10)
self.vr_extra_pub = self.create_publisher(Object3DArray, "vr/extra_object3d", 10)
self.ros_state_pub = self.create_publisher(ROSState, "vr/ros_state", 10)
self.seq_state_pub = self.create_publisher(L2SequenceMsg,
"vr/SequenceUpdate", 10)
self.debug_pub = self.create_publisher(String, "debug_json", 10)
self.create_subscription(L2SequenceMsg, "vr/Sequence",
self.handle_start_sequence, qos_profile_sensor_data,
callback_group=cb_group)
self.create_subscription(Object3DArray, "vr/Object3D", self.set_vr_state, qos_profile_sensor_data, callback_group=cb_group)
self.create_subscription(Simulation, "sim/simulation", self.set_sim_state, qos_profile_sensor_data, callback_group=cb_group)
self.create_subscription(ProjectItem, "vr/PutProjectItem",
self.handle_put_project_item,
qos_profile_sensor_data, callback_group=cb_group)
def init_parameters(self):
self.declare_parameter('comment', 'No comments', ParameterDescriptor())
self.declare_parameter('blind', False, ParameterDescriptor())
self.declare_parameter('offset', 0, ParameterDescriptor())
self.declare_parameter('favorite/numbers_integers', [1, 2],
ParameterDescriptor())
self.declare_parameter('favorite/numbers_doubles', [1.0, 2.0],
ParameterDescriptor())
self.comment = self.get_parameter('comment').value
self.blind = self.get_parameter('blind').value
self.offset = self.get_parameter('offset').value
def _describe_parameters_callback(self, request, response):
for name in request.names:
try:
descriptor = self._node.describe_parameter(name)
except ParameterNotDeclaredException:
descriptor = ParameterDescriptor()
response.descriptors.append(descriptor)
return response
def _declare_qos_parameters(entity_type: Union[Type[Publisher],
Type[Subscription]],
node: 'Node', topic_name: Text, qos: QoSProfile,
options: QoSOverridingOptions) -> QoSProfile:
"""
Declare QoS parameters for a Publisher or a Subscription.
:param entity_type: Either `rclpy.node.Publisher` or `rclpy.node.Subscription`.
:param node: Node used to declare the parameters.
:param topic_name: Topic name of the entity being created.
:param qos: Default QoS settings of the entity being created, that will be overridden
with the user provided QoS parameter overrides.
:param options: Options that indicates which parameters are going to be declared.
"""
if not issubclass(entity_type, (Publisher, Subscription)):
raise TypeError(
'Argument `entity_type` should be a subclass of Publisher or Subscription'
)
entity_type_str = 'publisher' if issubclass(entity_type,
Publisher) else 'subscription'
id_suffix = '' if options.entity_id is None else f'_{options.entity_id}'
name = f'qos_overrides.{topic_name}.{entity_type_str}{id_suffix}.' '{}'
description = '{}' f' for {entity_type_str} `{topic_name}` with id `{options.entity_id}`'
allowed_policies = _get_allowed_policies(entity_type)
for policy in options.policy_kinds:
if policy not in allowed_policies:
continue
policy_name = policy.name.lower()
descriptor = ParameterDescriptor()
descriptor.description = description.format(policy_name)
descriptor.read_only = True
try:
param = node.declare_parameter(
name.format(policy_name),
_get_qos_policy_parameter(qos, policy), descriptor)
except ParameterAlreadyDeclaredException:
param = node.get_parameter(name.format(policy_name))
_override_qos_policy_with_param(qos, policy, param)
if options.callback is not None:
result = options.callback(qos)
if not result.successful:
raise InvalidQosOverridesError(
f"{description.format('Provided QoS overrides')}, are not valid: {result.reason}"
)
def __init__(self):
super().__init__("listener")
parameter = self.declare_parameter("exit_after_receive", False,
ParameterDescriptor())
self._exit_after_receive = parameter.value
self.should_exit = False
qos_profile = QoSProfile(depth=1)
self._subscription = self.create_subscription(String, "chatter",
self._callback,
qos_profile)
def __init__(self):
super().__init__('infant_ear')
self.reset = False
pd_read_only = ParameterDescriptor(read_only=True)
self.chunk, self.alert, self.delta_vol_lo, self.delta_vol_hi, self.discomfort_rate, self.alert_rate, self.alert_decay = self.declare_parameters(
'', [('chunk_size', 0, pd_read_only), ('alert', .0),
('delta_vol_lo', 1.6), ('delta_vol_hi', 2.5),
('discomfort_rate', 60), ('alert_rate', 30),
('alert_decay', 3)])
self.pub_alert = self.create_publisher(Float64, 'ear_alert', 1)
self.pub_comfort = self.create_publisher(Float64, 'change_comfort', 10)
self.sub_reset = self.create_subscription(Empty, 'reset',
self.reset_callback, 1)
def __init__(self):
super().__init__('infant_eye')
self.reset = False
pd_read_only = ParameterDescriptor(read_only=True)
self.cam_id, self.tracker_half_life, self.tracker_timeout, self.base_alert = self.declare_parameters(
'', [('cam_id', 4, pd_read_only),
('tracker_half_life', 30, pd_read_only),
('tracker_timeout', 3, pd_read_only), ('base_alert', 300)])
self.pub_alert = self.create_publisher(Float64, 'eye_alert', 1)
self.pub_focus = self.create_publisher(Float64, 'eye_focus', 1)
self.sub_reset = self.create_subscription(Empty, 'reset',
self.reset_callback, 1)
def __init__(self, ls):
name = 'l2_sim_worker_'+uuid.uuid4().hex
super().__init__(name)
self.ls = ls
self.launch_ready = False
self.declare_parameter('report_path', '', ParameterDescriptor())
self.declare_parameter('object_config', '', ParameterDescriptor())
self.get_logger().info("Init")
self.sim_msg = Simulation(
ns=self.get_namespace(),
report_path=self.get_parameter('report_path').value,
worker_id=name,
object=Object3D(name=self.get_parameter('object_config').value))
self.sim_pub = self.create_publisher(Simulation, 'simulation', 10)
self.sim_pub_timer = self.create_timer(self.PUBLISH_PD, self.publish_sim)
self.getcli = self.create_client(GetObject3D, '/l2/storage/get_object3d')
self.get_logger().info("Waiting for storage")
self.getcli.wait_for_service(timeout_sec=5)
if not self.getcli.service_is_ready():
raise SimError("Timed out waiting for storage")
self.get_logger().info("GetObject3D %s" % self.sim_msg.object.name)
future = self.getcli.call_async(GetObject3D.Request(name=self.sim_msg.object.name))
future.add_done_callback(self.handle_response)
def __init__(self, comment, qos_profile):
super().__init__('counter')
self.get_logger().debug('Test debug message')
self.i = 0
self.comment = comment
self.declare_parameter('comment', 'Hello', ParameterDescriptor())
qos = self.get_qos(qos_profile)
self.pub = self.create_publisher(Count, 'count', qos)
self.timer = self.create_timer(1.0, self.timer_callback)
def __init__(self, node: Node):
self.node = node
self.list_update_frames = node.declare_parameter(
'list_update_frames', 10,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description=
'Update list every this frames. So update rate of the list is 20 (asciimatics frame rate) / list_update_frames.',
integer_range=[
IntegerRange(from_value=1, to_value=20, step=1)
])).value
self.selected_action = None
self.selected_node = None
self.selected_topic = None
def describe_parameter(self, name: str) -> ParameterDescriptor:
"""
Get the parameter descriptor of a given parameter.
:param name: Fully-qualified name of the parameter, including its namespace.
:return: ParameterDescriptor corresponding to the parameter,
or default ParameterDescriptor if parameter had not been declared before
and undeclared parameters are allowed.
:raises: ParameterNotDeclaredException if parameter had not been declared before
and undeclared parameters are not allowed.
"""
try:
return self._descriptors[name]
except KeyError:
if self._allow_undeclared_parameters:
return ParameterDescriptor()
else:
raise ParameterNotDeclaredException(name)
def __init__(self, h, w, l=None):
super().__init__('infant_visualizer')
self.reset = False
pd_read_only = ParameterDescriptor(read_only=True)
self.half_life, self.delta, self.period = self.declare_parameters(
'', [('half_life', 0.4, pd_read_only), ('delta', .8, pd_read_only),
('period', 3)])
self.figs = {}
resource_dir = os.path.join(get_package_share_directory('infant'),
'resource/')
for stance in ['low', 'normal', 'high']:
self.figs[stance] = {}
self.figs[stance]['plain'] = cv2.imread(
os.path.join(resource_dir, f'{stance}_plain.jpg'))
for style in ['rot', 'sat', 'unsat']:
if style == 'rot' and stance != 'low':
continue
self.figs[stance][style] = [
cv2.imread(
os.path.join(resource_dir,
f'{stance}_{style}/{i}.jpg'))
for i in range(1, 101)
]
self.alpha = np.log(2) / self.half_life.value
self.h, self.w = h, w
self.l = min(h, w) if l is None else l
self.sub_state = self.create_subscription(InfantState, 'state',
self.state_callback, 1)
self.sub_focus = self.create_subscription(Float64, 'eye_focus',
self.focus_callback, 1)
self.sub_pressure = self.create_subscription(Float64, 'touch_pressure',
self.pressure_callback, 1)
self.sub_reset = self.create_subscription(Empty, 'reset',
self.reset_callback, 1)
self.alert = None
self.comfort = None
self.stage = None
self.num = None
self.recovery = None
self.tgt_focus = None
self.tgt_pressure = None
def declare_parameter(
self,
name: str,
value: ParameterValue = ParameterValue(),
descriptor: ParameterDescriptor = ParameterDescriptor()
) -> Parameter:
"""
Declare and initialize a parameter.
This method, if successful, will result in any callback registered with
:func:`set_parameters_callback` to be called.
:param name: Fully-qualified name of the parameter, including its namespace.
:param value: Value of the parameter to declare.
:param descriptor: Descriptor for the parameter to declare.
:return: Parameter with the effectively assigned value.
:raises: ParameterAlreadyDeclaredException if the parameter had already been declared.
:raises: InvalidParameterException if the parameter name is invalid.
:raises: InvalidParameterValueException if the registered callback rejects the parameter.
"""
return self.declare_parameters('', [(name, value, descriptor)])[0]
def get_parameter(self, name: str) -> Parameter:
"""
Get a parameter by name.
:param name: Fully-qualified name of the parameter, including its namespace.
:return: The values for the given parameter names.
A default Parameter will be returned for an undeclared parameter if
undeclared parameters are allowed.
:raises: ParameterNotDeclaredException if undeclared parameters are not allowed,
and the parameter hadn't been declared beforehand.
"""
if self.has_parameter(name):
return self._parameters[name]
elif self._allow_undeclared_parameters:
# If undeclared parameters are allowed, the parameter might be in the initial set.
# If that's the case, first set and then return.
if name in self._initial_parameters:
self._parameters.update({name: self._initial_parameters[name]})
self._descriptors.update({name: ParameterDescriptor()})
return self._parameters[name]
return Parameter(name, Parameter.Type.NOT_SET, None)
else:
raise ParameterNotDeclaredException(name)
def thread_function(self, name):
device = None
if self.debug_mode:
print('Cmd file: ', self.cmd_file, ' args["device_id"]: ',
self.args['device_id'])
device = depthai.Device(self.cmd_file, self.args['device_id'])
else:
device = depthai.Device(self.args['device_id'], self.usb2_mode)
print('Available streams: ' + str(device.get_available_streams()))
# create the pipeline, here is the first connection with the device
self.pipeline = device.create_pipeline(config=self.config)
if self.pipeline is None:
print('Pipeline is not created.')
exit(3)
# add nn2depth to a parameter so clients can get at it to decode depth?
# setup a param for sharing the depth mapping for this particular device.
nn2depth = device.get_nn_to_depth_bbox_mapping()
nn2depthStr = json.dumps(nn2depth)
self.nn2depthParamName = "nn2depth" + self.targetDev
paramDefault = ""
paramDesc = ParameterDescriptor(type=ParameterType.PARAMETER_STRING,
description="Used ")
self.declare_parameter(self.nn2depthParamName, nn2depthStr, paramDesc)
t_start = time()
frame_count = {}
frame_count_prev = {}
nnet_prev = {}
nnet_prev["entries_prev"] = {}
nnet_prev["nnet_source"] = {}
frame_count['nn'] = {}
frame_count_prev['nn'] = {}
NN_cams = {'rgb', 'left', 'right'}
for cam in NN_cams:
nnet_prev["entries_prev"][cam] = []
nnet_prev["nnet_source"][cam] = []
frame_count['nn'][cam] = 0
frame_count_prev['nn'][cam] = 0
#-------------------------------------------------------
stream_windows = []
for s in self.stream_names:
if s == 'previewout':
for cam in NN_cams:
stream_windows.append(s + '-' + cam)
else:
stream_windows.append(s)
for w in stream_windows:
frame_count[w] = 0
frame_count_prev[w] = 0
tracklets = None
#-------------------------------------------------------
# start watchdog
reset_process_wd()
#-------------------------------------------------------
while True:
# retreive data from the device
# data is stored in packets, there are nnet (Neural NETwork) packets which have additional functions for NNet result interpretation
nnet_packets, data_packets = self.pipeline.get_available_nnet_and_data_packets(
True)
#-------------------------------------------------------
# TODO: should move the watchdog stuff to a shared class
#-------------------------------------------------------
packets_len = len(nnet_packets) + len(data_packets)
if packets_len != 0:
reset_process_wd()
else:
cur_time = monotonic()
if cur_time > wd_cutoff:
print("process watchdog timeout")
os._exit(10)
#-------------------------------------------------------
for _, nnet_packet in enumerate(nnet_packets):
meta = nnet_packet.getMetadata()
camera = 'rgb'
if meta != None:
camera = meta.getCameraName()
serializedEntry = self.decode_nn(nnet_packet,
config=self.config,
NN_json=self.NN_json)
self.nnmsg.data = str(serializedEntry)
self.nnResultPublisher.publish(self.nnmsg)
frame_count['metaout'] += 1
frame_count['nn'][camera] += 1
for packet in data_packets:
window_name = packet.stream_name
if packet.stream_name not in self.stream_names:
continue # skip streams that were automatically added
packetData = packet.getData()
if packetData is None:
print('Invalid packet data!')
continue
elif packet.stream_name == 'previewout':
#broadcast to previewout
meta = packet.getMetadata()
camera = 'rgb'
if meta != None:
camera = meta.getCameraName()
window_name = 'previewout-' + camera
# the format of previewout image is CHW (Chanel, Height, Width), but OpenCV needs HWC, so we
# change shape (3, 300, 300) -> (300, 300, 3)
data0 = packetData[0, :, :]
data1 = packetData[1, :, :]
data2 = packetData[2, :, :]
frame = cv2.merge([data0, data1, data2])
self.publishFrame(frame, self.previewPublisher,
self.previewmsg)
elif packet.stream_name == 'left':
frame_bgr = packetData
self.publishFrame(frame_bgr, self.leftPublisher,
self.leftmsg)
elif packet.stream_name == 'right':
frame_bgr = packetData
self.publishFrame(frame_bgr, self.rightPublisher,
self.rightmsg)
elif packet.stream_name == 'disparity':
frame_bgr = packetData
self.publishFrame(frame_bgr, self.disparityPublisher,
self.disparitymsg)
elif packet.stream_name.startswith('depth'):
frame = packetData
self.publishFrame(frame, self.depthPublisher,
self.depthmsg)
# TODO: maybe just publish the filepath?
elif packet.stream_name == 'jpegout':
jpg = packetData
mat = cv2.imdecode(jpg, cv2.IMREAD_COLOR)
cv2.imshow('jpegout', mat)
elif packet.stream_name == 'video':
videoFrame = packetData
videoFrame.tofile(video_file)
#mjpeg = packetData
#mat = cv2.imdecode(mjpeg, cv2.IMREAD_COLOR)
#cv2.imshow('mjpeg', mat)
elif packet.stream_name == 'meta_d2h':
str_ = packet.getDataAsStr()
self.d2hmsg.data = str_
self.d2hPublisher.publish(self.d2hmsg)
elif packet.stream_name == 'object_tracker':
tracklets = packet.getObjectTracker()
frame_count[window_name] += 1
key = cv2.waitKey(1)
def add(self,
param_name,
param_type=None,
default=None,
description=None,
min=None,
max=None,
step=None):
describtor = ParameterDescriptor()
describtor.name = param_name
if description is None:
describtor.description = param_name
else:
describtor.description = description
if param_type is None and default is not None:
param_type = type(default)
py2ros_param_type = {
None: ParameterType.PARAMETER_NOT_SET,
bool: ParameterType.PARAMETER_BOOL,
int: ParameterType.PARAMETER_INTEGER,
float: ParameterType.PARAMETER_DOUBLE,
str: ParameterType.PARAMETER_STRING
}
param_type = py2ros_param_type.get(param_type, param_type)
describtor.type = param_type
if param_type == ParameterType.PARAMETER_INTEGER:
if step is None:
step = 1
if all(x is not None or isinstance(x, int)
for x in [min, max, step]):
param_range = IntegerRange()
param_range.from_value = min
param_range.to_value = max
param_range.step = step
describtor.integer_range = [param_range]
if param_type == ParameterType.PARAMETER_DOUBLE:
if step is None:
step = 0.01
if all(x is not None for x in [min, max]):
param_range = FloatingPointRange()
param_range.from_value = float(min)
param_range.to_value = float(max)
param_range.step = float(step)
describtor.floating_point_range = [param_range]
type2default_default = {
ParameterType.PARAMETER_NOT_SET: 0,
ParameterType.PARAMETER_BOOL: False,
ParameterType.PARAMETER_INTEGER: 0,
ParameterType.PARAMETER_DOUBLE: 0.0,
ParameterType.PARAMETER_STRING: ""
}
if default is None:
default = type2default_default[param_type]
self.param_cache.append((param_name, default, describtor))
def _declare_and_fill_map(self, key, default_value, description, map):
param = self.declare_parameter(
key, default_value, ParameterDescriptor(description=description))
map[key] = param.value
def __init__(self):
super().__init__('depthai_publisher')
# setup a cli param for passing in command line arguments.
self.cliParamName = "cliArgs"
paramDefault = ""
paramDesc = ParameterDescriptor(
type=ParameterType.PARAMETER_STRING,
description=
"arguments that match the command line script depthai-demo.py input"
)
self.declare_parameter(self.cliParamName, paramDefault, paramDesc)
# get cli param
my_param = self.get_parameter(
self.cliParamName).get_parameter_value().string_value
new_argv = [sys.argv[0]]
if my_param is not "":
paramList = my_param.split(" ")
for param in paramList:
new_argv.append(param)
# setup argv for parsing.
self.old_argv = sys.argv
sys.argv = new_argv
# parse params
cliArgs = CliArgs()
self.args = vars(cliArgs.parse_args())
print(self.args)
configMan = DepthConfigManager(self.args)
self.cmd_file, self.debug_mode = configMan.getCommandFile()
self.usb2_mode = configMan.getUsb2Mode()
self.decode_nn = configMan.decode_nn
self.decode_nn_json = configMan.decode_nn_json
self.show_nn = configMan.show_nn
self.labels = configMan.labels
self.NN_json = configMan.NN_config
# This json file is sent to DepthAI. It communicates what options you'd like to enable and what model you'd like to run.
self.config = configMan.jsonConfig
self.targetDev = self.args['device_id'].replace(".", "_")
# setup publishers
self.previewPublisher = self.create_publisher(
UInt8MultiArray, 'preview' + self.targetDev, 10)
self.previewmsg = UInt8MultiArray()
self.leftPublisher = self.create_publisher(UInt8MultiArray,
'left' + self.targetDev, 10)
self.leftmsg = UInt8MultiArray()
self.rightPublisher = self.create_publisher(UInt8MultiArray,
'right' + self.targetDev,
10)
self.rightmsg = UInt8MultiArray()
self.disparityPublisher = self.create_publisher(
UInt8MultiArray, 'disparity' + self.targetDev, 10)
self.disparitymsg = UInt8MultiArray()
self.depthPublisher = self.create_publisher(UInt8MultiArray,
'depth' + self.targetDev,
10)
self.depthmsg = UInt8MultiArray()
self.d2hPublisher = self.create_publisher(String,
'd2h' + self.targetDev, 10)
self.d2hmsg = String()
self.nnResultPublisher = self.create_publisher(String,
'meta' + self.targetDev,
10)
self.nnmsg = String()
# Create a list of enabled streams ()
self.stream_names = [
stream if isinstance(stream, str) else stream['name']
for stream in configMan.stream_list
]
self.enable_object_tracker = 'object_tracker' in self.stream_names
def __init__(self):
# note the topic name will change if a device is specified. the name will be <stream>+<device_id>. eg preview3.
super().__init__('depthai_subscriber')
# setup our params
self.paramName = "cliArgs"
paramDefault = ""
paramDesc = ParameterDescriptor(
type=ParameterType.PARAMETER_STRING,
description=
"arguments that match the command line script depthai-demo.py input"
)
self.declare_parameter(self.paramName, paramDefault, paramDesc)
# get param
my_param = self.get_parameter(
self.paramName).get_parameter_value().string_value
new_argv = [sys.argv[0]]
if my_param is not "":
paramList = my_param.split(" ")
for param in paramList:
new_argv.append(param)
# setup argv for parsing.
self.old_argv = sys.argv
sys.argv = new_argv
# parse params
cliArgs = CliArgs()
self.args = vars(cliArgs.parse_args())
print(self.args)
self.configMan = DepthConfigManager(self.args)
# "." is not allowed in topic names so we replaced "." with "_" on the server.
self.targetDev = self.args['device_id'].replace(".", "_")
# get nn2depth params, if necessary.
if self.args['draw_bb_depth']:
self.nn2depth = self.get_nn2depth_sync(self.targetDev)
print(self.nn2depth)
# subscribe to topics
self.previewSubName = 'preview' + self.targetDev
self.previewSub = self.create_subscription(UInt8MultiArray,
self.previewSubName,
self.preview_callback, 10)
self.previewSub # prevent unused variable warning
self.leftSubName = 'left' + self.targetDev
self.leftSub = self.create_subscription(UInt8MultiArray,
self.leftSubName,
self.left_callback, 10)
self.leftSub # prevent unused variable warning
self.rightSubName = 'right' + self.targetDev
self.rightSub = self.create_subscription(UInt8MultiArray,
self.rightSubName,
self.right_callback, 10)
self.rightSub # prevent unused variable warning
self.disparitySubName = 'disparity' + self.targetDev
self.disparitySub = self.create_subscription(UInt8MultiArray,
self.disparitySubName,
self.disparity_callback,
10)
self.disparitySub # prevent unused variable warning
self.depthSubName = 'depth' + self.targetDev
self.depthSub = self.create_subscription(UInt8MultiArray,
self.depthSubName,
self.depth_callback, 10)
self.depthSub # prevent unused variable warning
self.d2hSubName = 'd2h' + self.targetDev
self.d2hSub = self.create_subscription(String, self.d2hSubName,
self.d2h_callback, 10)
self.depthSub # prevent unused variable warning
self.metaSubName = 'meta' + self.targetDev
self.metaSub = self.create_subscription(String, self.metaSubName,
self.meta_callback, 10)
self.metaSub # prevent unused variable warning
def setup_parameters(self):
# simplified parameters declaration
# all parameters are of type ParameterType.PARAMETER_DOUBLE
# and FloatingPointRange step is 0.0 (means no discrete step)
# [name, description, default_value, from_value, to_value]
params_simplified: List[Tuple[str, str, Any, Any, Any]] = [
('acceleration', 'Acceleration', 0.2, 0.01, 1.0),
('decceleration', 'Decceleration', 0.2, 0.01, 1.0),
# PWM duty for speed (lesser number ~ faster)
# current meaning: AVOID is used during sharp turns,
# MIN is used during other turns,
# MAX elsewhere
# self.VP_SPEED_MIN = 0.138522954 # ~ 9300
# self.VP_SPEED_MAX = 0.158483034 # ~ 9350
# self.VP_SPEED_AVOID = 0.098602794 # ~ 9200
# note: speed_min (Level-2 speed) and speed_max (Level-1 speed)
# descriptions are NOT swapped. The naming has a historical reason.
('speed_min', 'Level-2 speed', 0.05, 0.0, 0.3),
('speed_max', 'Level-1 speed', 0.05, 0.0, 0.3),
('speed_avoid', 'Level-3 speed', 0.05, 0.0, 0.3),
# PWM duty for turning
# TODO: Why the values in VP_* are NEGATIVE but parameters are POSITIVE?
# # left
# self.VP_TURN_MIN_L = -0.677419355 # ~ -2100
# self.VP_TURN_MAX_L = -1 # ~ -3100
# self.VP_TURN_AVOID_L = -1.419354839 # ~ -4400
('turn_l_min', 'Level-1 turn ratio (left)', 0.677419355, 0.0, 2.0),
('turn_l_max', 'Level-2 turn ratio (left)', 1.000000000, 0.0, 2.0),
('turn_l_avoid', 'Level-3 turn ratio (left)', 1.419354839, 0.0,
2.0),
# # right
# self.VP_TURN_MIN_R = -0.677419355 # ~ -2100
# self.VP_TURN_MAX_R = -0.709677419 # ~ -2200
# self.VP_TURN_AVOID_R = -1.290322581 # ~ -4000
('turn_r_min', 'Level-1 turn ratio (right)', 0.677419355, 0.0, 2.0
),
('turn_r_max', 'Level-2 turn ratio (right)', 0.709677419, 0.0,
2.0),
('turn_r_avoid', 'Level-3 turn ratio (right)', 1.290322581, 0.0,
2.0),
# Angle to switch between modes
# self.ANGLE_SWITCH = math.radians(20.0) # 20 degrees
# self.ANGLE_HYSTER = math.radians(5) # 5 degrees
('switch_l12', 'Level-1 to Level-2 angle switch', 20.0, 0.0, 90.0),
('hysteresis_l12', 'Level-1 to Level-2 angle hysteresis', 5.0, 0.0,
20.0),
# self.ANGLE_SWITCH_A = math.radians(41.0) # 41 degrees
# self.ANGLE_HYSTER_A = math.radians(5.0) # 5 degrees
('switch_l23', 'Level-2 to Level-3 angle switch', 41.0, 0.0, 90.0),
('hysteresis_l23', 'Level-2 to Level-3 angle hysteresis', 5.0, 0.0,
20.0),
]
params_def: List[Tuple[str, Any, ParameterDescriptor]] = \
[
(
name,
value,
ParameterDescriptor(
type=ParameterType.PARAMETER_DOUBLE,
description=desc,
floating_point_range=[FloatingPointRange(
from_value=from_value,
to_value=to_value,
step=0.0,
)],
)
)
for name, desc, value, from_value, to_value in params_simplified
]
self.declare_parameters(
namespace='',
parameters=params_def,
)
pass
def __init__(
self,
node_name: str,
*,
context: Context = None,
cli_args: List[str] = None,
namespace: str = None,
use_global_arguments: bool = True,
start_parameter_services: bool = True,
initial_parameters: List[Parameter] = None,
allow_undeclared_parameters: bool = False,
automatically_declare_initial_parameters: bool = True) -> None:
"""
Constructor.
:param node_name: A name to give to this node. Validated by :func:`validate_node_name`.
:param context: The context to be associated with, or ``None`` for the default global
context.
:param cli_args: A list of strings of command line args to be used only by this node.
:param namespace: The namespace to which relative topic and service names will be prefixed.
Validated by :func:`validate_namespace`.
:param use_global_arguments: ``False`` if the node should ignore process-wide command line
args.
:param start_parameter_services: ``False`` if the node should not create parameter
services.
:param initial_parameters: A list of parameters to be set during node creation.
:param allow_undeclared_parameters: True if undeclared parameters are allowed.
This flag affects the behavior of parameter-related operations.
:param automatically_declare_initial_parameters: True if initial parameters have to be
declared upon node creation, false otherwise.
"""
self.__handle = None
self._context = get_default_context() if context is None else context
self._parameters: dict = {}
self.__publishers: List[Publisher] = []
self.__subscriptions: List[Subscription] = []
self.__clients: List[Client] = []
self.__services: List[Service] = []
self.__timers: List[WallTimer] = []
self.__guards: List[GuardCondition] = []
self.__waitables: List[Waitable] = []
self._default_callback_group = MutuallyExclusiveCallbackGroup()
self._parameters_callback = None
self._allow_undeclared_parameters = allow_undeclared_parameters
self._initial_parameters = {}
self._descriptors = {}
namespace = namespace or ''
if not self._context.ok():
raise NotInitializedException('cannot create node')
try:
self.__handle = Handle(
_rclpy.rclpy_create_node(node_name, namespace,
self._context.handle, cli_args,
use_global_arguments))
except ValueError:
# these will raise more specific errors if the name or namespace is bad
validate_node_name(node_name)
# emulate what rcl_node_init() does to accept '' and relative namespaces
if not namespace:
namespace = '/'
if not namespace.startswith('/'):
namespace = '/' + namespace
validate_namespace(namespace)
# Should not get to this point
raise RuntimeError('rclpy_create_node failed for unknown reason')
with self.handle as capsule:
self._logger = get_logger(
_rclpy.rclpy_get_node_logger_name(capsule))
# Clock that has support for ROS time.
self._clock = ROSClock()
self._time_source = TimeSource(node=self)
self._time_source.attach_clock(self._clock)
self.__executor_weakref = None
self._parameter_event_publisher = self.create_publisher(
ParameterEvent,
'parameter_events',
qos_profile=qos_profile_parameter_events)
with self.handle as capsule:
self._initial_parameters = _rclpy.rclpy_get_node_parameters(
Parameter, capsule)
# Combine parameters from params files with those from the node constructor and
# use the set_parameters_atomically API so a parameter event is published.
if initial_parameters is not None:
self._initial_parameters.update(
{p.name: p
for p in initial_parameters})
if automatically_declare_initial_parameters:
self._parameters.update(self._initial_parameters)
self._descriptors.update(
{p: ParameterDescriptor()
for p in self._parameters})
if start_parameter_services:
self._parameter_service = ParameterService(self)
def setup_parameters(self):
# TODO: uninitialized values for statically typed params do not cause failure?
# (name, value, descriptor)
params_def: List[Tuple[str, Any, ParameterDescriptor]] = [
('drive_battery.cells', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description=
'number of cells of the drive battery (the one for the motor and the servo)',
integer_range=[
IntegerRange(
from_value=1,
to_value=5,
step=1,
)
],
)),
('drive_battery.cell_voltage', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_DOUBLE,
description='voltage of a cell of the battery [V]',
floating_point_range=[
FloatingPointRange(
from_value=1.0,
to_value=10.0,
step=0.1,
)
],
)),
# (
# # TODO
# 'motor.to_erpm',
# None,
# ParameterDescriptor(
# type=ParameterType.PARAMETER_DOUBLE,
# description='TODO',
# )
# ),
# (
# # TODO
# 'motor.erpm_max',
# None,
# ParameterDescriptor(
# type=ParameterType.PARAMETER_DOUBLE,
# # TODO: better description, correct unit
# description='TODO',
# )
# ),
(
'motor.back_emf',
None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
# TODO: better description, correct unit
description='constant velocity of the motor in [Kv]',
)),
('motor.poles', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description='number of poles in the motor',
)),
('motor.pinion', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description='number of teeth on the gear of the motor',
)),
('differential.spur', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description=
'number of teeth on the gear connected with motor gear',
)),
('differential.pinion', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description='number of teeth on the gear before the wheels',
)),
('differential.ring', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description=
'number of teeth on the wheel gear connected with differential gear',
)),
('wheels.radius', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_DOUBLE,
description='radius of the wheels [m]',
)),
('pwm.throttle.calm_value', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description='TODO',
)),
('pwm.throttle.forward.min', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description='TODO',
)),
('pwm.throttle.forward.max', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description='TODO',
)),
('pwm.throttle.backward.min', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description='TODO',
)),
('pwm.throttle.backward.max', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description='TODO',
)),
('pwm.steering.calm_value', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description='TODO',
)),
('pwm.steering.left.min', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description='TODO',
)),
('pwm.steering.left.max', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description='TODO',
)),
('pwm.steering.right.min', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description='TODO',
)),
('pwm.steering.right.max', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description='TODO',
)),
('angular_steering.left_max', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_DOUBLE,
description='in degrees',
floating_point_range=[
FloatingPointRange(
from_value=0.0,
to_value=90.0,
step=0.0,
)
],
)),
('angular_steering.right_max', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_DOUBLE,
description='in degrees',
floating_point_range=[
FloatingPointRange(
from_value=0.0,
to_value=90.0,
step=0.0,
)
],
)),
('simulation.throttle_modifier', 1.0,
ParameterDescriptor(
type=ParameterType.PARAMETER_DOUBLE,
description='Throttle modifier for simulation mode.',
)),
('simulation.steering_modifier', 1.0,
ParameterDescriptor(
type=ParameterType.PARAMETER_DOUBLE,
description='Steering modifier for simulation mode.',
)),
('run_mode', None,
ParameterDescriptor(
type=ParameterType.PARAMETER_STRING,
read_only=True,
description='Drive API mode',
additional_constraints=
'allowed values are basic, basic_vesc, simulation',
)),
('publish_rate', 10,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
read_only=True,
description='Drive API publish rate [Hz]',
)),
]
self.declare_parameters(
namespace='',
parameters=params_def,
)
pass
def declare_parameters(
self, namespace: str,
parameters: List[Tuple[str, Optional[ParameterValue],
Optional[ParameterDescriptor]]]
) -> List[Parameter]:
"""
Declare a list of parameters.
This method, if successful, will result in any callback registered with
:func:`set_parameters_callback` to be called once for each parameter.
If one of those calls fail, an exception will be raised and the remaining parameters will
not be declared.
Parameters declared up to that point will not be undeclared.
:param namespace: Namespace for parameters.
:param parameters: Tuple with parameters to declare, with a name, value and descriptor.
:return: Parameter list with the effectively assigned values for each of them.
:raises: ParameterAlreadyDeclaredException if the parameter had already been declared.
:raises: InvalidParameterException if the parameter name is invalid.
:raises: InvalidParameterValueException if the registered callback rejects any parameter.
"""
parameter_list = []
descriptor_list = []
for parameter_tuple in parameters:
name = parameter_tuple[0]
assert isinstance(name, str)
# Get value from initial parameters, of from tuple if it doesn't exist.
if name in self._initial_parameters:
value = self._initial_parameters[name].get_parameter_value()
elif parameter_tuple[1] is None:
value = ParameterValue()
else:
value = parameter_tuple[1]
assert isinstance(value, ParameterValue)
descriptor = parameter_tuple[2]
if descriptor is None:
descriptor = ParameterDescriptor()
assert isinstance(descriptor, ParameterDescriptor)
# Note(jubeira): declare_parameters verifies the name, but set_parameters doesn't.
full_name = namespace + name
validate_parameter_name(full_name)
parameter_list.append(
Parameter.from_parameter_msg(
ParameterMsg(name=full_name, value=value)))
descriptor_list.append(descriptor)
parameters_already_declared = [
parameter.name for parameter in parameter_list
if parameter.name in self._parameters
]
if any(parameters_already_declared):
raise ParameterAlreadyDeclaredException(
parameters_already_declared)
# Call the callback once for each of the parameters, using method that doesn't
# check whether the parameter was declared beforehand or not.
self._set_parameters(parameter_list,
descriptor_list=descriptor_list,
raise_on_failure=True)
return self.get_parameters(
[parameter.name for parameter in parameter_list])
def test_declare_parameter(self):
result_initial_foo = self.node.declare_parameter(
'initial_foo', ParameterValue(), ParameterDescriptor())
result_foo = self.node.declare_parameter(
'foo',
ParameterValue(type=Parameter.Type.INTEGER.value,
integer_value=42), ParameterDescriptor())
result_bar = self.node.declare_parameter(
'bar',
ParameterValue(type=Parameter.Type.STRING.value,
string_value='hello'), ParameterDescriptor())
result_baz = self.node.declare_parameter(
'baz',
ParameterValue(type=Parameter.Type.DOUBLE.value,
double_value=2.41), ParameterDescriptor())
# OK cases.
self.assertIsInstance(result_initial_foo, Parameter)
self.assertIsInstance(result_foo, Parameter)
self.assertIsInstance(result_bar, Parameter)
self.assertIsInstance(result_baz, Parameter)
self.assertEqual(result_initial_foo.value, 4321)
self.assertEqual(result_foo.value, 42)
self.assertEqual(result_bar.value, 'hello')
self.assertEqual(result_baz.value, 2.41)
self.assertEqual(self.node.get_parameter('initial_foo').value, 4321)
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)
# Error cases.
with self.assertRaises(ParameterAlreadyDeclaredException):
self.node.declare_parameter(
'foo',
ParameterValue(type=Parameter.Type.STRING.value,
string_value='raise'), ParameterDescriptor())
with self.assertRaises(InvalidParameterException):
self.node.declare_parameter(
'123foo',
ParameterValue(type=Parameter.Type.STRING.value,
string_value='raise'), ParameterDescriptor())
with self.assertRaises(InvalidParameterException):
self.node.declare_parameter(
'foo??',
ParameterValue(type=Parameter.Type.STRING.value,
string_value='raise'), ParameterDescriptor())
self.node.set_parameters_callback(self.reject_parameter_callback)
with self.assertRaises(InvalidParameterValueException):
self.node.declare_parameter(
'reject_me',
ParameterValue(type=Parameter.Type.STRING.value,
string_value='raise'), ParameterDescriptor())
with self.assertRaises(AssertionError):
self.node.declare_parameter(
1,
ParameterValue(type=Parameter.Type.STRING.value,
string_value='wrong_name_type'),
ParameterDescriptor())
with self.assertRaises(AssertionError):
self.node.declare_parameter('wrong_parameter_value_type', 1234,
ParameterDescriptor())
with self.assertRaises(AssertionError):
self.node.declare_parameter('wrong_parameter_descriptor_type',
ParameterValue(), ParameterValue())
def __init__(self):
super().__init__('jinjaturtle',
automatically_declare_parameters_from_overrides=True)
#allow_undeclared_parameters=True)
#self.get_logger().info(str(self.get_parameters_by_prefix('template_params')))
#self.get_logger().info( str(os.listdir(self.template_file_path)) )
param_list = [
( 'message_module', self.message_module_name,
ParameterDescriptor(description='The module containing the message class')),
( 'message_class', self.message_class_name,
ParameterDescriptor(description='The message class')),
( 'template_file', '',
ParameterDescriptor(description='a path to a jinja2 template file')),
( 'template_data', None,
ParameterDescriptor(description='a jinja2 template')),
( 'template_params', None,
ParameterDescriptor(description='Namespace for additional parameters to send to the template')),
]
for (param_name, param_value, param_descr) in param_list:
try:
self.declare_parameter(param_name, param_value, param_descr)
except ParameterAlreadyDeclaredException:
self.set_descriptor(param_name, param_descr)
if self.get_parameter('template_params').value != None:
self.get_logger().warn('template_params is used as a namespace, ignoring this parameter')
# XXX should we attempt to jsonloads this?
for param_name, param in self.get_parameters_by_prefix('template_params').items():
self.get_logger().info(f'found parameter {param_name} = {param.value}')
self.template_params[param_name] = param.value
# set the message type
# XXX test param type is actually a string
# try:
self.message_module_name = self.get_parameter('message_module').get_parameter_value().string_value
self.message_class_name = self.get_parameter('message_class').get_parameter_value().string_value
# except Exception as err:
# self.get_logger().error(f"message types must be strings {str(self.message_module_name)} {str(self.message_class_name)}")
# raise err
try:
message_module = import_module(self.message_module_name)
self.message_class = getattr(message_module, self.message_class_name)
except AttributeError as err:
self.get_logger().error(f"could not import {self.message_class_name} from {self.message_module_name}")
raise(err)
except ModuleNotFoundError as err:
self.get_logger().error(f"could not find module {self.message_module_name}")
raise(err)
# we automatically declare override to fill the template_params namespace, so
# we can't register the callback before declaration.
# register the callback
self.add_on_set_parameters_callback(self.cb_params)
# run the callback to load the template
self.cb_params( [
self.get_parameter('template_data'),
self.get_parameter('template_file'),
self.get_parameter('template_params'),
])
#register the pub/sub
self.publisher_ = self.create_publisher(String, 'string_msg', 10)
self.subscription = self.create_subscription(
self.message_class,
'raw_msg',
self.sub_callback,
10)
self.get_logger().info(f"listenning for a {self.message_module_name} {self.message_class_name}")
def __init__(self):
super().__init__('node1', namespace = '')
# Params
#____________________________________________
# p1 - INTEGER_ARRAY
# Description:
self.param_p1 = self.declare_parameter('p1', 32, descriptor=ParameterDescriptor(name='p1', type=7, description='', additional_constraints='', read_only=False, floating_point_range=[], integer_range=[]))
# You can use your parameter p1 with type INTEGER_ARRAY
# with self.get_parameter('p1')._value
# You can also use your parameter from terminal or yaml file.
#_____
# Publishers
#____________________________________________
# Subscribers
#____________________________________________
# sub1
# Qos profile
qos_profile_sub1 = QoSPresetProfiles.SYSTEM_DEFAULT.value
self.subscriber_sub1 = self.create_subscription(M1, 'topic/path1', self.subscriber_call_sub1, qos_profile = qos_profile_sub1)
self.subscriber_sub1
#_____
# Servers
#____________________________________________
# s1
# Qos profile
qos_profile_s1 = QoSProfile(history = QoSHistoryPolicy.KEEP_ALL, durability = QoSDurabilityPolicy.SYSTEM_DEFAULT, reliability = QoSReliabilityPolicy.RELIABLE, depth =0)
# Additional qos settings
qos_profile_s1.liveliness = QoSLivelinessPolicy.SYSTEM_DEFAULT
qos_profile_s1.deadline.sec = 0
qos_profile_s1.deadline.nsec = 0
qos_profile_s1.lifespan.sec = 0
qos_profile_s1.lifespan.nsec = 0
qos_profile_s1.liveliness_lease_duration.sec = 0
qos_profile_s1.liveliness_lease_duration.nsec = 0
qos_profile_s1.avoid_ros_namespace_conventions = False
self.server_s1 = self.create_service(Addtwo, 'service1', self.server_call_s1, qos_profile = qos_profile_s1)
#_____
# s2
# Qos profile
qos_profile_s2 = QoSProfile(history = QoSHistoryPolicy.KEEP_ALL, durability = QoSDurabilityPolicy.SYSTEM_DEFAULT, reliability = QoSReliabilityPolicy.RELIABLE, depth =0)
# Additional qos settings
qos_profile_s2.liveliness = QoSLivelinessPolicy.SYSTEM_DEFAULT
qos_profile_s2.deadline.sec = 0
qos_profile_s2.deadline.nsec = 0
qos_profile_s2.lifespan.sec = 0
qos_profile_s2.lifespan.nsec = 0
qos_profile_s2.liveliness_lease_duration.sec = 0
qos_profile_s2.liveliness_lease_duration.nsec = 0
qos_profile_s2.avoid_ros_namespace_conventions = False
self.server_s2 = self.create_service(SetBool, 'service2', self.server_call_s2, qos_profile = qos_profile_s2)
#_____
# Clients
#____________________________________________
# Action Servers
#____________________________________________
# as1
self.action_server_as1 = ActionServer(self, Increase, 'as1', execute_callback=self.action_execute_call_as1, goal_callback=self.action_goal_call_as1, cancel_callback=self.action_cancel_call_as1)
#_____
# Action Clients
#____________________________________________
# ac2
self.action_client_ac2 = ActionClient(self, Dec, 'ac2')
def get_descriptor(self):
return ParameterDescriptor(name=self.name, type=self.type_.value)
def test_declare_parameters(self):
parameters = [
('foo',
ParameterValue(type=Parameter.Type.INTEGER.value,
integer_value=42), ParameterDescriptor()),
('bar',
ParameterValue(type=Parameter.Type.STRING.value,
string_value='hello'), ParameterDescriptor()),
('baz',
ParameterValue(type=Parameter.Type.DOUBLE.value,
double_value=2.41), ParameterDescriptor()),
]
result = self.node.declare_parameters('', parameters)
# OK cases.
self.assertIsInstance(result, list)
self.assertIsInstance(result[0], Parameter)
self.assertIsInstance(result[1], Parameter)
self.assertIsInstance(result[2], Parameter)
self.assertEqual(result[0].value, 42)
self.assertEqual(result[1].value, 'hello')
self.assertEqual(result[2].value, 2.41)
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)
result = self.node.declare_parameters('/namespace/', parameters)
# OK cases.
self.assertIsInstance(result, list)
self.assertIsInstance(result[0], Parameter)
self.assertIsInstance(result[1], Parameter)
self.assertIsInstance(result[2], Parameter)
self.assertEqual(result[0].value, 42)
self.assertEqual(result[1].value, 'hello')
self.assertEqual(result[2].value, 2.41)
self.assertEqual(self.node.get_parameter('/namespace/foo').value, 42)
self.assertEqual(
self.node.get_parameter('/namespace/bar').value, 'hello')
self.assertEqual(self.node.get_parameter('/namespace/baz').value, 2.41)
# Error cases.
with self.assertRaises(ParameterAlreadyDeclaredException):
self.node.declare_parameters('', parameters)
# Declare a new set of parameters; the first one is not already declared,
# but 2nd and 3rd one are.
parameters = [
('foobar',
ParameterValue(type=Parameter.Type.INTEGER.value,
integer_value=43), ParameterDescriptor()),
('bar',
ParameterValue(type=Parameter.Type.STRING.value,
string_value='hello'), ParameterDescriptor()),
('baz',
ParameterValue(type=Parameter.Type.DOUBLE.value,
double_value=2.41), ParameterDescriptor()),
]
with self.assertRaises(ParameterAlreadyDeclaredException):
self.node.declare_parameters('', parameters)
# Declare a new set; the third one shall fail because of its name.
parameters = [
('foobarbar',
ParameterValue(type=Parameter.Type.INTEGER.value,
integer_value=44), ParameterDescriptor()),
('barbarbar',
ParameterValue(type=Parameter.Type.STRING.value,
string_value='world'), ParameterDescriptor()),
('baz??wrong_name',
ParameterValue(type=Parameter.Type.DOUBLE.value,
double_value=2.41), ParameterDescriptor()),
]
with self.assertRaises(InvalidParameterException):
self.node.declare_parameters('', parameters)
# Declare a new set; the third one shall be rejected by the callback.
parameters = [
('im_ok',
ParameterValue(type=Parameter.Type.INTEGER.value,
integer_value=44), ParameterDescriptor()),
('im_also_ok',
ParameterValue(type=Parameter.Type.STRING.value,
string_value='world'), ParameterDescriptor()),
('reject_me',
ParameterValue(type=Parameter.Type.DOUBLE.value,
double_value=2.41), ParameterDescriptor()),
]
self.node.set_parameters_callback(self.reject_parameter_callback)
with self.assertRaises(InvalidParameterValueException):
self.node.declare_parameters('', parameters)
with self.assertRaises(AssertionError):
self.node.declare_parameters(
'', [(1,
ParameterValue(type=Parameter.Type.STRING.value,
string_value='wrong_name_type'),
ParameterDescriptor())])
with self.assertRaises(AssertionError):
self.node.declare_parameters(
'',
[('wrong_parameter_value_type', 1234, ParameterDescriptor())])
with self.assertRaises(AssertionError):
self.node.declare_parameters(
'', [('wrong_parameter_descriptor_tpye', ParameterValue(),
ParameterValue())])
