From 82ef6c1eb6930a26ca523ad560a61fea8935a674 Mon Sep 17 00:00:00 2001 From: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com> Date: Wed, 8 Feb 2023 09:40:12 -0800 Subject: [PATCH] Unlatching topics (#674) * Changed heartbeat topic to be unlatched. * Changed echoed command to not latching. * no subscribers and info is obtained by service, so no latched topic needed * removing latched on frequently published topics * Made command topic to not atching. * no need for these to be latched * sub and pub in same node, no need to latch * no need to be latched no subs and not recorded, rviz should be opened when published * Made ground bridge topics not latched. * Fixed heartbeat topic latched comment. --------- Co-authored-by: Katie Browne --- behaviors/arm/src/arm_nodelet.cc | 2 +- .../src/rapid_guest_science_data.cc | 4 +--- .../ground_dds_ros_bridge/src/rapid_position.cc | 2 +- .../pmc_actuator_nodelet/pmc_actuator_nodelet.cc | 2 +- management/access_control/src/access_control.cc | 2 +- management/cpu_mem_monitor/src/cpu_mem_monitor.cc | 6 ++---- management/data_bagger/src/astrobee_recorder.cc | 2 +- management/disk_monitor/src/disk_monitor_node.cc | 2 +- management/executive/tools/data_to_disk_pub.cc | 3 +-- management/executive/tools/plan_pub.cc | 3 +-- management/executive/tools/simple_move.cc | 2 +- management/executive/tools/teleop_tool.cc | 2 +- management/executive/tools/zones_pub.cc | 3 +-- mobility/mapper/src/mapper_nodelet.cc | 8 ++++---- mobility/planner_qp/planner_qp/src/planner_qp.cc | 2 +- .../planner_qp/traj_opt_ros/src/ros_bridge.cpp | 4 ++-- shared/ff_util/src/ff_nodelet/ff_nodelet.cc | 15 ++++++++------- .../gazebo_model_plugin_pmc.cc | 2 +- .../src/interactive_marker_teleop.cpp | 2 +- .../src/sparse_mapping_display.cc | 2 +- 20 files changed, 32 insertions(+), 38 deletions(-) diff --git a/behaviors/arm/src/arm_nodelet.cc b/behaviors/arm/src/arm_nodelet.cc index 68a17c844a..0eb679d40a 100644 --- a/behaviors/arm/src/arm_nodelet.cc +++ b/behaviors/arm/src/arm_nodelet.cc @@ -518,7 +518,7 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { sub_joint_states_ = nh->subscribe(TOPIC_JOINT_STATES, 1, &ArmNodelet::JointStateCallback, this); pub_joint_goals_ = nh->advertise( - TOPIC_JOINT_GOALS, 1, true); + TOPIC_JOINT_GOALS, 1); // Subscribe to Proximal Joint Servo Enabling service client_enable_prox_servo_ = nh->serviceClient( diff --git a/communications/ground_dds_ros_bridge/src/rapid_guest_science_data.cc b/communications/ground_dds_ros_bridge/src/rapid_guest_science_data.cc index 1280e949b6..2fa0227d59 100644 --- a/communications/ground_dds_ros_bridge/src/rapid_guest_science_data.cc +++ b/communications/ground_dds_ros_bridge/src/rapid_guest_science_data.cc @@ -31,9 +31,7 @@ RapidGuestScienceDataToRos::RapidGuestScienceDataToRos( "RapidGuestScienceDataToRos", queue_size) { // advertise ros topic, make it latched - pub_ = nh_.advertise(publish_topic_, - queue_size, - true); + pub_ = nh_.advertise(publish_topic_, queue_size); // connect to ddsEventLoop try { diff --git a/communications/ground_dds_ros_bridge/src/rapid_position.cc b/communications/ground_dds_ros_bridge/src/rapid_position.cc index 43230e271b..cf1441b0d3 100644 --- a/communications/ground_dds_ros_bridge/src/rapid_position.cc +++ b/communications/ground_dds_ros_bridge/src/rapid_position.cc @@ -30,7 +30,7 @@ RapidPositionToRos::RapidPositionToRos(const std::string& subscribe_topic, "RapidPositionToRos", queue_size) { // advertise ros topic, make it latched - pub_ = nh_.advertise(publish_topic_, queue_size, true); + pub_ = nh_.advertise(publish_topic_, queue_size); // connect to ddsEventLoop try { diff --git a/hardware/pmc_actuator/src/pmc_actuator_nodelet/pmc_actuator_nodelet.cc b/hardware/pmc_actuator/src/pmc_actuator_nodelet/pmc_actuator_nodelet.cc index 4cd6779451..aa3691684a 100644 --- a/hardware/pmc_actuator/src/pmc_actuator_nodelet/pmc_actuator_nodelet.cc +++ b/hardware/pmc_actuator/src/pmc_actuator_nodelet/pmc_actuator_nodelet.cc @@ -101,7 +101,7 @@ class PmcActuatorNodelet : public ff_util::FreeFlyerNodelet { // State publisher as a latched topic pub_state_ = nh->advertise( - TOPIC_HARDWARE_PMC_STATE, pub_queue_size_, true); + TOPIC_HARDWARE_PMC_STATE, pub_queue_size_); // PMC enable/disable service srv_ = nh->advertiseService( diff --git a/management/access_control/src/access_control.cc b/management/access_control/src/access_control.cc index e0c77e457d..f6e6aaddab 100644 --- a/management/access_control/src/access_control.cc +++ b/management/access_control/src/access_control.cc @@ -61,7 +61,7 @@ void AccessControl::Initialize(ros::NodeHandle *nh) { failed_cmd_pub_ = nh->advertise( TOPIC_MANAGEMENT_ACCESS_CONTROL_CMD, pub_queue_size_, - true); + false); state_.controller = "No Controller"; diff --git a/management/cpu_mem_monitor/src/cpu_mem_monitor.cc b/management/cpu_mem_monitor/src/cpu_mem_monitor.cc index b8ea750a05..12c5881e2e 100644 --- a/management/cpu_mem_monitor/src/cpu_mem_monitor.cc +++ b/management/cpu_mem_monitor/src/cpu_mem_monitor.cc @@ -46,13 +46,11 @@ void CpuMemMonitor::Initialize(ros::NodeHandle *nh) { // All state messages are latching cpu_state_pub_ = nh->advertise( TOPIC_MANAGEMENT_CPU_MONITOR_STATE, - pub_queue_size_, - true); + pub_queue_size_); // All state messages are latching mem_state_pub_ = nh->advertise( TOPIC_MANAGEMENT_MEM_MONITOR_STATE, - pub_queue_size_, - true); + pub_queue_size_); // Timer for asserting the cpu load too high fault assert_cpu_load_fault_timer_ = nh->createTimer( diff --git a/management/data_bagger/src/astrobee_recorder.cc b/management/data_bagger/src/astrobee_recorder.cc index 647e2fc63f..ecfd864d4e 100644 --- a/management/data_bagger/src/astrobee_recorder.cc +++ b/management/data_bagger/src/astrobee_recorder.cc @@ -561,7 +561,7 @@ void Recorder::doCheckMaster(ros::TimerEvent const& e, ros::NodeHandle& node_han } void Recorder::doTrigger() { - ros::Publisher pub = node_handle_.advertise("snapshot_trigger", 1, true); + ros::Publisher pub = node_handle_.advertise("snapshot_trigger", 1); pub.publish(std_msgs::Empty()); ros::Timer terminate_timer = node_handle_.createTimer(ros::Duration(1.0), boost::bind(&ros::shutdown)); diff --git a/management/disk_monitor/src/disk_monitor_node.cc b/management/disk_monitor/src/disk_monitor_node.cc index fa8f8722f8..07d3936baa 100644 --- a/management/disk_monitor/src/disk_monitor_node.cc +++ b/management/disk_monitor/src/disk_monitor_node.cc @@ -62,7 +62,7 @@ void DiskMonitor::Initialize(ros::NodeHandle * nh) { // Setup the publisher pub_disk_stats_ = nh->advertise( - TOPIC_MANAGEMENT_DISK_MONITOR_STATE, pub_queue_size_, true); + TOPIC_MANAGEMENT_DISK_MONITOR_STATE, pub_queue_size_); stats_timer_ = nh->createTimer(ros::Duration(update_freq_), &DiskMonitor::OnCheckTimer, this); diff --git a/management/executive/tools/data_to_disk_pub.cc b/management/executive/tools/data_to_disk_pub.cc index d1c61e1a3f..3cae1b9cc8 100644 --- a/management/executive/tools/data_to_disk_pub.cc +++ b/management/executive/tools/data_to_disk_pub.cc @@ -145,8 +145,7 @@ int main(int argc, char** argv) { std::bind(&on_connect, std::placeholders::_1, cf)); // After the zones are received, commands a set zones to the executive - command_pub = n.advertise( - TOPIC_COMMAND, 5, true); + command_pub = n.advertise(TOPIC_COMMAND, 5, false); // Subscriber that receives confirmation that the zones were received ros::Subscriber cf_acK_pub = n.subscribe(TOPIC_MANAGEMENT_EXEC_CF_ACK, 10, &on_cf_ack); diff --git a/management/executive/tools/plan_pub.cc b/management/executive/tools/plan_pub.cc index 23acd5c236..c553213c78 100644 --- a/management/executive/tools/plan_pub.cc +++ b/management/executive/tools/plan_pub.cc @@ -164,8 +164,7 @@ int main(int argc, char** argv) { std::string sub_topic_plan = TOPIC_COMMUNICATIONS_DDS_PLAN; std::string sub_topic_command = TOPIC_COMMAND; - command_pub = n.advertise( - sub_topic_command, 5, true); + command_pub = n.advertise(sub_topic_command, 5); ros::Subscriber cf_ack_sub = n.subscribe(TOPIC_MANAGEMENT_EXEC_CF_ACK, 10, &on_cf_ack); diff --git a/management/executive/tools/simple_move.cc b/management/executive/tools/simple_move.cc index 1bc9e17716..555e1d6dc6 100644 --- a/management/executive/tools/simple_move.cc +++ b/management/executive/tools/simple_move.cc @@ -60,7 +60,7 @@ int main(int argc, char** argv) { // Make publisher to publish the command ros::Publisher cmd_publisher; - cmd_publisher = nh.advertise(TOPIC_COMMAND, 10, true); + cmd_publisher = nh.advertise(TOPIC_COMMAND, 10); // Make subscriber to receive command acks to see if the command completed // successfully diff --git a/management/executive/tools/teleop_tool.cc b/management/executive/tools/teleop_tool.cc index 5e44341a81..f2a9c2ad17 100644 --- a/management/executive/tools/teleop_tool.cc +++ b/management/executive/tools/teleop_tool.cc @@ -728,7 +728,7 @@ int main(int argc, char** argv) { tf2_ros::TransformListener tf_listener(tf_buffer); // Initialize publishers - cmd_pub = nh.advertise(TOPIC_COMMAND, 10, true); + cmd_pub = nh.advertise(TOPIC_COMMAND, 10); // Initialize subscribers ros::Subscriber ack_sub, agent_state_sub, fault_state_sub, dock_sub, move_sub; diff --git a/management/executive/tools/zones_pub.cc b/management/executive/tools/zones_pub.cc index 3c990e1348..69244619ed 100644 --- a/management/executive/tools/zones_pub.cc +++ b/management/executive/tools/zones_pub.cc @@ -144,8 +144,7 @@ int main(int argc, char** argv) { std::bind(&on_connect, std::placeholders::_1, cf)); // After the zones are received, commands a set zones to the executive - command_pub = n.advertise( - TOPIC_COMMAND, 5, true); + command_pub = n.advertise(TOPIC_COMMAND, 5); // Subscriber that receives confirmation that the zones were received ros::Subscriber cf_acK_pub = n.subscribe(TOPIC_MANAGEMENT_EXEC_CF_ACK, 10, &on_cf_ack); diff --git a/mobility/mapper/src/mapper_nodelet.cc b/mobility/mapper/src/mapper_nodelet.cc index d895093506..14fd9a38ed 100644 --- a/mobility/mapper/src/mapper_nodelet.cc +++ b/mobility/mapper/src/mapper_nodelet.cc @@ -106,13 +106,13 @@ void MapperNodelet::Initialize(ros::NodeHandle *nh) { cam_frustum_pub_ = nh->advertise( TOPIC_MAPPER_FRUSTRUM_MARKERS, 1); free_space_cloud_pub_ = nh->advertise( - TOPIC_MAPPER_OCTOMAP_FREE_CLOUD, 1, true); + TOPIC_MAPPER_OCTOMAP_FREE_CLOUD, 1); obstacle_cloud_pub_ = nh->advertise( - TOPIC_MAPPER_OCTOMAP_CLOUD, 1, true); + TOPIC_MAPPER_OCTOMAP_CLOUD, 1); inflated_free_space_cloud_pub_ = nh->advertise( - TOPIC_MAPPER_OCTOMAP_INFLATED_FREE_CLOUD, 1, true); + TOPIC_MAPPER_OCTOMAP_INFLATED_FREE_CLOUD, 1); inflated_obstacle_cloud_pub_ = nh->advertise( - TOPIC_MAPPER_OCTOMAP_INFLATED_CLOUD, 1, true); + TOPIC_MAPPER_OCTOMAP_INFLATED_CLOUD, 1); hazard_pub_ = nh->advertise( TOPIC_MOBILITY_HAZARD, 1); diff --git a/mobility/planner_qp/planner_qp/src/planner_qp.cc b/mobility/planner_qp/planner_qp/src/planner_qp.cc index 619bcb9bec..58e67c92a9 100644 --- a/mobility/planner_qp/planner_qp/src/planner_qp.cc +++ b/mobility/planner_qp/planner_qp/src/planner_qp.cc @@ -102,7 +102,7 @@ class Planner : public planner::PlannerImplementation { nh_ = nh; viz_pub_ = nh->advertise( - "/mob/planner_qp/safe_flight_cooridor", 5, true); + "/mob/planner_qp/safe_flight_cooridor", 5); // Setup a timer to animate timer_a_ = diff --git a/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp b/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp index bf0365be19..7881d796f1 100644 --- a/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp +++ b/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp @@ -27,14 +27,14 @@ TrajRosBridge &TrajRosBridge::instance() { ros::Publisher TrajRosBridge::getPub(std::string topic) { if (instance().pubs_.find(topic) == instance().pubs_.end()) { instance().pubs_[topic] = - instance().nh_.advertise(topic, 1, true); + instance().nh_.advertise(topic, 1); } return instance().pubs_[topic]; } ros::Publisher TrajRosBridge::getInfoPub(std::string topic) { if (instance().pubs_.find(topic) == instance().pubs_.end()) { instance().pubs_[topic] = - instance().nh_.advertise(topic, 1, true); + instance().nh_.advertise(topic, 1); } return instance().pubs_[topic]; } diff --git a/shared/ff_util/src/ff_nodelet/ff_nodelet.cc b/shared/ff_util/src/ff_nodelet/ff_nodelet.cc index 596a477b54..14d7be0026 100644 --- a/shared/ff_util/src/ff_nodelet/ff_nodelet.cc +++ b/shared/ff_util/src/ff_nodelet/ff_nodelet.cc @@ -96,18 +96,15 @@ void FreeFlyerNodelet::Setup(ros::NodeHandle & nh, ros::NodeHandle & nh_mt, std: heartbeat_.nodelet_manager = ros::this_node::getName(); // Immediately, setup a publisher for faults coming from this node - // Topic needs to be latched for initialization faults pub_heartbeat_ = nh_.advertise( - TOPIC_HEARTBEAT, heartbeat_queue_size_, true); + TOPIC_HEARTBEAT, heartbeat_queue_size_); pub_diagnostics_ = nh_.advertise( TOPIC_DIAGNOSTICS, 5); - // Setup a heartbeat timer for this node if auto start was requested - if (autostart_hb_timer_) { - // Don't autostart until nodelet finishes initialization function - timer_heartbeat_ = nh_.createTimer(ros::Rate(1.0), + // Setup a heartbeat timer regardless of if we use it or not + // Don't autostart until nodelet finishes initialization function + timer_heartbeat_ = nh_.createTimer(ros::Rate(1.0), &FreeFlyerNodelet::HeartbeatCallback, this, false, false); - } // Defer the initialization of the node to prevent a race condition with // nodelet registration. See this issue for more details: @@ -315,6 +312,10 @@ void FreeFlyerNodelet::InitCallback(ros::TimerEvent const& ev) { // was if (heartbeat_.faults.size() > 0) { PublishHeartbeat(); + // Start heartbeat timer to ensure the system monitor gets the + // initialization fault. Doesn't matter if the node doesn't want to publish + // a heartbeat as the system monitor will unload the nodelet. + timer_heartbeat_.start(); return; } diff --git a/simulation/src/gazebo_model_plugin_pmc/gazebo_model_plugin_pmc.cc b/simulation/src/gazebo_model_plugin_pmc/gazebo_model_plugin_pmc.cc index 0ba5654689..25fb0889ce 100644 --- a/simulation/src/gazebo_model_plugin_pmc/gazebo_model_plugin_pmc.cc +++ b/simulation/src/gazebo_model_plugin_pmc/gazebo_model_plugin_pmc.cc @@ -187,7 +187,7 @@ class GazeboModelPluginPmc : public FreeFlyerModelPlugin { // State publisher as a latched topic pub_state_ = nh->advertise( - TOPIC_HARDWARE_PMC_STATE, 1, true); + TOPIC_HARDWARE_PMC_STATE, 1); // Subscibe to PMC commands sub_command_ = nh->subscribe(TOPIC_HARDWARE_PMC_COMMAND, 5, diff --git a/tools/interactive_marker_teleop/src/interactive_marker_teleop.cpp b/tools/interactive_marker_teleop/src/interactive_marker_teleop.cpp index 4661d8a94b..9b2641282f 100644 --- a/tools/interactive_marker_teleop/src/interactive_marker_teleop.cpp +++ b/tools/interactive_marker_teleop/src/interactive_marker_teleop.cpp @@ -46,7 +46,7 @@ InteractiveMarkerTeleop::InteractiveMarkerTeleop(ros::NodeHandle& nh) : nh(nh) { snapMarkerToAstrobee(); // publish and acknowledge command actions - cmd_publisher_ = nh.advertise(TOPIC_COMMAND, 10, true); + cmd_publisher_ = nh.advertise(TOPIC_COMMAND, 10); // ack_subscriber_ = nh.subscribe(TOPIC_MANAGEMENT_ACK, 10, &AckCallback); // TODO(jdekarske) status goes in the // marker text } diff --git a/tools/localization_rviz_plugins/src/sparse_mapping_display.cc b/tools/localization_rviz_plugins/src/sparse_mapping_display.cc index 356e80d0f8..9faed117ca 100644 --- a/tools/localization_rviz_plugins/src/sparse_mapping_display.cc +++ b/tools/localization_rviz_plugins/src/sparse_mapping_display.cc @@ -46,7 +46,7 @@ SparseMappingDisplay::SparseMappingDisplay() { if (!config.GetStr("world_vision_map_filename", &map_file)) LogFatal("SparseMappingDisplay: Failed to load map file."); map_.reset(new sparse_mapping::SparseMap(map_file, true)); - map_cloud_publisher_ = nh_.advertise("sparse_mapping/map_cloud", 1, true); + map_cloud_publisher_ = nh_.advertise("sparse_mapping/map_cloud", 1); } void SparseMappingDisplay::onInitialize() { drawMap(); }