Add clear message queue functionality

README.md

@@ -22,6 +22,7 @@ It models characteristics of vehicle to vehicle communication, such as delay and
   * **car2x_topic2** (_optional_): Topic for car2x message type 2
   * **car2x_topic3** (_optional_): Topic for car2x message type 3
 
+  * **clear_queue_topic** (_optional_): Topic to clear the message queue
 
 * parameters that can be dynamically reconfigured:
 

cfg/CommModule.rosif

@@ -49,6 +49,7 @@ gen.add_verbosity_param(configurable=True)
 gen.add("communication_internal_in_topic", paramtype='std::string', description="Topicname for subscriber", default="default_communication_internal_in_topic")
 gen.add("communication_internal_out_topic", paramtype='std::string', description="Topicname for publisher", default="default_communication_internal_out_topic")
 gen.add("communication_external_topic", paramtype='std::string', description="Topicname for publisher", default="default_communication_external_topic")
+gen.add("clear_queue_topic", paramtype='std::string', description="Topicname for subscriber", default="default_communication_clear_queue_topic")
 
 # network-simulation-related
 gen.add("time_delay", paramtype='double', description="Time delay in seconds", min=0, max=10, default=0, configurable=True, global_scope=False)

launch/sample_communication.launch

@@ -14,7 +14,7 @@
    <arg name="car2x_topic1" default="dummy_car2x_topic1"/>
    <arg name="car2x_topic2" default="dummy_car2x_topic2"/>
    <arg name="car2x_topic3" default="dummy_car2x_topic3"/>
-
+   <arg name="clear_queue_topic" default="default_communication_clear_queue_topic"/>
 
     <!-- LAUNCH NODES -->
     <!-- For Car2X-Topic 1 -->
@@ -23,6 +23,7 @@
             <param name="communication_external_topic" value="$(arg global_communication_ns)/$(arg car2x_topic1)" />
             <param name="communication_internal_in_topic" value="/$(arg vehicle_ns)/$(arg internal_communication_subns)/in/$(arg car2x_topic1)" />
             <param name="communication_internal_out_topic" value="/$(arg vehicle_ns)/$(arg internal_communication_subns)/out/$(arg car2x_topic1)" />
+            <param name="clear_queue_topic" value="$(arg clear_queue_topic)" />
 
             <!-- Vehicle-related -->
             <param name="vehicle_id" value="$(arg vehicle_id)" />
@@ -36,6 +37,7 @@
             <param name="communication_external_topic" value="$(arg global_communication_ns)/$(arg car2x_topic2)" />
             <param name="communication_internal_in_topic" value="/$(arg vehicle_ns)/$(arg internal_communication_subns)/in/$(arg car2x_topic2)" />
             <param name="communication_internal_out_topic" value="/$(arg vehicle_ns)/$(arg internal_communication_subns)/out/$(arg car2x_topic2)" />
+            <param name="clear_queue_topic" value="$(arg clear_queue_topic)" />
 
             <!-- Vehicle-related -->
             <param name="vehicle_id" value="$(arg vehicle_id)" />
@@ -49,6 +51,7 @@
             <param name="communication_external_topic" value="$(arg global_communication_ns)/$(arg car2x_topic3)" />
             <param name="communication_internal_in_topic" value="/$(arg vehicle_ns)/$(arg internal_communication_subns)/in/$(arg car2x_topic3)" />
             <param name="communication_internal_out_topic" value="/$(arg vehicle_ns)/$(arg internal_communication_subns)/out/$(arg car2x_topic3)" />
+            <param name="clear_queue_topic" value="$(arg clear_queue_topic)" />
 
             <!-- Vehicle-related -->
             <param name="vehicle_id" value="$(arg vehicle_id)" />

src/comm_module/comm_module.cpp

@@ -60,8 +60,20 @@ CommModule::CommModule(ros::NodeHandle node_handle, ros::NodeHandle private_node
                                                    params_.msg_queue_size,
                                                    boost::bind(&CommModule::subCallbackInEx, this, node_handle, _1));
 
-    pubExInTimer_ = node_handle.createTimer(ros::Duration(backupTimerDuration_), &CommModule::timerCallback, this);
-}
+        subClearQueue_ = node_handle.subscribe<std_msgs::Bool>(params_.clear_queue_topic,
+                                                               params_.msg_queue_size,
+                                                               &CommModule::subCallbackClearQueue, this);
+
+        pubExInTimer_ = node_handle.createTimer(ros::Duration(backupTimerDuration_), &CommModule::timerCallback, this);
+    }
+
+    void CommModule::subCallbackClearQueue(const std_msgs::Bool::ConstPtr &msg){
+        if(msg->data){
+            while(!messageBuffer_.empty()){
+                messageBuffer_.pop();
+            }
+        }
+    }
 
 
 void CommModule::subCallbackExIn(ros::NodeHandle& nh, const boost::shared_ptr<const ShapeShifter> msg) {

src/comm_module/comm_module.hpp

@@ -37,6 +37,7 @@
 #include <ros/console.h>
 #include <ros/ros.h>
 #include <topic_tools/shape_shifter.h>
+#include <std_msgs/Bool.h>
 
 #include "sim_sample_communication_ros_tool/CommModuleInterface.h"
 
@@ -58,6 +59,7 @@ class CommModule {
     ros::Publisher pubExIn_;
     ros::Subscriber subExIn_;
     ros::Subscriber subInEx_;
+    ros::Subscriber subClearQueue_;
     ros::Timer pubExInTimer_;
 
     dynamic_reconfigure::Server<CommModuleConfig> reconfigSrv_; // Dynamic reconfiguration service
@@ -70,6 +72,7 @@ class CommModule {
     bool pubInExInitialized_ = false;
     bool pubExInInitialized_ = false;
 
+    void subCallbackClearQueue(const std_msgs::Bool::ConstPtr &msg);
     void subCallbackExIn(ros::NodeHandle&, const boost::shared_ptr<const topic_tools::ShapeShifter>);
     void subCallbackInEx(ros::NodeHandle&, const boost::shared_ptr<const topic_tools::ShapeShifter>);
     void timerCallback(const ros::TimerEvent&);