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 <[email protected]>
nasa · Feb 8, 2023 · a3ae212 · a3ae212
1 parent b75be9b
commit a3ae212
Show file tree

Hide file tree

Showing 20 changed files with 32 additions and 38 deletions.
diff --git 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<sensor_msgs::JointState>(
-      TOPIC_JOINT_GOALS, 1, true);
+      TOPIC_JOINT_GOALS, 1);
 
     // Subscribe to Proximal Joint Servo Enabling service
     client_enable_prox_servo_ = nh->serviceClient<ff_hw_msgs::SetEnabled>(

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
@@ -31,9 +31,7 @@ RapidGuestScienceDataToRos::RapidGuestScienceDataToRos(
                    "RapidGuestScienceDataToRos",
                    queue_size) {
   // advertise ros topic, make it latched
-  pub_ = nh_.advertise<ff_msgs::GuestScienceData>(publish_topic_,
-                                                  queue_size,
-                                                  true);
+  pub_ = nh_.advertise<ff_msgs::GuestScienceData>(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
@@ -30,7 +30,7 @@ RapidPositionToRos::RapidPositionToRos(const std::string& subscribe_topic,
                    "RapidPositionToRos",
                    queue_size) {
   // advertise ros topic, make it latched
-  pub_ = nh_.advertise<ff_msgs::EkfState>(publish_topic_, queue_size, true);
+  pub_ = nh_.advertise<ff_msgs::EkfState>(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
@@ -101,7 +101,7 @@ class PmcActuatorNodelet : public ff_util::FreeFlyerNodelet {
 
     // State publisher as a latched topic
     pub_state_ = nh->advertise<ff_hw_msgs::PmcState>(
-      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
@@ -61,7 +61,7 @@ void AccessControl::Initialize(ros::NodeHandle *nh) {
   failed_cmd_pub_ = nh->advertise<ff_msgs::CommandStamped>(
                                             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
@@ -46,13 +46,11 @@ void CpuMemMonitor::Initialize(ros::NodeHandle *nh) {
   // All state messages are latching
   cpu_state_pub_ = nh->advertise<ff_msgs::CpuStateStamped>(
                                             TOPIC_MANAGEMENT_CPU_MONITOR_STATE,
-                                            pub_queue_size_,
-                                            true);
+                                            pub_queue_size_);
   // All state messages are latching
   mem_state_pub_ = nh->advertise<ff_msgs::MemStateStamped>(
                                             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
@@ -561,7 +561,7 @@ void Recorder::doCheckMaster(ros::TimerEvent const& e, ros::NodeHandle& node_han
 }
 
 void Recorder::doTrigger() {
-    ros::Publisher pub = node_handle_.advertise<std_msgs::Empty>("snapshot_trigger", 1, true);
+    ros::Publisher pub = node_handle_.advertise<std_msgs::Empty>("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
@@ -62,7 +62,7 @@ void DiskMonitor::Initialize(ros::NodeHandle * nh) {
 
   // Setup the publisher
   pub_disk_stats_ = nh->advertise<ff_msgs::DiskStateStamped>(
-                    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
@@ -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<ff_msgs::CommandStamped>(
-                                                    TOPIC_COMMAND, 5, true);
+  command_pub = n.advertise<ff_msgs::CommandStamped>(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
@@ -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<ff_msgs::CommandStamped>(
-                                                    sub_topic_command, 5, true);
+  command_pub = n.advertise<ff_msgs::CommandStamped>(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
@@ -60,7 +60,7 @@ int main(int argc, char** argv) {
 
   // Make publisher to publish the command
   ros::Publisher cmd_publisher;
-  cmd_publisher = nh.advertise<ff_msgs::CommandStamped>(TOPIC_COMMAND, 10, true);
+  cmd_publisher = nh.advertise<ff_msgs::CommandStamped>(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
@@ -728,7 +728,7 @@ int main(int argc, char** argv) {
   tf2_ros::TransformListener tf_listener(tf_buffer);
 
   // Initialize publishers
-  cmd_pub = nh.advertise<ff_msgs::CommandStamped>(TOPIC_COMMAND, 10, true);
+  cmd_pub = nh.advertise<ff_msgs::CommandStamped>(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
@@ -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<ff_msgs::CommandStamped>(
-                                                    TOPIC_COMMAND, 5, true);
+  command_pub = n.advertise<ff_msgs::CommandStamped>(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
@@ -106,13 +106,13 @@ void MapperNodelet::Initialize(ros::NodeHandle *nh) {
   cam_frustum_pub_ = nh->advertise<visualization_msgs::Marker>(
     TOPIC_MAPPER_FRUSTRUM_MARKERS, 1);
   free_space_cloud_pub_ = nh->advertise<sensor_msgs::PointCloud2>(
-    TOPIC_MAPPER_OCTOMAP_FREE_CLOUD, 1, true);
+    TOPIC_MAPPER_OCTOMAP_FREE_CLOUD, 1);
   obstacle_cloud_pub_ = nh->advertise<sensor_msgs::PointCloud2>(
-    TOPIC_MAPPER_OCTOMAP_CLOUD, 1, true);
+    TOPIC_MAPPER_OCTOMAP_CLOUD, 1);
   inflated_free_space_cloud_pub_ = nh->advertise<sensor_msgs::PointCloud2>(
-    TOPIC_MAPPER_OCTOMAP_INFLATED_FREE_CLOUD, 1, true);
+    TOPIC_MAPPER_OCTOMAP_INFLATED_FREE_CLOUD, 1);
   inflated_obstacle_cloud_pub_ = nh->advertise<sensor_msgs::PointCloud2>(
-    TOPIC_MAPPER_OCTOMAP_INFLATED_CLOUD, 1, true);
+    TOPIC_MAPPER_OCTOMAP_INFLATED_CLOUD, 1);
   hazard_pub_ = nh->advertise<ff_msgs::Hazard>(
     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
@@ -102,7 +102,7 @@ class Planner : public planner::PlannerImplementation {
     nh_ = nh;
 
     viz_pub_ = nh->advertise<visualization_msgs::MarkerArray>(
-        "/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
@@ -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<traj_opt_msgs::Trajectory>(topic, 1, true);
+        instance().nh_.advertise<traj_opt_msgs::Trajectory>(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<traj_opt_msgs::SolverInfo>(topic, 1, true);
+        instance().nh_.advertise<traj_opt_msgs::SolverInfo>(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
@@ -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<ff_msgs::Heartbeat>(
-    TOPIC_HEARTBEAT, heartbeat_queue_size_, true);
+    TOPIC_HEARTBEAT, heartbeat_queue_size_);
   pub_diagnostics_ = nh_.advertise<diagnostic_msgs::DiagnosticArray>(
     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
@@ -187,7 +187,7 @@ class GazeboModelPluginPmc : public FreeFlyerModelPlugin {
 
     // State publisher as a latched topic
     pub_state_ = nh->advertise<ff_hw_msgs::PmcState>(
-      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
@@ -46,7 +46,7 @@ InteractiveMarkerTeleop::InteractiveMarkerTeleop(ros::NodeHandle& nh) : nh(nh) {
 
   snapMarkerToAstrobee();
   // publish and acknowledge command actions
-  cmd_publisher_ = nh.advertise<ff_msgs::CommandStamped>(TOPIC_COMMAND, 10, true);
+  cmd_publisher_ = nh.advertise<ff_msgs::CommandStamped>(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
@@ -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<sensor_msgs::PointCloud2>("sparse_mapping/map_cloud", 1, true);
+  map_cloud_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>("sparse_mapping/map_cloud", 1);
 }
 
 void SparseMappingDisplay::onInitialize() { drawMap(); }