Skip to content

Commit

Permalink
Merge pull request #44 from botsandus/AUTO-3278_dyn_CM_min_max_pcl
Browse files Browse the repository at this point in the history
AUTO-3278 [DEX] make CM pointcloud min_height and max_height
  • Loading branch information
doisyg authored Nov 21, 2024
2 parents 37badc5 + f88dda8 commit 5015798
Show file tree
Hide file tree
Showing 2 changed files with 33 additions and 0 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,13 @@ class PointCloud : public Source
*/
void dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);

/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(
std::vector<rclcpp::Parameter> parameters);

// ----- Variables -----

/// @brief PointCloud data subscriber
Expand Down
26 changes: 26 additions & 0 deletions nav2_collision_monitor/src/pointcloud.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,10 @@ void PointCloud::configure()
data_sub_ = node->create_subscription<sensor_msgs::msg::PointCloud2>(
source_topic, pointcloud_qos,
std::bind(&PointCloud::dataCallback, this, std::placeholders::_1));

// Add callback for dynamic parameters
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind(&PointCloud::dynamicParametersCallback, this, std::placeholders::_1));
}

bool PointCloud::getData(
Expand Down Expand Up @@ -123,4 +127,26 @@ void PointCloud::dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
data_ = msg;
}

rcl_interfaces::msg::SetParametersResult
PointCloud::dynamicParametersCallback(
std::vector<rclcpp::Parameter> parameters)
{
rcl_interfaces::msg::SetParametersResult result;

for (auto parameter : parameters) {
const auto & param_type = parameter.get_type();
const auto & param_name = parameter.get_name();

if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
if (param_name == source_name_ + "." + "min_height") {
min_height_ = parameter.as_double();
} else if (param_name == source_name_ + "." + "max_height") {
max_height_ = parameter.as_double();
}
}
}
result.successful = true;
return result;
}

} // namespace nav2_collision_monitor

0 comments on commit 5015798

Please sign in to comment.