Skip to content

Commit

Permalink
update geofence control pub to transient_local (#215)
Browse files Browse the repository at this point in the history
<!-- Thanks for the contribution, this is awesome. -->

# PR Details
This PR updates the QoS for incoming_geofence_control publisher to be of
type transient_local. In order to be consistent with other publishers
and subscribers to the topic.

## Description

<!--- Describe your changes in detail -->

## Related GitHub Issue

<!--- This project only accepts pull requests related to open GitHub
issues or Jira Keys -->
<!--- If suggesting a new feature or change, please discuss it in an
issue first -->
<!--- If fixing a bug, there should be an issue describing it with steps
to reproduce -->
<!--- Please DO NOT name partially fixed issues, instead open an issue
specific to this fix -->
<!--- Please link to the issue here: -->
usdot-fhwa-stol/carma-platform#2290

## Related Jira Key

<!-- e.g. CAR-123 -->

## Motivation and Context

<!--- Why is this change required? What problem does it solve? -->

## How Has This Been Tested?

<!--- Please describe in detail how you tested your changes. -->
<!--- Include details of your testing environment, and the tests you ran
to -->
<!--- see how your change affects other areas of the code, etc. -->

## Types of changes

<!--- What types of changes does your code introduce? Put an `x` in all
the boxes that apply: -->

- [x] Defect fix (non-breaking change that fixes an issue)
- [ ] New feature (non-breaking change that adds functionality)
- [ ] Breaking change (fix or feature that cause existing functionality
to change)

## Checklist:

<!--- Go over all the following points, and put an `x` in all the boxes
that apply. -->
<!--- If you're unsure about any of these, don't hesitate to ask. We're
here to help! -->

- [ ] I have added any new packages to the sonar-scanner.properties file
- [ ] My change requires a change to the documentation.
- [ ] I have updated the documentation accordingly.
- [x] I have read the
[**CONTRIBUTING**](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/Contributing.md)
document.
- [ ] I have added tests to cover my changes.
- [ ] All new and existing tests passed.
  • Loading branch information
adev4a authored Jan 19, 2024
2 parents 70500e9 + ecfdb7a commit 0643839
Showing 1 changed file with 15 additions and 5 deletions.
20 changes: 15 additions & 5 deletions carma-messenger-core/j2735_convertor/src/j2735_convertor_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -101,22 +101,32 @@ namespace j2735_convertor
outbound_sdsm_options.callback_group = outbound_sdsm_cb_group;
outbound_sdsm_sub_ = create_subscription<carma_v2x_msgs::msg::SensorDataSharingMessage>("outgoing_sdsm", 1, std::bind(&Node::SdsmHandler,
this, std_ph::_1), outbound_sdsm_options); // Queue size of 1 as we should never publish outdated PSMs

// SDSM j3224 Publisher
outbound_j3224_sdsm_pub_ = create_publisher<j3224_v2x_msgs::msg::SensorDataSharingMessage>("outgoing_j3224_sdsm", 1);

// End of SDSM pub/sub ///////////////////////////////////////////////////////


// Incoming geofence pub/sub
converted_geofence_control_pub_ = create_publisher<carma_v2x_msgs::msg::TrafficControlMessage>("incoming_geofence_control", 50);
// NOTE: Currently, intra-process comms must be disabled for the following publisher that are transient_local: https://github.com/ros2/rclcpp/issues/1753
rclcpp::PublisherOptions intra_proc_disabled;
intra_proc_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for this PublisherOptions object

// Create a publisher that will send all previously published messages to late-joining subscribers ONLY If the subscriber is transient_local too
auto pub_qos_transient_local = rclcpp::QoS(rclcpp::KeepAll()); // A publisher with this QoS will store all messages that it has sent on the topic
pub_qos_transient_local.transient_local(); // A publisher with this QoS will re-send all (when KeepAll is used) messages to all late-joining subscribers
// NOTE: The subscriber's QoS must be set to transient_local() as well for earlier messages to be resent to the later-joiner.

converted_geofence_control_pub_ = create_publisher<carma_v2x_msgs::msg::TrafficControlMessage>("incoming_geofence_control", pub_qos_transient_local, intra_proc_disabled);

converted_geofence_request_pub_ = create_publisher<carma_v2x_msgs::msg::TrafficControlRequest>("incoming_geofence_request", 50);

auto j2735_geofence_control_cb_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions j2735_geofence_control_options;
j2735_geofence_control_options.callback_group = j2735_geofence_control_cb_group;
j2735_geofence_control_sub_ = create_subscription<j2735_v2x_msgs::msg::TrafficControlMessage>("incoming_j2735_geofence_control", 50, std::bind(&Node::j2735ControlMessageHandler, this, std_ph::_1), j2735_geofence_control_options);


auto j2735_geofence_request_cb_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions j2735_geofence_request_options;
Expand All @@ -128,8 +138,8 @@ namespace j2735_convertor
rclcpp::SubscriptionOptions outbound_geofence_control_options;
outbound_geofence_control_options.callback_group = outbound_geofence_control_cb_group;
outbound_geofence_control_sub_ = create_subscription<carma_v2x_msgs::msg::TrafficControlMessage>("outgoing_geofence_control", 50, std::bind(&Node::ControlMessageHandler, this, std_ph::_1), outbound_geofence_control_options);


auto outbound_geofence_request_cb_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions outbound_geofence_request_options;
outbound_geofence_request_options.callback_group = outbound_geofence_request_cb_group;
Expand Down

0 comments on commit 0643839

Please sign in to comment.