feat: add XYZI sensor type and parameterize topics/frame (upstream PR #4) - #1
Open
h-wata wants to merge 5 commits into
Open
feat: add XYZI sensor type and parameterize topics/frame (upstream PR #4)#1h-wata wants to merge 5 commits into
h-wata wants to merge 5 commits into
Conversation
…eggedrobotics#4) Based on upstream PR leggedrobotics#4 by @jeldriks. Adds robot_description_topic subscription (falls back to topic when the robot_description parameter is empty), sensor_frame launch parameter, out_pointcloud_topic parameter (replacing the /cloud_out launch remap), and Generic XYZI (pcl::PointXYZI) sensor type (lidar_sensor_type=6), including the missing shape-publishing branch for it. Resolves conflicts with fork-local TinyXML2 migration (bcff853), QoS BEST_EFFORT change (3826a6c), and static lib export (8910134) by keeping the SensorDataQoS publisher/subscriber setup and adding the new out_pointcloud_topic parameter on top of it instead of reverting to the upstream remapping-based approach.
self_see_filter.h must be included before the point_*.h custom PCL point type headers, otherwise pcl::traits::fieldList<> specializations for those types (e.g. PointHesai) are not visible when self_see_filter.h's templates are instantiated, breaking the build with "no type named 'type' in struct pcl::traits::fieldList<...>". Guard the block with clang-format off/on so automatic re-sorting cannot reintroduce this.
- launch: sensor_frame/in_pointcloud_topic/out_pointcloud_topic only override filter_config YAML when explicitly passed on the CLI (via -p, positioned after --params-file in the same --ros-args block); YAML wins over unset launch defaults, matching pre-PR behavior. Other launch args are unaffected (unconditional, as before). - self_filter: warn every 10s while waiting for /robot_description, so a missing publisher no longer leaves the node silently idle - self_filter: guard setupFilter() against double execution - remove dead try/catch around use_sim_time declare/set (rclcpp always pre-declares it, so declare_parameter always throws and the set_parameter call never ran) - README: document description_name -> robot_description_topic rename impact and the zero_for_removed_points default mismatch between the launch file and the node's own declared default
W1 re-review (PCH-023 blocking finding dict-keys-still-silently-override-yaml) found that lidar_sensor_type / robot_description_topic / zero_for_removed_points were still left in an unconditional parameters= dict, so filter_config YAML values for those keys were silently discarded (the same defect class B1 targeted, left unfixed for 3 of the 5 remaining keys). Confirmed live against head 79d54d5 before fixing: YAML zero_for_removed_points/lidar_sensor_type/ robot_description_topic were all silently overridden by the launch defaults. - launch: apply the same sentinel + -p override mechanism (CLI > YAML > node's own declared default) to zero_for_removed_points, lidar_sensor_type, and robot_description_topic, matching sensor_frame/in_pointcloud_topic/ out_pointcloud_topic from the prior commit - launch: quote string-typed CLI override values (-p key:="value") so a numeric/bool-looking value (e.g. sensor_frame:=123) doesn't crash the node with InvalidParameterTypeException - robot_description and use_sim_time remain unconditional (launch argument always wins) with documented justification: robot_description can be an arbitrarily large multi-line URDF/XACRO string and -p's YAML-scalar parsing would fold embedded newlines into spaces, silently corrupting it; use_sim_time is a cross-cutting runtime setting expected to be driven by the overall launch context, not a per-filter config value - README: document the CLI > YAML > node-default priority for the six conditional keys, the two keys that always ignore filter_config and why, and the two disclosed default-value changes this causes when neither YAML nor CLI sets zero_for_removed_points (true -> false) or lidar_sensor_type (2 -> Ouster -> 0/XYZ), both now matching the node's own declared defaults
W1's second re-review (PCH-026 blocking finding lidar-sensor-type-default-silently-degrades-output) found that the previous fix's sentinel treatment of lidar_sensor_type changed its effective "nothing set" fallback from the old forced launch default (2, Ouster) to the node's own declared default (0, XYZ). A caller relying on the old implicit default now gets XYZ instead of Ouster, which silently drops fields (intensity, t, reflectivity, ring, ambient, range) from the output point cloud with no error and no warning -- confirmed live: 9 fields -> 3 fields, same otherwise-valid startup. Reverting the default back to 2 was explicitly ruled out: that would resurrect the exact "launch default silently overrides filter_config YAML" defect this PR chain already fixed for lidar_sensor_type. The fix instead adds a runtime signal: self_filter.cpp checks get_node_parameters_interface()->get_parameter_overrides() right after declaring lidar_sensor_type -- if the key is absent from the overrides map, neither filter_config nor the CLI supplied a value, so a WARN fires naming the default and the fields it drops. Confirmed at parent commit ac4f697 that this WARN does not fire (gate wasn't present yet), and that it fires only in the neither-YAML-nor-CLI case, not when either sets the parameter.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
Ports the changes from upstream leggedrobotics#4 (by @jeldriks) onto this fork's
main, which has already diverged with fork-local commits (TinyXML2 migration, SensorDataQoS BEST_EFFORT, static lib export).robot_description_topicparameter: subscribes to/robot_description(transient-local) and picks it up automatically fromrobot_state_publisherwhen therobot_descriptionparameter is left empty; the parameter still takes priority when non-empty. If nothing arrives, the node logs aWARNevery 10 seconds until it does.sensor_frameas a launch argument (previously only settable via the filter_config YAML).out_pointcloud_topicas a real node parameter (used directly for the publisher topic), replacing the previous/cloud_outlaunch-file remap.in_pointcloud_topicwas already parameterized on this fork.pcl::PointXYZI) sensor type aslidar_sensor_type=6, including the corresponding shape-publishing branch incloudCallback(upstream PR Subscribe to /robot_description, provide sensor_frame via launch file and add XYZI sensor type leggedrobotics/robot_self_filter#4 only added the sensor construction case + one shape-publishing case in a base that predates this fork's Ouster/Hesai/Robosense/Pandar additions).std_msgsdependency for the description-topic subscription).filter_configYAML vs. launch argumentssensor_frame,in_pointcloud_topic,out_pointcloud_topic,lidar_sensor_type,robot_description_topic, andzero_for_removed_pointsfollow this priority: an explicit CLI argument (e.g.sensor_frame:=...) wins, otherwisefilter_configYAML wins, otherwise the node's own declared default applies. This is implemented by passingfilter_configand any explicit CLI overrides through the samearguments=list, in that order — a-p key:=valueargument only beats a--params-fileargument for the same key when it comes after it in the same--ros-argsblock, and--params-fileentries otherwise resolve by plain last-one-wins regardless of whether the key is node-specific or wildcard-scoped. String-valued overrides are quoted (-p key:="value") so a numeric- or bool-looking CLI value (e.g.sensor_frame:=123) is passed as a string instead of crashing the node withInvalidParameterTypeException.robot_descriptionanduse_sim_timeare the two exceptions — the launch argument always wins overfilter_configfor these:robot_descriptioncan be an arbitrarily large, multi-line URDF/XACRO string.-p's value is parsed as a YAML plain scalar, which folds embedded newlines into spaces and would silently corrupt it, so this key is excluded from the CLI-override mechanism above.use_sim_timeis a cross-cutting runtime setting expected to be driven by the overall launch context, not by a per-filter config file.Disclosed default change: when neither
filter_confignor the CLI setszero_for_removed_pointsorlidar_sensor_type, this launch file used to forcetrue/2(Ouster) unconditionally regardless of whatfilter_configsaid. It now falls through to the node's own declared default (false/0, XYZ) like every other parameter above, restoringfilter_config's authority. A launch invocation that omitted these and relied on the old forced values will observe different behavior. Iflidar_sensor_typefalls through to its default this way, the node logs aWARNat startup, since silently defaulting to XYZ drops extra fields (intensity, ring, etc.) from sensor types that have them.Breaking change
This PR removes the launch argument
description_nameand replaces it withrobot_description_topic. Callers passingdescription_name:=...are affected.Compatibility with fork-local commits
rclcpp::SensorDataQoS();out_pointcloud_topicis layered on top of it instead of reverting to upstream's remap-based approach.Verification
All of the following were run against an isolated
colcon build(--build-base/--install-baseoutside the shared workspace build dir,--cmake-force-configure) of this branch's actual binary, not inferred from reading the code:colcon build: succeeds, no tests defined (pre-existing).lidar_sensor_type:=6) and XYZ (lidar_sensor_type:=0, default) both start cleanly with a minimal inline URDF, no "sensor type not handled" errors.sensor_frame:=123starts the node successfully withsensor_frameas the string"123", not a crash.robot_description_topicset via YAML is reflected in the periodic/robot_description-waitWARNmessage, not the node's default topic name.lidar_sensor_typeunset (neither YAML nor CLI):WARNlogged at startup. Set via YAML or CLI: noWARN. Confirmed thisWARNis absent at the parent commit (gate not yet present).Credit: based on upstream PR leggedrobotics#4 by @jeldriks.
Ready for re-review.